rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-21 13:28:58 +01:00
Code Issues Releases Activity

gpsassist:code review comments

Browse Source 
1. Defines
2. Name of adjustments changed to vtolSelfTuningStats
This commit is contained in:
abeck70 2015-01-09 07:51:37 +11:00
parent b1c94e83d0
commit 2b6d79467c
8 changed files with 37 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
flight/modules/ManualControl/manualcontrol.c
View File

@ -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 {

24
flight/modules/PathFollower/pathfollower.c
View File

@ -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

2
flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc
View File

@ -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

2
flight/targets/boards/revolution/firmware/UAVObjects.inc
View File

@ -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

2
flight/targets/boards/revoproto/firmware/UAVObjects.inc
View File

@ -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

2
flight/targets/boards/simposix/firmware/UAVObjects.inc
View File

@ -24,7 +24,7 @@
# (all architectures)
UAVOBJSRCFILENAMES =
UAVOBJSRCFILENAMES += adjustments
UAVOBJSRCFILENAMES += vtolselftuningstats
UAVOBJSRCFILENAMES += accessorydesired
UAVOBJSRCFILENAMES += actuatorcommand
UAVOBJSRCFILENAMES += actuatordesired

4
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

@ -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 \

4
shared/uavobjectdefinition/adjustments.xml → shared/uavobjectdefinition/vtolselftuningstats.xml
View File

@ -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"/>