/** * 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); }