Merge branch 'master' into abo_RTH_spiral_climb

.github/workflows/docs.yml

@@ -1,5 +1,9 @@
 name: Make sure docs are updated
 on:
+  pull_request:
+    paths:
+      - src/main/fc/settings.yaml
+      - docs/Settings.md
   push:
     paths:
       - src/main/fc/settings.yaml

docs/Safehomes.md

@@ -53,15 +53,17 @@ Parameters:
   * `<lat>` - Latitude (WGS84), in degrees * 1E7 (for example 123456789 means 12.3456789).
   * `<lon>` - Longitude.
 
+Note that coordinates from Google Maps only have five or six decimals, so you need to pad zero decimals until you have seven before removing the decimal period to set the correct safehome location. For example, coordinates 54.353319 -4.517927 obtained from Google Maps need to be entered as 543533190 -45179270, coordiniates 43.54648 -7.86545 as 435464800 -78654500 and 51.309842 -0.095651 as 513098420 -00956510.
+
 Safehomes are saved along with your regular settings and will appear in `diff` and `dump` output.  Use `save` to save any changes, as with other settings. 
 
 ### `safehome` example
 
 ```
 # safehome
-safehome 0 1 543533193 -45179273
-safehome 1 1 435464846 -78654544
-safehome 2 0 0 0 
+safehome 0 1 543533190 -45179270
+safehome 1 1 435464800 -78654500
+safehome 2 1 513098420 -00956510
 safehome 3 0 0 0 
 safehome 4 0 0 0 
 safehome 5 0 0 0 

docs/development/Cmake usage.md

@@ -67,3 +67,7 @@ target_stm32f405xg(OMNIBUSF4V3_S6_SS)
 # except for an inverter on UART6.
 target_stm32f405xg(OMNIBUSF4V3)
 ```
+
+## Adding (or removing) a source file
+
+In the cmake system, project source files are listed in `src/main/CMakeLists.txt`. New source files must be added to this list to be considered by the build system.

src/main/config/config_eeprom.c

@@ -118,8 +118,13 @@ bool isEEPROMContentValid(void)
             // Found the end.  Stop scanning.
             break;
         }
-        if (p + record->size >= &__config_end
-            || record->size < sizeof(*record)) {
+
+        if (p + sizeof(*record) >= &__config_end) {
+            // Too big. Further checking for size doesn't make sense
+            return false;
+        }
+
+        if (p + record->size >= &__config_end || record->size < sizeof(*record)) {
             // Too big or too small.
             return false;
         }
@@ -152,13 +157,21 @@ static const configRecord_t *findEEPROM(const pgRegistry_t *reg, configRecordFla
     p += sizeof(configHeader_t);             // skip header
     while (true) {
         const configRecord_t *record = (const configRecord_t *)p;
-        if (record->size == 0
-            || p + record->size >= &__config_end
-            || record->size < sizeof(*record))
+        // Ensure that the record header fits into config memory, otherwise accessing size and flags may cause a hardfault.
+        if (p + sizeof(*record) >= &__config_end) {
             break;
-        if (pgN(reg) == record->pgn
-            && (record->flags & CR_CLASSIFICATION_MASK) == classification)
+        }
+
+        // Check that record header makes sense
+        if (record->size == 0 || p + record->size >= &__config_end || record->size < sizeof(*record)) {
+            break;
+        }
+
+        // Check if this is the record we're looking for (check for size)
+        if (pgN(reg) == record->pgn && (record->flags & CR_CLASSIFICATION_MASK) == classification) {
             return record;
+        }
+
         p += record->size;
     }
     // record not found

src/main/fc/cli.c

@@ -1457,7 +1457,7 @@ static void cliWaypoints(char *cmdline)
 
             if (!(validArgumentCount == 6 || validArgumentCount == 8)) {
                 cliShowParseError();
-            } else if (!(action == 0 || action == NAV_WP_ACTION_WAYPOINT || action == NAV_WP_ACTION_RTH || action == NAV_WP_ACTION_JUMP || action == NAV_WP_ACTION_HOLD_TIME || action == NAV_WP_ACTION_LAND) || (p1 < 0) || !(flag == 0 || flag == NAV_WP_FLAG_LAST)) {
+            } else if (!(action == 0 || action == NAV_WP_ACTION_WAYPOINT || action == NAV_WP_ACTION_RTH || action == NAV_WP_ACTION_JUMP || action == NAV_WP_ACTION_HOLD_TIME || action == NAV_WP_ACTION_LAND || action == NAV_WP_ACTION_SET_POI || action == NAV_WP_ACTION_SET_HEAD) || !(flag == 0 || flag == NAV_WP_FLAG_LAST)) {
                 cliShowParseError();
             } else {
                 posControl.waypointList[i].action = action;

src/main/fc/fc_init.c

@@ -218,7 +218,7 @@ void init(void)
 #endif
 
     initEEPROM();
-    //ensureEEPROMContainsValidData();
+    ensureEEPROMContainsValidData();
     readEEPROM();
 
     // Re-initialize system clock to their final values (if necessary)

src/main/fc/fc_msp.c

@@ -1515,6 +1515,20 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
     return true;
 }
 
+static mspResult_e mspFcSafeHomeOutCommand(sbuf_t *dst, sbuf_t *src)
+{
+    const uint8_t safe_home_no = sbufReadU8(src);    // get the home number
+    if(safe_home_no < MAX_SAFE_HOMES) {
+        sbufWriteU8(dst, safe_home_no);
+        sbufWriteU8(dst, safeHomeConfig(safe_home_no)->enabled);
+        sbufWriteU32(dst, safeHomeConfig(safe_home_no)->lat);
+        sbufWriteU32(dst, safeHomeConfig(safe_home_no)->lon);
+        return MSP_RESULT_ACK;
+    } else {
+         return MSP_RESULT_ERROR;
+    }
+}
+
 #ifdef USE_NAV
 static void mspFcWaypointOutCommand(sbuf_t *dst, sbuf_t *src)
 {
@@ -2892,6 +2906,19 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
         return MSP_RESULT_ERROR; // will only be reached if the rollback is not ready
         break;
 #endif
+    case MSP2_INAV_SET_SAFEHOME:
+        if (dataSize == 10) {
+             uint8_t i;
+             if (!sbufReadU8Safe(&i, src) || i >= MAX_SAFE_HOMES) {
+                 return MSP_RESULT_ERROR;
+             }
+             safeHomeConfigMutable(i)->enabled = sbufReadU8(src);
+             safeHomeConfigMutable(i)->lat = sbufReadU32(src);
+             safeHomeConfigMutable(i)->lon = sbufReadU32(src);
+        } else {
+            return MSP_RESULT_ERROR;
+        }
+        break;
 
     default:
         return MSP_RESULT_ERROR;
@@ -3195,6 +3222,10 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
         break;
 #endif
 
+    case MSP2_INAV_SAFEHOME:
+         *ret = mspFcSafeHomeOutCommand(dst, src);
+         break;
+
     default:
         // Not handled
         return false;

src/main/fc/settings.yaml

@@ -2107,7 +2107,7 @@ groups:
         min: 0
         max: 5000
       - name: nav_overrides_motor_stop
-        description: "When set OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF always the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV"
+        description: "When set to OFF the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV"
         default_value: "ALL_NAV"
         field: general.flags.nav_overrides_motor_stop
         table: nav_overrides_motor_stop

src/main/io/gps_ublox.c

@@ -21,6 +21,7 @@
 #include <ctype.h>
 #include <string.h>
 #include <math.h>
+#include <stdarg.h>
 
 #include "platform.h"
 #include "build/build_config.h"
@@ -67,8 +68,18 @@
 #define UBX_VALID_GPS_DATE_TIME(valid) (UBX_VALID_GPS_DATE(valid) && UBX_VALID_GPS_TIME(valid))
 
 // SBAS_AUTO, SBAS_EGNOS, SBAS_WAAS, SBAS_MSAS, SBAS_GAGAN, SBAS_NONE
+// note PRNs last upadted 2020-12-18
+
+#define SBASMASK1_BASE 120
+#define SBASMASK1_BITS(prn) (1 << (prn-SBASMASK1_BASE))
+
 static const uint32_t ubloxScanMode1[] = {
-    0x00000000, 0x00000851, 0x0004E004, 0x00020200, 0x00000180, 0x00000000,
+    0x00000000, // AUTO
+    (SBASMASK1_BITS(123) | SBASMASK1_BITS(126) | SBASMASK1_BITS(136)), // SBAS
+    (SBASMASK1_BITS(131) | SBASMASK1_BITS(133) | SBASMASK1_BITS(138)), // WAAS
+    (SBASMASK1_BITS(129) | SBASMASK1_BITS(137)), // MAAS
+    (SBASMASK1_BITS(127) | SBASMASK1_BITS(128)), // GAGAN
+    0x00000000, // NONE
 };
 
 static const char * baudInitDataNMEA[GPS_BAUDRATE_COUNT] = {
@@ -101,11 +112,35 @@ typedef struct {
     uint16_t time;
 } ubx_rate;
 
+typedef struct {
+     uint8_t gnssId;
+     uint8_t resTrkCh;
+     uint8_t maxTrkCh;
+     uint8_t reserved1;
+// flags
+     uint8_t enabled;
+     uint8_t undefined0;
+     uint8_t sigCfgMask;
+     uint8_t undefined1;
+} ubx_gnss_element_t;
+
+typedef struct {
+     uint8_t msgVer;
+     uint8_t numTrkChHw;
+     uint8_t numTrkChUse;
+     uint8_t numConfigBlocks;
+     ubx_gnss_element_t config[0];
+} ubx_gnss_msg_t;
+
+#define MAX_GNSS 7
+#define MAX_GNSS_SIZE_BYTES (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)*MAX_GNSS)
+
 typedef union {
-    uint8_t bytes[60]; // sizeof Galileo config
+    uint8_t bytes[MAX_GNSS_SIZE_BYTES]; // placeholder
     ubx_sbas sbas;
     ubx_msg msg;
     ubx_rate rate;
+    ubx_gnss_msg_t gnss;
 } ubx_payload;
 
 // UBX support
@@ -409,25 +444,63 @@ static const uint8_t default_payload[] = {
     0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
 };
 
-// Note the organisation of the bytes reflects the structure of the payload
-// 4 bytes then 8*number of elements (7)
-static const uint8_t galileo_payload[] =  {
-    0x00, 0x00, 0x20, 0x07,			    // GNSS    / min / max / enable
-    0x00, 0x08, 0x10, 0x00, 0x01, 0x00, 0x01, 0x01, // GPS     / 8 / 16 / Y
-    0x01, 0x01, 0x03, 0x00, 0x01, 0x00, 0x01, 0x01, // SBAS    / 1 /  3 / Y
-    0x02, 0x04, 0x08, 0x00, 0x01, 0x00, 0x01, 0x01, // Galileo / 4 /  8 / Y
-    0x03, 0x08, 0x10, 0x00, 0x00, 0x00, 0x01, 0x01, // BeiDou  / 8 / 16 / N
-    0x04, 0x00, 0x08, 0x00, 0x00, 0x00, 0x01, 0x01, // IMES    / 0 /  8 / N
-    0x05, 0x00, 0x03, 0x00, 0x00, 0x00, 0x01, 0x01, // QZSS    / 0 /  3 / N
-    0x06, 0x08, 0x0e, 0x00, 0x01, 0x00, 0x01, 0x01  // GLONASS / 8 / 14 / Y
-};
+#define GNSSID_SBAS 1
+#define GNSSID_GALILEO 2
 
-static void configureGalileo(void)
+static int configureGNSS_SBAS(ubx_gnss_element_t * gnss_block)
 {
+    gnss_block->gnssId = GNSSID_SBAS;
+    gnss_block->maxTrkCh = 3;
+    gnss_block->sigCfgMask = 1;
+    if (gpsState.gpsConfig->sbasMode == SBAS_NONE) {
+         gnss_block->enabled = 0;
+         gnss_block->resTrkCh = 0;
+    } else {
+         gnss_block->enabled = 1;
+         gnss_block->resTrkCh = 1;
+    }
+
+    return 1;
+}
+
+static int configureGNSS_GALILEO(ubx_gnss_element_t * gnss_block)
+{
+    if (!capGalileo) {
+        return 0;
+    }
+
+    gnss_block->gnssId = GNSSID_GALILEO;
+    gnss_block->maxTrkCh = 8;
+    gnss_block->sigCfgMask = 1;
+    if (gpsState.gpsConfig->ubloxUseGalileo) {
+        gnss_block->enabled = 1;
+        gnss_block->resTrkCh = 4;
+    } else {
+        gnss_block->enabled = 0;
+        gnss_block->resTrkCh = 0;
+    }
+
+    return 1;
+}
+
+static void configureGNSS(void)
+{
+    int blocksUsed = 0;
+
     send_buffer.message.header.msg_class = CLASS_CFG;
     send_buffer.message.header.msg_id = MSG_CFG_GNSS;
-    send_buffer.message.header.length = sizeof(galileo_payload);
-    memcpy(send_buffer.message.payload.bytes, galileo_payload, sizeof(galileo_payload));
+    send_buffer.message.payload.gnss.msgVer = 0;
+    send_buffer.message.payload.gnss.numTrkChHw = 0; // read only, so unset
+    send_buffer.message.payload.gnss.numTrkChUse = 32;
+
+    /* SBAS, always generated */
+    blocksUsed += configureGNSS_SBAS(&send_buffer.message.payload.gnss.config[blocksUsed]);
+
+    /* Galileo */
+    blocksUsed += configureGNSS_GALILEO(&send_buffer.message.payload.gnss.config[blocksUsed]);
+
+    send_buffer.message.payload.gnss.numConfigBlocks = blocksUsed;
+    send_buffer.message.header.length = (sizeof(ubx_gnss_msg_t) + sizeof(ubx_gnss_element_t)* blocksUsed);
     sendConfigMessageUBLOX();
 }
 
@@ -840,12 +913,10 @@ STATIC_PROTOTHREAD(gpsConfigure)
     configureSBAS();
     ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);
 
-    // Enable GALILEO
-    if (gpsState.gpsConfig->ubloxUseGalileo && capGalileo) {
-        // If GALILEO is not supported by the hardware we'll get a NAK,
-        // however GPS would otherwise be perfectly initialized, so we are just waiting for any response
+    // Configure GNSS for M8N and later
+    if (gpsState.hwVersion >= 80000) {
          gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT);
-        configureGalileo();
+         configureGNSS();
          ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS);
     }
 

src/main/io/osd.c

@@ -2067,6 +2067,13 @@ static bool osdDrawSingleElement(uint8_t item)
             break;
         }
 
+    case OSD_VERSION:
+        {
+            tfp_sprintf(buff, "INAV %s", FC_VERSION_STRING);
+            displayWrite(osdDisplayPort, elemPosX, elemPosY, buff);
+            break;
+        }
+
     case OSD_MAIN_BATT_CELL_VOLTAGE:
         {
             osdDisplayBatteryVoltage(elemPosX, elemPosY, getBatteryRawAverageCellVoltage(), 3, 2);
@@ -3030,10 +3037,11 @@ static void osdShowArmed(void)
     dateTime_t dt;
     char buf[MAX(32, FORMATTED_DATE_TIME_BUFSIZE)];
     char craftNameBuf[MAX_NAME_LENGTH];
+    char versionBuf[30];
     char *date;
     char *time;
-    // We need 10 visible rows
-    uint8_t y = MIN((osdDisplayPort->rows / 2) - 1, osdDisplayPort->rows - 10 - 1);
+    // We need 12 visible rows
+    uint8_t y = MIN((osdDisplayPort->rows / 2) - 1, osdDisplayPort->rows - 12 - 1);
 
     displayClearScreen(osdDisplayPort);
     displayWrite(osdDisplayPort, 12, y, "ARMED");
@@ -3080,7 +3088,11 @@ static void osdShowArmed(void)
 
         displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(date)) / 2, y, date);
         displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(time)) / 2, y + 1, time);
+        y += 3;
     }
+
+    tfp_sprintf(versionBuf, "INAV VERSION: %s", FC_VERSION_STRING);
+    displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(versionBuf)) / 2, y, versionBuf);
 }
 
 static void osdFilterData(timeUs_t currentTimeUs) {

src/main/io/osd.h

@@ -218,6 +218,7 @@ typedef enum {
     OSD_GVAR_3,
     OSD_TPA,
     OSD_NAV_FW_CONTROL_SMOOTHNESS,
+    OSD_VERSION,
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;
 

src/main/msp/msp_protocol_v2_inav.h

@@ -72,3 +72,6 @@
 #define MSP2_INAV_FWUPDT_EXEC                   0x2035
 #define MSP2_INAV_FWUPDT_ROLLBACK_PREPARE       0x2036
 #define MSP2_INAV_FWUPDT_ROLLBACK_EXEC          0x2037
+
+#define MSP2_INAV_SAFEHOME                      0x2038
+#define MSP2_INAV_SET_SAFEHOME                  0x2039

src/main/navigation/navigation.c

@@ -158,8 +158,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .max_throttle = 1700,
         .min_throttle = 1200,
         .pitch_to_throttle = 10,                // pwm units per degree of pitch (10pwm units ~ 1% throttle)
-        .pitch_to_throttle_smooth = 0,
-        .pitch_to_throttle_thresh = 0,
+        .pitch_to_throttle_smooth = 6,
+        .pitch_to_throttle_thresh = 50,
         .loiter_radius = 5000,                  // 50m
 
         //Fixed wing landing

src/main/sensors/gyro.c

@@ -104,7 +104,7 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState;
 PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 10);
 
 PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
-    .gyro_lpf = GYRO_LPF_42HZ,      // 42HZ value is defined for Invensense/TDK gyros
+    .gyro_lpf = GYRO_LPF_256HZ,
     .gyro_soft_lpf_hz = 60,
     .gyro_soft_lpf_type = FILTER_BIQUAD,
     .gyro_align = ALIGN_DEFAULT,

src/main/target/FF_F35_LIGHTNING/target.h

@@ -27,6 +27,8 @@
 #define BEEPER                  PA1
 #define BEEPER_INVERTED
 
+#define USE_DSHOT
+
 // MPU interrupt
 #define USE_EXTI
 #define GYRO_INT_EXTI            PC4

src/main/target/KAKUTEF4/target.h

@@ -70,6 +70,9 @@
 #   define BARO_I2C_BUS            BUS_I2C1
 #   define USE_BARO_MS5611
 #   define USE_BARO_BMP280
+#else // V1 does not have I2C exposed, common_post.h will pull in USE_*_MSP
+#   define USE_BARO
+#   define USE_MAG
 #endif
 
 #define USE_OSD