Merge remote-tracking branch 'origin/amorale/OP-1374_autoenable_pathfollowers' into rel-14.06

flight/libraries/inc/sanitycheck.h

@@ -32,6 +32,14 @@
 
 #include <systemalarms.h>
 
+typedef enum {
+    FRAME_TYPE_MULTIROTOR,
+    FRAME_TYPE_HELI,
+    FRAME_TYPE_FIXED_WING,
+    FRAME_TYPE_GROUND,
+    FRAME_TYPE_CUSTOM,
+} FrameType_t;
+
 #define SANITYCHECK_STATUS_ERROR_NONE         SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE
 #define SANITYCHECK_STATUS_ERROR_FLIGHTMODE   SYSTEMALARMS_EXTENDEDALARMSTATUS_FLIGHTMODE
 
@@ -45,4 +53,6 @@
 
 extern int32_t configuration_check();
 
+FrameType_t GetCurrentFrameType();
+
 #endif /* SANITYCHECK_H */

flight/libraries/sanitycheck.c

@@ -94,26 +94,8 @@ int32_t configuration_check()
 
 
     // Classify airframe type
-    bool multirotor;
+    bool multirotor = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR);
-    uint8_t airframe_type;
 
-    SystemSettingsAirframeTypeGet(&airframe_type);
-    switch (airframe_type) {
-    case SYSTEMSETTINGS_AIRFRAMETYPE_QUADX:
-    case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP:
-    case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA:
-    case SYSTEMSETTINGS_AIRFRAMETYPE_OCTO:
-    case SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX:
-    case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV:
-    case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP:
-    case SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX:
-    case SYSTEMSETTINGS_AIRFRAMETYPE_TRI:
-    case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX:
-        multirotor = true;
-        break;
-    default:
-        multirotor = false;
-    }
 
     // For each available flight mode position sanity check the available
     // modes
@@ -272,3 +254,42 @@ static bool check_stabilization_settings(int index, bool multirotor, bool copter
 
     return true;
 }
+
+FrameType_t GetCurrentFrameType()
+{
+    uint8_t airframe_type;
+
+    SystemSettingsAirframeTypeGet(&airframe_type);
+    switch ((SystemSettingsAirframeTypeOptions)airframe_type) {
+    case SYSTEMSETTINGS_AIRFRAMETYPE_QUADX:
+    case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP:
+    case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA:
+    case SYSTEMSETTINGS_AIRFRAMETYPE_OCTO:
+    case SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX:
+    case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV:
+    case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP:
+    case SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX:
+    case SYSTEMSETTINGS_AIRFRAMETYPE_TRI:
+    case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX:
+        return FRAME_TYPE_MULTIROTOR;
+
+    case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING:
+    case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON:
+    case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL:
+        return FRAME_TYPE_FIXED_WING;
+
+    case SYSTEMSETTINGS_AIRFRAMETYPE_HELICP:
+        return FRAME_TYPE_HELI;
+
+    case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLECAR:
+    case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEDIFFERENTIAL:
+    case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEMOTORCYCLE:
+        return FRAME_TYPE_GROUND;
+
+    case SYSTEMSETTINGS_AIRFRAMETYPE_VTOL:
+    case SYSTEMSETTINGS_AIRFRAMETYPE_CUSTOM:
+        return FRAME_TYPE_CUSTOM;
+    }
+    // anyway it should not reach here
+    return FRAME_TYPE_CUSTOM;
+}

flight/modules/FixedWingPathFollower/fixedwingpathfollower.c

@@ -62,6 +62,7 @@
 #include "velocitystate.h"
 #include "taskinfo.h"
 #include <pios_struct_helper.h>
+#include <sanitycheck.h>
 
 #include "sin_lookup.h"
 #include "paths.h"
@@ -112,7 +113,10 @@ int32_t FixedWingPathFollowerInitialize()
     HwSettingsInitialize();
     HwSettingsOptionalModulesData optionalModules;
     HwSettingsOptionalModulesGet(&optionalModules);
-    if (optionalModules.FixedWingPathFollower == HWSETTINGS_OPTIONALMODULES_ENABLED) {
+    FrameType_t frameType = GetCurrentFrameType();
+
+    if ((optionalModules.FixedWingPathFollower == HWSETTINGS_OPTIONALMODULES_ENABLED) ||
+        (frameType == FRAME_TYPE_FIXED_WING)) {
         followerEnabled = true;
         FixedWingPathFollowerSettingsInitialize();
         FixedWingPathFollowerStatusInitialize();

flight/modules/StateEstimation/filtercf.c

@@ -234,7 +234,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float
         float magBias[3];
         RevoCalibrationmag_biasArrayGet(magBias);
         // don't trust Mag for initial orientation if it has not been calibrated
-        if (magBias[0] < 1e-6f && magBias[1] < 1e-6f && magBias[2] < 1e-6f) {
+        if (fabsf(magBias[0]) < 1e-6f && fabsf(magBias[1]) < 1e-6f && fabsf(magBias[2]) < 1e-6f) {
             this->magCalibrated = false;
             mag[0] = 100.0f;
             mag[1] = 0.0f;

flight/modules/System/systemmod.c

@@ -96,11 +96,12 @@ static xQueueHandle objectPersistenceQueue;
 static enum { STACKOVERFLOW_NONE = 0, STACKOVERFLOW_WARNING = 1, STACKOVERFLOW_CRITICAL = 3 } stackOverflow;
 static bool mallocFailed;
 static HwSettingsData bootHwSettings;
+static FrameType_t bootFrameType;
 static struct PIOS_FLASHFS_Stats fsStats;
 
 // Private functions
 static void objectUpdatedCb(UAVObjEvent *ev);
-static void hwSettingsUpdatedCb(UAVObjEvent *ev);
+static void checkSettingsUpdatedCb(UAVObjEvent *ev);
 #ifdef DIAG_TASKS
 static void taskMonitorForEachCallback(uint16_t task_id, const struct pios_task_info *task_info, void *context);
 static void callbackSchedulerForEachCallback(int16_t callback_id, const struct pios_callback_info *callback_info, void *context);
@@ -195,8 +196,10 @@ static void systemTask(__attribute__((unused)) void *parameters)
 
     // Load a copy of HwSetting active at boot time
     HwSettingsGet(&bootHwSettings);
+    bootFrameType = GetCurrentFrameType();
     // Whenever the configuration changes, make sure it is safe to fly
-    HwSettingsConnectCallback(hwSettingsUpdatedCb);
+    HwSettingsConnectCallback(checkSettingsUpdatedCb);
+    SystemSettingsConnectCallback(checkSettingsUpdatedCb);
 
 #ifdef DIAG_TASKS
     TaskInfoData taskInfoData;
@@ -396,13 +399,15 @@ static void objectUpdatedCb(UAVObjEvent *ev)
 /**
  * Called whenever hardware settings changed
  */
-static void hwSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
+static void checkSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
     HwSettingsData currentHwSettings;
 
     HwSettingsGet(&currentHwSettings);
+    FrameType_t currentFrameType = GetCurrentFrameType();
     // check whether the Hw Configuration has changed from the one used at boot time
-    if (memcmp(&bootHwSettings, &currentHwSettings, sizeof(HwSettingsData)) != 0) {
+    if ((memcmp(&bootHwSettings, &currentHwSettings, sizeof(HwSettingsData)) != 0) ||
+        (currentFrameType != bootFrameType)) {
         ExtendedAlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL, SYSTEMALARMS_EXTENDEDALARMSTATUS_REBOOTREQUIRED, 0);
     }
 }

flight/modules/VtolPathFollower/vtolpathfollower.c

@@ -73,6 +73,7 @@
 
 #include "paths.h"
 #include "CoordinateConversions.h"
+#include <sanitycheck.h>
 
 #include "cameradesired.h"
 #include "poilearnsettings.h"
@@ -128,8 +129,10 @@ int32_t VtolPathFollowerInitialize()
     HwSettingsOptionalModulesData optionalModules;
 
     HwSettingsOptionalModulesGet(&optionalModules);
+    FrameType_t frameType = GetCurrentFrameType();
 
-    if (optionalModules.VtolPathFollower == HWSETTINGS_OPTIONALMODULES_ENABLED) {
+    if ((optionalModules.VtolPathFollower == HWSETTINGS_OPTIONALMODULES_ENABLED) ||
+        (frameType == FRAME_TYPE_MULTIROTOR)) {
         VtolPathFollowerSettingsInitialize();
         NedAccelInitialize();
         PathDesiredInitialize();

shared/uavobjectdefinition/attitudesettings.xml

@@ -4,8 +4,8 @@
         <field name="BoardRotation" units="deg" type="float" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0"/>
         <field name="AccelKp" units="channel" type="float" elements="1" defaultvalue="0.05"/>
         <field name="AccelKi" units="channel" type="float" elements="1" defaultvalue="0.0001"/>
-        <field name="MagKi" units="" type="float" elements="1" defaultvalue="0"/>
+        <field name="MagKi" units="" type="float" elements="1" defaultvalue="0.000001"/>
-        <field name="MagKp" units="" type="float" elements="1" defaultvalue="0"/>
+        <field name="MagKp" units="" type="float" elements="1" defaultvalue="0.01"/>
         <field name="AccelTau" units="" type="float" elements="1" defaultvalue="0"/>
         <field name="YawBiasRate" units="channel" type="float" elements="1" defaultvalue="0.000001"/>
         <field name="ZeroDuringArming" units="channel" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>