//! 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; } } } }