rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

OP-1309 get rid of this horrible bound() function code duplication throughout entire flight code and put it into libraries/math

Browse Source
This commit is contained in:
Corvus Corax 2014-04-26 17:31:20 +02:00
parent 599661ba18
commit e9d1a2af4b
16 changed files with 197 additions and 177 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
flight/libraries/math/mathmisc.c Normal file
View File

@ -0,0 +1,49 @@
/**
******************************************************************************
* @addtogroup OpenPilot Math Utilities
* @{
* @addtogroup Reuseable math functions
* @{
*
* @file mathmisc.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Reuseable math functions
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
// returns min(boundary1,boundary2) if val<min(boundary1,boundary2)
// returns max(boundary1,boundary2) if val>max(boundary1,boundary2)
// returns val if min(boundary1,boundary2)<=val<=max(boundary1,boundary2)
float boundf(float val, float boundary1, float boundary2)
{
if (boundary1 > boundary2) {
float tmp = boundary2;
boundary2 = boundary1;
boundary1 = tmp;
}
if (!(val >= boundary1)) {
val = boundary1;
}
if (!(val <= boundary2)) {
val = boundary2;
}
return val;
}

39
flight/libraries/math/mathmisc.h Normal file
View File

@ -0,0 +1,39 @@
/**
******************************************************************************
* @addtogroup OpenPilot Math Utilities
* @{
* @addtogroup Reuseable math functions
* @{
*
* @file mathmisc.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Reuseable math functions
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef MATHMISC_H
#define MATHMISC_H
// returns min(boundary1,boundary2) if val<min(boundary1,boundary2)
// returns max(boundary1,boundary2) if val>max(boundary1,boundary2)
// returns val if min(boundary1,boundary2)<=val<=max(boundary1,boundary2)
float boundf(float val, float boundary1, float boundary2);
#endif /* MATHMISC_H */

21
flight/libraries/math/pid.c
View File

@ -30,11 +30,9 @@
#include "openpilot.h"
#include "pid.h"
#include <mathmisc.h>
#include <pios_math.h>
// ! Private method
static float bound(float val, float range);
// ! Store the shared time constant for the derivative cutoff.
static float deriv_tau = 7.9577e-3f;
@ -52,7 +50,7 @@ float pid_apply(struct pid *pid, const float err, float dT)
{
// Scale up accumulator by 1000 while computing to avoid losing precision
pid->iAccumulator += err * (pid->i * dT * 1000.0f);
pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f);
pid->iAccumulator = boundf(pid->iAccumulator, pid->iLim * -1000.0f, pid->iLim * 1000.0f);
// Calculate DT1 term
float diff = (err - pid->lastErr);
@ -84,7 +82,7 @@ float pid_apply_setpoint(struct pid *pid, const float factor, const float setpoi
// Scale up accumulator by 1000 while computing to avoid losing precision
pid->iAccumulator += err * (factor * pid->i * dT * 1000.0f);
pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f);
pid->iAccumulator = boundf(pid->iAccumulator, pid->iLim * -1000.0f, pid->iLim * 1000.0f);
// Calculate DT1 term,
float dterm = 0;
@ -142,16 +140,3 @@ void pid_configure(struct pid *pid, float p, float i, float d, float iLim)
pid->d = d;
pid->iLim = iLim;
}
/**
* Bound input value between limits
*/
static float bound(float val, float range)
{
if (val < -range) {
val = -range;
} else if (val > range) {
val = range;
}
return val;
}

15
flight/modules/CameraStab/camerastab.c
View File

@ -79,7 +79,6 @@ static struct CameraStab_data {
// Private functions
static void attitudeUpdated(UAVObjEvent *ev);
static float bound(float val, float limit);
#ifdef USE_GIMBAL_FF
static void applyFeedForward(uint8_t index, float dT, float *attitude, CameraStabSettingsData *cameraStab);
@ -186,8 +185,9 @@ static void attitudeUpdated(UAVObjEvent *ev)
input_rate = accessory.AccessoryVal *
cast_struct_to_array(cameraStab.InputRate, cameraStab.InputRate.Roll)[i];
if (fabsf(input_rate) > cameraStab.MaxAxisLockRate) {
csd->inputs[i] = bound(csd->inputs[i] + input_rate * 0.001f * dT_millis,
cast_struct_to_array(cameraStab.InputRange, cameraStab.InputRange.Roll)[i]);
csd->inputs[i] = boundf(csd->inputs[i] + input_rate * 0.001f * dT_millis,
-cast_struct_to_array(cameraStab.InputRange, cameraStab.InputRange.Roll)[i],
cast_struct_to_array(cameraStab.InputRange, cameraStab.InputRange.Roll)[i]);
}
break;
default:
@ -229,7 +229,7 @@ static void attitudeUpdated(UAVObjEvent *ev)
// bounding for elevon mixing occurs on the unmixed output
// to limit the range of the mixed output you must limit the range
// of both the unmixed pitch and unmixed roll
float output = bound((attitude + csd->inputs[i]) / cast_struct_to_array(cameraStab.OutputRange, cameraStab.OutputRange.Roll)[i], 1.0f);
float output = boundf((attitude + csd->inputs[i]) / cast_struct_to_array(cameraStab.OutputRange, cameraStab.OutputRange.Roll)[i], -1.0f, 1.0f);
// set output channels
switch (i) {
@ -282,13 +282,6 @@ static void attitudeUpdated(UAVObjEvent *ev)
}
}
float bound(float val, float limit)
{
return (val > limit) ? limit :
(val < -limit) ? -limit :
val;
}
#ifdef USE_GIMBAL_FF
void applyFeedForward(uint8_t index, float dT_millis, float *attitude, CameraStabSettingsData *cameraStab)
{

77
flight/modules/FixedWingPathFollower/fixedwingpathfollower.c
View File

@ -85,7 +85,6 @@ static void updatePathVelocity();
static uint8_t updateFixedDesiredAttitude();
static void updateFixedAttitude();
static void airspeedStateUpdatedCb(UAVObjEvent *ev);
static float bound(float val, float min, float max);
/**
* Initialise the module, called on startup
@ -277,9 +276,9 @@ static void updatePathVelocity()
case PATHDESIRED_MODE_DRIVEVECTOR:
default:
groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) *
bound(progress.fractional_progress, 0, 1);
boundf(progress.fractional_progress, 0, 1);
altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) *
bound(progress.fractional_progress, 0, 1);
boundf(progress.fractional_progress, 0, 1);
break;
}
// make sure groundspeed is not zero
@ -427,15 +426,15 @@ static uint8_t updateFixedDesiredAttitude()
// Desired ground speed
groundspeedDesired = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East);
indicatedAirspeedDesired = bound(groundspeedDesired + indicatedAirspeedStateBias,
fixedwingpathfollowerSettings.HorizontalVelMin,
fixedwingpathfollowerSettings.HorizontalVelMax);
indicatedAirspeedDesired = boundf(groundspeedDesired + indicatedAirspeedStateBias,
fixedwingpathfollowerSettings.HorizontalVelMin,
fixedwingpathfollowerSettings.HorizontalVelMax);
// Airspeed error
airspeedError = indicatedAirspeedDesired - indicatedAirspeedState;
// Vertical speed error
descentspeedDesired = bound(
descentspeedDesired = boundf(
velocityDesired.Down,
-fixedwingpathfollowerSettings.VerticalVelMax,
fixedwingpathfollowerSettings.VerticalVelMax);
@ -473,14 +472,14 @@ static uint8_t updateFixedDesiredAttitude()
*/
// compute saturated integral error thrust response. Make integral leaky for better performance. Approximately 30s time constant.
if (fixedwingpathfollowerSettings.PowerPI.Ki > 0) {
powerIntegral = bound(powerIntegral + -descentspeedError * dT,
-fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki,
fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki
) * (1.0f - 1.0f / (1.0f + 30.0f / dT));
powerIntegral = boundf(powerIntegral + -descentspeedError * dT,
-fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki,
fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki
) * (1.0f - 1.0f / (1.0f + 30.0f / dT));
} else { powerIntegral = 0; }
// Compute the cross feed from vertical speed to pitch, with saturation
float speedErrorToPowerCommandComponent = bound(
float speedErrorToPowerCommandComponent = boundf(
(airspeedError / fixedwingpathfollowerSettings.HorizontalVelMin) * fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Kp,
-fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Max,
fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Max
@ -497,9 +496,9 @@ static uint8_t updateFixedDesiredAttitude()
fixedwingpathfollowerStatus.Command.Power = powerCommand;
// set thrust
stabDesired.Thrust = bound(fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand,
fixedwingpathfollowerSettings.ThrustLimit.Min,
fixedwingpathfollowerSettings.ThrustLimit.Max);
stabDesired.Thrust = boundf(fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand,
fixedwingpathfollowerSettings.ThrustLimit.Min,
fixedwingpathfollowerSettings.ThrustLimit.Max);
// Error condition: plane cannot hold altitude at current speed.
fixedwingpathfollowerStatus.Errors.Lowpower = 0;
@ -529,16 +528,16 @@ static uint8_t updateFixedDesiredAttitude()
if (fixedwingpathfollowerSettings.SpeedPI.Ki > 0) {
// Integrate with saturation
airspeedErrorInt = bound(airspeedErrorInt + airspeedError * dT,
-fixedwingpathfollowerSettings.SpeedPI.ILimit / fixedwingpathfollowerSettings.SpeedPI.Ki,
fixedwingpathfollowerSettings.SpeedPI.ILimit / fixedwingpathfollowerSettings.SpeedPI.Ki);
airspeedErrorInt = boundf(airspeedErrorInt + airspeedError * dT,
-fixedwingpathfollowerSettings.SpeedPI.ILimit / fixedwingpathfollowerSettings.SpeedPI.Ki,
fixedwingpathfollowerSettings.SpeedPI.ILimit / fixedwingpathfollowerSettings.SpeedPI.Ki);
}
// Compute the cross feed from vertical speed to pitch, with saturation
float verticalSpeedToPitchCommandComponent = bound(-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Kp,
-fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Max,
fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Max
);
float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Kp,
-fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Max,
fixedwingpathfollowerSettings.VerticalToPitchCrossFeed.Max
);
// Compute the pitch command as err*Kp + errInt*Ki + X_feed.
pitchCommand = -(airspeedError * fixedwingpathfollowerSettings.SpeedPI.Kp
@ -549,9 +548,9 @@ static uint8_t updateFixedDesiredAttitude()
fixedwingpathfollowerStatus.ErrorInt.Speed = airspeedErrorInt;
fixedwingpathfollowerStatus.Command.Speed = pitchCommand;
stabDesired.Pitch = bound(fixedwingpathfollowerSettings.PitchLimit.Neutral + pitchCommand,
fixedwingpathfollowerSettings.PitchLimit.Min,
fixedwingpathfollowerSettings.PitchLimit.Max);
stabDesired.Pitch = boundf(fixedwingpathfollowerSettings.PitchLimit.Neutral + pitchCommand,
fixedwingpathfollowerSettings.PitchLimit.Min,
fixedwingpathfollowerSettings.PitchLimit.Max);
// Error condition: high speed dive
fixedwingpathfollowerStatus.Errors.Pitchcontrol = 0;
@ -606,9 +605,9 @@ static uint8_t updateFixedDesiredAttitude()
courseError -= 360.0f;
}
courseIntegral = bound(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI.Ki,
-fixedwingpathfollowerSettings.CoursePI.ILimit,
fixedwingpathfollowerSettings.CoursePI.ILimit);
courseIntegral = boundf(courseIntegral + courseError * dT * fixedwingpathfollowerSettings.CoursePI.Ki,
-fixedwingpathfollowerSettings.CoursePI.ILimit,
fixedwingpathfollowerSettings.CoursePI.ILimit);
courseCommand = (courseError * fixedwingpathfollowerSettings.CoursePI.Kp +
courseIntegral);
@ -616,10 +615,10 @@ static uint8_t updateFixedDesiredAttitude()
fixedwingpathfollowerStatus.ErrorInt.Course = courseIntegral;
fixedwingpathfollowerStatus.Command.Course = courseCommand;
stabDesired.Roll = bound(fixedwingpathfollowerSettings.RollLimit.Neutral +
courseCommand,
fixedwingpathfollowerSettings.RollLimit.Min,
fixedwingpathfollowerSettings.RollLimit.Max);
stabDesired.Roll = boundf(fixedwingpathfollowerSettings.RollLimit.Neutral +
courseCommand,
fixedwingpathfollowerSettings.RollLimit.Min,
fixedwingpathfollowerSettings.RollLimit.Max);
// TODO: find a check to determine loss of directional control. Likely needs some check of derivative
@ -642,20 +641,6 @@ static uint8_t updateFixedDesiredAttitude()
return result;
}
/**
* Bound input value between limits
*/
static float bound(float val, float min, float max)
{
if (val < min) {
val = min;
} else if (val > max) {
val = max;
}
return val;
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
FixedWingPathFollowerSettingsGet(&fixedwingpathfollowerSettings);

67
flight/modules/Stabilization/stabilization.c
View File

@ -116,7 +116,6 @@ static uint8_t cruise_control_flight_mode_switch_pos_enable[FLIGHTMODESETTINGS_F
// Private functions
static void stabilizationTask(void *parameters);
static float bound(float val, float range);
static void ZeroPids(void);
static void SettingsUpdatedCb(UAVObjEvent *ev);
static void BankUpdatedCb(UAVObjEvent *ev);
@ -342,11 +341,11 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// Store to rate desired variable for storing to UAVO
rateDesiredAxis[i] =
bound(stabDesiredAxis[i], cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]);
boundf(stabDesiredAxis[i], -cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i], cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]);
// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
actuatorDesiredAxis[i] = boundf(actuatorDesiredAxis[i], -1.0f, 1.0f);
break;
@ -358,12 +357,13 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// Compute the outer loop
rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT);
rateDesiredAxis[i] = bound(rateDesiredAxis[i],
cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i]);
rateDesiredAxis[i] = boundf(rateDesiredAxis[i],
-cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i],
cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i]);
// Compute the inner loop
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
actuatorDesiredAxis[i] = boundf(actuatorDesiredAxis[i], -1.0f, 1.0f);
break;
@ -382,7 +382,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// Compute what Rate mode would give for this stick angle's rate
// Save Rate's rate in a temp for later merging with Attitude's rate
float rateDesiredAxisRate;
rateDesiredAxisRate = bound(stabDesiredAxis[i], 1.0f)
rateDesiredAxisRate = boundf(stabDesiredAxis[i], -1.0f, 1.0f)
* cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i];
// Compute what Attitude mode would give for this stick angle's rate
@ -398,9 +398,11 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// Compute the outer loop just like Attitude mode does
float rateDesiredAxisAttitude;
rateDesiredAxisAttitude = pid_apply(&pids[PID_ROLL + i], attitude_error, dT);
rateDesiredAxisAttitude = bound(rateDesiredAxisAttitude,
cast_struct_to_array(stabBank.ManualRate,
stabBank.ManualRate.Roll)[i]);
rateDesiredAxisAttitude = boundf(rateDesiredAxisAttitude,
-cast_struct_to_array(stabBank.ManualRate,
stabBank.ManualRate.Roll)[i],
cast_struct_to_array(stabBank.ManualRate,
stabBank.ManualRate.Roll)[i]);
// Compute the weighted average rate desired
// Using max() rather than sqrt() for cpu speed;
@ -452,7 +454,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// actuatorDesiredAxis[i] is the weighted average
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor,
rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
actuatorDesiredAxis[i] = boundf(actuatorDesiredAxis[i], -1.0f, 1.0f);
break;
}
@ -479,12 +481,12 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
}
float weak_leveling = local_error[i] * weak_leveling_kp;
weak_leveling = bound(weak_leveling, weak_leveling_max);
weak_leveling = boundf(weak_leveling, -weak_leveling_max, weak_leveling_max);
// Compute desired rate as input biased towards leveling
rateDesiredAxis[i] = stabDesiredAxis[i] + weak_leveling;
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
actuatorDesiredAxis[i] = boundf(actuatorDesiredAxis[i], -1.0f, 1.0f);
break;
}
@ -501,26 +503,28 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
} else {
// For weaker commands or no command simply attitude lock (almost) on no gyro change
axis_lock_accum[i] += (stabDesiredAxis[i] - gyro_filtered[i]) * dT;
axis_lock_accum[i] = bound(axis_lock_accum[i], max_axis_lock);
axis_lock_accum[i] = boundf(axis_lock_accum[i], -max_axis_lock, max_axis_lock);
rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], axis_lock_accum[i], dT);
}
rateDesiredAxis[i] = bound(rateDesiredAxis[i],
cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]);
rateDesiredAxis[i] = boundf(rateDesiredAxis[i],
-cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i],
cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]);
actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor, rateDesiredAxis[i], gyro_filtered[i], dT);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
actuatorDesiredAxis[i] = boundf(actuatorDesiredAxis[i], -1.0f, 1.0f);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE:
// Store to rate desired variable for storing to UAVO
rateDesiredAxis[i] = bound(stabDesiredAxis[i],
cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]);
rateDesiredAxis[i] = boundf(stabDesiredAxis[i],
-cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i],
cast_struct_to_array(stabBank.ManualRate, stabBank.ManualRate.Roll)[i]);
// Run the relay controller which also estimates the oscillation parameters
stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
actuatorDesiredAxis[i] = boundf(actuatorDesiredAxis[i], -1.0f, 1.0f);
break;
@ -531,17 +535,18 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
// Compute the outer loop like attitude mode
rateDesiredAxis[i] = pid_apply(&pids[PID_ROLL + i], local_error[i], dT);
rateDesiredAxis[i] = bound(rateDesiredAxis[i],
cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i]);
rateDesiredAxis[i] = boundf(rateDesiredAxis[i],
-cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i],
cast_struct_to_array(stabBank.MaximumRate, stabBank.MaximumRate.Roll)[i]);
// Run the relay controller which also estimates the oscillation parameters
stabilization_relay_rate(rateDesiredAxis[i] - gyro_filtered[i], &actuatorDesiredAxis[i], i, reinit);
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
actuatorDesiredAxis[i] = boundf(actuatorDesiredAxis[i], -1.0f, 1.0f);
break;
case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE:
actuatorDesiredAxis[i] = bound(stabDesiredAxis[i], 1.0f);
actuatorDesiredAxis[i] = boundf(stabDesiredAxis[i], -1.0f, 1.0f);
break;
default:
error = true;
@ -662,20 +667,6 @@ static void ZeroPids(void)
}
/**
* Bound input value between limits
*/
static float bound(float val, float range)
{
if (val < -range) {
return -range;
} else if (val > range) {
return range;
}
return val;
}
static void SettingsBankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
if (cur_flight_mode < 0 || cur_flight_mode >= FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM) {

18
flight/modules/Stabilization/virtualflybar.c
View File

@ -40,9 +40,6 @@
static float vbar_integral[MAX_AXES];
static float vbar_decay = 0.991f;
// ! Private methods
static float bound(float val, float range);
int stabilization_virtual_flybar(float gyro, float command, float *output, float dT, bool reinit, uint32_t axis, StabilizationSettingsData *settings)
{
float gyro_gain = 1.0f;
@ -54,7 +51,7 @@ int stabilization_virtual_flybar(float gyro, float command, float *output, float
// Track the angle of the virtual flybar which includes a slow decay
vbar_integral[axis] = vbar_integral[axis] * vbar_decay + gyro * dT;
vbar_integral[axis] = bound(vbar_integral[axis], settings->VbarMaxAngle);
vbar_integral[axis] = boundf(vbar_integral[axis], -settings->VbarMaxAngle, settings->VbarMaxAngle);
// Command signal can indicate how much to disregard the gyro feedback (fast flips)
if (settings->VbarGyroSuppress > 0) {
@ -105,16 +102,3 @@ int stabilization_virtual_flybar_pirocomp(float z_gyro, float dT)
return 0;
}
/**
* Bound input value between limits
*/
static float bound(float val, float range)
{
if (val < -range) {
val = -range;
} else if (val > range) {
val = range;
}
return val;
}

80
flight/modules/VtolPathFollower/vtolpathfollower.c
View File

@ -101,7 +101,6 @@ static void updatePathVelocity();
static void updateEndpointVelocity();
static void updateFixedAttitude(float *attitude);
static void updateVtolDesiredAttitude(bool yaw_attitude);
static float bound(float val, float min, float max);
static bool vtolpathfollower_enabled;
static void accessoryUpdated(UAVObjEvent *ev);
@ -394,7 +393,7 @@ static void updatePathVelocity()
break;
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_DRIVEENDPOINT:
groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * bound(progress.fractional_progress, 0, 1);
groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * boundf(progress.fractional_progress, 0, 1);
if (progress.fractional_progress > 1) {
groundspeed = 0;
}
@ -403,7 +402,7 @@ static void updatePathVelocity()
case PATHDESIRED_MODE_DRIVEVECTOR:
default:
groundspeed = pathDesired.StartingVelocity
+ (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * bound(progress.fractional_progress, 0, 1);
+ (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * boundf(progress.fractional_progress, 0, 1);
if (progress.fractional_progress > 1) {
groundspeed = 0;
}
@ -427,14 +426,14 @@ static void updatePathVelocity()
velocityDesired.North += progress.correction_direction[0] * error_speed * scale;
velocityDesired.East += progress.correction_direction[1] * error_speed * scale;
float altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) * bound(progress.fractional_progress, 0, 1);
float altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) * boundf(progress.fractional_progress, 0, 1);
float downError = altitudeSetpoint - positionState.Down;
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki,
-vtolpathfollowerSettings.VerticalPosPI.ILimit,
vtolpathfollowerSettings.VerticalPosPI.ILimit);
downPosIntegral = boundf(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki,
-vtolpathfollowerSettings.VerticalPosPI.ILimit,
vtolpathfollowerSettings.VerticalPosPI.ILimit);
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral);
velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
velocityDesired.Down = boundf(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
// update pathstatus
pathStatus.error = progress.error;
@ -472,15 +471,15 @@ void updateEndpointVelocity()
// Compute desired north command
northError = pathDesired.End.North - positionState.North;
northPosIntegral = bound(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki,
-vtolpathfollowerSettings.HorizontalPosPI.ILimit,
vtolpathfollowerSettings.HorizontalPosPI.ILimit);
northPosIntegral = boundf(northPosIntegral + northError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki,
-vtolpathfollowerSettings.HorizontalPosPI.ILimit,
vtolpathfollowerSettings.HorizontalPosPI.ILimit);
northCommand = (northError * vtolpathfollowerSettings.HorizontalPosPI.Kp + northPosIntegral);
eastError = pathDesired.End.East - positionState.East;
eastPosIntegral = bound(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki,
-vtolpathfollowerSettings.HorizontalPosPI.ILimit,
vtolpathfollowerSettings.HorizontalPosPI.ILimit);
eastPosIntegral = boundf(eastPosIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalPosPI.Ki,
-vtolpathfollowerSettings.HorizontalPosPI.ILimit,
vtolpathfollowerSettings.HorizontalPosPI.ILimit);
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalPosPI.Kp + eastPosIntegral);
// Limit the maximum velocity
@ -494,11 +493,11 @@ void updateEndpointVelocity()
velocityDesired.East = eastCommand * scale;
downError = pathDesired.End.Down - positionState.Down;
downPosIntegral = bound(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki,
-vtolpathfollowerSettings.VerticalPosPI.ILimit,
vtolpathfollowerSettings.VerticalPosPI.ILimit);
downPosIntegral = boundf(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki,
-vtolpathfollowerSettings.VerticalPosPI.ILimit,
vtolpathfollowerSettings.VerticalPosPI.ILimit);
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral);
velocityDesired.Down = bound(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
velocityDesired.Down = boundf(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
VelocityDesiredSet(&velocityDesired);
}
@ -595,18 +594,18 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
// Compute desired north command
northError = velocityDesired.North - northVel;
northVelIntegral = bound(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki,
-vtolpathfollowerSettings.HorizontalVelPID.ILimit,
vtolpathfollowerSettings.HorizontalVelPID.ILimit);
northVelIntegral = boundf(northVelIntegral + northError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki,
-vtolpathfollowerSettings.HorizontalVelPID.ILimit,
vtolpathfollowerSettings.HorizontalVelPID.ILimit);
northCommand = (northError * vtolpathfollowerSettings.HorizontalVelPID.Kp + northVelIntegral
- nedAccel.North * vtolpathfollowerSettings.HorizontalVelPID.Kd
+ velocityDesired.North * vtolpathfollowerSettings.VelocityFeedforward);
// Compute desired east command
eastError = velocityDesired.East - eastVel;
eastVelIntegral = bound(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki,
-vtolpathfollowerSettings.HorizontalVelPID.ILimit,
vtolpathfollowerSettings.HorizontalVelPID.ILimit);
eastVelIntegral = boundf(eastVelIntegral + eastError * dT * vtolpathfollowerSettings.HorizontalVelPID.Ki,
-vtolpathfollowerSettings.HorizontalVelPID.ILimit,
vtolpathfollowerSettings.HorizontalVelPID.ILimit);
eastCommand = (eastError * vtolpathfollowerSettings.HorizontalVelPID.Kp + eastVelIntegral
- nedAccel.East * vtolpathfollowerSettings.HorizontalVelPID.Kd
+ velocityDesired.East * vtolpathfollowerSettings.VelocityFeedforward);
@ -615,22 +614,22 @@ static void updateVtolDesiredAttitude(bool yaw_attitude)
downError = velocityDesired.Down - downVel;
// Must flip this sign
downError = -downError;
downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.Ki,
-vtolpathfollowerSettings.VerticalVelPID.ILimit,
vtolpathfollowerSettings.VerticalVelPID.ILimit);
downVelIntegral = boundf(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.Ki,
-vtolpathfollowerSettings.VerticalVelPID.ILimit,
vtolpathfollowerSettings.VerticalVelPID.ILimit);
downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID.Kp + downVelIntegral
- nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID.Kd);
stabDesired.Thrust = bound(downCommand + thrustOffset, 0, 1);
stabDesired.Thrust = boundf(downCommand + thrustOffset, 0, 1);
// Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the
// craft should move similarly for 5 deg roll versus 5 deg pitch
stabDesired.Pitch = bound(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) +
-eastCommand * sinf(DEG2RAD(attitudeState.Yaw)),
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
stabDesired.Roll = bound(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) +
eastCommand * cosf(DEG2RAD(attitudeState.Yaw)),
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
stabDesired.Pitch = boundf(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) +
-eastCommand * sinf(DEG2RAD(attitudeState.Yaw)),
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
stabDesired.Roll = boundf(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) +
eastCommand * cosf(DEG2RAD(attitudeState.Yaw)),
-vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch);
if (vtolpathfollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_FALSE) {
// For now override thrust with manual control. Disable at your risk, quad goes to China.
@ -692,19 +691,6 @@ static void updateNedAccel()
NedAccelSet(&accelData);
}
/**
* Bound input value between limits
*/
static float bound(float val, float min, float max)
{
if (val < min) {
val = min;
} else if (val > max) {
val = max;
}
return val;
}
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
VtolPathFollowerSettingsGet(&vtolpathfollowerSettings);

1
flight/targets/boards/coptercontrol/firmware/inc/openpilot.h
View File

@ -39,6 +39,7 @@
#include <uavtalk.h>
#include "alarms.h"
#include <mathmisc.h>
/* Global Functions */
void OpenPilotInit(void);

1
flight/targets/boards/oplinkmini/firmware/inc/openpilot.h
View File

@ -40,6 +40,7 @@
#include <uavtalk.h>
#include "alarms.h"
#include <mathmisc.h>
/* Global Functions */
void OpenPilotInit(void);

1
flight/targets/boards/osd/firmware/inc/openpilot.h
View File

@ -39,6 +39,7 @@
#include <uavtalk.h>
#include "alarms.h"
#include <mathmisc.h>
/* Global Functions */
void OpenPilotInit(void);

1
flight/targets/boards/revolution/firmware/inc/openpilot.h
View File

@ -40,6 +40,7 @@
#include <uavtalk.h>
#include "alarms.h"
#include <mathmisc.h>
/* Global Functions */
void OpenPilotInit(void);

1
flight/targets/boards/revoproto/firmware/inc/openpilot.h
View File

@ -40,6 +40,7 @@
#include <uavtalk.h>
#include "alarms.h"
#include <mathmisc.h>
/* Global Functions */
void OpenPilotInit(void);

1
flight/targets/boards/simposix/firmware/Makefile
View File

@ -96,6 +96,7 @@ SRC += $(FLIGHTLIB)/sanitycheck.c
SRC += $(MATHLIB)/sin_lookup.c
SRC += $(MATHLIB)/pid.c
SRC += $(MATHLIB)/mathmisc.c
SRC += $(PIOSCORECOMMON)/pios_task_monitor.c
SRC += $(PIOSCORECOMMON)/pios_dosfs_logfs.c

1
flight/targets/boards/simposix/firmware/inc/openpilot.h
View File

@ -40,6 +40,7 @@
#include <uavtalk.h>
#include "alarms.h"
#include <mathmisc.h>
/* Global Functions */
void OpenPilotInit(void);

1
make/apps-defs.mk
View File

@ -106,6 +106,7 @@ SRC += $(FLIGHTLIB)/sanitycheck.c
SRC += $(FLIGHTLIB)/CoordinateConversions.c
SRC += $(MATHLIB)/sin_lookup.c
SRC += $(MATHLIB)/pid.c
SRC += $(MATHLIB)/mathmisc.c
SRC += $(FLIGHTLIB)/printf-stdarg.c
## Modules