mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-15 07:29:15 +01:00
gpsassist:code review comments
1. Defines 2. Name of adjustments changed to vtolSelfTuningStats
This commit is contained in:
parent
b1c94e83d0
commit
2b6d79467c
@ -37,7 +37,7 @@
|
||||
#include <sanitycheck.h>
|
||||
#include <manualcontrolsettings.h>
|
||||
#include <manualcontrolcommand.h>
|
||||
#include <adjustments.h>
|
||||
#include <vtolselftuningstats.h>
|
||||
#include <flightmodesettings.h>
|
||||
#include <flightstatus.h>
|
||||
#include <systemsettings.h>
|
||||
@ -58,6 +58,10 @@
|
||||
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR
|
||||
#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
|
||||
|
||||
#define ASSISTEDCONTROL_NEUTRALTHROTTLERANGE_FACTOR 0.2f
|
||||
#define ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_LO 0.92f
|
||||
#define ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_HI 1.08f
|
||||
|
||||
|
||||
// defined handlers
|
||||
|
||||
@ -157,7 +161,7 @@ int32_t ManualControlInitialize()
|
||||
FlightModeSettingsInitialize();
|
||||
SystemSettingsInitialize();
|
||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||
AdjustmentsInitialize();
|
||||
VtolSelfTuningStatsInitialize();
|
||||
StabilizationSettingsInitialize();
|
||||
VtolPathFollowerSettingsInitialize();
|
||||
#endif
|
||||
@ -184,8 +188,8 @@ static void manualControlTask(void)
|
||||
ManualControlCommandData cmd;
|
||||
ManualControlCommandGet(&cmd);
|
||||
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
|
||||
VtolPathFollowerSettingsData vtolPathFollowerSettings;
|
||||
VtolPathFollowerSettingsGet(&vtolPathFollowerSettings);
|
||||
VtolPathFollowerSettingsThrustLimitsData thrustLimits;
|
||||
VtolPathFollowerSettingsThrustLimitsGet(&thrustLimits);
|
||||
#endif
|
||||
|
||||
FlightModeSettingsData modeSettings;
|
||||
@ -237,10 +241,12 @@ static void manualControlTask(void)
|
||||
bool throttleNeutral = false;
|
||||
bool throttleNeutralOrLow = false;
|
||||
float neutralThrustOffset = 0.0f;
|
||||
AdjustmentsNeutralThrustOffsetGet(&neutralThrustOffset);
|
||||
float throttleRangeDelta = (vtolPathFollowerSettings.ThrustLimits.Neutral + neutralThrustOffset) * 0.2f;
|
||||
float throttleNeutralLow = (vtolPathFollowerSettings.ThrustLimits.Neutral + neutralThrustOffset) - throttleRangeDelta;
|
||||
float throttleNeutralHi = (vtolPathFollowerSettings.ThrustLimits.Neutral + neutralThrustOffset) + throttleRangeDelta;
|
||||
VtolSelfTuningStatsNeutralThrustOffsetGet(&neutralThrustOffset);
|
||||
|
||||
|
||||
float throttleRangeDelta = (thrustLimits.Neutral + neutralThrustOffset) * ASSISTEDCONTROL_NEUTRALTHROTTLERANGE_FACTOR;
|
||||
float throttleNeutralLow = (thrustLimits.Neutral + neutralThrustOffset) - throttleRangeDelta;
|
||||
float throttleNeutralHi = (thrustLimits.Neutral + neutralThrustOffset) + throttleRangeDelta;
|
||||
if (cmd.Thrust > throttleNeutralLow && cmd.Thrust < throttleNeutralHi) {
|
||||
throttleNeutral = true;
|
||||
}
|
||||
@ -266,8 +272,8 @@ static void manualControlTask(void)
|
||||
|
||||
// calculate hi and low value of +-8% as a mini-deadband
|
||||
// for use in auto-override in brake sequence
|
||||
thrustLo = 0.92f * thrustAtBrakeStart;
|
||||
thrustHi = 1.08f * thrustAtBrakeStart;
|
||||
thrustLo = ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_LO * thrustAtBrakeStart;
|
||||
thrustHi = ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_HI * thrustAtBrakeStart;
|
||||
|
||||
// The purpose for auto throttle assist is to go from a mid to high thrust range to a
|
||||
// neutral vertical-holding/maintaining ~50% thrust range. It is not designed/intended
|
||||
@ -277,7 +283,7 @@ static void manualControlTask(void)
|
||||
// kick in...the flyer needs to manually manage throttle to slow down decent,
|
||||
// and the next time they put in a bit of stick, revert to primary, and then
|
||||
// sticks-off it will brake and hold with auto-thrust
|
||||
if (thrustAtBrakeStart < vtolPathFollowerSettings.ThrustLimits.Min) {
|
||||
if (thrustAtBrakeStart < thrustLimits.Min) {
|
||||
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None
|
||||
}
|
||||
} else {
|
||||
|
@ -75,7 +75,7 @@
|
||||
#include <manualcontrolcommand.h>
|
||||
#include <systemsettings.h>
|
||||
#include <stabilizationbank.h>
|
||||
#include <adjustments.h>
|
||||
#include <vtolselftuningstats.h>
|
||||
#include <pathsummary.h>
|
||||
|
||||
|
||||
@ -209,7 +209,7 @@ int32_t PathFollowerInitialize()
|
||||
ManualControlCommandInitialize();
|
||||
SystemSettingsInitialize();
|
||||
StabilizationBankInitialize();
|
||||
AdjustmentsInitialize();
|
||||
VtolSelfTuningStatsInitialize();
|
||||
|
||||
|
||||
// reset integrals
|
||||
@ -1219,7 +1219,7 @@ static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction)
|
||||
AttitudeStateData attitudeState;
|
||||
StabilizationBankData stabSettings;
|
||||
SystemSettingsData systemSettings;
|
||||
AdjustmentsData adjustments;
|
||||
VtolSelfTuningStatsData vtolSelfTuningStats;
|
||||
|
||||
float northError;
|
||||
float northCommand;
|
||||
@ -1237,7 +1237,7 @@ static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction)
|
||||
VelocityDesiredGet(&velocityDesired);
|
||||
AttitudeStateGet(&attitudeState);
|
||||
StabilizationBankGet(&stabSettings);
|
||||
AdjustmentsGet(&adjustments);
|
||||
VtolSelfTuningStatsGet(&vtolSelfTuningStats);
|
||||
|
||||
|
||||
if (pathDesired.Mode != PATHDESIRED_MODE_BRAKE) {
|
||||
@ -1327,14 +1327,14 @@ static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction)
|
||||
neutralThrustEst.have_correction = true;
|
||||
|
||||
// Write a new adjustment value
|
||||
// adjustments.NeutralThrustOffset was incremental adjusted above
|
||||
AdjustmentsData new_adjustments;
|
||||
// vtolSelfTuningStats.NeutralThrustOffset was incremental adjusted above
|
||||
VtolSelfTuningStatsData new_vtolSelfTuningStats;
|
||||
// add the average remaining i value to the
|
||||
new_adjustments.NeutralThrustOffset = adjustments.NeutralThrustOffset + neutralThrustEst.correction;
|
||||
new_adjustments.NeutralThrustCorrection = neutralThrustEst.correction; // the i term thrust correction value applied
|
||||
new_adjustments.NeutralThrustAccumulator = global.PIDvel[2].iAccumulator; // the actual iaccumulator value after correction
|
||||
new_adjustments.NeutralThrustRange = neutralThrustEst.max - neutralThrustEst.min;
|
||||
AdjustmentsSet(&new_adjustments);
|
||||
new_vtolSelfTuningStats.NeutralThrustOffset = vtolSelfTuningStats.NeutralThrustOffset + neutralThrustEst.correction;
|
||||
new_vtolSelfTuningStats.NeutralThrustCorrection = neutralThrustEst.correction; // the i term thrust correction value applied
|
||||
new_vtolSelfTuningStats.NeutralThrustAccumulator = global.PIDvel[2].iAccumulator; // the actual iaccumulator value after correction
|
||||
new_vtolSelfTuningStats.NeutralThrustRange = neutralThrustEst.max - neutralThrustEst.min;
|
||||
VtolSelfTuningStatsSet(&new_vtolSelfTuningStats);
|
||||
}
|
||||
} else {
|
||||
// start a tick count
|
||||
@ -1351,7 +1351,7 @@ static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction)
|
||||
} // else we already have a correction for this PH run
|
||||
|
||||
// Generally in braking the downError will be an increased altitude. We really will rely on cruisecontrol to backoff.
|
||||
stabDesired.Thrust = boundf(adjustments.NeutralThrustOffset + downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max);
|
||||
stabDesired.Thrust = boundf(vtolSelfTuningStats.NeutralThrustOffset + downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max);
|
||||
|
||||
|
||||
// DEBUG HACK: allow user to skew compass on purpose to see if emergency failsafe kicks in
|
||||
|
@ -19,7 +19,7 @@
|
||||
# These are the UAVObjects supposed to be build as part of the OpenPilot target
|
||||
# (all architectures)
|
||||
UAVOBJSRCFILENAMES =
|
||||
UAVOBJSRCFILENAMES += adjustments
|
||||
UAVOBJSRCFILENAMES += vtolselftuningstats
|
||||
UAVOBJSRCFILENAMES += accelgyrosettings
|
||||
UAVOBJSRCFILENAMES += accessorydesired
|
||||
UAVOBJSRCFILENAMES += actuatorcommand
|
||||
|
@ -19,7 +19,7 @@
|
||||
# These are the UAVObjects supposed to be build as part of the OpenPilot target
|
||||
# (all architectures)
|
||||
UAVOBJSRCFILENAMES =
|
||||
UAVOBJSRCFILENAMES += adjustments
|
||||
UAVOBJSRCFILENAMES += vtolselftuningstats
|
||||
UAVOBJSRCFILENAMES += accelgyrosettings
|
||||
UAVOBJSRCFILENAMES += accessorydesired
|
||||
UAVOBJSRCFILENAMES += actuatorcommand
|
||||
|
@ -19,7 +19,7 @@
|
||||
# These are the UAVObjects supposed to be build as part of the OpenPilot target
|
||||
# (all architectures)
|
||||
UAVOBJSRCFILENAMES =
|
||||
UAVOBJSRCFILENAMES += adjustments
|
||||
UAVOBJSRCFILENAMES += vtolselftuningstats
|
||||
UAVOBJSRCFILENAMES += accelgyrosettings
|
||||
UAVOBJSRCFILENAMES += accessorydesired
|
||||
UAVOBJSRCFILENAMES += actuatorcommand
|
||||
|
@ -24,7 +24,7 @@
|
||||
# (all architectures)
|
||||
|
||||
UAVOBJSRCFILENAMES =
|
||||
UAVOBJSRCFILENAMES += adjustments
|
||||
UAVOBJSRCFILENAMES += vtolselftuningstats
|
||||
UAVOBJSRCFILENAMES += accessorydesired
|
||||
UAVOBJSRCFILENAMES += actuatorcommand
|
||||
UAVOBJSRCFILENAMES += actuatordesired
|
||||
|
@ -27,7 +27,7 @@ OTHER_FILES += UAVObjects.pluginspec
|
||||
|
||||
# Add in all of the synthetic/generated uavobject files
|
||||
HEADERS += \
|
||||
$$UAVOBJECT_SYNTHETICS/adjustments.h \
|
||||
$$UAVOBJECT_SYNTHETICS/vtolselftuningstats.h \
|
||||
$$UAVOBJECT_SYNTHETICS/accelgyrosettings.h \
|
||||
$$UAVOBJECT_SYNTHETICS/accessorydesired.h \
|
||||
$$UAVOBJECT_SYNTHETICS/barosensor.h \
|
||||
@ -133,7 +133,7 @@ HEADERS += \
|
||||
$$UAVOBJECT_SYNTHETICS/perfcounter.h
|
||||
|
||||
SOURCES += \
|
||||
$$UAVOBJECT_SYNTHETICS/adjustments.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/vtolselftuningstats.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/accelgyrosettings.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/barosensor.cpp \
|
||||
|
@ -1,6 +1,6 @@
|
||||
<xml>
|
||||
<object name="Adjustments" singleinstance="true" settings="false" category="Navigation">
|
||||
<description>Automatically calculated adjustments to parameters used into auto flight modes. Can come from @ref PathFollower </description>
|
||||
<object name="VtolSelfTuningStats" singleinstance="true" settings="false" category="Navigation">
|
||||
<description>Automatically calculated adjustments to parameters used into vtol auto flight modes. Can come from @ref PathFollower </description>
|
||||
|
||||
<field name="NeutralThrustOffset" units="" type="float" elements="1" defaultvalue="0"/>
|
||||
<field name="NeutralThrustCorrection" units="" type="float" elements="1" defaultvalue="0"/>
|
Loading…
x
Reference in New Issue
Block a user