//! When the control system requests to disarm the FC
static int32_t control_event_disarm()
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (flightStatus.Armed != FLIGHTSTATUS_ARMED_DISARMED) {
flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
FlightStatusSet(&flightStatus);
}
return 0;
}
//! When the control system requests to start arming the FC
static int32_t control_event_arming()
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMING) {
flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMING;
FlightStatusSet(&flightStatus);
}
return 0;
}
/**
* @brief Update the flightStatus object only if value changed. Reduces callbacks
* @param[in] val The new value
*/
static void setArmedIfChanged(uint8_t val)
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if (flightStatus.Armed != val) {
flightStatus.Armed = val;
FlightStatusSet(&flightStatus);
}
}
/**
* 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);
}
}
/**
* Telemetry transmit task. Processes queue events and periodic updates.
*/
static void telemetryRxTask(void *parameters)
{
uint32_t inputPort;
uint8_t c;
// Task loop
while (1) {
#if defined(PIOS_INCLUDE_USB_HID)
// Determine input port (USB takes priority over telemetry port)
if (PIOS_USB_HID_CheckAvailable(0)) {
inputPort = PIOS_COM_TELEM_USB;
} else
#endif /* PIOS_INCLUDE_USB_HID */
{
inputPort = telemetryPort;
}
mavlink_channel_t mavlink_chan = MAVLINK_COMM_0;
// Block until a byte is available
PIOS_COM_ReceiveBuffer(inputPort, &c, 1, portMAX_DELAY);
// And process it
if (mavlink_parse_char(mavlink_chan, c, &rx_msg, &rx_status))
{
// Handle packet with waypoint component
mavlink_wpm_message_handler(&rx_msg);
// Handle packet with parameter component
mavlink_pm_message_handler(mavlink_chan, &rx_msg);
switch (rx_msg.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
{
// Check if this is the gcs
mavlink_heartbeat_t beat;
mavlink_msg_heartbeat_decode(&rx_msg, &beat);
if (beat.type == MAV_TYPE_GCS)
{
// Got heartbeat from the GCS, we're good!
lastOperatorHeartbeat = xTaskGetTickCount() * portTICK_RATE_MS;
}
}
break;
case MAVLINK_MSG_ID_SET_MODE:
{
mavlink_set_mode_t mode;
mavlink_msg_set_mode_decode(&rx_msg, &mode);
// Check if this system should change the mode
if (mode.target_system == mavlink_system.sysid)
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
switch (mode.base_mode)
{
case MAV_MODE_MANUAL_ARMED:
{
flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL;
flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED;
}
break;
case MAV_MODE_MANUAL_DISARMED:
{
flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL;
flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
}
break;
case MAV_MODE_PREFLIGHT:
{
flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED;
}
break;
case MAV_MODE_STABILIZE_ARMED:
{
flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_STABILIZED1;
flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED;
}
break;
case MAV_MODE_GUIDED_ARMED:
{
flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_STABILIZED2;
flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED;
}
break;
case MAV_MODE_AUTO_ARMED:
{
flightStatus.FlightMode = FLIGHTSTATUS_FLIGHTMODE_STABILIZED3;
flightStatus.Armed = FLIGHTSTATUS_ARMED_ARMED;
}
break;
}
bool newHilEnabled = (mode.base_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL);
if (newHilEnabled != hilEnabled)
{
if (newHilEnabled)
{
// READ-ONLY flag write to ActuatorCommand
UAVObjMetadata meta;
UAVObjHandle handle = ActuatorCommandHandle();
UAVObjGetMetadata(handle, &meta);
meta.access = ACCESS_READONLY;
UAVObjSetMetadata(handle, &meta);
mavlink_missionlib_send_gcs_string("ENABLING HIL SIMULATION");
mavlink_missionlib_send_gcs_string("+++++++++++++++++++++++");
mavlink_missionlib_send_gcs_string("BLOCKING ALL ACTUATORS");
}
else
{
// READ-ONLY flag write to ActuatorCommand
UAVObjMetadata meta;
UAVObjHandle handle = ActuatorCommandHandle();
UAVObjGetMetadata(handle, &meta);
meta.access = ACCESS_READWRITE;
UAVObjSetMetadata(handle, &meta);
mavlink_missionlib_send_gcs_string("DISABLING HIL SIMULATION");
mavlink_missionlib_send_gcs_string("+++++++++++++++++++++++");
mavlink_missionlib_send_gcs_string("ACTIVATING ALL ACTUATORS");
}
}
hilEnabled = newHilEnabled;
FlightStatusSet(&flightStatus);
// Check HIL
bool hilEnabled = (mode.base_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL);
enableHil(hilEnabled);
}
}
break;
case MAVLINK_MSG_ID_HIL_STATE:
{
if (hilEnabled)
{
mavlink_hil_state_t hil;
mavlink_msg_hil_state_decode(&rx_msg, &hil);
// Write GPSPosition
GPSPositionData gps;
GPSPositionGet(&gps);
gps.Altitude = hil.alt/10;
gps.Latitude = hil.lat/10;
gps.Longitude = hil.lon/10;
GPSPositionSet(&gps);
// Write PositionActual
PositionActualData pos;
PositionActualGet(&pos);
// FIXME WRITE POSITION HERE
PositionActualSet(&pos);
// Write AttitudeActual
AttitudeActualData att;
AttitudeActualGet(&att);
att.Roll = hil.roll;
att.Pitch = hil.pitch;
att.Yaw = hil.yaw;
// FIXME
//att.RollSpeed = hil.rollspeed;
//att.PitchSpeed = hil.pitchspeed;
//att.YawSpeed = hil.yawspeed;
// Convert to quaternion formulation
RPY2Quaternion(&attitudeActual.Roll, &attitudeActual.q1);
// Write AttitudeActual
AttitudeActualSet(&att);
// Write AttitudeRaw
AttitudeRawData raw;
AttitudeRawGet(&raw);
raw.gyros[0] = hil.rollspeed;
raw.gyros[1] = hil.pitchspeed;
raw.gyros[2] = hil.yawspeed;
raw.accels[0] = hil.xacc;
raw.accels[1] = hil.yacc;
raw.accels[2] = hil.zacc;
// raw.magnetometers[0] = hil.xmag;
// raw.magnetometers[0] = hil.ymag;
// raw.magnetometers[0] = hil.zmag;
AttitudeRawSet(&raw);
}
}
break;
case MAVLINK_MSG_ID_COMMAND_LONG:
{
// FIXME Implement
}
break;
}
}
}
}
