/** * Clear the accumulators and derivatives for all the axes */ static void zero_pids(void) { for(uint32_t i = 0; i < PID_MAX; i++) pid_zero(&pids[i]); for(uint8_t i = 0; i < 3; i++) axis_lock_accum[i] = 0.0f; }
/** * Module initialization */ int32_t StabilizationInitialize() { // Initialize variables StabilizationDesiredInitialize(); StabilizationSettingsInitialize(); StabilizationStatusInitialize(); StabilizationBankInitialize(); StabilizationSettingsBank1Initialize(); StabilizationSettingsBank2Initialize(); StabilizationSettingsBank3Initialize(); RateDesiredInitialize(); ManualControlCommandInitialize(); // only used for PID bank selection based on flight mode switch sin_lookup_initalize(); stabilizationOuterloopInit(); stabilizationInnerloopInit(); #ifdef REVOLUTION stabilizationAltitudeloopInit(); #endif pid_zero(&stabSettings.outerPids[0]); pid_zero(&stabSettings.outerPids[1]); pid_zero(&stabSettings.outerPids[2]); pid_zero(&stabSettings.innerPids[0]); pid_zero(&stabSettings.innerPids[1]); pid_zero(&stabSettings.innerPids[2]); return 0; }
/** * reset integrals */ void FixedWingFlyController::resetGlobals() { pid_zero(&PIDposH[0]); pid_zero(&PIDposH[1]); pid_zero(&PIDposV); pid_zero(&PIDcourse); pid_zero(&PIDspeed); pid_zero(&PIDpower); pathStatus->path_time = 0.0f; }
/** * Module thread, should not return. */ static void vtolPathFollowerTask(void *parameters) { SystemSettingsData systemSettings; FlightStatusData flightStatus; portTickType lastUpdateTime; VtolPathFollowerSettingsConnectCallback(SettingsUpdatedCb); PathDesiredConnectCallback(SettingsUpdatedCb); VtolPathFollowerSettingsGet(&guidanceSettings); PathDesiredGet(&pathDesired); // Main task loop lastUpdateTime = xTaskGetTickCount(); while (1) { // Conditions when this runs: // 1. Must have VTOL type airframe // 2. Flight mode is PositionHold and PathDesired.Mode is Endpoint OR // FlightMode is PathPlanner and PathDesired.Mode is Endpoint or Path SystemSettingsGet(&systemSettings); if ( (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_VTOL) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADP) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_QUADX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXA) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTO) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX) && (systemSettings.AirframeType != SYSTEMSETTINGS_AIRFRAMETYPE_TRI) ) { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_WARNING); vTaskDelay(1000); continue; } // Continue collecting data if not enough time vTaskDelayUntil(&lastUpdateTime, MS2TICKS(guidanceSettings.UpdatePeriod)); // Convert the accels into the NED frame updateNedAccel(); FlightStatusGet(&flightStatus); // Check the combinations of flightmode and pathdesired mode switch(flightStatus.FlightMode) { /* This combination of RETURNTOHOME and HOLDPOSITION looks strange but * is correct. RETURNTOHOME mode uses HOLDPOSITION with the position * set to home */ case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME: if (pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) { updateEndpointVelocity(); updateVtolDesiredAttitude(); } else { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR); } break; case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: if (pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) { updateEndpointVelocity(); updateVtolDesiredAttitude(); } else { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR); } break; case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT || pathDesired.Mode == PATHDESIRED_MODE_HOLDPOSITION) { updateEndpointVelocity(); updateVtolDesiredAttitude(); } else if (pathDesired.Mode == PATHDESIRED_MODE_FLYVECTOR || pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLELEFT || pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLERIGHT) { updatePathVelocity(); updateVtolDesiredAttitude(); } else { AlarmsSet(SYSTEMALARMS_ALARM_PATHFOLLOWER,SYSTEMALARMS_ALARM_ERROR); } break; default: for (uint32_t i = 0; i < VTOL_PID_NUM; i++) pid_zero(&vtol_pids[i]); // Track throttle before engaging this mode. Cheap system ident StabilizationDesiredData stabDesired; StabilizationDesiredGet(&stabDesired); throttleOffset = stabDesired.Throttle; break; } AlarmsClear(SYSTEMALARMS_ALARM_PATHFOLLOWER); } }