//! Initialize the transmitter control mode
int32_t transmitter_control_initialize()
{
AccessoryDesiredInitialize();
ManualControlCommandInitialize();
FlightStatusInitialize();
StabilizationDesiredInitialize();
ReceiverActivityInitialize();
ManualControlSettingsInitialize();
// Both the gimbal and coptercontrol do not support loitering
#if !defined(SMALLF1) && !defined(GIMBAL)
LoiterCommandInitialize();
#endif
/* For now manual instantiate extra instances of Accessory Desired. In future */
/* should be done dynamically this includes not even registering it if not used */
AccessoryDesiredCreateInstance();
AccessoryDesiredCreateInstance();
/* No pending control events */
pending_control_event = CONTROL_EVENTS_NONE;
/* Initialize the RcvrActivty FSM */
lastActivityTime = PIOS_Thread_Systime();
resetRcvrActivity(&activity_fsm);
// Use callback to update the settings when they change
ManualControlSettingsConnectCallbackCtx(UAVObjCbSetFlag, &settings_updated);
settings_updated = true;
// Main task loop
lastSysTime = PIOS_Thread_Systime();
return 0;
}
//! Initialize the transmitter control mode
int32_t transmitter_control_initialize()
{
/* Check the assumptions about uavobject enum's are correct */
if(!assumptions)
return -1;
AccessoryDesiredInitialize();
ManualControlCommandInitialize();
FlightStatusInitialize();
StabilizationDesiredInitialize();
ReceiverActivityInitialize();
ManualControlSettingsInitialize();
// Both the gimbal and coptercontrol do not support loitering
#if !defined(COPTERCONTROL) && !defined(GIMBAL)
LoiterCommandInitialize();
#endif
/* For now manual instantiate extra instances of Accessory Desired. In future */
/* should be done dynamically this includes not even registering it if not used */
AccessoryDesiredCreateInstance();
AccessoryDesiredCreateInstance();
/* Reset the state machine for processing stick movements */
arm_state = ARM_STATE_DISARMED;
/* No pending control events */
pending_control_event = CONTROL_EVENTS_NONE;
/* Initialize the RcvrActivty FSM */
lastActivityTime = xTaskGetTickCount();
resetRcvrActivity(&activity_fsm);
// Use callback to update the settings when they change
ManualControlSettingsConnectCallback(manual_control_settings_updated);
manual_control_settings_updated(NULL);
// Main task loop
lastSysTime = xTaskGetTickCount();
return 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;
}
