/** * Register a new object, adds object to local list and connects the queue depending on the object's * telemetry settings. * \param[in] obj Object to connect */ static void registerObject(UAVObjHandle obj) { if (UAVObjIsMetaobject(obj)) { /* Only connect change notifications for meta objects. No periodic updates */ UAVObjConnectQueue(obj, priorityQueue, EV_MASK_ALL_UPDATES); return; } else { UAVObjMetadata metadata; UAVObjUpdateMode updateMode; UAVObjGetMetadata(obj, &metadata); updateMode = UAVObjGetTelemetryUpdateMode(&metadata); /* Only create a periodic event for objects that are periodic */ if ((updateMode == UPDATEMODE_PERIODIC) || (updateMode == UPDATEMODE_THROTTLED)) { // Setup object for periodic updates UAVObjEvent ev = { .obj = obj, .instId = UAVOBJ_ALL_INSTANCES, .event = EV_UPDATED_PERIODIC, }; EventPeriodicQueueCreate(&ev, queue, 0); } // Setup object for telemetry updates updateObject(obj, EV_NONE); }
/** * Processes queue events */ static void processObjEvent(UAVObjEvent * ev) { UAVObjMetadata metadata; UAVObjUpdateMode updateMode; FlightTelemetryStatsData flightStats; int32_t retries; int32_t success; if (ev->obj == 0) { updateTelemetryStats(); } else if (ev->obj == GCSTelemetryStatsHandle()) { gcsTelemetryStatsUpdated(); } else { // Only process event if connected to GCS or if object FlightTelemetryStats is updated FlightTelemetryStatsGet(&flightStats); // Get object metadata UAVObjGetMetadata(ev->obj, &metadata); updateMode = UAVObjGetTelemetryUpdateMode(&metadata); if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED || ev->obj == FlightTelemetryStatsHandle()) { // Act on event retries = 0; success = -1; if (ev->event == EV_UPDATED || ev->event == EV_UPDATED_MANUAL || ((ev->event == EV_UPDATED_PERIODIC) && (updateMode != UPDATEMODE_THROTTLED))) { // Send update to GCS (with retries) while (retries < MAX_RETRIES && success == -1) { success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS); // call blocks until ack is received or timeout ++retries; } // Update stats txRetries += (retries - 1); if (success == -1) { ++txErrors; } } else if (ev->event == EV_UPDATE_REQ) { // Request object update from GCS (with retries) while (retries < MAX_RETRIES && success == -1) { success = UAVTalkSendObjectRequest(uavTalkCon, ev->obj, ev->instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout ++retries; } // Update stats txRetries += (retries - 1); if (success == -1) { ++txErrors; } } // If this is a metaobject then make necessary telemetry updates if (UAVObjIsMetaobject(ev->obj)) { updateObject(UAVObjGetLinkedObj(ev->obj), EV_NONE); // linked object will be the actual object the metadata are for } } if((updateMode == UPDATEMODE_THROTTLED) && !UAVObjIsMetaobject(ev->obj)) { // If this is UPDATEMODE_THROTTLED, the event mask changes on every event. updateObject(ev->obj, ev->event); } } }
/** * Update object's queue connections and timer, depending on object's settings * \param[in] obj Object to updates */ static void updateObject(UAVObjHandle obj, int32_t eventType) { UAVObjMetadata metadata; UAVObjUpdateMode updateMode; int32_t eventMask; // Get metadata UAVObjGetMetadata(obj, &metadata); updateMode = UAVObjGetTelemetryUpdateMode(&metadata); // Setup object depending on update mode if (updateMode == UPDATEMODE_PERIODIC) { // Set update period setUpdatePeriod(obj, metadata.telemetryUpdatePeriod); // Connect queue eventMask = EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ; if (UAVObjIsMetaobject(obj)) { eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) } UAVObjConnectQueue(obj, priorityQueue, eventMask); } else if (updateMode == UPDATEMODE_ONCHANGE) { // Set update period setUpdatePeriod(obj, 0); // Connect queue eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; if (UAVObjIsMetaobject(obj)) { eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) } UAVObjConnectQueue(obj, priorityQueue, eventMask); } else if (updateMode == UPDATEMODE_THROTTLED) { if ((eventType == EV_UPDATED_PERIODIC) || (eventType == EV_NONE)) { // If we received a periodic update, we can change back to update on change eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; // Set update period on initialization and metadata change if (eventType == EV_NONE) setUpdatePeriod(obj, metadata.telemetryUpdatePeriod); } else { // Otherwise, we just received an object update, so switch to periodic for the timeout period to prevent more updates eventMask = EV_UPDATED_PERIODIC | EV_UPDATED_MANUAL | EV_UPDATE_REQ; } if (UAVObjIsMetaobject(obj)) { eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) } UAVObjConnectQueue(obj, priorityQueue, eventMask); } else if (updateMode == UPDATEMODE_MANUAL) { // Set update period setUpdatePeriod(obj, 0); // Connect queue eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ; if (UAVObjIsMetaobject(obj)) { eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) } UAVObjConnectQueue(obj, priorityQueue, eventMask); } }
/** * Processes queue events */ static void processObjEvent(UAVObjEvent * ev) { UAVObjMetadata metadata; FlightTelemetryStatsData flightStats; int32_t retries; int32_t success; if (ev->obj == 0) { updateTelemetryStats(); } else if (ev->obj == GCSTelemetryStatsHandle()) { gcsTelemetryStatsUpdated(); } else if (ev->obj == TelemetrySettingsHandle()) { updateSettings(); } else { // Only process event if connected to GCS or if object FlightTelemetryStats is updated FlightTelemetryStatsGet(&flightStats); if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED || ev->obj == FlightTelemetryStatsHandle()) { // Get object metadata UAVObjGetMetadata(ev->obj, &metadata); // Act on event retries = 0; success = -1; if (ev->event == EV_UPDATED || ev->event == EV_UPDATED_MANUAL) { // Send update to GCS (with retries) while (retries < MAX_RETRIES && success == -1) { success = UAVTalkSendObject(ev->obj, ev->instId, metadata.telemetryAcked, REQ_TIMEOUT_MS); // call blocks until ack is received or timeout ++retries; } // Update stats txRetries += (retries - 1); if (success == -1) { ++txErrors; } } else if (ev->event == EV_UPDATE_REQ) { // Request object update from GCS (with retries) while (retries < MAX_RETRIES && success == -1) { success = UAVTalkSendObjectRequest(ev->obj, ev->instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout ++retries; } // Update stats txRetries += (retries - 1); if (success == -1) { ++txErrors; } } // If this is a metaobject then make necessary telemetry updates if (UAVObjIsMetaobject(ev->obj)) { updateObject(UAVObjGetLinkedObj(ev->obj)); // linked object will be the actual object the metadata are for } } } }
/** * Update object's queue connections and timer, depending on object's settings * \param[in] obj Object to updates */ static void updateObject(UAVObjHandle obj) { UAVObjMetadata metadata; int32_t eventMask; // Get metadata UAVObjGetMetadata(obj, &metadata); // Setup object depending on update mode if (metadata.telemetryUpdateMode == UPDATEMODE_PERIODIC) { // Set update period setUpdatePeriod(obj, metadata.telemetryUpdatePeriod); // Connect queue eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ; if (UAVObjIsMetaobject(obj)) { eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) } UAVObjConnectQueue(obj, priorityQueue, eventMask); } else if (metadata.telemetryUpdateMode == UPDATEMODE_ONCHANGE) { // Set update period setUpdatePeriod(obj, 0); // Connect queue eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ; if (UAVObjIsMetaobject(obj)) { eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) } UAVObjConnectQueue(obj, priorityQueue, eventMask); } else if (metadata.telemetryUpdateMode == UPDATEMODE_MANUAL) { // Set update period setUpdatePeriod(obj, 0); // Connect queue eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ; if (UAVObjIsMetaobject(obj)) { eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events) } UAVObjConnectQueue(obj, priorityQueue, eventMask); } else if (metadata.telemetryUpdateMode == UPDATEMODE_NEVER) { // Set update period setUpdatePeriod(obj, 0); // Disconnect queue UAVObjDisconnectQueue(obj, priorityQueue); } }
static bool enableHil(bool enable) { // READ-ONLY flag write to ActuatorCommand UAVObjHandle handle = ActuatorCommandHandle(); UAVObjMetadata commandMeta; UAVObjGetMetadata(handle, &commandMeta); if (enable) { // Disable actuators commandMeta.access = ACCESS_READONLY; } else { // Enable actuators commandMeta.access = ACCESS_READWRITE; } return (UAVObjSetMetadata(handle, &commandMeta) == 0); }
/** * Register a new object: connect the update callback * \param[in] obj Object to connect */ static void register_object(UAVObjHandle obj) { // check whether we want to log this object UAVObjMetadata meta_data; if (UAVObjGetMetadata(obj, &meta_data) < 0){ return; } if (meta_data.loggingUpdatePeriod == 0){ return; } uint16_t period = MAX(meta_data.loggingUpdatePeriod, get_minimum_logging_period()); if (period == 1) { // log every update UAVObjConnectCallback(obj, obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED); } else { // log updates throttled UAVObjConnectCallbackThrottled(obj, obj_updated_callback, NULL, EV_UPDATED | EV_UNPACKED, period); } }
/** * Module task */ static void manualControlTask(void *parameters) { ManualControlSettingsData settings; ManualControlCommandData cmd; portTickType lastSysTime; float flightMode = 0; uint8_t disconnected_count = 0; uint8_t connected_count = 0; enum { CONNECTED, DISCONNECTED } connection_state = DISCONNECTED; // Make sure unarmed on power up ManualControlCommandGet(&cmd); cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE; ManualControlCommandSet(&cmd); armState = ARM_STATE_DISARMED; // Main task loop lastSysTime = xTaskGetTickCount(); while (1) { float scaledChannel[MANUALCONTROLCOMMAND_CHANNEL_NUMELEM]; // Wait until next update vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS); PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL); // Read settings ManualControlSettingsGet(&settings); if (ManualControlCommandReadOnly(&cmd)) { FlightTelemetryStatsData flightTelemStats; FlightTelemetryStatsGet(&flightTelemStats); if(flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { /* trying to fly via GCS and lost connection. fall back to transmitter */ UAVObjMetadata metadata; UAVObjGetMetadata(&cmd, &metadata); metadata.access = ACCESS_READWRITE; UAVObjSetMetadata(&cmd, &metadata); } } if (!ManualControlCommandReadOnly(&cmd)) { // Check settings, if error raise alarm if (settings.Roll >= MANUALCONTROLSETTINGS_ROLL_NONE || settings.Pitch >= MANUALCONTROLSETTINGS_PITCH_NONE || settings.Yaw >= MANUALCONTROLSETTINGS_YAW_NONE || settings.Throttle >= MANUALCONTROLSETTINGS_THROTTLE_NONE || settings.FlightMode >= MANUALCONTROLSETTINGS_FLIGHTMODE_NONE) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; ManualControlCommandSet(&cmd); continue; } // Read channel values in us // TODO: settings.InputMode is currently ignored because PIOS will not allow runtime // selection of PWM and PPM. The configuration is currently done at compile time in // the pios_config.h file. for (int n = 0; n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) { #if defined(PIOS_INCLUDE_PWM) cmd.Channel[n] = PIOS_PWM_Get(n); #elif defined(PIOS_INCLUDE_PPM) cmd.Channel[n] = PIOS_PPM_Get(n); #elif defined(PIOS_INCLUDE_SPEKTRUM) cmd.Channel[n] = PIOS_SPEKTRUM_Get(n); #endif scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n], 0); } // Scale channels to -1 -> +1 range cmd.Roll = scaledChannel[settings.Roll]; cmd.Pitch = scaledChannel[settings.Pitch]; cmd.Yaw = scaledChannel[settings.Yaw]; cmd.Throttle = scaledChannel[settings.Throttle]; flightMode = scaledChannel[settings.FlightMode]; if (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) cmd.Accessory1 = scaledChannel[settings.Accessory1]; else cmd.Accessory1 = 0; if (settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) cmd.Accessory2 = scaledChannel[settings.Accessory2]; else cmd.Accessory2 = 0; if (settings.Accessory3 != MANUALCONTROLSETTINGS_ACCESSORY3_NONE) cmd.Accessory3 = scaledChannel[settings.Accessory3]; else cmd.Accessory3 = 0; // Note here the code is ass if (flightMode < -FLIGHT_MODE_LIMIT) cmd.FlightMode = settings.FlightModePosition[0]; else if (flightMode > FLIGHT_MODE_LIMIT) cmd.FlightMode = settings.FlightModePosition[2]; else cmd.FlightMode = settings.FlightModePosition[1]; // Update the ManualControlCommand object ManualControlCommandSet(&cmd); // This seems silly to set then get, but the reason is if the GCS is // the control input, the set command will be blocked by the read only // setting and the get command will pull the right values from telemetry } else ManualControlCommandGet(&cmd); /* Under GCS control */ // Implement hysteresis loop on connection status // Must check both Max and Min in case they reversed if (!ManualControlCommandReadOnly(&cmd) && cmd.Channel[settings.Throttle] < settings.ChannelMax[settings.Throttle] - CONNECTION_OFFSET && cmd.Channel[settings.Throttle] < settings.ChannelMin[settings.Throttle] - CONNECTION_OFFSET) { if (disconnected_count++ > 10) { connection_state = DISCONNECTED; connected_count = 0; disconnected_count = 0; } else disconnected_count++; } else { if (connected_count++ > 10) { connection_state = CONNECTED; connected_count = 0; disconnected_count = 0; } else connected_count++; } if (connection_state == DISCONNECTED) { cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; cmd.Throttle = -1; // Shut down engine with no control cmd.Roll = 0; cmd.Yaw = 0; cmd.Pitch = 0; //cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); ManualControlCommandSet(&cmd); } else { cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE; AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); ManualControlCommandSet(&cmd); } // // Arming and Disarming mechanism // // Look for state changes and write in newArmState uint8_t newCmdArmed = cmd.Armed; // By default, keep the arming state the same if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) { // In this configuration we always disarm newCmdArmed = MANUALCONTROLCOMMAND_ARMED_FALSE; } else { // In all other cases, we will not change the arm state when disconnected if (connection_state == CONNECTED) { if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) { // In this configuration, we go into armed state as soon as the throttle is low, never disarm if (cmd.Throttle < 0) { newCmdArmed = MANUALCONTROLCOMMAND_ARMED_TRUE; } } else { // When the configuration is not "Always armed" and no "Always disarmed", // the state will not be changed when the throttle is not low if (cmd.Throttle < 0) { static portTickType armedDisarmStart; float armingInputLevel = 0; // Calc channel see assumptions7 switch ( (settings.Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) { case ARMING_CHANNEL_ROLL: armingInputLevel = cmd.Roll; break; case ARMING_CHANNEL_PITCH: armingInputLevel = cmd.Pitch; break; case ARMING_CHANNEL_YAW: armingInputLevel = cmd.Yaw; break; } bool manualArm = false; bool manualDisarm = false; if (connection_state == CONNECTED) { // Should use RC input only if RX is connected if (armingInputLevel <= -0.90) manualArm = true; else if (armingInputLevel >= +0.90) manualDisarm = true; } // Swap arm-disarming see assumptions8 if ((settings.Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) { bool temp = manualArm; manualArm = manualDisarm; manualDisarm = temp; } switch(armState) { case ARM_STATE_DISARMED: newCmdArmed = MANUALCONTROLCOMMAND_ARMED_FALSE; if (manualArm) { if (okToArm()) // only allow arming if it's OK too { armedDisarmStart = lastSysTime; armState = ARM_STATE_ARMING_MANUAL; } } break; case ARM_STATE_ARMING_MANUAL: if (manualArm) { if (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS) armState = ARM_STATE_ARMED; } else armState = ARM_STATE_DISARMED; break; case ARM_STATE_ARMED: // When we get here, the throttle is low, // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled armedDisarmStart = lastSysTime; armState = ARM_STATE_DISARMING_TIMEOUT; newCmdArmed = MANUALCONTROLCOMMAND_ARMED_TRUE; break; case ARM_STATE_DISARMING_TIMEOUT: // We get here when armed while throttle low, even when the arming timeout is not enabled if (settings.ArmedTimeout != 0) if (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings.ArmedTimeout) armState = ARM_STATE_DISARMED; // Switch to disarming due to manual control when needed if (manualDisarm) { armedDisarmStart = lastSysTime; armState = ARM_STATE_DISARMING_MANUAL; } break; case ARM_STATE_DISARMING_MANUAL: if (manualDisarm) { if (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS) armState = ARM_STATE_DISARMED; } else armState = ARM_STATE_ARMED; break; } // End Switch } else { // The throttle is not low, in case we where arming or disarming, abort switch(armState) { case ARM_STATE_DISARMING_MANUAL: case ARM_STATE_DISARMING_TIMEOUT: armState = ARM_STATE_ARMED; break; case ARM_STATE_ARMING_MANUAL: armState = ARM_STATE_DISARMED; break; default: // Nothing needs to be done in the other states break; } } } } } // Update cmd object when needed if (newCmdArmed != cmd.Armed) { cmd.Armed = newCmdArmed; ManualControlCommandSet(&cmd); } // // End of arming/disarming // // Depending on the mode update the Stabilization or Actuator objects switch(PARSE_FLIGHT_MODE(cmd.FlightMode)) { case FLIGHTMODE_UNDEFINED: // This reflects a bug in the code architecture! AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); break; case FLIGHTMODE_MANUAL: updateActuatorDesired(&cmd); break; case FLIGHTMODE_STABILIZED: updateStabilizationDesired(&cmd, &settings); break; case FLIGHTMODE_GUIDANCE: // TODO: Implement break; } } }
/** * Module task */ static void manualControlTask(void *parameters) { ManualControlSettingsData settings; StabilizationSettingsData stabSettings; ManualControlCommandData cmd; ActuatorDesiredData actuator; AttitudeDesiredData attitude; RateDesiredData rate; portTickType lastSysTime; float flightMode; uint8_t disconnected_count = 0; uint8_t connected_count = 0; enum { CONNECTED, DISCONNECTED } connection_state = DISCONNECTED; // Make sure unarmed on power up ManualControlCommandGet(&cmd); cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE; ManualControlCommandSet(&cmd); armState = ARM_STATE_DISARMED; // Main task loop lastSysTime = xTaskGetTickCount(); while (1) { float scaledChannel[MANUALCONTROLCOMMAND_CHANNEL_NUMELEM]; // Wait until next update vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS); PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL); // Read settings ManualControlSettingsGet(&settings); StabilizationSettingsGet(&stabSettings); if (ManualControlCommandReadOnly(&cmd)) { FlightTelemetryStatsData flightTelemStats; FlightTelemetryStatsGet(&flightTelemStats); if(flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { /* trying to fly via GCS and lost connection. fall back to transmitter */ UAVObjMetadata metadata; UAVObjGetMetadata(&cmd, &metadata); metadata.access = ACCESS_READWRITE; UAVObjSetMetadata(&cmd, &metadata); } } if (!ManualControlCommandReadOnly(&cmd)) { // Check settings, if error raise alarm if (settings.Roll >= MANUALCONTROLSETTINGS_ROLL_NONE || settings.Pitch >= MANUALCONTROLSETTINGS_PITCH_NONE || settings.Yaw >= MANUALCONTROLSETTINGS_YAW_NONE || settings.Throttle >= MANUALCONTROLSETTINGS_THROTTLE_NONE || settings.FlightMode >= MANUALCONTROLSETTINGS_FLIGHTMODE_NONE) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; ManualControlCommandSet(&cmd); continue; } // Read channel values in us // TODO: settings.InputMode is currently ignored because PIOS will not allow runtime // selection of PWM and PPM. The configuration is currently done at compile time in // the pios_config.h file. for (int n = 0; n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) { #if defined(PIOS_INCLUDE_PWM) cmd.Channel[n] = PIOS_PWM_Get(n); #elif defined(PIOS_INCLUDE_PPM) cmd.Channel[n] = PIOS_PPM_Get(n); #elif defined(PIOS_INCLUDE_SPEKTRUM) cmd.Channel[n] = PIOS_SPEKTRUM_Get(n); #endif scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]); } // Scale channels to -1 -> +1 range cmd.Roll = scaledChannel[settings.Roll]; cmd.Pitch = scaledChannel[settings.Pitch]; cmd.Yaw = scaledChannel[settings.Yaw]; cmd.Throttle = scaledChannel[settings.Throttle]; flightMode = scaledChannel[settings.FlightMode]; if (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) cmd.Accessory1 = scaledChannel[settings.Accessory1]; else cmd.Accessory1 = 0; if (settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) cmd.Accessory2 = scaledChannel[settings.Accessory2]; else cmd.Accessory2 = 0; if (settings.Accessory3 != MANUALCONTROLSETTINGS_ACCESSORY3_NONE) cmd.Accessory3 = scaledChannel[settings.Accessory3]; else cmd.Accessory3 = 0; if (flightMode < -FLIGHT_MODE_LIMIT) { // Position 1 for(int i = 0; i < 3; i++) { cmd.StabilizationSettings[i] = settings.Pos1StabilizationSettings[i]; // See assumptions1 } cmd.FlightMode = settings.Pos1FlightMode; // See assumptions2 } else if (flightMode > FLIGHT_MODE_LIMIT) { // Position 3 for(int i = 0; i < 3; i++) { cmd.StabilizationSettings[i] = settings.Pos3StabilizationSettings[i]; // See assumptions5 } cmd.FlightMode = settings.Pos3FlightMode; // See assumptions6 } else { // Position 2 for(int i = 0; i < 3; i++) { cmd.StabilizationSettings[i] = settings.Pos2StabilizationSettings[i]; // See assumptions3 } cmd.FlightMode = settings.Pos2FlightMode; // See assumptions4 } // Update the ManualControlCommand object ManualControlCommandSet(&cmd); // This seems silly to set then get, but the reason is if the GCS is // the control input, the set command will be blocked by the read only // setting and the get command will pull the right values from telemetry } else ManualControlCommandGet(&cmd); /* Under GCS control */ // Implement hysteresis loop on connection status // Must check both Max and Min in case they reversed if (!ManualControlCommandReadOnly(&cmd) && cmd.Channel[settings.Throttle] < settings.ChannelMax[settings.Throttle] - CONNECTION_OFFSET && cmd.Channel[settings.Throttle] < settings.ChannelMin[settings.Throttle] - CONNECTION_OFFSET) { if (disconnected_count++ > 10) { connection_state = DISCONNECTED; connected_count = 0; disconnected_count = 0; } else disconnected_count++; } else { if (connected_count++ > 10) { connection_state = CONNECTED; connected_count = 0; disconnected_count = 0; } else connected_count++; } if (connection_state == DISCONNECTED) { cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; cmd.Throttle = -1; // Shut down engine with no control cmd.Roll = 0; cmd.Yaw = 0; cmd.Pitch = 0; //cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); ManualControlCommandSet(&cmd); } else { cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE; AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); ManualControlCommandSet(&cmd); } // Arming and Disarming mechanism if (cmd.Throttle < 0) { // Throttle is low, in this condition the arming state could change uint8_t newCmdArmed = cmd.Armed; static portTickType armedDisarmStart; // Look for state changes and write in newArmState if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_NONE) { // No channel assigned to arming -> arm immediately when throttle is low newCmdArmed = MANUALCONTROLCOMMAND_ARMED_TRUE; } else { float armStickLevel; uint8_t channel = settings.Arming/2; // 0=Channel1, 1=Channel1_Rev, 2=Channel2, .... bool reverse = (settings.Arming%2)==1; bool manualArm = false; bool manualDisarm = false; if (connection_state == CONNECTED) { // Should use RC input only if RX is connected armStickLevel = scaledChannel[channel]; if (reverse) armStickLevel =-armStickLevel; if (armStickLevel <= -0.90) manualArm = true; else if (armStickLevel >= +0.90) manualDisarm = true; } switch(armState) { case ARM_STATE_DISARMED: newCmdArmed = MANUALCONTROLCOMMAND_ARMED_FALSE; if (manualArm) { armedDisarmStart = lastSysTime; armState = ARM_STATE_ARMING_MANUAL; } break; case ARM_STATE_ARMING_MANUAL: if (manualArm) { if (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS) armState = ARM_STATE_ARMED; } else armState = ARM_STATE_DISARMED; break; case ARM_STATE_ARMED: // When we get here, the throttle is low, // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled armedDisarmStart = lastSysTime; armState = ARM_STATE_DISARMING_TIMEOUT; newCmdArmed = MANUALCONTROLCOMMAND_ARMED_TRUE; break; case ARM_STATE_DISARMING_TIMEOUT: // We get here when armed while throttle low, even when the arming timeout is not enabled if (settings.ArmedTimeout != 0) if (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings.ArmedTimeout) armState = ARM_STATE_DISARMED; // Switch to disarming due to manual control when needed if (manualDisarm) { armedDisarmStart = lastSysTime; armState = ARM_STATE_DISARMING_MANUAL; } break; case ARM_STATE_DISARMING_MANUAL: if (manualDisarm) { if (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS) armState = ARM_STATE_DISARMED; } else armState = ARM_STATE_ARMED; break; } } // Update cmd object when needed if (newCmdArmed != cmd.Armed) { cmd.Armed = newCmdArmed; ManualControlCommandSet(&cmd); } } else { // The throttle is not low, in case we where arming or disarming, abort switch(armState) { case ARM_STATE_DISARMING_MANUAL: case ARM_STATE_DISARMING_TIMEOUT: armState = ARM_STATE_ARMED; break; case ARM_STATE_ARMING_MANUAL: armState = ARM_STATE_DISARMED; break; default: // Nothing needs to be done in the other states break; } } // End of arming/disarming // Depending on the mode update the Stabilization or Actuator objects if (cmd.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL) { actuator.Roll = cmd.Roll; actuator.Pitch = cmd.Pitch; actuator.Yaw = cmd.Yaw; actuator.Throttle = cmd.Throttle; ActuatorDesiredSet(&actuator); } else if (cmd.FlightMode == MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED) { attitude.Roll = cmd.Roll * stabSettings.RollMax; attitude.Pitch = cmd.Pitch * stabSettings.PitchMax; attitude.Yaw = fmod(cmd.Yaw * 180.0, 360); attitude.Throttle = (cmd.Throttle < 0) ? -1 : cmd.Throttle; rate.Roll = cmd.Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL]; rate.Pitch = cmd.Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH]; rate.Yaw = cmd.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; AttitudeDesiredSet(&attitude); RateDesiredSet(&rate); } } }
/** * 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; } } } }
/** * Processes queue events */ static void processObjEvent(UAVObjEvent * ev) { UAVObjMetadata metadata; // FlightTelemetryStatsData flightStats; // GCSTelemetryStatsData gcsTelemetryStatsData; // int32_t retries; // int32_t success; if (ev->obj == 0) { updateTelemetryStats(); } else if (ev->obj == GCSTelemetryStatsHandle()) { gcsTelemetryStatsUpdated(); } else if (ev->obj == TelemetrySettingsHandle()) { updateSettings(); } else { // Get object metadata UAVObjGetMetadata(ev->obj, &metadata); // If this is a metaobject then make necessary telemetry updates if (UAVObjIsMetaobject(ev->obj)) { updateObject(UAVObjGetLinkedObj(ev->obj)); // linked object will be the actual object the metadata are for } mavlink_message_t msg; mavlink_system.sysid = 20; mavlink_system.compid = MAV_COMP_ID_IMU; mavlink_system.type = MAV_TYPE_FIXED_WING; uint8_t mavClass = MAV_AUTOPILOT_OPENPILOT; AlarmsClear(SYSTEMALARMS_ALARM_TELEMETRY); // Setup type and object id fields uint32_t objId = UAVObjGetID(ev->obj); // uint64_t timeStamp = 0; switch(objId) { case BAROALTITUDE_OBJID: { BaroAltitudeGet(&baroAltitude); pressure.press_abs = baroAltitude.Pressure*10.0f; pressure.temperature = baroAltitude.Temperature*100.0f; mavlink_msg_scaled_pressure_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &pressure); // Copy the message to the send buffer uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg); // Send buffer PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len); break; } case FLIGHTTELEMETRYSTATS_OBJID: { // FlightTelemetryStatsData flightTelemetryStats; FlightTelemetryStatsGet(&flightStats); // XXX this is a hack to make it think it got a confirmed // connection flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_CONNECTED; GCSTelemetryStatsGet(&gcsTelemetryStatsData); gcsTelemetryStatsData.Status = GCSTELEMETRYSTATS_STATUS_CONNECTED; // // // //mavlink_msg_heartbeat_send(MAVLINK_COMM_0,mavlink_system.type,mavClass); // mavlink_msg_heartbeat_pack(mavlink_system.sysid, mavlink_system.compid, &msg, mavlink_system.type, mavClass); // // Copy the message to the send buffer // uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg); // // Send buffer // PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len); break; } case SYSTEMSTATS_OBJID: { FlightStatusData flightStatus; FlightStatusGet(&flightStatus); uint8_t system_state = MAV_STATE_UNINIT; uint8_t base_mode = 0; uint8_t custom_mode = 0; // Set flight mode switch (flightStatus.FlightMode) { case FLIGHTSTATUS_FLIGHTMODE_MANUAL: base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; break; case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: base_mode |= MAV_MODE_FLAG_AUTO_ENABLED; break; case FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL: base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; break; default: base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; break; } // Set arming state switch (flightStatus.Armed) { case FLIGHTSTATUS_ARMED_ARMING: case FLIGHTSTATUS_ARMED_ARMED: system_state = MAV_STATE_ACTIVE; base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; break; case FLIGHTSTATUS_ARMED_DISARMED: system_state = MAV_STATE_STANDBY; base_mode &= !MAV_MODE_FLAG_SAFETY_ARMED; break; } // Set HIL if (hilEnabled) base_mode |= MAV_MODE_FLAG_HIL_ENABLED; mavlink_msg_heartbeat_send(MAVLINK_COMM_0, mavlink_system.type, mavClass, base_mode, custom_mode, system_state); SystemStatsData stats; SystemStatsGet(&stats); FlightBatteryStateData flightBatteryData; FlightBatteryStateGet(&flightBatteryData); FlightBatterySettingsData flightBatterySettings; FlightBatterySettingsGet(&flightBatterySettings); uint16_t batteryVoltage = (uint16_t)(flightBatteryData.Voltage*1000.0f); int16_t batteryCurrent = -1; // -1: Not present / not estimated int8_t batteryPercent = -1; // -1: Not present / not estimated // if (flightBatterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_CURRENTFACTOR] == 0) // { // Factor is zero, sensor is not present // Estimate remaining capacity based on lipo curve batteryPercent = 100.0f*((flightBatteryData.Voltage - 9.6f)/(12.6f - 9.6f)); // } // else // { // // Use capacity and current // batteryPercent = 100.0f*((flightBatterySettings.Capacity - flightBatteryData.ConsumedEnergy) / flightBatterySettings.Capacity); // batteryCurrent = flightBatteryData.Current*100; // } mavlink_msg_sys_status_send(MAVLINK_COMM_0, 0xFF, 0xFF, 0xFF, ((uint16_t)stats.CPULoad*10), batteryVoltage, batteryCurrent, batteryPercent, 0, 0, 0, 0, 0, 0); // // Copy the message to the send buffer // uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg); // // Send buffer // PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len); break; } case ATTITUDERAW_OBJID: { AttitudeRawGet(&attitudeRaw); // Copy data attitude_raw.xacc = attitudeRaw.accels[ATTITUDERAW_ACCELS_X]; attitude_raw.yacc = attitudeRaw.accels[ATTITUDERAW_ACCELS_Y]; attitude_raw.zacc = attitudeRaw.accels[ATTITUDERAW_ACCELS_Z]; attitude_raw.xgyro = attitudeRaw.gyros[ATTITUDERAW_GYROS_X]; attitude_raw.ygyro = attitudeRaw.gyros[ATTITUDERAW_GYROS_Y]; attitude_raw.zgyro = attitudeRaw.gyros[ATTITUDERAW_GYROS_Z]; attitude_raw.xmag = attitudeRaw.magnetometers[ATTITUDERAW_MAGNETOMETERS_X]; attitude_raw.ymag = attitudeRaw.magnetometers[ATTITUDERAW_MAGNETOMETERS_Y]; attitude_raw.zmag = attitudeRaw.magnetometers[ATTITUDERAW_MAGNETOMETERS_Z]; mavlink_msg_raw_imu_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &attitude_raw); // Copy the message to the send buffer uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg); // Send buffer PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len); if (hilEnabled) { mavlink_hil_controls_t controls; // Copy data controls.roll_ailerons = 0.1; controls.pitch_elevator = 0.1; controls.yaw_rudder = 0.0; controls.throttle = 0.8; mavlink_msg_hil_controls_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &controls); // Copy the message to the send buffer len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg); // Send buffer PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len); } break; } case ATTITUDEMATRIX_OBJID: { AttitudeMatrixGet(&attitudeMatrix); // Copy data attitude.roll = attitudeMatrix.Roll; attitude.pitch = attitudeMatrix.Pitch; attitude.yaw = attitudeMatrix.Yaw; attitude.rollspeed = attitudeMatrix.AngularRates[0]; attitude.pitchspeed = attitudeMatrix.AngularRates[1]; attitude.yawspeed = attitudeMatrix.AngularRates[2]; mavlink_msg_attitude_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &attitude); // Copy the message to the send buffer uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg); // Send buffer PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len); break; } case GPSPOSITION_OBJID: { GPSPositionGet(&gpsPosition); gps_raw.time_usec = 0; gps_raw.lat = gpsPosition.Latitude*10; gps_raw.lon = gpsPosition.Longitude*10; gps_raw.alt = gpsPosition.Altitude*10; gps_raw.eph = gpsPosition.HDOP*100; gps_raw.epv = gpsPosition.VDOP*100; gps_raw.cog = gpsPosition.Heading*100; gps_raw.satellites_visible = gpsPosition.Satellites; gps_raw.fix_type = gpsPosition.Status; mavlink_msg_gps_raw_int_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &gps_raw); // Copy the message to the send buffer uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg); // Send buffer PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len); // mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, gps_raw.usec, gps_raw.lat, gps_raw.lon, gps_raw.alt, gps_raw.eph, gps_raw.epv, gps_raw.hdg, gps_raw.satellites_visible, gps_raw.fix_type, 0); break; } case POSITIONACTUAL_OBJID: { PositionActualData pos; PositionActualGet(&pos); mavlink_local_position_ned_t m_pos; m_pos.time_boot_ms = 0; m_pos.x = pos.North; m_pos.y = pos.East; m_pos.z = pos.Down; m_pos.vx = 0.0f; m_pos.vy = 0.0f; m_pos.vz = 0.0f; mavlink_msg_local_position_ned_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &m_pos); // Copy the message to the send buffer uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg); // Send buffer PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len); } break; case ACTUATORCOMMAND_OBJID: { mavlink_rc_channels_scaled_t rc; float val; ManualControlCommandRollGet(&val); rc.chan1_scaled = val*1000; ManualControlCommandPitchGet(&val); rc.chan2_scaled = val*1000; ManualControlCommandYawGet(&val); rc.chan3_scaled = val*1000; ManualControlCommandThrottleGet(&val); rc.chan4_scaled = val*1000; ActuatorCommandData act; ActuatorCommandGet(&act); rc.chan5_scaled = act.Channel[0]; rc.chan6_scaled = act.Channel[1]; rc.chan7_scaled = act.Channel[2]; rc.chan8_scaled = act.Channel[3]; ManualControlCommandData cmd; ManualControlCommandGet(&cmd); rc.rssi = ((uint8_t)(cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_TRUE))*255; rc.port = 0; mavlink_msg_rc_channels_scaled_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &rc); // Copy the message to the send buffer uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg); // Send buffer PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEM_RF, mavlinkTxBuf, len); break; } case MANUALCONTROLCOMMAND_OBJID: { mavlink_rc_channels_scaled_t rc; float val; ManualControlCommandRollGet(&val); rc.chan1_scaled = val*1000; ManualControlCommandPitchGet(&val); rc.chan2_scaled = val*1000; ManualControlCommandYawGet(&val); rc.chan3_scaled = val*1000; ManualControlCommandThrottleGet(&val); rc.chan4_scaled = val*1000; rc.chan5_scaled = 0; rc.chan6_scaled = 0; rc.chan7_scaled = 0; rc.chan8_scaled = 0; ManualControlCommandData cmd; ManualControlCommandGet(&cmd); rc.rssi = ((uint8_t)(cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_TRUE))*255; rc.port = 0; mavlink_msg_rc_channels_scaled_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &rc); // Copy the message to the send buffer uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg); // Send buffer PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEM_RF, mavlinkTxBuf, len); break; } default: { //printf("unknown object: %x\n",(unsigned int)objId); break; } } } }