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

OP-1154 Initial version with UAVO's but no GCS GUI

Browse Source
This commit is contained in:
Cliff Geerdes 2014-01-11 16:22:20 -05:00
parent 6d3dca6ee3
commit ccc09721cd
3 changed files with 90 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
flight/libraries/math/sin_lookup.c
View File

@ -105,7 +105,14 @@ float sin_lookup_deg(float angle)
return 0;
}
int i_ang = ((int32_t)angle) % 360;
// <bug, was> int i_ang = ((int32_t)angle) % 360;
// 1073741760 is a multiple of 360 that is close to 0x3fffffff
// so angle can be a very large number of positive or negative rotations
// this is the fastest fix (no tests, one integer addition)
// and has the largest range since float mantissas are 23-4 bit
// we could halve the lookup table size
// we could interpolate for greater accuracy
int i_ang = ((int32_t)(angle + 0.5f) + (int32_t)1073741760) % 360;
if (i_ang >= 180) { // for 180 to 360 deg
return -sin_table[i_ang - 180];
} else { // for 0 to 179 deg

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

@ -92,6 +92,13 @@ bool lowThrottleZeroAxis[MAX_AXES];
float vbar_decay = 0.991f;
struct pid pids[PID_MAX];
static uint8_t rattitude_anti_windup;
static float cruise_control_min_throttle;
static float cruise_control_max_throttle;
static uint8_t cruise_control_max_angle;
static float cruise_control_max_power_factor;
static float cruise_control_power_trim;
static int8_t cruise_control_inverted_power_switch;
static float cruise_control_neutral_thrust;
// Private functions
static void stabilizationTask(void *parameters);
@ -556,6 +563,63 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
}
}
// modify throttle according to 1/cos(bank angle)
// to maintain same altitdue with changing bank angle
// but without manually adjusting throttle
// do it here and all the various flight modes (e.g. Altitude Hold) can use it
if (cruise_control_max_power_factor > 0.0001f) {
static uint8_t toggle;
static float factor;
float angle;
// get attitude state and calculate angle
// do it every 8th iteration to save CPU
if ((toggle++ & 7) == 0) {
// spherical right triangle
// 0 <= acosf() <= Pi
angle = RAD2DEG(acosf(cos_lookup_deg(attitudeState.Roll) * cos_lookup_deg(attitudeState.Pitch)));
// if past the cutoff angle (60 to 180 (180 means never))
if (angle > cruise_control_max_angle) {
// -1 reversed collective, 0 zero power, or 1 normal power
// these are all unboosted
factor = cruise_control_inverted_power_switch;
} else {
// avoid singularity
if (angle > 89.999f && angle < 90.001f) {
factor = cruise_control_max_power_factor;
} else {
factor = 1.0f / fabsf(cos_lookup_deg(angle));
if (factor > cruise_control_max_power_factor) {
factor = cruise_control_max_power_factor;
}
}
// factor in the power trim, no effect at 1.0, linear effect increases with factor
factor = (factor - 1.0f) * cruise_control_power_trim + 1.0f;
// if inverted and they want negative boost
if (angle > 90.0f && cruise_control_inverted_power_switch == (int8_t) -1) {
factor = -factor;
// as long as throttle is getting reversed
// we may as well do pitch and yaw for a complete "invert switch"
actuatorDesired.Pitch = -actuatorDesired.Pitch;
actuatorDesired.Yaw = -actuatorDesired.Yaw;
}
}
}
// also don't adjust throttle if <= 0, leaves neg alone and zero throttle stops motors
if (actuatorDesired.Throttle > cruise_control_min_throttle)
{
// quad example factor of 2 at hover power of 40%: (0.4 - 0.0) * 2.0 + 0.0 = 0.8
// CP heli example factor of 2 at hover stick of 60%: (0.6 - 0.5) * 2.0 + 0.5 = 0.7
actuatorDesired.Throttle = (actuatorDesired.Throttle - cruise_control_neutral_thrust) * factor + cruise_control_neutral_thrust;
if (actuatorDesired.Throttle > cruise_control_max_throttle) {
actuatorDesired.Throttle = cruise_control_max_throttle;
} else if (actuatorDesired.Throttle < cruise_control_min_throttle) {
actuatorDesired.Throttle = cruise_control_min_throttle;
}
}
}
if (PARSE_FLIGHT_MODE(flightStatus.FlightMode) != FLIGHTMODE_MANUAL) {
ActuatorDesiredSet(&actuatorDesired);
} else {
@ -606,9 +670,9 @@ static void ZeroPids(void)
static float bound(float val, float range)
{
if (val < -range) {
val = -range;
return -range;
} else if (val > range) {
val = range;
return range;
}
return val;
}
@ -755,6 +819,14 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
// Rattitude flight mode anti-windup factor
rattitude_anti_windup = settings.RattitudeAntiWindup;
cruise_control_min_throttle = (float) settings.CruiseControlMinThrottle / 100.0f;
cruise_control_max_throttle = (float) settings.CruiseControlMaxThrottle / 100.0f;
cruise_control_max_angle = settings.CruiseControlMaxAngle;
cruise_control_max_power_factor = settings.CruiseControlMaxPowerFactor;
cruise_control_power_trim = settings.CruiseControlPowerTrim / 100.0f;
cruise_control_inverted_power_switch = settings.CruiseControlInvertedPowerSwitch;
cruise_control_neutral_thrust = (float) settings.CruiseControlNeutralThrust / 100.0f;
}

8
shared/uavobjectdefinition/stabilizationsettings.xml
View File

@ -35,6 +35,14 @@
<field name="RattitudeAntiWindup" units="" type="uint8" elements="1" defaultvalue="10"/>
<field name="CruiseControlMinThrottle" units="%" type="uint8" elements="1" defaultvalue="5"/>
<field name="CruiseControlMaxThrottle" units="%" type="uint8" elements="1" defaultvalue="90"/>
<field name="CruiseControlMaxAngle" units="deg" type="uint8" elements="1" defaultvalue="105"/>
<field name="CruiseControlMaxPowerFactor" units="x" type="float" elements="1" defaultvalue="3.0"/>
<field name="CruiseControlPowerTrim" units="%" type="float" elements="1" defaultvalue="100.0"/>
<field name="CruiseControlInvertedPowerSwitch" units="" type="int8" elements="1" defaultvalue="0"/>
<field name="CruiseControlNeutralThrust" units="%" type="uint8" elements="1" defaultvalue="0"/>
<field name="LowThrottleZeroIntegral" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>
<field name="LowThrottleZeroAxis" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="FALSE,TRUE" defaultvalue="FALSE,FALSE,FALSE"/>