From fdbe58e8e6f6ecbf4ff44381b75d88699a610eae Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 29 Jun 2014 16:06:54 +0200 Subject: [PATCH 1/5] OP-1375 Update Mag Ki and Kp default settings --- shared/uavobjectdefinition/attitudesettings.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/shared/uavobjectdefinition/attitudesettings.xml b/shared/uavobjectdefinition/attitudesettings.xml index 208fd49ba..1c1ad6b8d 100644 --- a/shared/uavobjectdefinition/attitudesettings.xml +++ b/shared/uavobjectdefinition/attitudesettings.xml @@ -4,8 +4,8 @@ - - + + From 52a909185b6cf209372d9f44e9de4e6ced53e724 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 29 Jun 2014 17:22:41 +0200 Subject: [PATCH 2/5] OP-1374 - Add sanitycheck API to detect frame type --- flight/libraries/inc/sanitycheck.h | 10 +++++ flight/libraries/sanitycheck.c | 59 ++++++++++++++++++++---------- 2 files changed, 50 insertions(+), 19 deletions(-) diff --git a/flight/libraries/inc/sanitycheck.h b/flight/libraries/inc/sanitycheck.h index f324486ae..9576d1030 100644 --- a/flight/libraries/inc/sanitycheck.h +++ b/flight/libraries/inc/sanitycheck.h @@ -32,6 +32,14 @@ #include +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 */ diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index cda3b443a..83e498447 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -94,26 +94,8 @@ int32_t configuration_check() // Classify airframe type - bool multirotor; - uint8_t airframe_type; + bool multirotor = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR); - 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; +} From 3ae85f6434bff4a4efa2c717e14e2af9086e9ff1 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 29 Jun 2014 17:23:15 +0200 Subject: [PATCH 3/5] OP-1374 - Issue a Boot Alarm (reboot) if frame type is changed --- flight/modules/System/systemmod.c | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/flight/modules/System/systemmod.c b/flight/modules/System/systemmod.c index 8595642dc..aa6a8f54d 100644 --- a/flight/modules/System/systemmod.c +++ b/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(¤tHwSettings); + FrameType_t currentFrameType = GetCurrentFrameType(); // check whether the Hw Configuration has changed from the one used at boot time - if (memcmp(&bootHwSettings, ¤tHwSettings, sizeof(HwSettingsData)) != 0) { + if ((memcmp(&bootHwSettings, ¤tHwSettings, sizeof(HwSettingsData)) != 0) || + (currentFrameType != bootFrameType)) { ExtendedAlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL, SYSTEMALARMS_EXTENDEDALARMSTATUS_REBOOTREQUIRED, 0); } } From 30491c799238ef9b61d52dbdb1f59ec7984ee02c Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 29 Jun 2014 17:23:57 +0200 Subject: [PATCH 4/5] OP-1374 - Autostart the relevant PathFollower for known frame types --- .../modules/FixedWingPathFollower/fixedwingpathfollower.c | 6 +++++- flight/modules/VtolPathFollower/vtolpathfollower.c | 5 ++++- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c index 5cdbe7c9e..556a10db8 100644 --- a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c +++ b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -62,6 +62,7 @@ #include "velocitystate.h" #include "taskinfo.h" #include +#include #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(); diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c index 259d81fe4..cc09e0b6b 100644 --- a/flight/modules/VtolPathFollower/vtolpathfollower.c +++ b/flight/modules/VtolPathFollower/vtolpathfollower.c @@ -73,6 +73,7 @@ #include "paths.h" #include "CoordinateConversions.h" +#include #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(); From 80d3d7c9540fbe69d34d0176572dc8c18e4e7c1c Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 29 Jun 2014 18:36:46 +0200 Subject: [PATCH 5/5] OP-1374 hotfix to filtercf on checking of magnetometer calibration fabsf() --- flight/modules/StateEstimation/filtercf.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index 807618004..15b61a665 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/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;