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

Added alarm check when trying to change state to ARMED mode (from DISAMRED mode), if any warnings are set then it will not go into ARMED mode - unless the GPS is not required, in which case the GPS alarm will be ignored (if using Simple or Indoor AHRS modes). These checks are done in the okToArm() function.

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2820 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
pip 2011-02-20 16:42:31 +00:00 committed by pip
parent 4c03220268
commit df26016c64
1 changed files with 62 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
flight/Modules/ManualControl/manualcontrol.c
View File

@ -42,6 +42,8 @@
#include "attitudedesired.h"
#include "flighttelemetrystats.h"
#include "ahrs_comm_objects.h"
// Private constants
#if defined(PIOS_MANUAL_STACK_SIZE)
#define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE
@ -75,6 +77,7 @@ static ArmState_t armState;
static void manualControlTask(void *parameters);
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral);
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time);
static bool okToArm(void);
#define assumptions1 ( \
((int)MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_NONE == (int)MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE) && \
@ -377,9 +380,14 @@ static void manualControlTask(void *parameters)
switch(armState) {
case ARM_STATE_DISARMED:
newCmdArmed = MANUALCONTROLCOMMAND_ARMED_FALSE;
if (manualArm) {
armedDisarmStart = lastSysTime;
armState = ARM_STATE_ARMING_MANUAL;
if (manualArm)
{
if (okToArm()) // only allow arming if it's OK too
{
armedDisarmStart = lastSysTime;
armState = ARM_STATE_ARMING_MANUAL;
}
}
break;
@ -535,6 +543,57 @@ static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time)
return ((((portTICK_RATE_MS) -1) - start_time) + end_time) * portTICK_RATE_MS;
}
static bool okToArm(void)
{ // return TRUE is it's OK to arm
bool ok = true;
// read AHRS settings
AHRSSettingsData AHRSSettings;
AHRSSettingsGet(&AHRSSettings);
// read alarms
SystemAlarmsData alarms;
SystemAlarmsGet(&alarms);
// // fetch the GPS alarm state
// SystemAlarmsAlarmOptions gps_alarm = AlarmsGet(SYSTEMALARMS_ALARM_GPS);
switch (AHRSSettings.Algorithm)
{
case AHRSSETTINGS_ALGORITHM_SIMPLE:
case AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR:
case AHRSSETTINGS_ALGORITHM_INSGPS_INDOOR_NOMAG:
// Go through alarms
for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++)
{
if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_WARNING)
{ // found an alarm thats set
if (i != SYSTEMALARMS_ALARM_GPS)
{ // it's not the gps alarm
ok = false;
break;
}
}
}
break;
case AHRSSETTINGS_ALGORITHM_INSGPS_OUTDOOR:
if (AlarmsHasWarnings())
ok = false;
break;
default: // unknown AHRS algorithum
if (AlarmsHasWarnings())
ok = false;
break;
}
return ok;
}
//
//static void armingMechanism(uint8_t* armingState, const ManualControlSettingsData* settings, const ManualControlCommandData* cmd)
//{