Merge pull request #12 from iNavFlight/master

Merge from master

Author: MrD-RC

docs/Programming Framework.md

@@ -73,6 +73,7 @@ IPF can be edited using INAV Configurator user interface, of via CLI
 | 37            | MAP_OUTPUT         | Scales `Operand A` from [`0` : `1000`] to [`0` : `Operand B`]. Note: input will be constrained and then scaled |
 | 38            | RC_CHANNEL_OVERRIDE | Overrides channel set by `Operand A` to value of `Operand B` |
 | 39            | SET_HEADING_TARGET  | Sets heading-hold target to `Operand A`, in degrees. Value wraps-around. |
+| 40            | MOD           | Divide `Operand A` by `Operand B` and returns the remainder |
 
 
 ### Operands

docs/Settings.md

@@ -101,6 +101,7 @@
 | failsafe_throttle_low_delay | 100 | If failsafe activated when throttle is low for this much time - bypass failsafe and disarm, in 10th of seconds. 0 = No timeout |
 | fixed_wing_auto_arm | OFF | Auto-arm fixed wing aircraft on throttle above min_check, and disarming with stick commands are disabled, so power cycle is required to disarm. Requires enabled motorstop and no arm switch configured. |
 | flaperon_throw_offset | 200 | Defines throw range in us for both ailerons that will be passed to servo mixer via input source 14 (`FEATURE FLAPS`) when FLAPERON mode is activated. |
+| flip_over_after_crash_power_factor | 65 | flip over after crash power factor |
 | fpv_mix_degrees |  |  |
 | frsky_coordinates_format | 0 | D-Series telemetry only: FRSKY_FORMAT_DMS (default), FRSKY_FORMAT_NMEA |
 | frsky_default_latitude | 0.000 | D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. |

src/main/CMakeLists.txt

@@ -48,6 +48,8 @@ main_sources(COMMON_SRC
     common/typeconversion.h
     common/uvarint.c
     common/uvarint.h
+    common/circular_queue.c
+    common/circular_queue.h
 
     config/config_eeprom.c
     config/config_eeprom.h
@@ -154,6 +156,8 @@ main_sources(COMMON_SRC
     drivers/compass/compass_mpu9250.h
     drivers/compass/compass_qmc5883l.c
     drivers/compass/compass_qmc5883l.h
+    drivers/compass/compass_rm3100.c
+    drivers/compass/compass_rm3100.h
     drivers/compass/compass_msp.c
     drivers/compass/compass_msp.h
 

src/main/common/circular_queue.c

@@ -0,0 +1,66 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include "circular_queue.h"
+
+void circularBufferInit(circularBuffer_t *circular_buffer, uint8_t *buffer, size_t buffer_size,
+                          size_t buffer_element_size) {
+    circular_buffer->buffer = buffer;
+    circular_buffer->bufferSize = buffer_size;
+    circular_buffer->elementSize = buffer_element_size;
+    circular_buffer->head = 0;
+    circular_buffer->tail = 0;
+    circular_buffer->size = 0;
+}
+
+void circularBufferPushElement(circularBuffer_t *circularBuffer, uint8_t *element) {
+    if (circularBufferIsFull(circularBuffer))
+        return;
+
+    memcpy(circularBuffer->buffer + circularBuffer->tail, element, circularBuffer->elementSize);
+
+    circularBuffer->tail += circularBuffer->elementSize;
+    circularBuffer->tail %= circularBuffer->bufferSize;
+    circularBuffer->size += 1;
+}
+
+void circularBufferPopHead(circularBuffer_t *circularBuffer, uint8_t *element) {
+    memcpy(element, circularBuffer->buffer + circularBuffer->head, circularBuffer->elementSize);
+
+    circularBuffer->head += circularBuffer->elementSize;
+    circularBuffer->head %= circularBuffer->bufferSize;
+    circularBuffer->size -= 1;
+}
+
+int circularBufferIsFull(circularBuffer_t *circularBuffer) {
+    return circularBufferCountElements(circularBuffer) * circularBuffer->elementSize == circularBuffer->bufferSize;
+}
+
+int circularBufferIsEmpty(circularBuffer_t *circularBuffer) {
+    return circularBuffer==NULL || circularBufferCountElements(circularBuffer) == 0;
+}
+
+size_t circularBufferCountElements(circularBuffer_t *circularBuffer) {
+    return circularBuffer->size;
+}

src/main/common/circular_queue.h

@@ -0,0 +1,47 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#ifndef INAV_CIRCULAR_QUEUE_H
+#define INAV_CIRCULAR_QUEUE_H
+
+#include "stdint.h"
+#include "string.h"
+
+typedef struct circularBuffer_s{
+    size_t head;
+    size_t tail;
+    size_t bufferSize;
+    uint8_t * buffer;
+    size_t elementSize;
+    size_t size;
+}circularBuffer_t;
+
+void    circularBufferInit(circularBuffer_t * circularBuffer, uint8_t * buffer, size_t bufferSize, size_t bufferElementSize);
+void    circularBufferPushElement(circularBuffer_t * circularBuffer, uint8_t * element);
+void    circularBufferPopHead(circularBuffer_t * circularBuffer, uint8_t * element);
+int     circularBufferIsFull(circularBuffer_t * circularBuffer);
+int     circularBufferIsEmpty(circularBuffer_t *circularBuffer);
+size_t  circularBufferCountElements(circularBuffer_t * circularBuffer);
+
+#endif //INAV_CIRCULAR_QUEUE_H

src/main/common/maths.h

@@ -88,6 +88,8 @@
 #define _ABS_I(x, var) _ABS_II(x, var)
 #define ABS(x) _ABS_I(x, _CHOOSE_VAR(_abs, __COUNTER__))
 
+#define power3(x) ((x)*(x)*(x))
+
 // Floating point Euler angles.
 typedef struct fp_angles {
     float roll;

src/main/drivers/bus.h

@@ -118,6 +118,7 @@ typedef enum {
     DEVHW_QMC5883,
     DEVHW_MAG3110,
     DEVHW_LIS3MDL,
+    DEVHW_RM3100,
 
     /* Temp sensor chips */
     DEVHW_LM75_0,

src/main/drivers/compass/compass_rm3100.c

@@ -0,0 +1,179 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <math.h>
+
+#include "platform.h"
+
+#ifdef USE_MAG_RM3100
+
+#include "build/build_config.h"
+#include "build/debug.h"
+
+#include "common/axis.h"
+#include "common/maths.h"
+#include "common/utils.h"
+
+#include "drivers/time.h"
+#include "drivers/bus_i2c.h"
+
+#include "sensors/boardalignment.h"
+#include "sensors/sensors.h"
+
+#include "drivers/sensor.h"
+#include "drivers/compass/compass.h"
+
+#include "drivers/compass/compass_rm3100.h"
+
+#define RM3100_REG_POLL        0x00
+#define RM3100_REG_CMM         0x01
+#define RM3100_REG_CCX1        0x04
+#define RM3100_REG_CCX0        0x05
+#define RM3100_REG_CCY1        0x06
+#define RM3100_REG_CCY0        0x07
+#define RM3100_REG_CCZ1        0x08
+#define RM3100_REG_CCZ0        0x09
+#define RM3100_REG_TMRC        0x0B
+#define RM3100_REG_MX          0x24
+#define RM3100_REG_MY          0x27
+#define RM3100_REG_MZ          0x2A
+#define RM3100_REG_BIST        0x33
+#define RM3100_REG_STATUS      0x34
+#define RM3100_REG_HSHAKE      0x35
+#define RM3100_REG_REVID       0x36
+
+#define RM3100_REVID           0x22
+
+#define CCX_DEFAULT_MSB        0x00
+#define CCX_DEFAULT_LSB        0xC8
+#define CCY_DEFAULT_MSB        CCX_DEFAULT_MSB
+#define CCY_DEFAULT_LSB        CCX_DEFAULT_LSB
+#define CCZ_DEFAULT_MSB        CCX_DEFAULT_MSB
+#define CCZ_DEFAULT_LSB        CCX_DEFAULT_LSB
+#define CMM_DEFAULT            0x71    // Continuous mode
+#define TMRC_DEFAULT           0x94
+
+
+static bool deviceInit(magDev_t * mag)
+{
+    busWrite(mag->busDev, RM3100_REG_TMRC, TMRC_DEFAULT);
+
+    busWrite(mag->busDev, RM3100_REG_CMM, CMM_DEFAULT);
+
+    busWrite(mag->busDev, RM3100_REG_CCX1, CCX_DEFAULT_MSB);
+    busWrite(mag->busDev, RM3100_REG_CCX0, CCX_DEFAULT_LSB);
+
+    busWrite(mag->busDev, RM3100_REG_CCY1, CCY_DEFAULT_MSB);
+    busWrite(mag->busDev, RM3100_REG_CCY0, CCY_DEFAULT_LSB);
+
+    busWrite(mag->busDev, RM3100_REG_CCZ1, CCZ_DEFAULT_MSB);
+    busWrite(mag->busDev, RM3100_REG_CCZ0, CCZ_DEFAULT_LSB);
+
+    return true;
+}
+
+static bool deviceRead(magDev_t * mag)
+{
+    uint8_t status;
+
+#pragma pack(push, 1)
+    struct {
+        uint8_t x[3];
+        uint8_t y[3];
+        uint8_t z[3];
+    } rm_report;
+#pragma pack(pop)
+
+    mag->magADCRaw[X] = 0;
+    mag->magADCRaw[Y] = 0;
+    mag->magADCRaw[Z] = 0;
+
+    /* Check if new measurement is ready */
+    bool ack = busRead(mag->busDev, RM3100_REG_STATUS, &status);
+
+    if (!ack || (status & 0x80) == 0) {
+        return false;
+    }
+
+    ack = busReadBuf(mag->busDev, RM3100_REG_MX, (uint8_t *)&rm_report, sizeof(rm_report));
+    if (!ack) {
+        return false;
+    }
+
+    int32_t xraw;
+    int32_t yraw;
+    int32_t zraw;
+
+    /* Rearrange mag data */
+    xraw = ((rm_report.x[0] << 24) | (rm_report.x[1] << 16) | (rm_report.x[2]) << 8);
+    yraw = ((rm_report.y[0] << 24) | (rm_report.y[1] << 16) | (rm_report.y[2]) << 8);
+    zraw = ((rm_report.z[0] << 24) | (rm_report.z[1] << 16) | (rm_report.z[2]) << 8);
+
+    /* Truncate to 16-bit integers and pass along */
+    mag->magADCRaw[X] = (int16_t)(xraw >> 16);
+    mag->magADCRaw[Y] = (int16_t)(yraw >> 16);
+    mag->magADCRaw[Z] = (int16_t)(zraw >> 16);
+
+    return true;
+}
+
+#define DETECTION_MAX_RETRY_COUNT   5
+static bool deviceDetect(magDev_t * mag)
+{
+    for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
+        uint8_t revid = 0;
+        bool ack = busRead(mag->busDev, RM3100_REG_REVID, &revid);
+
+        if (ack && revid == RM3100_REVID) {
+            return true;
+        }
+    }
+
+    return false;
+}
+
+bool rm3100MagDetect(magDev_t * mag)
+{
+    busSetSpeed(mag->busDev, BUS_SPEED_STANDARD);
+
+    mag->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_RM3100, mag->magSensorToUse, OWNER_COMPASS);
+    if (mag->busDev == NULL) {
+        return false;
+    }
+
+    if (!deviceDetect(mag)) {
+        busDeviceDeInit(mag->busDev);
+        return false;
+    }
+
+    mag->init = deviceInit;
+    mag->read = deviceRead;
+
+    return true;
+}
+
+#endif

src/main/drivers/compass/compass_rm3100.h

@@ -0,0 +1,27 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+bool rm3100MagDetect(magDev_t *mag);