rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

LP-443 - Handle arming

Browse Source
This commit is contained in:
Alessio Morale 2016-07-18 01:18:06 +02:00
parent ce87ce105e
commit 4f045bc73a
3 changed files with 7 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
flight/modules/StateEstimation/filterekf.c
View File

@ -202,6 +202,7 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
float dT; float dT;
uint16_t sensors = 0; uint16_t sensors = 0;
INSSetArmed(state->armed);
this->work.updated |= state->updated; this->work.updated |= state->updated;
// check magnetometer alarm, discard any magnetometer readings if not OK // check magnetometer alarm, discard any magnetometer readings if not OK

3
flight/modules/StateEstimation/inc/stateestimation.h
View File

@ -70,8 +70,9 @@ typedef struct {
float airspeed[2]; float airspeed[2];
float baro[1]; float baro[1];
float auxMag[3]; float auxMag[3];
uint8_t magStatus;
float boardMag[3]; float boardMag[3];
uint8_t magStatus;
bool armed;
sensorUpdates updated; sensorUpdates updated;
} stateEstimation; } stateEstimation;

8
flight/modules/StateEstimation/stateestimation.c
View File

@ -355,7 +355,6 @@ static void StateEstimationCb(void)
static stateEstimation states; static stateEstimation states;
static uint32_t last_time; static uint32_t last_time;
static uint16_t bootDelay = 64; static uint16_t bootDelay = 64;
// after system startup, first few sensor readings might be messed up, delay until everything has settled // after system startup, first few sensor readings might be messed up, delay until everything has settled
if (bootDelay) { if (bootDelay) {
bootDelay--; bootDelay--;
@ -373,12 +372,13 @@ static void StateEstimationCb(void)
} else { } else {
last_time = PIOS_DELAY_GetRaw(); last_time = PIOS_DELAY_GetRaw();
} }
FlightStatusArmedOptions fsarmed;
FlightStatusArmedGet(&fsarmed);
states.armed = fsarmed != FLIGHTSTATUS_ARMED_DISARMED;
// check if a new filter chain should be initialized // check if a new filter chain should be initialized
if (fusionAlgorithm != revoSettings.FusionAlgorithm) { if (fusionAlgorithm != revoSettings.FusionAlgorithm) {
FlightStatusData fs; if (fsarmed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == FILTER_INIT_FORCE) {
FlightStatusGet(&fs);
if (fs.Armed == FLIGHTSTATUS_ARMED_DISARMED || fusionAlgorithm == FILTER_INIT_FORCE) {
const filterPipeline *newFilterChain; const filterPipeline *newFilterChain;
switch ((RevoSettingsFusionAlgorithmOptions)revoSettings.FusionAlgorithm) { switch ((RevoSettingsFusionAlgorithmOptions)revoSettings.FusionAlgorithm) {
case REVOSETTINGS_FUSIONALGORITHM_BASICCOMPLEMENTARY: case REVOSETTINGS_FUSIONALGORITHM_BASICCOMPLEMENTARY: