mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-15 07:29:15 +01:00
Sanity Check. Add the first set of 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"
This commit is contained in:
parent
b9ebeb2c2e
commit
23422d3cd4
@ -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;
|
||||
}
|
Loading…
x
Reference in New Issue
Block a user