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

Sanity Check. Add the first set of checks:

Browse Source 
1. If a flight mode switch allows autotune and autotune module not running
2. If airframe is a multirotor and either manual is available or a stabilization mode uses "none"
This commit is contained in:
James Cotton 2012-10-25 14:41:59 -05:00
parent b9ebeb2c2e
commit 23422d3cd4
1 changed files with 137 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

139
flight/Libraries/sanitycheck.c
View File

@ -27,11 +27,22 @@
*/
#include "openpilot.h"
#include <pios_board_info.h>
#include "sanitycheck.h"
#include "hwsettings.h"
#include "taskinfo.h"
#include "manualcontrolsettings.h"
#include "systemalarms.h"
#include "systemsettings.h"
/****************************
* Current checks:
* 1. If a flight mode switch allows autotune and autotune module not running
* 2. If airframe is a multirotor and either manual is available or a stabilization mode uses "none"
****************************/
//! Check a stabilization mode switch position for safety
static int32_t check_stabilization_settings(int index, bool multirotor);
/**
* Run a preflight check over the hardware configuration
@ -39,9 +50,133 @@
*/
int32_t configuration_check()
{
// TODO: Check manual control settings for any modes that can be accessed without the
int32_t status = SYSTEMALARMS_ALARM_OK;
// TODO: Check manual control settings for any modes that can be accessed without the
// necessary module running
AlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, SYSTEMALARMS_ALARM_CRITICAL);
// TODO: Get board type
const struct pios_board_info * bdinfo = &pios_board_info_blob;
bool coptercontrol = bdinfo->board_type == 0x04;
// Classify airframe type
bool multirotor = true;
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:
multirotor = true;
break;
default:
multirotor = false;
}
// Get the running modules
uint8_t running[TASKINFO_RUNNING_NUMELEM];
TaskInfoRunningGet(running);
// For each available flight mode position sanity check the available
// modes
uint8_t num_modes;
uint8_t modes[MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM];
ManualControlSettingsFlightModeNumberGet(&num_modes);
ManualControlSettingsFlightModePositionGet(modes);
for(int i = 0; i < num_modes; i++) {
switch(modes[i]) {
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL:
if (multirotor)
status = SYSTEMALARMS_ALARM_ERROR;
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1:
status = (status == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(1, multirotor) : status;
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2:
status = (status == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(2, multirotor) : status;
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3:
status = (status == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(3, multirotor) : status;
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_AUTOTUNE:
if (running[TASKINFO_RUNNING_AUTOTUNE] != TASKINFO_RUNNING_TRUE)
status = SYSTEMALARMS_ALARM_ERROR;
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD:
if (coptercontrol)
status = SYSTEMALARMS_ALARM_ERROR;
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL:
if (coptercontrol)
status = SYSTEMALARMS_ALARM_ERROR;
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
if (coptercontrol)
status = SYSTEMALARMS_ALARM_ERROR;
break;
default:
// Uncovered modes are automatically an error
status = SYSTEMALARMS_ALARM_ERROR;
}
}
// TODO: Check on a multirotor no axis supports "None"
if(status != SYSTEMALARMS_ALARM_OK)
AlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, status);
else
AlarmsClear(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION);
return 0;
}
/**
* Checks the stabiliation settings for a paritcular mode and makes
* sure it is appropriate for the airframe
* @param[in] index Which stabilization mode to check
* @returns SYSTEMALARMS_ALARM_OK or SYSTEMALARMS_ALARM_ERROR
*/
static int32_t check_stabilization_settings(int index, bool multirotor)
{
// Make sure the modes are all similar
if (MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NUMELEM ||
MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NUMELEM)
return SYSTEMALARMS_ALARM_ERROR;
uint8_t modes[MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM];
// Get the different axis modes for this switch position
switch(index) {
case 1:
ManualControlSettingsStabilization1SettingsGet(modes);
break;
case 2:
ManualControlSettingsStabilization2SettingsGet(modes);
break;
case 3:
ManualControlSettingsStabilization3SettingsGet(modes);
break;
default:
return SYSTEMALARMS_ALARM_ERROR;
}
// For multirotors verify that nothing is set to "none"
if (multirotor) {
for(int i = 0; i < NELEMENTS(modes); i++) {
if (modes[i] == MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE)
return SYSTEMALARMS_ALARM_ERROR;
}
}
// Warning: This assumes that certain conditions in the XML file are met. That
// MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE has the same numeric value for each channel
// and is the same for STABILIZATIONDESIRED_STABILIZATIONMODE_NONE
return SYSTEMALARMS_ALARM_OK;
}