static void UpdateStabilizationDesired(bool doingIdent) { StabilizationDesiredData stabDesired; StabilizationDesiredGet(&stabDesired); uint8_t rollMax, pitchMax; float manualRate[STABILIZATIONSETTINGS_MANUALRATE_NUMELEM]; StabilizationSettingsRollMaxGet(&rollMax); StabilizationSettingsPitchMaxGet(&pitchMax); StabilizationSettingsManualRateGet(manualRate); ManualControlCommandRollGet(&stabDesired.Roll); stabDesired.Roll *= rollMax; ManualControlCommandPitchGet(&stabDesired.Pitch); stabDesired.Pitch *= pitchMax; ManualControlCommandYawGet(&stabDesired.Yaw); stabDesired.Yaw *= manualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; if (doingIdent) { stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT; } else { stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; } ManualControlCommandThrottleGet(&stabDesired.Throttle); StabilizationDesiredSet(&stabDesired); }
static float get_pid_scale_source_value() { float value; switch (stabSettings.stabBank.ThrustPIDScaleSource) { case STABILIZATIONBANK_THRUSTPIDSCALESOURCE_MANUALCONTROLTHROTTLE: ManualControlCommandThrottleGet(&value); break; case STABILIZATIONBANK_THRUSTPIDSCALESOURCE_STABILIZATIONDESIREDTHRUST: StabilizationDesiredThrustGet(&value); break; case STABILIZATIONBANK_THRUSTPIDSCALESOURCE_ACTUATORDESIREDTHRUST: ActuatorDesiredThrustGet(&value); break; default: ActuatorDesiredThrustGet(&value); break; } if (value < 0) { value = 0.0f; } return value; }
/** * @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; } } } }
/** * 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; }
/** * Compute desired attitude from the desired velocity * @param[in] dT the time since last evaluation * * Takes in @ref NedActual which has the acceleration in the * NED frame as the feedback term and then compares the * @ref VelocityActual against the @ref VelocityDesired */ int32_t vtol_follower_control_attitude(float dT) { vtol_follower_control_accel(dT); AccelDesiredData accelDesired; AccelDesiredGet(&accelDesired); StabilizationDesiredData stabDesired; float northCommand = accelDesired.North; float eastCommand = accelDesired.East; // Project the north and east acceleration signals into body frame float yaw; AttitudeActualYawGet(&yaw); float forward_accel_desired = -northCommand * cosf(yaw * DEG2RAD) + -eastCommand * sinf(yaw * DEG2RAD); float right_accel_desired = -northCommand * sinf(yaw * DEG2RAD) + eastCommand * cosf(yaw * DEG2RAD); // Set the angle that would achieve the desired acceleration given the thrust is enough for a hover stabDesired.Pitch = bound_min_max(RAD2DEG * atanf(forward_accel_desired / GRAVITY), -guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch); stabDesired.Roll = bound_min_max(RAD2DEG * atanf(right_accel_desired / GRAVITY), -guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch); stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; // Calculate the throttle setting or use pass through from transmitter if (guidanceSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) { ManualControlCommandThrottleGet(&stabDesired.Throttle); } else { float downCommand = accelDesired.Down; AltitudeHoldStateData altitudeHoldState; altitudeHoldState.VelocityDesired = downCommand; altitudeHoldState.Integral = vtol_pids[DOWN_VELOCITY].iAccumulator / 1000.0f; altitudeHoldState.AngleGain = 1.0f; if (altitudeHoldSettings.AttitudeComp > 0) { // Throttle desired is at this point the mount desired in the up direction, we can // account for the attitude if desired AttitudeActualData attitudeActual; AttitudeActualGet(&attitudeActual); // Project a unit vector pointing up into the body frame and // get the z component float fraction = attitudeActual.q1 * attitudeActual.q1 - attitudeActual.q2 * attitudeActual.q2 - attitudeActual.q3 * attitudeActual.q3 + attitudeActual.q4 * attitudeActual.q4; // Add ability to scale up the amount of compensation to achieve // level forward flight fraction = powf(fraction, (float) altitudeHoldSettings.AttitudeComp / 100.0f); // Dividing by the fraction remaining in the vertical projection will // attempt to compensate for tilt. This acts like the thrust is linear // with the output which isn't really true. If the fraction is starting // to go negative we are inverted and should shut off throttle downCommand = (fraction > 0.1f) ? (downCommand / fraction) : 0.0f; altitudeHoldState.AngleGain = 1.0f / fraction; } altitudeHoldState.Throttle = downCommand; AltitudeHoldStateSet(&altitudeHoldState); stabDesired.Throttle = bound_min_max(downCommand, 0, 1); } // Various ways to control the yaw that are essentially manual passthrough. However, because we do not have a fine // grained mechanism of manual setting the yaw as it normally would we need to duplicate that code here float manual_rate[STABILIZATIONSETTINGS_MANUALRATE_NUMELEM]; switch(guidanceSettings.YawMode) { case VTOLPATHFOLLOWERSETTINGS_YAWMODE_RATE: /* This is awkward. This allows the transmitter to control the yaw while flying navigation */ ManualControlCommandYawGet(&yaw); StabilizationSettingsManualRateGet(manual_rate); stabDesired.Yaw = manual_rate[STABILIZATIONSETTINGS_MANUALRATE_YAW] * yaw; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; break; case VTOLPATHFOLLOWERSETTINGS_YAWMODE_AXISLOCK: ManualControlCommandYawGet(&yaw); StabilizationSettingsManualRateGet(manual_rate); stabDesired.Yaw = manual_rate[STABILIZATIONSETTINGS_MANUALRATE_YAW] * yaw; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; break; case VTOLPATHFOLLOWERSETTINGS_YAWMODE_ATTITUDE: { uint8_t yaw_max; StabilizationSettingsYawMaxGet(&yaw_max); ManualControlCommandYawGet(&yaw); stabDesired.Yaw = yaw_max * yaw; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; } break; case VTOLPATHFOLLOWERSETTINGS_YAWMODE_PATH: { // Face forward on the path VelocityDesiredData velocityDesired; VelocityDesiredGet(&velocityDesired); float total_vel2 = velocityDesired.East*velocityDesired.East + velocityDesired.North*velocityDesired.North; float path_direction = atan2f(velocityDesired.East, velocityDesired.North) * RAD2DEG; if (total_vel2 > 1) { stabDesired.Yaw = path_direction; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; } else { stabDesired.Yaw = 0; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; } } break; case VTOLPATHFOLLOWERSETTINGS_YAWMODE_POI: stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_POI; break; } StabilizationDesiredSet(&stabDesired); 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); }
/** * Processes queue events */ static void processObjEvent(UAVObjEvent * ev) { UAVObjMetadata metadata; // FlightTelemetryStatsData flightStats; // GCSTelemetryStatsData gcsTelemetryStatsData; // int32_t retries; // int32_t success; if (ev->obj == 0) { updateTelemetryStats(); } else if (ev->obj == GCSTelemetryStatsHandle()) { gcsTelemetryStatsUpdated(); } else if (ev->obj == TelemetrySettingsHandle()) { updateSettings(); } else { // Get object metadata UAVObjGetMetadata(ev->obj, &metadata); // If this is a metaobject then make necessary telemetry updates if (UAVObjIsMetaobject(ev->obj)) { updateObject(UAVObjGetLinkedObj(ev->obj)); // linked object will be the actual object the metadata are for } mavlink_message_t msg; mavlink_system.sysid = 20; mavlink_system.compid = MAV_COMP_ID_IMU; mavlink_system.type = MAV_TYPE_FIXED_WING; uint8_t mavClass = MAV_AUTOPILOT_OPENPILOT; AlarmsClear(SYSTEMALARMS_ALARM_TELEMETRY); // Setup type and object id fields uint32_t objId = UAVObjGetID(ev->obj); // uint64_t timeStamp = 0; switch(objId) { case BAROALTITUDE_OBJID: { BaroAltitudeGet(&baroAltitude); pressure.press_abs = baroAltitude.Pressure*10.0f; pressure.temperature = baroAltitude.Temperature*100.0f; mavlink_msg_scaled_pressure_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &pressure); // Copy the message to the send buffer uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg); // Send buffer PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len); break; } case FLIGHTTELEMETRYSTATS_OBJID: { // FlightTelemetryStatsData flightTelemetryStats; FlightTelemetryStatsGet(&flightStats); // XXX this is a hack to make it think it got a confirmed // connection flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_CONNECTED; GCSTelemetryStatsGet(&gcsTelemetryStatsData); gcsTelemetryStatsData.Status = GCSTELEMETRYSTATS_STATUS_CONNECTED; // // // //mavlink_msg_heartbeat_send(MAVLINK_COMM_0,mavlink_system.type,mavClass); // mavlink_msg_heartbeat_pack(mavlink_system.sysid, mavlink_system.compid, &msg, mavlink_system.type, mavClass); // // Copy the message to the send buffer // uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg); // // Send buffer // PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len); break; } case SYSTEMSTATS_OBJID: { FlightStatusData flightStatus; FlightStatusGet(&flightStatus); uint8_t system_state = MAV_STATE_UNINIT; uint8_t base_mode = 0; uint8_t custom_mode = 0; // Set flight mode switch (flightStatus.FlightMode) { case FLIGHTSTATUS_FLIGHTMODE_MANUAL: base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; break; case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: base_mode |= MAV_MODE_FLAG_AUTO_ENABLED; break; case FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL: base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; break; default: base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; break; } // Set arming state switch (flightStatus.Armed) { case FLIGHTSTATUS_ARMED_ARMING: case FLIGHTSTATUS_ARMED_ARMED: system_state = MAV_STATE_ACTIVE; base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; break; case FLIGHTSTATUS_ARMED_DISARMED: system_state = MAV_STATE_STANDBY; base_mode &= !MAV_MODE_FLAG_SAFETY_ARMED; break; } // Set HIL if (hilEnabled) base_mode |= MAV_MODE_FLAG_HIL_ENABLED; mavlink_msg_heartbeat_send(MAVLINK_COMM_0, mavlink_system.type, mavClass, base_mode, custom_mode, system_state); SystemStatsData stats; SystemStatsGet(&stats); FlightBatteryStateData flightBatteryData; FlightBatteryStateGet(&flightBatteryData); FlightBatterySettingsData flightBatterySettings; FlightBatterySettingsGet(&flightBatterySettings); uint16_t batteryVoltage = (uint16_t)(flightBatteryData.Voltage*1000.0f); int16_t batteryCurrent = -1; // -1: Not present / not estimated int8_t batteryPercent = -1; // -1: Not present / not estimated // if (flightBatterySettings.SensorCalibrations[FLIGHTBATTERYSETTINGS_SENSORCALIBRATIONS_CURRENTFACTOR] == 0) // { // Factor is zero, sensor is not present // Estimate remaining capacity based on lipo curve batteryPercent = 100.0f*((flightBatteryData.Voltage - 9.6f)/(12.6f - 9.6f)); // } // else // { // // Use capacity and current // batteryPercent = 100.0f*((flightBatterySettings.Capacity - flightBatteryData.ConsumedEnergy) / flightBatterySettings.Capacity); // batteryCurrent = flightBatteryData.Current*100; // } mavlink_msg_sys_status_send(MAVLINK_COMM_0, 0xFF, 0xFF, 0xFF, ((uint16_t)stats.CPULoad*10), batteryVoltage, batteryCurrent, batteryPercent, 0, 0, 0, 0, 0, 0); // // Copy the message to the send buffer // uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg); // // Send buffer // PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len); break; } case ATTITUDERAW_OBJID: { AttitudeRawGet(&attitudeRaw); // Copy data attitude_raw.xacc = attitudeRaw.accels[ATTITUDERAW_ACCELS_X]; attitude_raw.yacc = attitudeRaw.accels[ATTITUDERAW_ACCELS_Y]; attitude_raw.zacc = attitudeRaw.accels[ATTITUDERAW_ACCELS_Z]; attitude_raw.xgyro = attitudeRaw.gyros[ATTITUDERAW_GYROS_X]; attitude_raw.ygyro = attitudeRaw.gyros[ATTITUDERAW_GYROS_Y]; attitude_raw.zgyro = attitudeRaw.gyros[ATTITUDERAW_GYROS_Z]; attitude_raw.xmag = attitudeRaw.magnetometers[ATTITUDERAW_MAGNETOMETERS_X]; attitude_raw.ymag = attitudeRaw.magnetometers[ATTITUDERAW_MAGNETOMETERS_Y]; attitude_raw.zmag = attitudeRaw.magnetometers[ATTITUDERAW_MAGNETOMETERS_Z]; mavlink_msg_raw_imu_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &attitude_raw); // Copy the message to the send buffer uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg); // Send buffer PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len); if (hilEnabled) { mavlink_hil_controls_t controls; // Copy data controls.roll_ailerons = 0.1; controls.pitch_elevator = 0.1; controls.yaw_rudder = 0.0; controls.throttle = 0.8; mavlink_msg_hil_controls_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &controls); // Copy the message to the send buffer len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg); // Send buffer PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len); } break; } case ATTITUDEMATRIX_OBJID: { AttitudeMatrixGet(&attitudeMatrix); // Copy data attitude.roll = attitudeMatrix.Roll; attitude.pitch = attitudeMatrix.Pitch; attitude.yaw = attitudeMatrix.Yaw; attitude.rollspeed = attitudeMatrix.AngularRates[0]; attitude.pitchspeed = attitudeMatrix.AngularRates[1]; attitude.yawspeed = attitudeMatrix.AngularRates[2]; mavlink_msg_attitude_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &attitude); // Copy the message to the send buffer uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg); // Send buffer PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len); break; } case GPSPOSITION_OBJID: { GPSPositionGet(&gpsPosition); gps_raw.time_usec = 0; gps_raw.lat = gpsPosition.Latitude*10; gps_raw.lon = gpsPosition.Longitude*10; gps_raw.alt = gpsPosition.Altitude*10; gps_raw.eph = gpsPosition.HDOP*100; gps_raw.epv = gpsPosition.VDOP*100; gps_raw.cog = gpsPosition.Heading*100; gps_raw.satellites_visible = gpsPosition.Satellites; gps_raw.fix_type = gpsPosition.Status; mavlink_msg_gps_raw_int_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &gps_raw); // Copy the message to the send buffer uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg); // Send buffer PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len); // mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, gps_raw.usec, gps_raw.lat, gps_raw.lon, gps_raw.alt, gps_raw.eph, gps_raw.epv, gps_raw.hdg, gps_raw.satellites_visible, gps_raw.fix_type, 0); break; } case POSITIONACTUAL_OBJID: { PositionActualData pos; PositionActualGet(&pos); mavlink_local_position_ned_t m_pos; m_pos.time_boot_ms = 0; m_pos.x = pos.North; m_pos.y = pos.East; m_pos.z = pos.Down; m_pos.vx = 0.0f; m_pos.vy = 0.0f; m_pos.vz = 0.0f; mavlink_msg_local_position_ned_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &m_pos); // Copy the message to the send buffer uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg); // Send buffer PIOS_COM_SendBufferNonBlocking(telemetryPort, mavlinkTxBuf, len); } break; case ACTUATORCOMMAND_OBJID: { mavlink_rc_channels_scaled_t rc; float val; ManualControlCommandRollGet(&val); rc.chan1_scaled = val*1000; ManualControlCommandPitchGet(&val); rc.chan2_scaled = val*1000; ManualControlCommandYawGet(&val); rc.chan3_scaled = val*1000; ManualControlCommandThrottleGet(&val); rc.chan4_scaled = val*1000; ActuatorCommandData act; ActuatorCommandGet(&act); rc.chan5_scaled = act.Channel[0]; rc.chan6_scaled = act.Channel[1]; rc.chan7_scaled = act.Channel[2]; rc.chan8_scaled = act.Channel[3]; ManualControlCommandData cmd; ManualControlCommandGet(&cmd); rc.rssi = ((uint8_t)(cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_TRUE))*255; rc.port = 0; mavlink_msg_rc_channels_scaled_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &rc); // Copy the message to the send buffer uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg); // Send buffer PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEM_RF, mavlinkTxBuf, len); break; } case MANUALCONTROLCOMMAND_OBJID: { mavlink_rc_channels_scaled_t rc; float val; ManualControlCommandRollGet(&val); rc.chan1_scaled = val*1000; ManualControlCommandPitchGet(&val); rc.chan2_scaled = val*1000; ManualControlCommandYawGet(&val); rc.chan3_scaled = val*1000; ManualControlCommandThrottleGet(&val); rc.chan4_scaled = val*1000; rc.chan5_scaled = 0; rc.chan6_scaled = 0; rc.chan7_scaled = 0; rc.chan8_scaled = 0; ManualControlCommandData cmd; ManualControlCommandGet(&cmd); rc.rssi = ((uint8_t)(cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_TRUE))*255; rc.port = 0; mavlink_msg_rc_channels_scaled_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &rc); // Copy the message to the send buffer uint16_t len = mavlink_msg_to_send_buffer(mavlinkTxBuf, &msg); // Send buffer PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEM_RF, mavlinkTxBuf, len); break; } default: { //printf("unknown object: %x\n",(unsigned int)objId); break; } } } }
/** * Compute desired attitude from the desired velocity * @param[in] dT the time since last evaluation * @param[in] att_adj an adjustment to the attitude for loiter mode * * Takes in @ref NedActual which has the acceleration in the * NED frame as the feedback term and then compares the * @ref VelocityActual against the @ref VelocityDesired */ int32_t vtol_follower_control_attitude(float dT, const float *att_adj) { vtol_follower_control_accel(dT); float default_adj[2] = {0,0}; if (!att_adj) { att_adj = default_adj; } AccelDesiredData accelDesired; AccelDesiredGet(&accelDesired); StabilizationSettingsData stabSet; StabilizationSettingsGet(&stabSet); float northCommand = accelDesired.North; float eastCommand = accelDesired.East; // Project the north and east acceleration signals into body frame float yaw; AttitudeActualYawGet(&yaw); float forward_accel_desired = -northCommand * cosf(yaw * DEG2RAD) + -eastCommand * sinf(yaw * DEG2RAD); float right_accel_desired = -northCommand * sinf(yaw * DEG2RAD) + eastCommand * cosf(yaw * DEG2RAD); StabilizationDesiredData stabDesired; // Set the angle that would achieve the desired acceleration given the thrust is enough for a hover stabDesired.Pitch = bound_sym(RAD2DEG * atanf(forward_accel_desired / GRAVITY), guidanceSettings.MaxRollPitch) + att_adj[1]; stabDesired.Roll = bound_sym(RAD2DEG * atanf(right_accel_desired / GRAVITY), guidanceSettings.MaxRollPitch) + att_adj[0]; // Re-bound based on maximum attitude settings stabDesired.Pitch = bound_sym(stabDesired.Pitch, stabSet.PitchMax); stabDesired.Roll = bound_sym(stabDesired.Roll, stabSet.RollMax); stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; // Calculate the throttle setting or use pass through from transmitter if (guidanceSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) { ManualControlCommandThrottleGet(&stabDesired.Throttle); } else { float downCommand = vtol_follower_control_altitude(accelDesired.Down); stabDesired.Throttle = bound_min_max(downCommand, 0, 1); } // Various ways to control the yaw that are essentially manual passthrough. However, because we do not have a fine // grained mechanism of manual setting the yaw as it normally would we need to duplicate that code here switch(guidanceSettings.YawMode) { case VTOLPATHFOLLOWERSETTINGS_YAWMODE_RATE: /* This is awkward. This allows the transmitter to control the yaw while flying navigation */ ManualControlCommandYawGet(&yaw); stabDesired.Yaw = stabSet.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] * yaw; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; break; case VTOLPATHFOLLOWERSETTINGS_YAWMODE_AXISLOCK: ManualControlCommandYawGet(&yaw); stabDesired.Yaw = stabSet.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] * yaw; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; break; case VTOLPATHFOLLOWERSETTINGS_YAWMODE_ATTITUDE: { ManualControlCommandYawGet(&yaw); stabDesired.Yaw = stabSet.YawMax * yaw; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; } break; case VTOLPATHFOLLOWERSETTINGS_YAWMODE_PATH: { // Face forward on the path VelocityDesiredData velocityDesired; VelocityDesiredGet(&velocityDesired); float total_vel2 = velocityDesired.East*velocityDesired.East + velocityDesired.North*velocityDesired.North; float path_direction = atan2f(velocityDesired.East, velocityDesired.North) * RAD2DEG; if (total_vel2 > 1) { stabDesired.Yaw = path_direction; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; } else { stabDesired.Yaw = 0; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; } } break; case VTOLPATHFOLLOWERSETTINGS_YAWMODE_POI: stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_POI; break; } StabilizationDesiredSet(&stabDesired); return 0; }