//! Get any control events
enum control_events failsafe_control_get_events()
{
// For now ARM / DISARM events still come from the transmitter. This
// means the normal disarm timeout still applies. To be replaced later
// by a full state machine determining how long to stay in failsafe before
// disarming.
return transmitter_control_get_events();
}
//! Get any control events
enum control_events failsafe_control_get_events()
{
// For now ARM / DISARM events still come from the transmitter. This
// means the normal disarm timeout still applies. To be replaced later
// by a full state machine determining how long to stay in failsafe before
// disarming. THIS IS TRUE ONLY IF RTH IS NOT ACTIVE! Otherwise we don't
// want trasnmitter to disarm by timeout when RTH mission is active.
if (!rth_active)
return transmitter_control_get_events();
else
return CONTROL_EVENTS_NONE;
}
/**
* Module task
*/
static void manualControlTask(void *parameters)
{
/* Make sure disarmed on power up */
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
FlightStatusSet(&flightStatus);
// Main task loop
lastSysTime = PIOS_Thread_Systime();
// Select failsafe before run
failsafe_control_select(true);
while (1) {
// Process periodic data for each of the controllers, including reading
// all available inputs
failsafe_control_update();
transmitter_control_update();
tablet_control_update();
geofence_control_update();
// Initialize to invalid value to ensure first update sets FlightStatus
static FlightStatusControlSourceOptions last_control_selection = -1;
enum control_events control_events = CONTROL_EVENTS_NONE;
// Control logic to select the valid controller
FlightStatusControlSourceOptions control_selection = control_source_select();
bool reset_controller = control_selection != last_control_selection;
// This logic would be better collapsed into control_source_select but
// in the case of the tablet we need to be able to abort and fall back
// to failsafe for now
switch(control_selection) {
case FLIGHTSTATUS_CONTROLSOURCE_TRANSMITTER:
transmitter_control_select(reset_controller);
control_events = transmitter_control_get_events();
break;
case FLIGHTSTATUS_CONTROLSOURCE_TABLET:
{
static bool tablet_previously_succeeded = false;
if (tablet_control_select(reset_controller) == 0) {
control_events = tablet_control_get_events();
tablet_previously_succeeded = true;
} else {
// Failure in tablet control. This would be better if done
// at the selection stage before the tablet is even used.
failsafe_control_select(reset_controller || tablet_previously_succeeded);
control_events = failsafe_control_get_events();
tablet_previously_succeeded = false;
}
break;
}
case FLIGHTSTATUS_CONTROLSOURCE_GEOFENCE:
geofence_control_select(reset_controller);
control_events = geofence_control_get_events();
break;
case FLIGHTSTATUS_CONTROLSOURCE_FAILSAFE:
default:
failsafe_control_select(reset_controller);
control_events = failsafe_control_get_events();
break;
}
if (control_selection != last_control_selection) {
FlightStatusControlSourceSet(&control_selection);
last_control_selection = control_selection;
}
// TODO: This can evolve into a full FSM like I2C possibly
switch(control_events) {
case CONTROL_EVENTS_NONE:
break;
case CONTROL_EVENTS_ARM:
control_event_arm();
break;
case CONTROL_EVENTS_ARMING:
control_event_arming();
break;
case CONTROL_EVENTS_DISARM:
control_event_disarm();
break;
}
// Wait until next update
PIOS_Thread_Sleep_Until(&lastSysTime, UPDATE_PERIOD_MS);
PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL);
}
}
