StabilizationSettingsInitialize();
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
VtolSelfTuningStatsInitialize();
VtolPathFollowerSettingsInitialize();
VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
SystemSettingsConnectCallback(&SettingsUpdatedCb);
#endif
callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&manualControlTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_MANUALCONTROL, STACK_SIZE_BYTES);
return 0;
}
MODULE_INITCALL(ManualControlInitialize, ManualControlStart);
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
frameType = GetCurrentFrameType();
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
VtolPathFollowerSettingsTreatCustomCraftAsOptions TreatCustomCraftAs;
VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs);
if (frameType == FRAME_TYPE_CUSTOM) {
switch (TreatCustomCraftAs) {
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
frameType = FRAME_TYPE_FIXED_WING;
break;
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
frameType = FRAME_TYPE_MULTIROTOR;
break;
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND:
frameType = FRAME_TYPE_GROUND;
/**
* Run a preflight check over the hardware configuration
* and currently active modules
*/
int32_t configuration_check()
{
int32_t severity = SYSTEMALARMS_ALARM_OK;
SystemAlarmsExtendedAlarmStatusOptions alarmstatus = SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE;
uint8_t alarmsubstatus = 0;
// Get board type
const struct pios_board_info *bdinfo = &pios_board_info_blob;
bool coptercontrol = bdinfo->board_type == 0x04;
// Classify navigation capability
#ifdef REVOLUTION
RevoSettingsInitialize();
uint8_t revoFusion;
RevoSettingsFusionAlgorithmGet(&revoFusion);
bool navCapableFusion;
switch (revoFusion) {
case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAGGPSOUTDOOR:
case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13:
navCapableFusion = true;
break;
default:
navCapableFusion = false;
// check for hitl. hitl allows to feed position and velocity state via
// telemetry, this makes nav possible even with an unsuited algorithm
if (PositionStateHandle()) {
if (PositionStateReadOnly()) {
navCapableFusion = true;
}
}
}
#else
const bool navCapableFusion = false;
#endif /* ifdef REVOLUTION */
// Classify airframe type
bool multirotor = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR);
// For each available flight mode position sanity check the available
// modes
uint8_t num_modes;
uint8_t modes[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
ManualControlSettingsFlightModeNumberGet(&num_modes);
FlightModeSettingsFlightModePositionGet(modes);
for (uint32_t i = 0; i < num_modes; i++) {
switch (modes[i]) {
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL:
ADDSEVERITY(!multirotor);
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1:
ADDSEVERITY(check_stabilization_settings(1, multirotor, coptercontrol));
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2:
ADDSEVERITY(check_stabilization_settings(2, multirotor, coptercontrol));
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3:
ADDSEVERITY(check_stabilization_settings(3, multirotor, coptercontrol));
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED4:
ADDSEVERITY(check_stabilization_settings(4, multirotor, coptercontrol));
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED5:
ADDSEVERITY(check_stabilization_settings(5, multirotor, coptercontrol));
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED6:
ADDSEVERITY(check_stabilization_settings(6, multirotor, coptercontrol));
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE:
ADDSEVERITY(PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_AUTOTUNE));
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
{
// Revo supports PathPlanner and that must be OK or we are not sane
// PathPlan alarm is uninitialized if not running
// PathPlan alarm is warning or error if the flightplan is invalid
SystemAlarmsAlarmData alarms;
SystemAlarmsAlarmGet(&alarms);
ADDSEVERITY(alarms.PathPlan == SYSTEMALARMS_ALARM_OK);
}
// intentionally no break as this also needs pathfollower
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONVARIOFPV:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONVARIOLOS:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONVARIONSEW:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOCRUISE:
ADDSEVERITY(!coptercontrol);
ADDSEVERITY(navCapableFusion);
break;
default:
// Uncovered modes are automatically an error
ADDSEVERITY(false);
}
// mark the first encountered erroneous setting in status and substatus
if ((severity != SYSTEMALARMS_ALARM_OK) && (alarmstatus == SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE)) {
alarmstatus = SYSTEMALARMS_EXTENDEDALARMSTATUS_FLIGHTMODE;
alarmsubstatus = i;
}
}
uint8_t checks_disabled;
FlightModeSettingsDisableSanityChecksGet(&checks_disabled);
if (checks_disabled == FLIGHTMODESETTINGS_DISABLESANITYCHECKS_TRUE) {
severity = SYSTEMALARMS_ALARM_WARNING;
}
if (severity != SYSTEMALARMS_ALARM_OK) {
ExtendedAlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, severity, alarmstatus, alarmsubstatus);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION);
}
return 0;
}
