Esempio n. 1
0
/**
 * 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);
        }
    }
}
Esempio n. 2
0
/**
 * Called each time the GCS telemetry stats object is updated.
 * Trigger a flight telemetry stats update if a connection is not
 * yet established.
 */
static void gcsTelemetryStatsUpdated()
{
	FlightTelemetryStatsData flightStats;
	GCSTelemetryStatsData gcsStats;
	FlightTelemetryStatsGet(&flightStats);
	GCSTelemetryStatsGet(&gcsStats);
	if (flightStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED || gcsStats.Status != GCSTELEMETRYSTATS_STATUS_CONNECTED) {
		updateTelemetryStats();
	}
}
Esempio n. 3
0
/**
 * 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
			}
		}
	}
}
Esempio n. 4
0
/**
 * 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);
		}
	}
}
Esempio n. 6
0
/**
  * @brief Process inputs and arming
  *
  * This will always process the arming signals as for now the transmitter
  * is always in charge.  When a transmitter is not detected control will
  * fall back to the failsafe module.  If the flight mode is in tablet
  * control position then control will be ceeded to that module.
  */
int32_t transmitter_control_update()
{
	lastSysTime = PIOS_Thread_Systime();

	float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM];
	bool validChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM];

	if (settings_updated) {
		settings_updated = false;
		ManualControlSettingsGet(&settings);
	}

	/* Update channel activity monitor */
	uint8_t arm_status;
	FlightStatusArmedGet(&arm_status);

	if (arm_status == FLIGHTSTATUS_ARMED_DISARMED) {
		if (updateRcvrActivity(&activity_fsm)) {
			/* Reset the aging timer because activity was detected */
			lastActivityTime = lastSysTime;
		}
	}
	if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) {
		resetRcvrActivity(&activity_fsm);
		lastActivityTime = lastSysTime;
	}

	if (ManualControlCommandReadOnly()) {
		FlightTelemetryStatsData flightTelemStats;
		FlightTelemetryStatsGet(&flightTelemStats);
		if(flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
			/* trying to fly via GCS and lost connection.  fall back to transmitter */
			UAVObjMetadata metadata;
			ManualControlCommandGetMetadata(&metadata);
			UAVObjSetAccess(&metadata, ACCESS_READWRITE);
			ManualControlCommandSetMetadata(&metadata);
		}

		// Don't process anything else when GCS is overriding the objects
		return 0;
	}

	if (settings.RssiType != MANUALCONTROLSETTINGS_RSSITYPE_NONE) {
		int32_t value = 0;
		extern uintptr_t pios_rcvr_group_map[];
		switch (settings.RssiType) {
		case MANUALCONTROLSETTINGS_RSSITYPE_PWM:
			value = PIOS_RCVR_Read(pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM], settings.RssiChannelNumber);
			break;
		case MANUALCONTROLSETTINGS_RSSITYPE_PPM:
			value = PIOS_RCVR_Read(pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM], settings.RssiChannelNumber);
			break;
		case MANUALCONTROLSETTINGS_RSSITYPE_ADC:
#if defined(PIOS_INCLUDE_ADC)
			value = PIOS_ADC_GetChannelRaw(settings.RssiChannelNumber);
#endif
			break;
		case MANUALCONTROLSETTINGS_RSSITYPE_OPENLRS:
#if defined(PIOS_INCLUDE_OPENLRS_RCVR)
			value = PIOS_OpenLRS_RSSI_Get();
#endif /* PIOS_INCLUDE_OPENLRS_RCVR */
			break;
		case MANUALCONTROLSETTINGS_RSSITYPE_FRSKYPWM:
#if defined(PIOS_INCLUDE_FRSKY_RSSI)
			value = PIOS_FrSkyRssi_Get();
#endif /* PIOS_INCLUDE_FRSKY_RSSI */
			break;
		case MANUALCONTROLSETTINGS_RSSITYPE_SBUS:
#if defined(PIOS_INCLUDE_SBUS)
			value = PIOS_RCVR_Read(pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS], settings.RssiChannelNumber);
#endif /* PIOS_INCLUDE_SBUS */
			break;
		}
		if(value < 0)
			value = 0;
		if (settings.RssiMax == settings.RssiMin)
			cmd.Rssi = 0;
		else
			cmd.Rssi = ((float)(value - settings.RssiMin)/((float)settings.RssiMax-settings.RssiMin)) * 100;
		cmd.RawRssi = value;
	}

	bool valid_input_detected = true;

	// Read channel values in us
	for (uint8_t n = 0; 
	     n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM;
	     ++n) {
		extern uintptr_t pios_rcvr_group_map[];

		if (settings.ChannelGroups[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
			cmd.Channel[n] = PIOS_RCVR_INVALID;
			validChannel[n] = false;
		} else {
			cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[settings.ChannelGroups[n]],
							settings.ChannelNumber[n]);
		}

		// If a channel has timed out this is not valid data and we shouldn't update anything
		// until we decide to go to failsafe
		if(cmd.Channel[n] == (uint16_t) PIOS_RCVR_TIMEOUT) {
			valid_input_detected = false;
			validChannel[n] = false;
		} else {
			scaledChannel[n] = scaleChannel(n, cmd.Channel[n]);
			validChannel[n] = validInputRange(n, cmd.Channel[n], CONNECTION_OFFSET);
		}
	}

	// Check settings, if error raise alarm
	if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
		settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
		settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
		settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
		// Check all channel mappings are valid
		cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_INVALID ||
		cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_INVALID ||
		cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_INVALID ||
		cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_INVALID ||
		// Check the driver exists
		cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_NODRIVER ||
		cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_NODRIVER ||
		cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_NODRIVER ||
		cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_NODRIVER ||
		// Check the FlightModeNumber is valid
		settings.FlightModeNumber < 1 || settings.FlightModeNumber > MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM ||
		// If we've got more than one possible valid FlightMode, we require a configured FlightMode channel
		((settings.FlightModeNumber > 1) && (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE)) ||
		// Whenever FlightMode channel is configured, it needs to be valid regardless of FlightModeNumber settings
		((settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] < MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) && (
			cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_INVALID ||
			cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_NODRIVER ))) {

		set_manual_control_error(SYSTEMALARMS_MANUALCONTROL_SETTINGS);

		cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
		ManualControlCommandSet(&cmd);

		// Need to do this here since we don't process armed status.  Since this shouldn't happen in flight (changed config) 
		// immediately disarm
		pending_control_event = CONTROL_EVENTS_DISARM;

		return -1;
	}

	// the block above validates the input configuration. this simply checks that the range is valid if flight mode is configured.
	bool flightmode_valid_input = settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
	    validChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE];

	// because arming is optional we must determine if it is needed before checking it is valid
	bool arming_valid_input = (settings.Arming < MANUALCONTROLSETTINGS_ARMING_SWITCH) ||
		validChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ARMING];

	// decide if we have valid manual input or not
	valid_input_detected &= validChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] &&
	    validChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] &&
	    validChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] &&
	    validChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] &&
	    flightmode_valid_input &&
	    arming_valid_input;

	// Implement hysteresis loop on connection status
	if (valid_input_detected && (++connected_count > 10)) {
		cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
		connected_count = 0;
		disconnected_count = 0;
	} else if (!valid_input_detected && (++disconnected_count > 10)) {
		cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
		connected_count = 0;
		disconnected_count = 0;
	}

	if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
		// These values are not used but just put ManualControlCommand in a sane state.  When
		// Connected is false, then the failsafe submodule will be in control.

		cmd.Throttle = -1;
		cmd.Roll = 0;
		cmd.Yaw = 0;
		cmd.Pitch = 0;
		cmd.Collective = 0;

		set_manual_control_error(SYSTEMALARMS_MANUALCONTROL_NORX);

	} else if (valid_input_detected) {
		set_manual_control_error(SYSTEMALARMS_MANUALCONTROL_NONE);

		// Scale channels to -1 -> +1 range
		cmd.Roll           = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL];
		cmd.Pitch          = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH];
		cmd.Yaw            = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW];
		cmd.Throttle       = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE];
		cmd.ArmSwitch      = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ARMING] > 0 ? 
		                     MANUALCONTROLCOMMAND_ARMSWITCH_ARMED : MANUALCONTROLCOMMAND_ARMSWITCH_DISARMED;
		flight_mode_value  = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE];

		// Apply deadband for Roll/Pitch/Yaw stick inputs
		if (settings.Deadband) {
			applyDeadband(&cmd.Roll, settings.Deadband);
			applyDeadband(&cmd.Pitch, settings.Deadband);
			applyDeadband(&cmd.Yaw, settings.Deadband);
		}

		if(cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_INVALID &&
		   cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_NODRIVER &&
		   cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_TIMEOUT) {
			cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE];
		}
		   
		AccessoryDesiredData accessory;
		// Set Accessory 0
		if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != 
			MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
			accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0];
			if(AccessoryDesiredInstSet(0, &accessory) != 0) //These are allocated later and that allocation might fail
				set_manual_control_error(SYSTEMALARMS_MANUALCONTROL_ACCESSORY);
		}
		// Set Accessory 1
		if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != 
			MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
			accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1];
			if(AccessoryDesiredInstSet(1, &accessory) != 0) //These are allocated later and that allocation might fail
				set_manual_control_error(SYSTEMALARMS_MANUALCONTROL_ACCESSORY);
		}
		// Set Accessory 2
		if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != 
			MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
			accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2];
			if(AccessoryDesiredInstSet(2, &accessory) != 0) //These are allocated later and that allocation might fail
				set_manual_control_error(SYSTEMALARMS_MANUALCONTROL_ACCESSORY);
		}

	}

	// Process arming outside conditional so system will disarm when disconnected.  Notice this
	// is processed in the _update method instead of _select method so the state system is always
	// evalulated, even if not detected.
	process_transmitter_events(&cmd, &settings, valid_input_detected);
	
	// Update cmd object
	ManualControlCommandSet(&cmd);

	return 0;
}
Esempio n. 7
0
/**
 * Update telemetry statistics and handle connection handshake
 */
static void updateTelemetryStats()
{
	UAVTalkStats utalkStats;
	FlightTelemetryStatsData flightStats;
	GCSTelemetryStatsData gcsStats;
	uint8_t forceUpdate;
	uint8_t connectionTimeout;
	uint32_t timeNow;

	// Get stats
	UAVTalkGetStats(&utalkStats);
	UAVTalkResetStats();

	// Get object data
	FlightTelemetryStatsGet(&flightStats);
	GCSTelemetryStatsGet(&gcsStats);

	// Update stats object
	if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
		flightStats.RxDataRate = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0);
		flightStats.TxDataRate = (float)utalkStats.txBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0);
		flightStats.RxFailures += utalkStats.rxErrors;
		flightStats.TxFailures += txErrors;
		flightStats.TxRetries += txRetries;
		txErrors = 0;
		txRetries = 0;
	} else {
		flightStats.RxDataRate = 0;
		flightStats.TxDataRate = 0;
		flightStats.RxFailures = 0;
		flightStats.TxFailures = 0;
		flightStats.TxRetries = 0;
		txErrors = 0;
		txRetries = 0;
	}

	// Check for connection timeout
	timeNow = xTaskGetTickCount() * portTICK_RATE_MS;
	if (utalkStats.rxObjects > 0) {
		timeOfLastObjectUpdate = timeNow;
	}
	if ((timeNow - timeOfLastObjectUpdate) > CONNECTION_TIMEOUT_MS) {
		connectionTimeout = 1;
	} else {
		connectionTimeout = 0;
	}

	// Update connection state
	forceUpdate = 1;
	if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED) {
		// Wait for connection request
		if (gcsStats.Status == GCSTELEMETRYSTATS_STATUS_HANDSHAKEREQ) {
			flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_HANDSHAKEACK;
		}
	} else if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_HANDSHAKEACK) {
		// Wait for connection
		if (gcsStats.Status == GCSTELEMETRYSTATS_STATUS_CONNECTED) {
			flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_CONNECTED;
		} else if (gcsStats.Status == GCSTELEMETRYSTATS_STATUS_DISCONNECTED) {
			flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED;
		}
	} else if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
		if (gcsStats.Status != GCSTELEMETRYSTATS_STATUS_CONNECTED || connectionTimeout) {
			flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED;
		} else {
			forceUpdate = 0;
		}
	} else {
		flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED;
	}

	// Update the telemetry alarm
	if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
		AlarmsClear(SYSTEMALARMS_ALARM_TELEMETRY);
	} else {
		AlarmsSet(SYSTEMALARMS_ALARM_TELEMETRY, SYSTEMALARMS_ALARM_ERROR);
	}

	// Update object
	FlightTelemetryStatsSet(&flightStats);

	// Force telemetry update if not connected
	if (forceUpdate) {
		FlightTelemetryStatsUpdated();
	}
}
/**
 * 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;
		}
		}
	}
}
/**
 * Update telemetry statistics and handle connection handshake
 */
static void updateTelemetryStats()
{
	//	flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_CONNECTED;
	//	gcsStats.Status = GCSTELEMETRYSTATS_STATUS_CONNECTED;

	FlightTelemetryStatsData flightStats;
	GCSTelemetryStatsData gcsStats;
	uint8_t forceUpdate;
	uint8_t connectionTimeout;
	uint32_t timeNow;

	// Get object data
	FlightTelemetryStatsGet(&flightStats);
	GCSTelemetryStatsGet(&gcsStats);

	// Update stats object
	if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
		//flightStats.RxDataRate = (float)utalkStats.rxBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0);
		//flightStats.TxDataRate = (float)utalkStats.txBytes / ((float)STATS_UPDATE_PERIOD_MS / 1000.0);
		//flightStats.RxFailures += utalkStats.rxErrors;
		flightStats.TxFailures += txErrors;
		flightStats.TxRetries += txRetries;
		txErrors = 0;
		txRetries = 0;
	} else {
		flightStats.RxDataRate = 0;
		flightStats.TxDataRate = 0;
		flightStats.RxFailures = 0;
		flightStats.TxFailures = 0;
		flightStats.TxRetries = 0;
		txErrors = 0;
		txRetries = 0;
	}

	// Check for connection timeout
	timeNow = xTaskGetTickCount() * portTICK_RATE_MS;
	if ((timeNow - lastOperatorHeartbeat) > CONNECTION_TIMEOUT_MS) {
		connectionTimeout = 1;
	} else {
		connectionTimeout = 0;
	}

	// Update connection state
	forceUpdate = 1;

	// FIXME Hardcode value for now
	//	connectionTimeout = 0;

	// If the GCS heartbeat has been received in the last five seconds
	// set as connected
	if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED) {
		if (connectionTimeout == 0) {
			// Switching from disconnected to connected
			flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_CONNECTED;
			gcsStats.Status = GCSTELEMETRYSTATS_STATUS_CONNECTED;
		} else {
			// Not switching, no update needed
			forceUpdate = 0;
		}
	} else {
		// We were connected, checking if we're still connected
		if (connectionTimeout == 1) {
			flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED;
			gcsStats.Status = GCSTELEMETRYSTATS_STATUS_DISCONNECTED;
		} else {
			// We were connected and still are, no update needed
			forceUpdate = 0;
		}
	}

	// FIXME HARDCODED VALUES
	flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_CONNECTED;
	gcsStats.Status = GCSTELEMETRYSTATS_STATUS_CONNECTED;
	forceUpdate = 1;

	//	if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED) {
	//		// Wait for connection request
	//		if (gcsStats.Status == GCSTELEMETRYSTATS_STATUS_HANDSHAKEREQ) {
	//			flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_HANDSHAKEACK;
	//		}
	//	} else if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_HANDSHAKEACK) {
	//		// Wait for connection
	//		if (gcsStats.Status == GCSTELEMETRYSTATS_STATUS_CONNECTED) {
	//			flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_CONNECTED;
	//		} else if (gcsStats.Status == GCSTELEMETRYSTATS_STATUS_DISCONNECTED) {
	//			flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED;
	//		}
	//	} else if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
	//		if (gcsStats.Status != GCSTELEMETRYSTATS_STATUS_CONNECTED || connectionTimeout) {
	//			flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED;
	//		} else {
	//			forceUpdate = 0;
	//		}
	//	} else {
	//		flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED;
	//	}

	// Update the telemetry alarm
	if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
		AlarmsClear(SYSTEMALARMS_ALARM_TELEMETRY);
	} else {
		AlarmsSet(SYSTEMALARMS_ALARM_TELEMETRY, SYSTEMALARMS_ALARM_ERROR);
	}

	// Update object
	FlightTelemetryStatsSet(&flightStats);

	// Force telemetry update if not connected
	if (forceUpdate) {
		FlightTelemetryStatsUpdated();
	}
}