rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-21 11:54:15 +01:00
Code Issues Releases Activity

OP-1848 altvario TxPID support

Browse Source
This commit is contained in:
abeck70 2015-05-02 23:57:18 +10:00
parent 21c54c3a37
commit b479bd5436
3 changed files with 79 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
flight/modules/Stabilization/altitudeloop.cpp
View File

@ -185,7 +185,6 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
// initialise limits on thrust but note the FSM can override. // initialise limits on thrust but note the FSM can override.
controlDown.SetThrustLimits(altitudeHoldSettings.ThrustLimits.Min, altitudeHoldSettings.ThrustLimits.Max); controlDown.SetThrustLimits(altitudeHoldSettings.ThrustLimits.Min, altitudeHoldSettings.ThrustLimits.Max);
} }

132
flight/modules/TxPID/txpid.c
View File

@ -55,6 +55,7 @@
#include "accessorydesired.h" #include "accessorydesired.h"
#include "manualcontrolcommand.h" #include "manualcontrolcommand.h"
#include "stabilizationsettings.h" #include "stabilizationsettings.h"
#include "altitudeholdsettings.h"
#include "stabilizationbank.h" #include "stabilizationbank.h"
#include "stabilizationsettingsbank1.h" #include "stabilizationsettingsbank1.h"
#include "stabilizationsettingsbank2.h" #include "stabilizationsettingsbank2.h"
@ -95,6 +96,8 @@ int32_t TxPIDInitialize(void)
bool txPIDEnabled; bool txPIDEnabled;
HwSettingsOptionalModulesData optionalModules; HwSettingsOptionalModulesData optionalModules;
AltitudeHoldSettingsInitialize();
HwSettingsInitialize(); HwSettingsInitialize();
HwSettingsOptionalModulesGet(&optionalModules); HwSettingsOptionalModulesGet(&optionalModules);
@ -188,10 +191,13 @@ static void updatePIDs(UAVObjEvent *ev)
} }
StabilizationSettingsData stab; StabilizationSettingsData stab;
StabilizationSettingsGet(&stab); StabilizationSettingsGet(&stab);
AltitudeHoldSettingsData altitude;
AltitudeHoldSettingsGet(&altitude);
AccessoryDesiredData accessory; AccessoryDesiredData accessory;
uint8_t needsUpdateBank = 0; uint8_t needsUpdateBank = 0;
uint8_t needsUpdateStab = 0; uint8_t needsUpdateStab = 0;
uint8_t needsUpdateAltitude = 0;
// Loop through every enabled instance // Loop through every enabled instance
for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) { for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) {
@ -216,140 +222,155 @@ static void updatePIDs(UAVObjEvent *ev)
switch (TxPIDSettingsPIDsToArray(inst.PIDs)[i]) { switch (TxPIDSettingsPIDsToArray(inst.PIDs)[i]) {
case TXPIDSETTINGS_PIDS_ROLLRATEKP: case TXPIDSETTINGS_PIDS_ROLLRATEKP:
needsUpdateBank |= update(&bank.RollRatePID.Kp, value); needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLRATEKI: case TXPIDSETTINGS_PIDS_ROLLRATEKI:
needsUpdateBank |= update(&bank.RollRatePID.Ki, value); needsUpdateBank |= update(&bank.RollRatePID.Ki, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLRATEKD: case TXPIDSETTINGS_PIDS_ROLLRATEKD:
needsUpdateBank |= update(&bank.RollRatePID.Kd, value); needsUpdateBank |= update(&bank.RollRatePID.Kd, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT: case TXPIDSETTINGS_PIDS_ROLLRATEILIMIT:
needsUpdateBank |= update(&bank.RollRatePID.ILimit, value); needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLRATERESP: case TXPIDSETTINGS_PIDS_ROLLRATERESP:
needsUpdateBank |= update(&bank.ManualRate.Roll, value); needsUpdateBank |= update(&bank.ManualRate.Roll, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP: case TXPIDSETTINGS_PIDS_ROLLATTITUDEKP:
needsUpdateBank |= update(&bank.RollPI.Kp, value); needsUpdateBank |= update(&bank.RollPI.Kp, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI: case TXPIDSETTINGS_PIDS_ROLLATTITUDEKI:
needsUpdateBank |= update(&bank.RollPI.Ki, value); needsUpdateBank |= update(&bank.RollPI.Ki, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT: case TXPIDSETTINGS_PIDS_ROLLATTITUDEILIMIT:
needsUpdateBank |= update(&bank.RollPI.ILimit, value); needsUpdateBank |= update(&bank.RollPI.ILimit, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLATTITUDERESP: case TXPIDSETTINGS_PIDS_ROLLATTITUDERESP:
needsUpdateBank |= updateUint8(&bank.RollMax, value); needsUpdateBank |= updateUint8(&bank.RollMax, value);
break; break;
case TXPIDSETTINGS_PIDS_PITCHRATEKP: case TXPIDSETTINGS_PIDS_PITCHRATEKP:
needsUpdateBank |= update(&bank.PitchRatePID.Kp, value); needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
break; break;
case TXPIDSETTINGS_PIDS_PITCHRATEKI: case TXPIDSETTINGS_PIDS_PITCHRATEKI:
needsUpdateBank |= update(&bank.PitchRatePID.Ki, value); needsUpdateBank |= update(&bank.PitchRatePID.Ki, value);
break; break;
case TXPIDSETTINGS_PIDS_PITCHRATEKD: case TXPIDSETTINGS_PIDS_PITCHRATEKD:
needsUpdateBank |= update(&bank.PitchRatePID.Kd, value); needsUpdateBank |= update(&bank.PitchRatePID.Kd, value);
break; break;
case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT: case TXPIDSETTINGS_PIDS_PITCHRATEILIMIT:
needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value); needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
break; break;
case TXPIDSETTINGS_PIDS_PITCHRATERESP: case TXPIDSETTINGS_PIDS_PITCHRATERESP:
needsUpdateBank |= update(&bank.ManualRate.Pitch, value); needsUpdateBank |= update(&bank.ManualRate.Pitch, value);
break; break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP: case TXPIDSETTINGS_PIDS_PITCHATTITUDEKP:
needsUpdateBank |= update(&bank.PitchPI.Kp, value); needsUpdateBank |= update(&bank.PitchPI.Kp, value);
break; break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI: case TXPIDSETTINGS_PIDS_PITCHATTITUDEKI:
needsUpdateBank |= update(&bank.PitchPI.Ki, value); needsUpdateBank |= update(&bank.PitchPI.Ki, value);
break; break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT: case TXPIDSETTINGS_PIDS_PITCHATTITUDEILIMIT:
needsUpdateBank |= update(&bank.PitchPI.ILimit, value); needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
break; break;
case TXPIDSETTINGS_PIDS_PITCHATTITUDERESP: case TXPIDSETTINGS_PIDS_PITCHATTITUDERESP:
needsUpdateBank |= updateUint8(&bank.PitchMax, value); needsUpdateBank |= updateUint8(&bank.PitchMax, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP: case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKP:
needsUpdateBank |= update(&bank.RollRatePID.Kp, value); needsUpdateBank |= update(&bank.RollRatePID.Kp, value);
needsUpdateBank |= update(&bank.PitchRatePID.Kp, value); needsUpdateBank |= update(&bank.PitchRatePID.Kp, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI: case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKI:
needsUpdateBank |= update(&bank.RollRatePID.Ki, value); needsUpdateBank |= update(&bank.RollRatePID.Ki, value);
needsUpdateBank |= update(&bank.PitchRatePID.Ki, value); needsUpdateBank |= update(&bank.PitchRatePID.Ki, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD: case TXPIDSETTINGS_PIDS_ROLLPITCHRATEKD:
needsUpdateBank |= update(&bank.RollRatePID.Kd, value); needsUpdateBank |= update(&bank.RollRatePID.Kd, value);
needsUpdateBank |= update(&bank.PitchRatePID.Kd, value); needsUpdateBank |= update(&bank.PitchRatePID.Kd, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT: case TXPIDSETTINGS_PIDS_ROLLPITCHRATEILIMIT:
needsUpdateBank |= update(&bank.RollRatePID.ILimit, value); needsUpdateBank |= update(&bank.RollRatePID.ILimit, value);
needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value); needsUpdateBank |= update(&bank.PitchRatePID.ILimit, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLPITCHRATERESP: case TXPIDSETTINGS_PIDS_ROLLPITCHRATERESP:
needsUpdateBank |= update(&bank.ManualRate.Roll, value); needsUpdateBank |= update(&bank.ManualRate.Roll, value);
needsUpdateBank |= update(&bank.ManualRate.Pitch, value); needsUpdateBank |= update(&bank.ManualRate.Pitch, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP: case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKP:
needsUpdateBank |= update(&bank.RollPI.Kp, value); needsUpdateBank |= update(&bank.RollPI.Kp, value);
needsUpdateBank |= update(&bank.PitchPI.Kp, value); needsUpdateBank |= update(&bank.PitchPI.Kp, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI: case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEKI:
needsUpdateBank |= update(&bank.RollPI.Ki, value); needsUpdateBank |= update(&bank.RollPI.Ki, value);
needsUpdateBank |= update(&bank.PitchPI.Ki, value); needsUpdateBank |= update(&bank.PitchPI.Ki, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT: case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDEILIMIT:
needsUpdateBank |= update(&bank.RollPI.ILimit, value); needsUpdateBank |= update(&bank.RollPI.ILimit, value);
needsUpdateBank |= update(&bank.PitchPI.ILimit, value); needsUpdateBank |= update(&bank.PitchPI.ILimit, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDERESP: case TXPIDSETTINGS_PIDS_ROLLPITCHATTITUDERESP:
needsUpdateBank |= updateUint8(&bank.RollMax, value); needsUpdateBank |= updateUint8(&bank.RollMax, value);
needsUpdateBank |= updateUint8(&bank.PitchMax, value); needsUpdateBank |= updateUint8(&bank.PitchMax, value);
break; break;
case TXPIDSETTINGS_PIDS_YAWRATEKP: case TXPIDSETTINGS_PIDS_YAWRATEKP:
needsUpdateBank |= update(&bank.YawRatePID.Kp, value); needsUpdateBank |= update(&bank.YawRatePID.Kp, value);
break; break;
case TXPIDSETTINGS_PIDS_YAWRATEKI: case TXPIDSETTINGS_PIDS_YAWRATEKI:
needsUpdateBank |= update(&bank.YawRatePID.Ki, value); needsUpdateBank |= update(&bank.YawRatePID.Ki, value);
break; break;
case TXPIDSETTINGS_PIDS_YAWRATEKD: case TXPIDSETTINGS_PIDS_YAWRATEKD:
needsUpdateBank |= update(&bank.YawRatePID.Kd, value); needsUpdateBank |= update(&bank.YawRatePID.Kd, value);
break; break;
case TXPIDSETTINGS_PIDS_YAWRATEILIMIT: case TXPIDSETTINGS_PIDS_YAWRATEILIMIT:
needsUpdateBank |= update(&bank.YawRatePID.ILimit, value); needsUpdateBank |= update(&bank.YawRatePID.ILimit, value);
break; break;
case TXPIDSETTINGS_PIDS_YAWRATERESP: case TXPIDSETTINGS_PIDS_YAWRATERESP:
needsUpdateBank |= update(&bank.ManualRate.Yaw, value); needsUpdateBank |= update(&bank.ManualRate.Yaw, value);
break; break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKP: case TXPIDSETTINGS_PIDS_YAWATTITUDEKP:
needsUpdateBank |= update(&bank.YawPI.Kp, value); needsUpdateBank |= update(&bank.YawPI.Kp, value);
break; break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEKI: case TXPIDSETTINGS_PIDS_YAWATTITUDEKI:
needsUpdateBank |= update(&bank.YawPI.Ki, value); needsUpdateBank |= update(&bank.YawPI.Ki, value);
break; break;
case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT: case TXPIDSETTINGS_PIDS_YAWATTITUDEILIMIT:
needsUpdateBank |= update(&bank.YawPI.ILimit, value); needsUpdateBank |= update(&bank.YawPI.ILimit, value);
break; break;
case TXPIDSETTINGS_PIDS_YAWATTITUDERESP: case TXPIDSETTINGS_PIDS_YAWATTITUDERESP:
needsUpdateBank |= updateUint8(&bank.YawMax, value); needsUpdateBank |= updateUint8(&bank.YawMax, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLEXPO: case TXPIDSETTINGS_PIDS_ROLLEXPO:
needsUpdateBank |= updateInt8(&bank.StickExpo.Roll, value); needsUpdateBank |= updateInt8(&bank.StickExpo.Roll, value);
break; break;
case TXPIDSETTINGS_PIDS_PITCHEXPO: case TXPIDSETTINGS_PIDS_PITCHEXPO:
needsUpdateBank |= updateInt8(&bank.StickExpo.Pitch, value); needsUpdateBank |= updateInt8(&bank.StickExpo.Pitch, value);
break; break;
case TXPIDSETTINGS_PIDS_ROLLPITCHEXPO: case TXPIDSETTINGS_PIDS_ROLLPITCHEXPO:
needsUpdateBank |= updateInt8(&bank.StickExpo.Roll, value); needsUpdateBank |= updateInt8(&bank.StickExpo.Roll, value);
needsUpdateBank |= updateInt8(&bank.StickExpo.Pitch, value); needsUpdateBank |= updateInt8(&bank.StickExpo.Pitch, value);
break; break;
case TXPIDSETTINGS_PIDS_YAWEXPO: case TXPIDSETTINGS_PIDS_YAWEXPO:
needsUpdateBank |= updateInt8(&bank.StickExpo.Yaw, value); needsUpdateBank |= updateInt8(&bank.StickExpo.Yaw, value);
break; break;
case TXPIDSETTINGS_PIDS_GYROTAU: case TXPIDSETTINGS_PIDS_GYROTAU:
needsUpdateStab |= update(&stab.GyroTau, value); needsUpdateStab |= update(&stab.GyroTau, value);
break; break;
case TXPIDSETTINGS_PIDS_ACROPLUSFACTOR: case TXPIDSETTINGS_PIDS_ACROPLUSFACTOR:
needsUpdateBank |= update(&bank.AcroInsanityFactor, value); needsUpdateBank |= update(&bank.AcroInsanityFactor, value);
break;
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; break;
default: default:
PIOS_Assert(0); PIOS_Assert(0);
@ -359,6 +380,9 @@ static void updatePIDs(UAVObjEvent *ev)
if (needsUpdateStab) { if (needsUpdateStab) {
StabilizationSettingsSet(&stab); StabilizationSettingsSet(&stab);
} }
if (needsUpdateAltitude) {
AltitudeHoldSettingsSet(&altitude);
}
if (needsUpdateBank) { if (needsUpdateBank) {
switch (inst.BankNumber) { switch (inst.BankNumber) {
case 0: case 0:

2
shared/uavobjectdefinition/txpidsettings.xml
View File

@ -20,7 +20,7 @@
Roll+Pitch Attitude.Kp, Roll+Pitch Attitude.Ki, Roll+Pitch Attitude.ILimit, Roll+Pitch Attitude.Resp, Roll+Pitch Attitude.Kp, Roll+Pitch Attitude.Ki, Roll+Pitch Attitude.ILimit, Roll+Pitch Attitude.Resp,
Yaw Attitude.Kp, Yaw Attitude.Ki, Yaw Attitude.ILimit, Yaw Attitude.Resp, Yaw Attitude.Kp, Yaw Attitude.Ki, Yaw Attitude.ILimit, Yaw Attitude.Resp,
Roll.Expo, Pitch.Expo, Roll+Pitch.Expo, Yaw.Expo, Roll.Expo, Pitch.Expo, Roll+Pitch.Expo, Yaw.Expo,
GyroTau,AcroPlusFactor" GyroTau,AcroPlusFactor, Altitude Pos.Kp, Altitude Velocity.Kp, Altitude Velocity.Ki, Altitude Velocity.Kd, Altitude Velocity.Beta"
defaultvalue="Disabled"/> defaultvalue="Disabled"/>
<field name="MinPID" units="" type="float" elementnames="Instance1,Instance2,Instance3" defaultvalue="0"/> <field name="MinPID" units="" type="float" elementnames="Instance1,Instance2,Instance3" defaultvalue="0"/>
<field name="MaxPID" units="" type="float" elementnames="Instance1,Instance2,Instance3" defaultvalue="0"/> <field name="MaxPID" units="" type="float" elementnames="Instance1,Instance2,Instance3" defaultvalue="0"/>