/**
* Select and use failsafe control
* @param [in] reset_controller True if previously another controller was used
*/
int32_t failsafe_control_select(bool reset_controller)
{
if (reset_controller) {
FlightStatusArmedOptions armed;
FlightStatusArmedGet(&armed);
armed_when_enabled = (armed == FLIGHTSTATUS_ARMED_ARMED);
}
uint8_t flight_status;
FlightStatusFlightModeGet(&flight_status);
if (flight_status != FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 || reset_controller) {
flight_status = FLIGHTSTATUS_FLIGHTMODE_STABILIZED1;
FlightStatusFlightModeSet(&flight_status);
}
#ifdef GIMBAL
// Gimbals do not need failsafe
StabilizationDesiredData stabilization_desired;
StabilizationDesiredGet(&stabilization_desired);
stabilization_desired.Throttle = -1;
stabilization_desired.Roll = 0;
stabilization_desired.Pitch = 0;
stabilization_desired.Yaw = 0;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_POI;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_POI;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
StabilizationDesiredSet(&stabilization_desired);
#else
// Pick default values that will roughly cause a plane to circle down
// and a quad to fall straight down
StabilizationDesiredData stabilization_desired;
StabilizationDesiredGet(&stabilization_desired);
if (!armed_when_enabled) {
/* disable stabilization so outputs do not move when system was not armed */
stabilization_desired.Throttle = -1;
stabilization_desired.Roll = 0;
stabilization_desired.Pitch = 0;
stabilization_desired.Yaw = 0;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
} else {
/* Pick default values that will roughly cause a plane to circle down and */
/* a quad to fall straight down */
stabilization_desired.Throttle = -1;
stabilization_desired.Roll = -10;
stabilization_desired.Pitch = 0;
stabilization_desired.Yaw = -5;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
}
StabilizationDesiredSet(&stabilization_desired);
#endif
return 0;
}
/* check access level */
bool security(int neededlevel)
{
if (accesslevel < neededlevel)
// access level is insufficient
return false;
FlightStatusData data;
FlightStatusArmedGet(&data.Armed);
// in level 1 flightstatus has also to be disarmed
if ((accesslevel <= 1) && (data.Armed != FLIGHTSTATUS_ARMED_DISARMED))
return false;
// level 2 or higher is currently like root
// all checks are ok
return true;
}
void updatedCb(UAVObjEvent *ev)
{
if (!ev || ev->obj == FlightStatusHandle()) {
static uint8_t last_armed = 0xff;
static uint8_t last_flightmode = 0xff;
uint8_t armed;
uint8_t flightmode;
FlightStatusArmedGet(&armed);
FlightStatusFlightModeGet(&flightmode);
if (last_armed != armed || (armed && flightmode != last_flightmode)) {
if (armed) {
PIOS_NOTIFICATION_Default_Ext_Led_Play(flightModeMap[flightmode], NOTIFY_PRIORITY_BACKGROUND);
} else {
PIOS_NOTIFICATION_Default_Ext_Led_Play(¬ifications[NOTIFY_SEQUENCE_DISARMED], NOTIFY_PRIORITY_BACKGROUND);
}
}
last_armed = armed;
last_flightmode = flightmode;
}
}
/**
* @brief If requested accumulate accel values to calculate level
* @param[in] accelsData the scaled and normalized accels
*/
static void update_trimming(AccelsData * accelsData)
{
if (trim_requested) {
if (trim_samples >= MAX_TRIM_FLIGHT_SAMPLES) {
trim_requested = false;
} else {
uint8_t armed;
float throttle;
FlightStatusArmedGet(&armed);
ManualControlCommandThrottleGet(&throttle); // Until flight status indicates airborne
if ((armed == FLIGHTSTATUS_ARMED_ARMED) && (throttle > 0)) {
trim_samples++;
// Store the digitally scaled version since that is what we use for bias
trim_accels[0] += accelsData->x;
trim_accels[1] += accelsData->y;
trim_accels[2] += accelsData->z;
}
}
}
}
/**
* Select and use failsafe control
* @param [in] reset_controller True if previously another controller was used
*/
int32_t failsafe_control_select(bool reset_controller)
{
if (reset_controller) {
FlightStatusArmedOptions armed;
FlightStatusArmedGet(&armed);
armed_when_enabled = (armed == FLIGHTSTATUS_ARMED_ARMED);
rth_active = false;
/* Get configured failsafe behavior */
uint8_t failsafe_behavior;
ManualControlSettingsFailsafeBehaviorGet(&failsafe_behavior);
/* check whether RTH failsafe is configured and can be used */
if (failsafe_behavior == MANUALCONTROLSETTINGS_FAILSAFEBEHAVIOR_RTH
&& check_rth_preconditions_met())
rth_active = true;
}
uint8_t flight_mode_desired = rth_active ? FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME : FLIGHTSTATUS_FLIGHTMODE_STABILIZED1;
uint8_t flight_mode_actual;
FlightStatusFlightModeGet(&flight_mode_actual);
if (flight_mode_actual != flight_mode_desired || reset_controller) {
FlightStatusFlightModeSet(&flight_mode_desired);
}
#ifdef GIMBAL
// Gimbals do not need failsafe
StabilizationDesiredData stabilization_desired;
StabilizationDesiredGet(&stabilization_desired);
stabilization_desired.Throttle = -1;
stabilization_desired.Roll = 0;
stabilization_desired.Pitch = 0;
stabilization_desired.Yaw = 0;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_POI;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_POI;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
StabilizationDesiredSet(&stabilization_desired);
#else
if (!rth_active) {
// Pick default values that will roughly cause a plane to circle down
// and a quad to fall straight down
StabilizationDesiredData stabilization_desired;
StabilizationDesiredGet(&stabilization_desired);
if (!armed_when_enabled) {
/* disable stabilization so outputs do not move when system was not armed */
stabilization_desired.Throttle = -1;
stabilization_desired.Roll = 0;
stabilization_desired.Pitch = 0;
stabilization_desired.Yaw = 0;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
} else {
/* Pick default values that will roughly cause a plane to circle down and */
/* a quad to fall straight down */
stabilization_desired.Throttle = -1;
stabilization_desired.Roll = -10;
stabilization_desired.Pitch = 0;
stabilization_desired.Yaw = -5;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilization_desired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
}
StabilizationDesiredSet(&stabilization_desired);
}
#endif
return 0;
}
int32_t LoggingStart(void)
{
DebugLogSettingsConnectCallback(SettingsUpdatedCb);
DebugLogControlConnectCallback(ControlUpdatedCb);
FlightStatusConnectCallback(FlightStatusUpdatedCb);
SettingsUpdatedCb(DebugLogSettingsHandle());
UAVObjEvent ev = {
.obj = DebugLogSettingsHandle(),
.instId = 0,
.event = EV_UPDATED_PERIODIC,
.lowPriority = true,
};
EventPeriodicCallbackCreate(&ev, StatusUpdatedCb, 1000);
// invoke a periodic dispatcher callback - the event struct is a dummy, it could be filled with anything!
StatusUpdatedCb(&ev);
return 0;
}
MODULE_INITCALL(LoggingInitialize, LoggingStart);
static void StatusUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
PIOS_DEBUGLOG_Info(&status.Flight, &status.Entry, &status.FreeSlots, &status.UsedSlots);
DebugLogStatusSet(&status);
}
static void FlightStatusUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
FlightStatusGet(&flightstatus);
if (settings.LoggingEnabled == DEBUGLOGSETTINGS_LOGGINGENABLED_ONLYWHENARMED) {
if (flightstatus.Armed != FLIGHTSTATUS_ARMED_ARMED) {
PIOS_DEBUGLOG_Printf("FlightStatus Disarmed: On board Logging disabled.");
PIOS_DEBUGLOG_Enable(0);
} else {
PIOS_DEBUGLOG_Enable(1);
PIOS_DEBUGLOG_Printf("FlightStatus Armed: On board logging enabled.");
}
}
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
DebugLogSettingsGet(&settings);
if (settings.LoggingEnabled == DEBUGLOGSETTINGS_LOGGINGENABLED_ALWAYS) {
PIOS_DEBUGLOG_Enable(1);
PIOS_DEBUGLOG_Printf("On board logging enabled.");
} else if (settings.LoggingEnabled == DEBUGLOGSETTINGS_LOGGINGENABLED_DISABLED) {
PIOS_DEBUGLOG_Printf("On board logging disabled.");
PIOS_DEBUGLOG_Enable(0);
} else {
FlightStatusUpdatedCb(NULL);
}
}
static void ControlUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
DebugLogControlGet(&control);
if (control.Operation == DEBUGLOGCONTROL_OPERATION_RETRIEVE) {
memset(entry, 0, sizeof(DebugLogEntryData));
if (PIOS_DEBUGLOG_Read(entry, control.Flight, control.Entry) != 0) {
// reading from log failed, mark as non existent in output
entry->Flight = control.Flight;
entry->Entry = control.Entry;
entry->Type = DEBUGLOGENTRY_TYPE_EMPTY;
}
DebugLogEntrySet(entry);
} else if (control.Operation == DEBUGLOGCONTROL_OPERATION_FORMATFLASH) {
uint8_t armed;
FlightStatusArmedGet(&armed);
if (armed == FLIGHTSTATUS_ARMED_DISARMED) {
PIOS_DEBUGLOG_Format();
}
}
StatusUpdatedCb(ev);
}
/**
* @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;
}
/* int armed(): returns armed status */
void SystemArmed(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs)
{
FlightStatusData data;
FlightStatusArmedGet(&data.Armed);
ReturnValue->Val->Integer = (data.Armed == FLIGHTSTATUS_ARMED_ARMED);
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t TxPIDInitialize(void)
{
bool txPIDEnabled;
HwSettingsOptionalModulesData optionalModules;
#ifdef REVOLUTION
AltitudeHoldSettingsInitialize();
#endif
HwSettingsInitialize();
HwSettingsOptionalModulesGet(&optionalModules);
if (optionalModules.TxPID == HWSETTINGS_OPTIONALMODULES_ENABLED) {
txPIDEnabled = true;
} else {
txPIDEnabled = false;
}
if (txPIDEnabled) {
TxPIDSettingsInitialize();
TxPIDStatusInitialize();
AccessoryDesiredInitialize();
UAVObjEvent ev = {
.obj = AccessoryDesiredHandle(),
.instId = 0,
.event = 0,
.lowPriority = false,
};
EventPeriodicCallbackCreate(&ev, updatePIDs, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
#if (TELEMETRY_UPDATE_PERIOD_MS != 0)
// Change StabilizationSettings update rate from OnChange to periodic
// to prevent telemetry link flooding with frequent updates in case of
// control channel jitter.
// Warning: saving to flash with this code active will change the
// StabilizationSettings update rate permanently. Use Metadata via
// browser to reset to defaults (telemetryAcked=true, OnChange).
UAVObjMetadata metadata;
StabilizationSettingsInitialize();
StabilizationSettingsGetMetadata(&metadata);
metadata.telemetryAcked = 0;
metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
metadata.telemetryUpdatePeriod = TELEMETRY_UPDATE_PERIOD_MS;
StabilizationSettingsSetMetadata(&metadata);
AttitudeSettingsInitialize();
AttitudeSettingsGetMetadata(&metadata);
metadata.telemetryAcked = 0;
metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
metadata.telemetryUpdatePeriod = TELEMETRY_UPDATE_PERIOD_MS;
AttitudeSettingsSetMetadata(&metadata);
#endif /* if (TELEMETRY_UPDATE_PERIOD_MS != 0) */
return 0;
}
return -1;
}
/* stub: module has no module thread */
int32_t TxPIDStart(void)
{
return 0;
}
MODULE_INITCALL(TxPIDInitialize, TxPIDStart);
/**
* Update PIDs callback function
*/
static void updatePIDs(UAVObjEvent *ev)
{
if (ev->obj != AccessoryDesiredHandle()) {
return;
}
TxPIDSettingsData inst;
TxPIDSettingsGet(&inst);
if (inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_NEVER) {
return;
}
uint8_t armed;
FlightStatusArmedGet(&armed);
if ((inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_WHENARMED) &&
(armed == FLIGHTSTATUS_ARMED_DISARMED)) {
return;
}
StabilizationBankData bank;
switch (inst.BankNumber) {
case 0:
StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)&bank);
break;
case 1:
StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&bank);
break;
case 2:
StabilizationSettingsBank3Get((StabilizationSettingsBank3Data *)&bank);
break;
default:
return;
}
StabilizationSettingsData stab;
StabilizationSettingsGet(&stab);
AttitudeSettingsData att;
AttitudeSettingsGet(&att);
#ifdef REVOLUTION
AltitudeHoldSettingsData altitude;
AltitudeHoldSettingsGet(&altitude);
#endif
AccessoryDesiredData accessory;
TxPIDStatusData txpid_status;
TxPIDStatusGet(&txpid_status);
bool easyTuneEnabled = false;
uint8_t needsUpdateBank = 0;
uint8_t needsUpdateStab = 0;
uint8_t needsUpdateAtt = 0;
#ifdef REVOLUTION
uint8_t needsUpdateAltitude = 0;
#endif
// Loop through every enabled instance
for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) {
if (TxPIDSettingsPIDsToArray(inst.PIDs)[i] != TXPIDSETTINGS_PIDS_DISABLED) {
float value;
if (TxPIDSettingsInputsToArray(inst.Inputs)[i] == TXPIDSETTINGS_INPUTS_THROTTLE) {
ManualControlCommandThrottleGet(&value);
value = scale(value,
inst.ThrottleRange.Min,
inst.ThrottleRange.Max,
TxPIDSettingsMinPIDToArray(inst.MinPID)[i],
TxPIDSettingsMaxPIDToArray(inst.MaxPID)[i]);
} else if (AccessoryDesiredInstGet(
TxPIDSettingsInputsToArray(inst.Inputs)[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0,
&accessory) == 0) {
value = scale(accessory.AccessoryVal, -1.0f, 1.0f,
TxPIDSettingsMinPIDToArray(inst.MinPID)[i],
TxPIDSettingsMaxPIDToArray(inst.MaxPID)[i]);
} else {
continue;
}
TxPIDStatusCurPIDToArray(txpid_status.CurPID)[i] = value;
switch (TxPIDSettingsPIDsToArray(inst.PIDs)[i]) {
case TXPIDSETTINGS_PIDS_ROLLRATEKP:
needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_EASYTUNERATEROLL:
easyTuneEnabled = true;
needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
needsUpdateBank |= update(&bank.RollRatePID.Ki, value * inst.EasyTunePitchRollRateFactors.I);
needsUpdateBank |= update(&bank.RollRatePID.Kd, value * inst.EasyTunePitchRollRateFactors.D);
break;
case TXPIDSETTINGS_PIDS_EASYTUNERATEPITCH:
easyTuneEnabled = true;
needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
needsUpdateBank |= update(&bank.PitchRatePID.Ki, value * inst.EasyTunePitchRollRateFactors.I);
needsUpdateBank |= update(&bank.PitchRatePID.Kd, value * inst.EasyTunePitchRollRateFactors.D);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEKI:
needsUpdateBank |= update(&bank.RollRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEKD:
needsUpdateBank |= update(&bank.RollRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT:
needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATERESP:
needsUpdateBank |= updateUint16(&bank.ManualRate.Roll, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP:
needsUpdateBank |= update(&bank.RollPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI:
needsUpdateBank |= update(&bank.RollPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT:
needsUpdateBank |= update(&bank.RollPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDERESP:
needsUpdateBank |= updateUint8(&bank.RollMax, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKP:
needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKI:
needsUpdateBank |= update(&bank.PitchRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKD:
needsUpdateBank |= update(&bank.PitchRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT:
needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATERESP:
needsUpdateBank |= updateUint16(&bank.ManualRate.Pitch, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP:
needsUpdateBank |= update(&bank.PitchPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI:
needsUpdateBank |= update(&bank.PitchPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT:
needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDERESP:
needsUpdateBank |= updateUint8(&bank.PitchMax, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP:
needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI:
needsUpdateBank |= update(&bank.RollRatePID.Ki, value);
needsUpdateBank |= update(&bank.PitchRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD:
needsUpdateBank |= update(&bank.RollRatePID.Kd, value);
needsUpdateBank |= update(&bank.PitchRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT:
needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATERESP:
needsUpdateBank |= updateUint16(&bank.ManualRate.Roll, value);
needsUpdateBank |= updateUint16(&bank.ManualRate.Pitch, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP:
needsUpdateBank |= update(&bank.RollPI.Kp, value);
needsUpdateBank |= update(&bank.PitchPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI:
needsUpdateBank |= update(&bank.RollPI.Ki, value);
needsUpdateBank |= update(&bank.PitchPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT:
needsUpdateBank |= update(&bank.RollPI.ILimit, value);
needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDERESP:
needsUpdateBank |= updateUint8(&bank.RollMax, value);
needsUpdateBank |= updateUint8(&bank.PitchMax, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKP:
needsUpdateBank |= update(&bank.YawRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKI:
needsUpdateBank |= update(&bank.YawRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKD:
needsUpdateBank |= update(&bank.YawRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEILIMIT:
needsUpdateBank |= update(&bank.YawRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATERESP:
needsUpdateBank |= updateUint16(&bank.ManualRate.Yaw, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKP:
needsUpdateBank |= update(&bank.YawPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKI:
needsUpdateBank |= update(&bank.YawPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT:
needsUpdateBank |= update(&bank.YawPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDERESP:
needsUpdateBank |= updateUint8(&bank.YawMax, value);
break;
case TXPIDSETTINGS_PIDS_ROLLEXPO:
needsUpdateBank |= updateInt8(&bank.StickExpo.Roll, value);
break;
case TXPIDSETTINGS_PIDS_PITCHEXPO:
needsUpdateBank |= updateInt8(&bank.StickExpo.Pitch, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHEXPO:
needsUpdateBank |= updateInt8(&bank.StickExpo.Roll, value);
needsUpdateBank |= updateInt8(&bank.StickExpo.Pitch, value);
break;
case TXPIDSETTINGS_PIDS_YAWEXPO:
needsUpdateBank |= updateInt8(&bank.StickExpo.Yaw, value);
break;
case TXPIDSETTINGS_PIDS_GYROTAU:
needsUpdateStab |= update(&stab.GyroTau, value);
break;
case TXPIDSETTINGS_PIDS_ACROROLLFACTOR:
needsUpdateBank |= updateUint8(&bank.AcroInsanityFactor.Roll, value);
break;
case TXPIDSETTINGS_PIDS_ACROPITCHFACTOR:
needsUpdateBank |= updateUint8(&bank.AcroInsanityFactor.Pitch, value);
break;
case TXPIDSETTINGS_PIDS_ACROROLLPITCHFACTOR:
needsUpdateBank |= updateUint8(&bank.AcroInsanityFactor.Roll, value);
needsUpdateBank |= updateUint8(&bank.AcroInsanityFactor.Pitch, value);
break;
case TXPIDSETTINGS_PIDS_ACCELTAU:
needsUpdateAtt |= update(&att.AccelTau, value);
break;
case TXPIDSETTINGS_PIDS_ACCELKP:
needsUpdateAtt |= update(&att.AccelKp, value);
break;
case TXPIDSETTINGS_PIDS_ACCELKI:
needsUpdateAtt |= update(&att.AccelKi, value);
break;
#ifdef REVOLUTION
case TXPIDSETTINGS_PIDS_ALTITUDEPOSKP:
needsUpdateAltitude |= update(&altitude.VerticalPosP, value);
break;
case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKP:
needsUpdateAltitude |= update(&altitude.VerticalVelPID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKI:
needsUpdateAltitude |= update(&altitude.VerticalVelPID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKD:
needsUpdateAltitude |= update(&altitude.VerticalVelPID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYBETA:
needsUpdateAltitude |= update(&altitude.VerticalVelPID.Beta, value);
break;
#endif
default:
PIOS_Assert(0);
}
}
}
if (needsUpdateStab) {
StabilizationSettingsSet(&stab);
}
if (needsUpdateAtt) {
AttitudeSettingsSet(&att);
}
#ifdef REVOLUTION
if (needsUpdateAltitude) {
AltitudeHoldSettingsSet(&altitude);
}
#endif
if (easyTuneEnabled && (inst.EasyTuneRatePIDRecalculateYaw != TXPIDSETTINGS_EASYTUNERATEPIDRECALCULATEYAW_FALSE)) {
float newKp = (bank.RollRatePID.Kp + bank.PitchRatePID.Kp) * .5f * inst.EasyTuneYawRateFactors.P;
needsUpdateBank |= update(&bank.YawRatePID.Kp, newKp);
needsUpdateBank |= update(&bank.YawRatePID.Ki, newKp * inst.EasyTuneYawRateFactors.I);
needsUpdateBank |= update(&bank.YawRatePID.Kd, newKp * inst.EasyTuneYawRateFactors.D);
}
if (needsUpdateBank) {
switch (inst.BankNumber) {
case 0:
StabilizationSettingsBank1Set((StabilizationSettingsBank1Data *)&bank);
break;
case 1:
StabilizationSettingsBank2Set((StabilizationSettingsBank2Data *)&bank);
break;
case 2:
StabilizationSettingsBank3Set((StabilizationSettingsBank3Data *)&bank);
break;
default:
return;
}
}
if (needsUpdateStab ||
needsUpdateAtt ||
#ifdef REVOLUTION
needsUpdateAltitude ||
#endif /* REVOLUTION */
needsUpdateBank) {
TxPIDStatusSet(&txpid_status);;
}
}
/**
* Scales input val from [inMin..inMax] range to [outMin..outMax].
* If val is out of input range (inMin <= inMax), it will be bound.
* (outMin > outMax) is ok, in that case output will be decreasing.
*
* \returns scaled value
*/
static float scale(float val, float inMin, float inMax, float outMin, float outMax)
{
// bound input value
if (val > inMax) {
val = inMax;
}
if (val < inMin) {
val = inMin;
}
// normalize input value to [0..1]
if (inMax <= inMin) {
val = 0.0f;
} else {
val = (val - inMin) / (inMax - inMin);
}
// update output bounds
if (outMin > outMax) {
float t = outMin;
outMin = outMax;
outMax = t;
val = 1.0f - val;
}
return (outMax - outMin) * val + outMin;
}
/**
* Updates var using val if needed.
* \returns 1 if updated, 0 otherwise
*/
static uint8_t update(float *var, float val)
{
/* FIXME: this is not an entirely correct way
* to check if the two floating point
* numbers are 'not equal'.
* Epsilon of 1e-9 is probably okay for the range
* of numbers we see here*/
if (fabsf(*var - val) > 1e-9f) {
*var = val;
return 1;
}
return 0;
}
/**
* Updates var using val if needed.
* \returns 1 if updated, 0 otherwise
*/
static uint8_t updateUint16(uint16_t *var, float val)
{
uint16_t roundedVal = (uint16_t)roundf(val);
if (*var != roundedVal) {
*var = roundedVal;
return 1;
}
return 0;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t TxPIDInitialize(void)
{
bool module_enabled;
#ifdef MODULE_TxPID_BUILTIN
module_enabled = true;
#else
uint8_t module_state[MODULESETTINGS_ADMINSTATE_NUMELEM];
ModuleSettingsAdminStateGet(module_state);
if (module_state[MODULESETTINGS_ADMINSTATE_TXPID] == MODULESETTINGS_ADMINSTATE_ENABLED) {
module_enabled = true;
} else {
module_enabled = false;
}
#endif
if (module_enabled) {
TxPIDSettingsInitialize();
AccessoryDesiredInitialize();
UAVObjEvent ev = {
.obj = AccessoryDesiredHandle(),
.instId = 0,
.event = 0,
};
EventPeriodicCallbackCreate(&ev, updatePIDs, SAMPLE_PERIOD_MS);
#if (TELEMETRY_UPDATE_PERIOD_MS != 0)
// Change StabilizationSettings update rate from OnChange to periodic
// to prevent telemetry link flooding with frequent updates in case of
// control channel jitter.
// Warning: saving to flash with this code active will change the
// StabilizationSettings update rate permanently. Use Metadata via
// browser to reset to defaults (telemetryAcked=true, OnChange).
UAVObjMetadata metadata;
StabilizationSettingsInitialize();
StabilizationSettingsGetMetadata(&metadata);
metadata.telemetryAcked = 0;
metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
metadata.telemetryUpdatePeriod = TELEMETRY_UPDATE_PERIOD_MS;
StabilizationSettingsSetMetadata(&metadata);
#endif
return 0;
}
return -1;
}
MODULE_INITCALL(TxPIDInitialize, NULL);
/**
* Update PIDs callback function
*/
static void updatePIDs(UAVObjEvent* ev)
{
if (ev->obj != AccessoryDesiredHandle())
return;
TxPIDSettingsData inst;
TxPIDSettingsGet(&inst);
if (inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_NEVER)
return;
uint8_t armed;
FlightStatusArmedGet(&armed);
if ((inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_WHENARMED) &&
(armed == FLIGHTSTATUS_ARMED_DISARMED))
return;
StabilizationSettingsData stab;
StabilizationSettingsGet(&stab);
#ifdef UAVOBJ_INIT_vtolpathfollowersettings
VtolPathFollowerSettingsData vtolPathFollowerSettingsData;
// Check to make sure the settings UAVObject has been instantiated
if (VtolPathFollowerSettingsHandle()) {
VtolPathFollowerSettingsGet(&vtolPathFollowerSettingsData);
}
bool vtolPathFollowerSettingsNeedsUpdate = false;
#endif
AccessoryDesiredData accessory;
bool stabilizationSettingsNeedsUpdate = false;
// Loop through every enabled instance
for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) {
if (inst.PIDs[i] != TXPIDSETTINGS_PIDS_DISABLED) {
float value;
if (inst.Inputs[i] == TXPIDSETTINGS_INPUTS_THROTTLE) {
ManualControlCommandThrottleGet(&value);
value = scale(value,
inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MIN],
inst.ThrottleRange[TXPIDSETTINGS_THROTTLERANGE_MAX],
inst.MinPID[i], inst.MaxPID[i]);
} else if (AccessoryDesiredInstGet(inst.Inputs[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0) {
value = scale(accessory.AccessoryVal, -1.0, 1.0, inst.MinPID[i], inst.MaxPID[i]);
} else {
continue;
}
switch (inst.PIDs[i]) {
case TXPIDSETTINGS_PIDS_ROLLRATEKP:
stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEKI:
stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEKD:
stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT:
stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT], value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP:
stabilizationSettingsNeedsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI:
stabilizationSettingsNeedsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT:
stabilizationSettingsNeedsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT], value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKP:
stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKI:
stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKD:
stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT:
stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT], value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP:
stabilizationSettingsNeedsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI:
stabilizationSettingsNeedsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT:
stabilizationSettingsNeedsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP:
stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP], value);
stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI:
stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI], value);
stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD:
stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD], value);
stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT:
stabilizationSettingsNeedsUpdate |= update(&stab.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT], value);
stabilizationSettingsNeedsUpdate |= update(&stab.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP:
stabilizationSettingsNeedsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP], value);
stabilizationSettingsNeedsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI:
stabilizationSettingsNeedsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], value);
stabilizationSettingsNeedsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT:
stabilizationSettingsNeedsUpdate |= update(&stab.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT], value);
stabilizationSettingsNeedsUpdate |= update(&stab.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT], value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKP:
stabilizationSettingsNeedsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP], value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKI:
stabilizationSettingsNeedsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI], value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKD:
stabilizationSettingsNeedsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD], value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEILIMIT:
stabilizationSettingsNeedsUpdate |= update(&stab.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT], value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKP:
stabilizationSettingsNeedsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_KP], value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKI:
stabilizationSettingsNeedsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT:
stabilizationSettingsNeedsUpdate |= update(&stab.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT], value);
break;
case TXPIDSETTINGS_PIDS_GYROCUTOFF:
stabilizationSettingsNeedsUpdate |= update(&stab.GyroCutoff, value);
break;
case TXPIDSETTINGS_PIDS_ROLLVBARSENSITIVITY:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarSensitivity[STABILIZATIONSETTINGS_VBARSENSITIVITY_ROLL], value);
break;
case TXPIDSETTINGS_PIDS_PITCHVBARSENSITIVITY:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarSensitivity[STABILIZATIONSETTINGS_VBARSENSITIVITY_PITCH], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHVBARSENSITIVITY:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarSensitivity[STABILIZATIONSETTINGS_VBARSENSITIVITY_ROLL], value);
stabilizationSettingsNeedsUpdate |= update(&stab.VbarSensitivity[STABILIZATIONSETTINGS_VBARSENSITIVITY_PITCH], value);
break;
case TXPIDSETTINGS_PIDS_YAWVBARSENSITIVITY:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarSensitivity[STABILIZATIONSETTINGS_VBARSENSITIVITY_YAW], value);
break;
case TXPIDSETTINGS_PIDS_ROLLVBARKP:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KP], value);
break;
case TXPIDSETTINGS_PIDS_ROLLVBARKI:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KI], value);
break;
case TXPIDSETTINGS_PIDS_ROLLVBARKD:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KD], value);
break;
case TXPIDSETTINGS_PIDS_PITCHVBARKP:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KP], value);
break;
case TXPIDSETTINGS_PIDS_PITCHVBARKI:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KI], value);
break;
case TXPIDSETTINGS_PIDS_PITCHVBARKD:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KD], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHVBARKP:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KP], value);
stabilizationSettingsNeedsUpdate |= update(&stab.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KP], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHVBARKI:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KI], value);
stabilizationSettingsNeedsUpdate |= update(&stab.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KI], value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHVBARKD:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KD], value);
stabilizationSettingsNeedsUpdate |= update(&stab.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KD], value);
break;
case TXPIDSETTINGS_PIDS_YAWVBARKP:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarYawPID[STABILIZATIONSETTINGS_VBARYAWPID_KP], value);
break;
case TXPIDSETTINGS_PIDS_YAWVBARKI:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarYawPID[STABILIZATIONSETTINGS_VBARYAWPID_KI], value);
break;
case TXPIDSETTINGS_PIDS_YAWVBARKD:
stabilizationSettingsNeedsUpdate |= update(&stab.VbarYawPID[STABILIZATIONSETTINGS_VBARYAWPID_KD], value);
break;
#ifdef UAVOBJ_INIT_vtolpathfollowersettings
case TXPIDSETTINGS_PIDS_HORIZONTALPOSKP:
vtolPathFollowerSettingsNeedsUpdate |= update(&vtolPathFollowerSettingsData.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP], value);
break;
case TXPIDSETTINGS_PIDS_HORIZONTALPOSKI:
vtolPathFollowerSettingsNeedsUpdate |= update(&vtolPathFollowerSettingsData.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KI], value);
break;
case TXPIDSETTINGS_PIDS_HORIZONTALPOSILIMIT:
vtolPathFollowerSettingsNeedsUpdate |= update(&vtolPathFollowerSettingsData.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_ILIMIT], value);
break;
case TXPIDSETTINGS_PIDS_HORIZONTALVELKP:
vtolPathFollowerSettingsNeedsUpdate |= update(&vtolPathFollowerSettingsData.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KP], value);
break;
case TXPIDSETTINGS_PIDS_HORIZONTALVELKI:
vtolPathFollowerSettingsNeedsUpdate |= update(&vtolPathFollowerSettingsData.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KI], value);
break;
case TXPIDSETTINGS_PIDS_HORIZONTALVELKD:
vtolPathFollowerSettingsNeedsUpdate |= update(&vtolPathFollowerSettingsData.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD], value);
break;
#endif /* UAVOBJ_INIT_vtolpathfollowersettings */
default:
// Previously this would assert. But now the
// object may be missing and it's not worth a
// crash.
break;
}
}
}
// Update UAVOs, if necessary
if (stabilizationSettingsNeedsUpdate) {
StabilizationSettingsSet(&stab);
}
#ifdef UAVOBJ_INIT_vtolpathfollowersettings
if (vtolPathFollowerSettingsNeedsUpdate) {
// Check to make sure the settings UAVObject has been instantiated
if (VtolPathFollowerSettingsHandle()) {
VtolPathFollowerSettingsSet(&vtolPathFollowerSettingsData);
}
}
#endif
}
/**
* Scales input val from [inMin..inMax] range to [outMin..outMax].
* If val is out of input range (inMin <= inMax), it will be bound.
* (outMin > outMax) is ok, in that case output will be decreasing.
*
* \returns scaled value
*/
static float scale(float val, float inMin, float inMax, float outMin, float outMax)
{
// bound input value
if (val > inMax) val = inMax;
if (val < inMin) val = inMin;
// normalize input value to [0..1]
if (inMax <= inMin)
val = 0.0;
else
val = (val - inMin) / (inMax - inMin);
// update output bounds
if (outMin > outMax) {
float t = outMin;
outMin = outMax;
outMax = t;
val = 1.0f - val;
}
return (outMax - outMin) * val + outMin;
}
/**
* Updates var using val if needed.
* \returns 1 if updated, 0 otherwise
*/
static bool update(float *var, float val)
{
if (*var != val) {
*var = val;
return true;
}
return false;
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t TxPIDInitialize(void)
{
bool txPIDEnabled;
HwSettingsOptionalModulesData optionalModules;
HwSettingsInitialize();
HwSettingsOptionalModulesGet(&optionalModules);
if (optionalModules.TxPID == HWSETTINGS_OPTIONALMODULES_ENABLED) {
txPIDEnabled = true;
} else {
txPIDEnabled = false;
}
if (txPIDEnabled) {
TxPIDSettingsInitialize();
AccessoryDesiredInitialize();
UAVObjEvent ev = {
.obj = AccessoryDesiredHandle(),
.instId = 0,
.event = 0,
.lowPriority = false,
};
EventPeriodicCallbackCreate(&ev, updatePIDs, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
#if (TELEMETRY_UPDATE_PERIOD_MS != 0)
// Change StabilizationSettings update rate from OnChange to periodic
// to prevent telemetry link flooding with frequent updates in case of
// control channel jitter.
// Warning: saving to flash with this code active will change the
// StabilizationSettings update rate permanently. Use Metadata via
// browser to reset to defaults (telemetryAcked=true, OnChange).
UAVObjMetadata metadata;
StabilizationSettingsInitialize();
StabilizationSettingsGetMetadata(&metadata);
metadata.telemetryAcked = 0;
metadata.telemetryUpdateMode = UPDATEMODE_PERIODIC;
metadata.telemetryUpdatePeriod = TELEMETRY_UPDATE_PERIOD_MS;
StabilizationSettingsSetMetadata(&metadata);
#endif
return 0;
}
return -1;
}
/* stub: module has no module thread */
int32_t TxPIDStart(void)
{
return 0;
}
MODULE_INITCALL(TxPIDInitialize, TxPIDStart);
/**
* Update PIDs callback function
*/
static void updatePIDs(UAVObjEvent *ev)
{
if (ev->obj != AccessoryDesiredHandle()) {
return;
}
TxPIDSettingsData inst;
TxPIDSettingsGet(&inst);
if (inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_NEVER) {
return;
}
uint8_t armed;
FlightStatusArmedGet(&armed);
if ((inst.UpdateMode == TXPIDSETTINGS_UPDATEMODE_WHENARMED) &&
(armed == FLIGHTSTATUS_ARMED_DISARMED)) {
return;
}
StabilizationBankData bank;
switch (inst.BankNumber) {
case 0:
StabilizationSettingsBank1Get((StabilizationSettingsBank1Data *)&bank);
break;
case 1:
StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&bank);
break;
case 2:
StabilizationSettingsBank2Get((StabilizationSettingsBank2Data *)&bank);
break;
default:
return;
}
StabilizationSettingsData stab;
StabilizationSettingsGet(&stab);
AccessoryDesiredData accessory;
uint8_t needsUpdateBank = 0;
uint8_t needsUpdateStab = 0;
// Loop through every enabled instance
for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) {
if (cast_struct_to_array(inst.PIDs, inst.PIDs.Instance1)[i] != TXPIDSETTINGS_PIDS_DISABLED) {
float value;
if (cast_struct_to_array(inst.Inputs, inst.Inputs.Instance1)[i] == TXPIDSETTINGS_INPUTS_THROTTLE) {
ManualControlCommandThrottleGet(&value);
value = scale(value,
inst.ThrottleRange.Min,
inst.ThrottleRange.Max,
cast_struct_to_array(inst.MinPID, inst.MinPID.Instance1)[i],
cast_struct_to_array(inst.MaxPID, inst.MaxPID.Instance1)[i]);
} else if (AccessoryDesiredInstGet(
cast_struct_to_array(inst.Inputs, inst.Inputs.Instance1)[i] - TXPIDSETTINGS_INPUTS_ACCESSORY0,
&accessory) == 0) {
value = scale(accessory.AccessoryVal, -1.0f, 1.0f,
cast_struct_to_array(inst.MinPID, inst.MinPID.Instance1)[i],
cast_struct_to_array(inst.MaxPID, inst.MaxPID.Instance1)[i]);
} else {
continue;
}
switch (cast_struct_to_array(inst.PIDs, inst.PIDs.Instance1)[i]) {
case TXPIDSETTINGS_PIDS_ROLLRATEKP:
needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEKI:
needsUpdateBank |= update(&bank.RollRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEKD:
needsUpdateBank |= update(&bank.RollRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT:
needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP:
needsUpdateBank |= update(&bank.RollPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI:
needsUpdateBank |= update(&bank.RollPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT:
needsUpdateBank |= update(&bank.RollPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKP:
needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKI:
needsUpdateBank |= update(&bank.PitchRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEKD:
needsUpdateBank |= update(&bank.PitchRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT:
needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP:
needsUpdateBank |= update(&bank.PitchPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI:
needsUpdateBank |= update(&bank.PitchPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT:
needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP:
needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI:
needsUpdateBank |= update(&bank.RollRatePID.Ki, value);
needsUpdateBank |= update(&bank.PitchRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD:
needsUpdateBank |= update(&bank.RollRatePID.Kd, value);
needsUpdateBank |= update(&bank.PitchRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT:
needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP:
needsUpdateBank |= update(&bank.RollPI.Kp, value);
needsUpdateBank |= update(&bank.PitchPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI:
needsUpdateBank |= update(&bank.RollPI.Ki, value);
needsUpdateBank |= update(&bank.PitchPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT:
needsUpdateBank |= update(&bank.RollPI.ILimit, value);
needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKP:
needsUpdateBank |= update(&bank.YawRatePID.Kp, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKI:
needsUpdateBank |= update(&bank.YawRatePID.Ki, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEKD:
needsUpdateBank |= update(&bank.YawRatePID.Kd, value);
break;
case TXPIDSETTINGS_PIDS_YAWRATEILIMIT:
needsUpdateBank |= update(&bank.YawRatePID.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKP:
needsUpdateBank |= update(&bank.YawPI.Kp, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKI:
needsUpdateBank |= update(&bank.YawPI.Ki, value);
break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT:
needsUpdateBank |= update(&bank.YawPI.ILimit, value);
break;
case TXPIDSETTINGS_PIDS_GYROTAU:
needsUpdateStab |= update(&stab.GyroTau, value);
break;
default:
PIOS_Assert(0);
}
}
}
if (needsUpdateStab) {
StabilizationSettingsSet(&stab);
}
if (needsUpdateBank) {
switch (inst.BankNumber) {
case 0:
StabilizationSettingsBank1Set((StabilizationSettingsBank1Data *)&bank);
break;
case 1:
StabilizationSettingsBank2Set((StabilizationSettingsBank2Data *)&bank);
break;
case 2:
StabilizationSettingsBank3Set((StabilizationSettingsBank3Data *)&bank);
break;
default:
return;
}
}
}
/**
* Scales input val from [inMin..inMax] range to [outMin..outMax].
* If val is out of input range (inMin <= inMax), it will be bound.
* (outMin > outMax) is ok, in that case output will be decreasing.
*
* \returns scaled value
*/
static float scale(float val, float inMin, float inMax, float outMin, float outMax)
{
// bound input value
if (val > inMax) {
val = inMax;
}
if (val < inMin) {
val = inMin;
}
// normalize input value to [0..1]
if (inMax <= inMin) {
val = 0.0f;
} else {
val = (val - inMin) / (inMax - inMin);
}
// update output bounds
if (outMin > outMax) {
float t = outMin;
outMin = outMax;
outMax = t;
val = 1.0f - val;
}
return (outMax - outMin) * val + outMin;
}
/**
* Updates var using val if needed.
* \returns 1 if updated, 0 otherwise
*/
static uint8_t update(float *var, float val)
{
/* FIXME: this is not an entirely correct way
* to check if the two floating point
* numbers are 'not equal'.
* Epsilon of 1e-9 is probably okay for the range
* of numbers we see here*/
if (fabsf(*var - val) > 1e-9f) {
*var = val;
return 1;
}
return 0;
}
/**
* WARNING! This callback executes with critical flight control priority every
* time a gyroscope update happens do NOT put any time consuming calculations
* in this loop unless they really have to execute with every gyro update
*/
static void stabilizationInnerloopTask()
{
// watchdog and error handling
{
#ifdef PIOS_INCLUDE_WDG
PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION);
#endif
bool warn = false;
bool error = false;
bool crit = false;
// check if outer loop keeps executing
if (stabSettings.monitor.rateupdates > -64) {
stabSettings.monitor.rateupdates--;
}
if (stabSettings.monitor.rateupdates < -(2 * OUTERLOOP_SKIPCOUNT)) {
// warning if rate loop skipped more than 2 execution
warn = true;
}
if (stabSettings.monitor.rateupdates < -(4 * OUTERLOOP_SKIPCOUNT)) {
// critical if rate loop skipped more than 4 executions
crit = true;
}
// check if gyro keeps updating
if (stabSettings.monitor.gyroupdates < 1) {
// error if gyro didn't update at all!
error = true;
}
if (stabSettings.monitor.gyroupdates > 1) {
// warning if we missed a gyro update
warn = true;
}
if (stabSettings.monitor.gyroupdates > 3) {
// critical if we missed 3 gyro updates
crit = true;
}
stabSettings.monitor.gyroupdates = 0;
if (crit) {
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_CRITICAL);
} else if (error) {
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_ERROR);
} else if (warn) {
AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION, SYSTEMALARMS_ALARM_WARNING);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION);
}
}
RateDesiredData rateDesired;
ActuatorDesiredData actuator;
StabilizationStatusInnerLoopData enabled;
FlightStatusControlChainData cchain;
RateDesiredGet(&rateDesired);
ActuatorDesiredGet(&actuator);
StabilizationStatusInnerLoopGet(&enabled);
FlightStatusControlChainGet(&cchain);
float *rate = &rateDesired.Roll;
float *actuatorDesiredAxis = &actuator.Roll;
int t;
float dT;
dT = PIOS_DELTATIME_GetAverageSeconds(&timeval);
for (t = 0; t < AXES; t++) {
bool reinit = (cast_struct_to_array(enabled, enabled.Roll)[t] != previous_mode[t]);
previous_mode[t] = cast_struct_to_array(enabled, enabled.Roll)[t];
if (t < STABILIZATIONSTATUS_INNERLOOP_THRUST) {
if (reinit) {
stabSettings.innerPids[t].iAccumulator = 0;
}
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
case STABILIZATIONSTATUS_INNERLOOP_VIRTUALFLYBAR:
stabilization_virtual_flybar(gyro_filtered[t], rate[t], &actuatorDesiredAxis[t], dT, reinit, t, &stabSettings.settings);
break;
case STABILIZATIONSTATUS_INNERLOOP_RELAYTUNING:
rate[t] = boundf(rate[t],
-cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t],
cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t]
);
stabilization_relay_rate(rate[t] - gyro_filtered[t], &actuatorDesiredAxis[t], t, reinit);
break;
case STABILIZATIONSTATUS_INNERLOOP_AXISLOCK:
if (fabsf(rate[t]) > stabSettings.settings.MaxAxisLockRate) {
// While getting strong commands act like rate mode
axis_lock_accum[t] = 0;
} else {
// For weaker commands or no command simply attitude lock (almost) on no gyro change
axis_lock_accum[t] += (rate[t] - gyro_filtered[t]) * dT;
axis_lock_accum[t] = boundf(axis_lock_accum[t], -stabSettings.settings.MaxAxisLock, stabSettings.settings.MaxAxisLock);
rate[t] = axis_lock_accum[t] * stabSettings.settings.AxisLockKp;
}
// IMPORTANT: deliberately no "break;" here, execution continues with regular RATE control loop to avoid code duplication!
// keep order as it is, RATE must follow!
case STABILIZATIONSTATUS_INNERLOOP_RATE:
// limit rate to maximum configured limits (once here instead of 5 times in outer loop)
rate[t] = boundf(rate[t],
-cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t],
cast_struct_to_array(stabSettings.stabBank.MaximumRate, stabSettings.stabBank.MaximumRate.Roll)[t]
);
actuatorDesiredAxis[t] = pid_apply_setpoint(&stabSettings.innerPids[t], speedScaleFactor, rate[t], gyro_filtered[t], dT);
break;
case STABILIZATIONSTATUS_INNERLOOP_DIRECT:
default:
actuatorDesiredAxis[t] = rate[t];
break;
}
} else {
switch (cast_struct_to_array(enabled, enabled.Roll)[t]) {
case STABILIZATIONSTATUS_INNERLOOP_CRUISECONTROL:
actuatorDesiredAxis[t] = cruisecontrol_apply_factor(rate[t]);
break;
case STABILIZATIONSTATUS_INNERLOOP_DIRECT:
default:
actuatorDesiredAxis[t] = rate[t];
break;
}
}
actuatorDesiredAxis[t] = boundf(actuatorDesiredAxis[t], -1.0f, 1.0f);
}
actuator.UpdateTime = dT * 1000;
if (cchain.Stabilization == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
ActuatorDesiredSet(&actuator);
} else {
// Force all axes to reinitialize when engaged
for (t = 0; t < AXES; t++) {
previous_mode[t] = 255;
}
}
{
uint8_t armed;
FlightStatusArmedGet(&armed);
float throttleDesired;
ManualControlCommandThrottleGet(&throttleDesired);
if (armed != FLIGHTSTATUS_ARMED_ARMED ||
((stabSettings.settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE) && throttleDesired < 0)) {
// Force all axes to reinitialize when engaged
for (t = 0; t < AXES; t++) {
previous_mode[t] = 255;
}
}
}
PIOS_CALLBACKSCHEDULER_Schedule(callbackHandle, FAILSAFE_TIMEOUT_MS, CALLBACK_UPDATEMODE_LATER);
}
