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

OP-1760 code review fixes

Browse Source
This commit is contained in:
abeck70 2015-04-15 08:39:13 +10:00
parent a52969a76b
commit a8c6a19784
3 changed files with 24 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
flight/modules/PathFollower/vtolautotakeofffsm.cpp
View File

@ -109,7 +109,7 @@ int32_t VtolAutoTakeoffFSM::Initialize(VtolPathFollowerSettingsData *ptr_vtolPat
mAutoTakeoffData = (VtolAutoTakeoffFSMData_T *)pios_malloc(sizeof(VtolAutoTakeoffFSMData_T)); mAutoTakeoffData = (VtolAutoTakeoffFSMData_T *)pios_malloc(sizeof(VtolAutoTakeoffFSMData_T));
PIOS_Assert(mAutoTakeoffData); PIOS_Assert(mAutoTakeoffData);
} }
memset(mAutoTakeoffData, sizeof(VtolAutoTakeoffFSMData_T), 0); memset(mAutoTakeoffData, 0, sizeof(VtolAutoTakeoffFSMData_T));
vtolPathFollowerSettings = ptr_vtolPathFollowerSettings; vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
pathDesired = ptr_pathDesired; pathDesired = ptr_pathDesired;
flightStatus = ptr_flightStatus; flightStatus = ptr_flightStatus;
@ -120,7 +120,7 @@ int32_t VtolAutoTakeoffFSM::Initialize(VtolPathFollowerSettingsData *ptr_vtolPat
void VtolAutoTakeoffFSM::Inactive(void) void VtolAutoTakeoffFSM::Inactive(void)
{ {
memset(mAutoTakeoffData, sizeof(VtolAutoTakeoffFSMData_T), 0); memset(mAutoTakeoffData, 0, sizeof(VtolAutoTakeoffFSMData_T));
initFSM(); initFSM();
} }
@ -136,7 +136,7 @@ void VtolAutoTakeoffFSM::initFSM(void)
void VtolAutoTakeoffFSM::Activate() void VtolAutoTakeoffFSM::Activate()
{ {
memset(mAutoTakeoffData, sizeof(VtolAutoTakeoffFSMData_T), 0); memset(mAutoTakeoffData, 0, sizeof(VtolAutoTakeoffFSMData_T));
mAutoTakeoffData->currentState = AUTOTAKEOFF_STATE_INACTIVE; mAutoTakeoffData->currentState = AUTOTAKEOFF_STATE_INACTIVE;
mAutoTakeoffData->flLowAltitude = true; mAutoTakeoffData->flLowAltitude = true;
mAutoTakeoffData->flAltitudeHold = false; mAutoTakeoffData->flAltitudeHold = false;
@ -368,15 +368,21 @@ void VtolAutoTakeoffFSM::setup_checkstate(void)
// STATE: SLOWSTART spools up motors to vtol min over 2 seconds for effect. // STATE: SLOWSTART spools up motors to vtol min over 2 seconds for effect.
// PID loops may be cumulating I terms but that problem needs to be solved // PID loops may be cumulating I terms but that problem needs to be solved
#define SLOWSTART_INITIAL_THRUST 0.05f // assumed to be less than vtol min
void VtolAutoTakeoffFSM::setup_slowstart(void) void VtolAutoTakeoffFSM::setup_slowstart(void)
{ {
setStateTimeout(TIMEOUT_SLOWSTART); setStateTimeout(TIMEOUT_SLOWSTART);
mAutoTakeoffData->flZeroStabiHorizontal = true; // turn off positional controllers mAutoTakeoffData->flZeroStabiHorizontal = true; // turn off positional controllers
StabilizationDesiredData stabDesired; StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired); StabilizationDesiredGet(&stabDesired);
mAutoTakeoffData->sum1 = (vtolPathFollowerSettings->ThrustLimits.Min - 0.05f) / (float)TIMEOUT_SLOWSTART; float vtol_thrust_min = vtolPathFollowerSettings->ThrustLimits.Min;
mAutoTakeoffData->boundThrustMin = 0.05f; if (vtol_thrust_min < SLOWSTART_INITIAL_THRUST) {
mAutoTakeoffData->boundThrustMax = 0.05f; vtol_thrust_min = SLOWSTART_INITIAL_THRUST;
}
mAutoTakeoffData->sum1 = (vtol_thrust_min - SLOWSTART_INITIAL_THRUST) / (float)TIMEOUT_SLOWSTART;
mAutoTakeoffData->sum2 = vtol_thrust_min;
mAutoTakeoffData->boundThrustMin = SLOWSTART_INITIAL_THRUST;
mAutoTakeoffData->boundThrustMax = SLOWSTART_INITIAL_THRUST;
PositionStateData positionState; PositionStateData positionState;
PositionStateGet(&positionState); PositionStateGet(&positionState);
mAutoTakeoffData->expectedAutoTakeoffPositionNorth = positionState.North; mAutoTakeoffData->expectedAutoTakeoffPositionNorth = positionState.North;
@ -386,12 +392,12 @@ void VtolAutoTakeoffFSM::setup_slowstart(void)
void VtolAutoTakeoffFSM::run_slowstart(__attribute__((unused)) uint8_t flTimeout) void VtolAutoTakeoffFSM::run_slowstart(__attribute__((unused)) uint8_t flTimeout)
{ {
// increase thrust setpoint step by step // increase thrust setpoint step by step
if (mAutoTakeoffData->boundThrustMin < vtolPathFollowerSettings->ThrustLimits.Min) { if (mAutoTakeoffData->boundThrustMin < mAutoTakeoffData->sum2) {
mAutoTakeoffData->boundThrustMin += mAutoTakeoffData->sum1; mAutoTakeoffData->boundThrustMin += mAutoTakeoffData->sum1;
} }
mAutoTakeoffData->boundThrustMax += mAutoTakeoffData->sum1; mAutoTakeoffData->boundThrustMax += mAutoTakeoffData->sum1;
if (mAutoTakeoffData->boundThrustMax > vtolPathFollowerSettings->ThrustLimits.Min) { if (mAutoTakeoffData->boundThrustMax > mAutoTakeoffData->sum2) {
mAutoTakeoffData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Min; mAutoTakeoffData->boundThrustMax = mAutoTakeoffData->sum2;
} }
if (flTimeout) { if (flTimeout) {
@ -401,14 +407,15 @@ void VtolAutoTakeoffFSM::run_slowstart(__attribute__((unused)) uint8_t flTimeout
// STATE: THRUSTUP spools up motors to vtol min over 5 seconds for effect. // STATE: THRUSTUP spools up motors to vtol min over 5 seconds for effect.
// PID loops may be cumulating I terms but that problem needs to be solved // PID loops may be cumulating I terms but that problem needs to be solved
#define THRUSTUP_FINAL_THRUST_AS_RATIO_OF_VTOLMAX 0.8f
void VtolAutoTakeoffFSM::setup_thrustup(void) void VtolAutoTakeoffFSM::setup_thrustup(void)
{ {
setStateTimeout(TIMEOUT_THRUSTUP); setStateTimeout(TIMEOUT_THRUSTUP);
mAutoTakeoffData->flZeroStabiHorizontal = false; mAutoTakeoffData->flZeroStabiHorizontal = false;
StabilizationDesiredData stabDesired; StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired); StabilizationDesiredGet(&stabDesired);
mAutoTakeoffData->sum1 = (0.8f * vtolPathFollowerSettings->ThrustLimits.Max - mAutoTakeoffData->boundThrustMax) / (float)TIMEOUT_THRUSTUP; mAutoTakeoffData->sum2 = THRUSTUP_FINAL_THRUST_AS_RATIO_OF_VTOLMAX * vtolPathFollowerSettings->ThrustLimits.Max;
mAutoTakeoffData->sum2 = 0.8f * vtolPathFollowerSettings->ThrustLimits.Max; mAutoTakeoffData->sum1 = (mAutoTakeoffData->sum2 - mAutoTakeoffData->boundThrustMax) / (float)TIMEOUT_THRUSTUP;
mAutoTakeoffData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min; mAutoTakeoffData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
} }

6
flight/modules/PathFollower/vtolbrakefsm.cpp
View File

@ -101,7 +101,7 @@ int32_t VtolBrakeFSM::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollo
mBrakeData = (VtolBrakeFSMData_T *)pios_malloc(sizeof(VtolBrakeFSMData_T)); mBrakeData = (VtolBrakeFSMData_T *)pios_malloc(sizeof(VtolBrakeFSMData_T));
PIOS_Assert(mBrakeData); PIOS_Assert(mBrakeData);
} }
memset(mBrakeData, sizeof(VtolBrakeFSMData_T), 0); memset(mBrakeData, 0, sizeof(VtolBrakeFSMData_T));
vtolPathFollowerSettings = ptr_vtolPathFollowerSettings; vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
pathDesired = ptr_pathDesired; pathDesired = ptr_pathDesired;
flightStatus = ptr_flightStatus; flightStatus = ptr_flightStatus;
@ -113,7 +113,7 @@ int32_t VtolBrakeFSM::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollo
void VtolBrakeFSM::Inactive(void) void VtolBrakeFSM::Inactive(void)
{ {
memset(mBrakeData, sizeof(VtolBrakeFSMData_T), 0); memset(mBrakeData, 0, sizeof(VtolBrakeFSMData_T));
initFSM(); initFSM();
} }
@ -125,7 +125,7 @@ void VtolBrakeFSM::initFSM(void)
void VtolBrakeFSM::Activate() void VtolBrakeFSM::Activate()
{ {
memset(mBrakeData, sizeof(VtolBrakeFSMData_T), 0); memset(mBrakeData, 0, sizeof(VtolBrakeFSMData_T));
mBrakeData->currentState = BRAKE_STATE_INACTIVE; mBrakeData->currentState = BRAKE_STATE_INACTIVE;
setState(BRAKE_STATE_BRAKE, FSMBRAKESTATUS_STATEEXITREASON_NONE); setState(BRAKE_STATE_BRAKE, FSMBRAKESTATUS_STATEEXITREASON_NONE);
} }

6
flight/modules/PathFollower/vtollandfsm.cpp
View File

@ -128,7 +128,7 @@ int32_t VtolLandFSM::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollow
mLandData = (VtolLandFSMData_T *)pios_malloc(sizeof(VtolLandFSMData_T)); mLandData = (VtolLandFSMData_T *)pios_malloc(sizeof(VtolLandFSMData_T));
PIOS_Assert(mLandData); PIOS_Assert(mLandData);
} }
memset(mLandData, sizeof(VtolLandFSMData_T), 0); memset(mLandData, 0, sizeof(VtolLandFSMData_T));
vtolPathFollowerSettings = ptr_vtolPathFollowerSettings; vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
pathDesired = ptr_pathDesired; pathDesired = ptr_pathDesired;
flightStatus = ptr_flightStatus; flightStatus = ptr_flightStatus;
@ -139,7 +139,7 @@ int32_t VtolLandFSM::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollow
void VtolLandFSM::Inactive(void) void VtolLandFSM::Inactive(void)
{ {
memset(mLandData, sizeof(VtolLandFSMData_T), 0); memset(mLandData, 0, sizeof(VtolLandFSMData_T));
initFSM(); initFSM();
} }
@ -155,7 +155,7 @@ void VtolLandFSM::initFSM(void)
void VtolLandFSM::Activate() void VtolLandFSM::Activate()
{ {
memset(mLandData, sizeof(VtolLandFSMData_T), 0); memset(mLandData, 0, sizeof(VtolLandFSMData_T));
mLandData->currentState = LAND_STATE_INACTIVE; mLandData->currentState = LAND_STATE_INACTIVE;
mLandData->flLowAltitude = false; mLandData->flLowAltitude = false;
mLandData->flAltitudeHold = false; mLandData->flAltitudeHold = false;