rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

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

Browse Source
This commit is contained in:
Alessio Morale 2014-06-30 23:13:43 +02:00
commit 00ce8dadb3
7 changed files with 71 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
flight/libraries/inc/sanitycheck.h
View File

@ -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 */

59
flight/libraries/sanitycheck.c
View File

@ -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;
}

6
flight/modules/FixedWingPathFollower/fixedwingpathfollower.c
View File

@ -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();

2
flight/modules/StateEstimation/filtercf.c
View File

@ -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;

13
flight/modules/System/systemmod.c
View File

@ -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);
}
}

5
flight/modules/VtolPathFollower/vtolpathfollower.c
View File

@ -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();

4
shared/uavobjectdefinition/attitudesettings.xml
View File

@ -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="MagKp" 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.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"/>