/**
* 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;
}
}
}
/**
* @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;
}
/**
* 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);
}
}
}
/**
* Module task
*/
static void manualControlTask(void *parameters)
{
ManualControlSettingsData settings;
StabilizationSettingsData stabSettings;
ManualControlCommandData cmd;
ActuatorDesiredData actuator;
AttitudeDesiredData attitude;
portTickType lastSysTime;
portTickType armedDisarmStart = 0;
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);
// Main task loop
lastSysTime = xTaskGetTickCount();
while (1) {
// Wait until next update
vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS);
// Read settings
ManualControlSettingsGet(&settings);
StabilizationSettingsGet(&stabSettings);
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
}
// Calculate roll command in range +1 to -1
cmd.Roll = scaleChannel(cmd.Channel[settings.Roll], settings.ChannelMax[settings.Roll],
settings.ChannelMin[settings.Roll], settings.ChannelNeutral[settings.Roll]);
// Calculate pitch command in range +1 to -1
cmd.Pitch = scaleChannel(cmd.Channel[settings.Pitch], settings.ChannelMax[settings.Pitch],
settings.ChannelMin[settings.Pitch], settings.ChannelNeutral[settings.Pitch]);
// Calculate yaw command in range +1 to -1
cmd.Yaw = scaleChannel(cmd.Channel[settings.Yaw], settings.ChannelMax[settings.Yaw],
settings.ChannelMin[settings.Yaw], settings.ChannelNeutral[settings.Yaw]);
// Calculate throttle command in range +1 to -1
cmd.Throttle = scaleChannel(cmd.Channel[settings.Throttle], settings.ChannelMax[settings.Throttle],
settings.ChannelMin[settings.Throttle], settings.ChannelNeutral[settings.Throttle]);
if (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE)
cmd.Accessory1 = scaleChannel(cmd.Channel[settings.Accessory1], settings.ChannelMax[settings.Accessory1],
settings.ChannelMin[settings.Accessory1], settings.ChannelNeutral[settings.Accessory1]);
else
cmd.Accessory1 = 0;
if (settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE)
cmd.Accessory2 = scaleChannel(cmd.Channel[settings.Accessory2], settings.ChannelMax[settings.Accessory2],
settings.ChannelMin[settings.Accessory2], settings.ChannelNeutral[settings.Accessory2]);
else
cmd.Accessory2 = 0;
if (settings.Accessory3 != MANUALCONTROLSETTINGS_ACCESSORY3_NONE)
cmd.Accessory3 = scaleChannel(cmd.Channel[settings.Accessory3], settings.ChannelMax[settings.Accessory3],
settings.ChannelMin[settings.Accessory3], settings.ChannelNeutral[settings.Accessory3]);
else
cmd.Accessory3 = 0;
// Update flight mode
flightMode = scaleChannel(cmd.Channel[settings.FlightMode], settings.ChannelMax[settings.FlightMode],
settings.ChannelMin[settings.FlightMode], settings.ChannelNeutral[settings.FlightMode]);
if (flightMode < -FLIGHT_MODE_LIMIT) { // Position 1
for(int i = 0; i < 3; i++) {
if(settings.Pos1StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_NONE)
cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE;
else if(settings.Pos1StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_RATE)
cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE;
else if(settings.Pos1StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS1STABILIZATIONSETTINGS_ATTITUDE)
cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE;
}
if(settings.Pos1FlightMode == MANUALCONTROLSETTINGS_POS1FLIGHTMODE_MANUAL)
cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL;
else if(settings.Pos1FlightMode == MANUALCONTROLSETTINGS_POS1FLIGHTMODE_STABILIZED)
cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED;
else if(settings.Pos1FlightMode == MANUALCONTROLSETTINGS_POS1FLIGHTMODE_AUTO)
cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO;
} else if (flightMode > FLIGHT_MODE_LIMIT) { // Position 3
for(int i = 0; i < 3; i++) {
if(settings.Pos3StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS3STABILIZATIONSETTINGS_NONE)
cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE;
else if(settings.Pos3StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS3STABILIZATIONSETTINGS_RATE)
cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE;
else if(settings.Pos3StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS3STABILIZATIONSETTINGS_ATTITUDE)
cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE;
}
if(settings.Pos3FlightMode == MANUALCONTROLSETTINGS_POS3FLIGHTMODE_MANUAL)
cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL;
else if(settings.Pos3FlightMode == MANUALCONTROLSETTINGS_POS3FLIGHTMODE_STABILIZED)
cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED;
else if(settings.Pos3FlightMode == MANUALCONTROLSETTINGS_POS3FLIGHTMODE_AUTO)
cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO;
} else { // Position 2
for(int i = 0; i < 3; i++) {
if(settings.Pos2StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_NONE)
cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_NONE;
else if(settings.Pos2StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_RATE)
cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_RATE;
else if(settings.Pos2StabilizationSettings[i] == MANUALCONTROLSETTINGS_POS2STABILIZATIONSETTINGS_ATTITUDE)
cmd.StabilizationSettings[i] = MANUALCONTROLCOMMAND_STABILIZATIONSETTINGS_ATTITUDE;
}
if(settings.Pos2FlightMode == MANUALCONTROLSETTINGS_POS2FLIGHTMODE_MANUAL)
cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_MANUAL;
else if(settings.Pos2FlightMode == MANUALCONTROLSETTINGS_POS2FLIGHTMODE_STABILIZED)
cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_STABILIZED;
else if(settings.Pos2FlightMode == MANUALCONTROLSETTINGS_POS2FLIGHTMODE_AUTO)
cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO;
}
// 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);
}
/* Look for arm or disarm signal */
if ((cmd.Throttle <= 0.05) && (cmd.Roll <= -0.95)) {
if ((armedDisarmStart == 0) || (lastSysTime < armedDisarmStart)) // store when started, deal with rollover
armedDisarmStart = lastSysTime;
else if ((lastSysTime - armedDisarmStart) > (1000 * portTICK_RATE_MS))
cmd.Armed = MANUALCONTROLCOMMAND_ARMED_TRUE;
} else if ((cmd.Throttle <= 0.05) && (cmd.Roll >= 0.95)) {
if ((armedDisarmStart == 0) || (lastSysTime < armedDisarmStart)) // store when started, deal with rollover
armedDisarmStart = lastSysTime;
else if ((lastSysTime - armedDisarmStart) > (1000 * portTICK_RATE_MS))
cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE;
} else {
armedDisarmStart = 0;
}
// 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;
AttitudeDesiredSet(&attitude);
}
if (cmd.Accessory3 < -.5) { //TODO: Make what happens here depend on GCS
AHRSSettingsData attitudeSettings;
AHRSSettingsGet(&attitudeSettings);
// Hard coding a maximum bias of 15 for now... maybe mistake
attitudeSettings.PitchBias = cmd.Accessory1 * 15;
attitudeSettings.RollBias = cmd.Accessory2 * 15;
AHRSSettingsSet(&attitudeSettings);
}
}
}
