mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-18 03:52:11 +01:00
OP-1259 WIP to merge next in locally
This commit is contained in:
parent
6d7d10caba
commit
81f31e64a9
@ -113,6 +113,7 @@ static float cruise_control_power_trim;
|
|||||||
static int8_t cruise_control_inverted_power_switch;
|
static int8_t cruise_control_inverted_power_switch;
|
||||||
static float cruise_control_max_power_factor_angle;
|
static float cruise_control_max_power_factor_angle;
|
||||||
static float cruise_control_half_power_delay;
|
static float cruise_control_half_power_delay;
|
||||||
|
static float cruise_control_thrust_difference;
|
||||||
static uint8_t cruise_control_flight_mode_switch_pos_enable[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
|
static uint8_t cruise_control_flight_mode_switch_pos_enable[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
|
||||||
|
|
||||||
// Private functions
|
// Private functions
|
||||||
@ -122,6 +123,9 @@ static void ZeroPids(void);
|
|||||||
static void SettingsUpdatedCb(UAVObjEvent *ev);
|
static void SettingsUpdatedCb(UAVObjEvent *ev);
|
||||||
static void BankUpdatedCb(UAVObjEvent *ev);
|
static void BankUpdatedCb(UAVObjEvent *ev);
|
||||||
static void SettingsBankUpdatedCb(UAVObjEvent *ev);
|
static void SettingsBankUpdatedCb(UAVObjEvent *ev);
|
||||||
|
static float CruiseControlAngleToFactor(float angle);
|
||||||
|
static float CruiseControlFactorToThrust(float factor, float stick_thrust);
|
||||||
|
static float CruiseControlLimitThrust(float thrust);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Module initialization
|
* Module initialization
|
||||||
@ -433,7 +437,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
|
|||||||
#define STICK_VALUE_AT_MODE_TRANSITION 0.618033989f
|
#define STICK_VALUE_AT_MODE_TRANSITION 0.618033989f
|
||||||
|
|
||||||
// the following assumes the transition would otherwise be at 0.618033989f
|
// the following assumes the transition would otherwise be at 0.618033989f
|
||||||
// and that assumes that Att ramps up to max roll rate
|
// and THAT assumes that Att ramps up to max roll rate
|
||||||
// when a small number of degrees off of where it should be
|
// when a small number of degrees off of where it should be
|
||||||
|
|
||||||
// if below the transition angle (still in attitude mode)
|
// if below the transition angle (still in attitude mode)
|
||||||
@ -441,21 +445,24 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
|
|||||||
if (magnitude <= rattitude_mode_transition_stick_position) {
|
if (magnitude <= rattitude_mode_transition_stick_position) {
|
||||||
magnitude *= STICK_VALUE_AT_MODE_TRANSITION / rattitude_mode_transition_stick_position;
|
magnitude *= STICK_VALUE_AT_MODE_TRANSITION / rattitude_mode_transition_stick_position;
|
||||||
} else {
|
} else {
|
||||||
magnitude = (magnitude - rattitude_mode_transition_stick_position) * (1.0f-STICK_VALUE_AT_MODE_TRANSITION) / (1.0f - rattitude_mode_transition_stick_position) + STICK_VALUE_AT_MODE_TRANSITION;
|
magnitude = (magnitude - rattitude_mode_transition_stick_position)
|
||||||
|
* (1.0f-STICK_VALUE_AT_MODE_TRANSITION)
|
||||||
|
/ (1.0f - rattitude_mode_transition_stick_position)
|
||||||
|
+ STICK_VALUE_AT_MODE_TRANSITION;
|
||||||
}
|
}
|
||||||
rateDesiredAxis[i] = (1.0f - magnitude) * rateDesiredAxisAttitude
|
rateDesiredAxis[i] = (1.0f - magnitude) * rateDesiredAxisAttitude
|
||||||
+ magnitude * rateDesiredAxisRate;
|
+ magnitude * rateDesiredAxisRate;
|
||||||
|
|
||||||
// Compute the inner loop for the averaged rate
|
// Compute the inner loop for the averaged rate
|
||||||
// actuatorDesiredAxis[i] is the weighted average
|
// 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] = pid_apply_setpoint(&pids[PID_RATE_ROLL + i], speedScaleFactor,
|
||||||
|
rateDesiredAxis[i], gyro_filtered[i], dT);
|
||||||
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
|
actuatorDesiredAxis[i] = bound(actuatorDesiredAxis[i], 1.0f);
|
||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
|
case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR:
|
||||||
|
|
||||||
// Store for debugging output
|
// Store for debugging output
|
||||||
rateDesiredAxis[i] = stabDesiredAxis[i];
|
rateDesiredAxis[i] = stabDesiredAxis[i];
|
||||||
|
|
||||||
@ -564,42 +571,47 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
|
|||||||
// to maintain same altitude with changing bank angle
|
// to maintain same altitude with changing bank angle
|
||||||
// but without manually adjusting thrust
|
// but without manually adjusting thrust
|
||||||
// do it here and all the various flight modes (e.g. Altitude Hold) can use it
|
// do it here and all the various flight modes (e.g. Altitude Hold) can use it
|
||||||
uint8_t previous_flight_mode_switch_position = 254; // something invalid
|
|
||||||
if (flight_mode_switch_position < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM
|
if (flight_mode_switch_position < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM
|
||||||
&& cruise_control_flight_mode_switch_pos_enable[flight_mode_switch_position] != (uint8_t)0
|
&& cruise_control_flight_mode_switch_pos_enable[flight_mode_switch_position] != (uint8_t)0
|
||||||
&& cruise_control_max_power_factor > 0.0001f) {
|
&& cruise_control_max_power_factor > 0.0001f) {
|
||||||
static float factor;
|
static float factor;
|
||||||
|
static float previous_angle;
|
||||||
static uint32_t previous_time;
|
static uint32_t previous_time;
|
||||||
|
static bool previous_time_valid; // initially false
|
||||||
|
static bool inverted_flight_mode;
|
||||||
static uint8_t calc_count;
|
static uint8_t calc_count;
|
||||||
float angle;
|
static uint8_t previous_flight_mode_switch_position;
|
||||||
uint32_t time;
|
uint32_t time;
|
||||||
|
|
||||||
// flight mode has changed. there could be a time gap
|
// flight mode has changed. there could be a time gap. leave thrust alone.
|
||||||
|
// angle could be invalid because of the time spent with CC off
|
||||||
|
// if (cruise_control_flight_mode_switch_pos_enable[flight_mode_switch_position] != cruise_control_flight_mode_switch_pos_enable[previous_flight_mode_switch_position]) {
|
||||||
if (flight_mode_switch_position != previous_flight_mode_switch_position) {
|
if (flight_mode_switch_position != previous_flight_mode_switch_position) {
|
||||||
// flag to skip this loop because time diff may be invalid
|
previous_flight_mode_switch_position = flight_mode_switch_position;
|
||||||
previous_time = 0;
|
// flag to skip delay comp calculations and just use thrust as it is
|
||||||
|
// because time diff may be invalid
|
||||||
|
previous_time_valid = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
time = PIOS_DELAY_GetuS(); // good for 72 minutes, then it wraps (handled OK)
|
time = PIOS_DELAY_GetuS(); // good for 72 minutes, then it wraps (handled OK)
|
||||||
// get attitude state and calculate angle
|
// get attitude state and calculate angle
|
||||||
// do it every 8th iteration to save CPU
|
// do it every 8th iteration to save CPU
|
||||||
if (time != previous_time && calc_count++ >= 8) {
|
if ((time != previous_time && calc_count++ >= 8) || previous_time_valid == false) {
|
||||||
static float previous_angle;
|
float angle;
|
||||||
float rate;
|
|
||||||
// float thrust;
|
|
||||||
|
|
||||||
calc_count = 0;
|
calc_count = 0;
|
||||||
|
|
||||||
// spherical right triangle
|
// spherical right triangle
|
||||||
// 0 <= acosf() <= Pi
|
// 0 <= acosf() <= Pi(180)
|
||||||
angle = RAD2DEG(acosf(cos_lookup_deg(attitudeState.Roll) * cos_lookup_deg(attitudeState.Pitch)));
|
angle = RAD2DEG(acosf(cos_lookup_deg(attitudeState.Roll) * cos_lookup_deg(attitudeState.Pitch)));
|
||||||
|
|
||||||
// combined bank angle change rate in degrees per second
|
// combined bank angle change rate in degrees per second
|
||||||
// rate is currently calculated over the most recent CALCCOUNT loops
|
// rate is currently calculated over the most recent CALCCOUNT loops
|
||||||
|
// it may be that the angle doesn't change at all between two consecutive passes
|
||||||
|
// because the sensors aren't read that often
|
||||||
// keeping the sign for rate is important, it can be negative
|
// keeping the sign for rate is important, it can be negative
|
||||||
if (previous_time == 0UL) {
|
if (previous_time_valid) {
|
||||||
rate = 0.0f;
|
float rate;
|
||||||
} else {
|
|
||||||
// handle wrap around
|
// handle wrap around
|
||||||
// the assumption here is that it's been less than 0x7fffffff since prev_time
|
// the assumption here is that it's been less than 0x7fffffff since prev_time
|
||||||
// and thus likewise since time
|
// and thus likewise since time
|
||||||
@ -609,98 +621,94 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
|
|||||||
} else { // the wrap around case
|
} else { // the wrap around case
|
||||||
rate = (angle - previous_angle) / ((float) ((uint32_t) ((int32_t) time - (int32_t) previous_time)) / 1000000.0f);
|
rate = (angle - previous_angle) / ((float) ((uint32_t) ((int32_t) time - (int32_t) previous_time)) / 1000000.0f);
|
||||||
}
|
}
|
||||||
}
|
|
||||||
previous_time = time;
|
|
||||||
previous_angle = angle;
|
|
||||||
|
|
||||||
// define "within range" to be those transitions that should be executing now
|
// define "within range" to be those transitions that should be executing now
|
||||||
// - recall that each impulse transition is spread out over a range of time / angle
|
// - recall that each impulse transition is spread out over a range of time / angle
|
||||||
|
|
||||||
// there is only one transition and the high power level for it is either
|
// there is only one transition and the high power level for it is either
|
||||||
// = full thrust
|
// = full thrust
|
||||||
// = max power factor
|
// = max power factor
|
||||||
// = 1/fabsf(cos(angle))
|
// = 1/fabsf(cos(angle))
|
||||||
// OK, you could say there are two transitions in 360 degrees (90 and 270)
|
// OK, you could say there are two transitions in 360 degrees (90 and 270)
|
||||||
|
|
||||||
{
|
if (actuatorDesired.Thrust > 0.0f) {
|
||||||
float thrust;
|
float max_thrust;
|
||||||
{
|
// calculate the max positive thrust at the transition
|
||||||
float full_thrust_angle;
|
// <5% t<5
|
||||||
// calculate angle where thrust is full (as limited by max_thrust)
|
// 5.01% 19.3
|
||||||
full_thrust_angle = RAD2DEG(acosf(cruise_control_max_thrust / actuatorDesired.Thrust));
|
// 10% 38.6
|
||||||
// if full thrust comes before the artificially limited max_power_factor
|
// 50% max
|
||||||
if (full_thrust_angle < cruise_control_max_power_factor_angle) {
|
// 100% max
|
||||||
thrust = cruise_control_max_thrust;
|
thrust = CruiseControlFactorToThrust(CruiseControlAngleToFactor(cruise_control_max_angle), actuatorDesired.Thrust);
|
||||||
} else {
|
|
||||||
thrust = cruise_control_max_power_factor * actuatorDesired.Thrust;
|
/*
|
||||||
// 'full_thrust_angle' is now the angle that goes with 'thrust'
|
revisit all the GCS settings
|
||||||
full_thrust_angle = cruise_control_max_power_factor_angle;
|
51 <= maxt <= 100
|
||||||
|
No (0 <= mint <= 49 plus -51 -> -100)
|
||||||
|
0 <= mint <= 49 or maybe -100 -> 49
|
||||||
|
1 <= mpf <= 50
|
||||||
|
50 <= trim <= 200
|
||||||
|
?? <= maxa <= 255?
|
||||||
|
0 <= pdc <= 2?
|
||||||
|
*/
|
||||||
|
|
||||||
|
// determine if we are in range of the transition
|
||||||
|
|
||||||
|
//CP helis go -max to +max and have min set to 0 and max to +1
|
||||||
|
|
||||||
|
//min_thrust is defined to be where the power goes when inverted
|
||||||
|
|
||||||
|
// calculate the actual proportion of change in thrust
|
||||||
|
switch (cruise_control_inverted_power_switch) {
|
||||||
|
case -3:
|
||||||
|
case -2:
|
||||||
|
// CP heli case, stroke is max to -max
|
||||||
|
thrust = (thrust + thrust) / cruise_control_thrust_difference;
|
||||||
|
break;
|
||||||
|
case -1:
|
||||||
|
// CP heli case, stroke is max to -stick
|
||||||
|
thrust = (thrust + CruiseControlLimitThrust(actuatorDesired.Thrust)) / cruise_control_thrust_difference;
|
||||||
|
break;
|
||||||
|
case 0:
|
||||||
|
// normal multi-copter case, stroke is max to zero
|
||||||
|
// technically max to constant min_thrust
|
||||||
|
// can be used by CP
|
||||||
|
thrust = (thrust - cruise_control_min_thrust) / cruise_control_thrust_difference;
|
||||||
|
//shit, cruise_control_thrust_difference is a problem, maybe remove it
|
||||||
|
//thrust_difference is always max-min, and now for CP it needs to be max+max here
|
||||||
|
//maybe not a problem since we don't limit thrust on the negative side any more
|
||||||
|
//CP: max=1.0, min=-1.0 diff=2.0
|
||||||
|
//MR: max=0.9, min=0.1 diff=0.8
|
||||||
|
break;
|
||||||
|
case 1:
|
||||||
|
// simply turn off boost, stroke is max to +stick
|
||||||
|
thrust = (thrust - CruiseControlLimitThrust(actuatorDesired.Thrust)) / cruise_control_thrust_difference;
|
||||||
|
break;
|
||||||
|
case 2:
|
||||||
|
// CP heli case, no transition, stroke is zero
|
||||||
|
thrust = 0.0f;
|
||||||
|
break;
|
||||||
}
|
}
|
||||||
// if the transition is outside the max thrust regions
|
// multiply this proportion of max stroke, times the max stroke time, to get this stroke time
|
||||||
if (full_thrust_angle < cruise_control_max_angle
|
// we only want half of this time before the transition (and half after the transition)
|
||||||
|| 180.0f - cruise_control_max_angle < full_thrust_angle ) {
|
thrust *= cruise_control_half_power_delay;
|
||||||
// max thrust is 1/cos(transition angle)
|
// times angular rate gives angle that this stroke will take to execute
|
||||||
thrust = 1.0f / fabsf(cos_lookup_deg(cruise_control_max_angle));
|
thrust *= fabsf(rate);
|
||||||
|
// if the transition is within range we use it, else we just use the current calculated thrust
|
||||||
|
if (cruise_control_max_angle - thrust < angle
|
||||||
|
&& angle < cruise_control_max_angle + thrust) {
|
||||||
|
// default to a little above max angle
|
||||||
|
angle = cruise_control_max_angle + 0.01f;
|
||||||
|
// if roll direction is downward then thrust value is taken from below max angle
|
||||||
|
if (rate < 0.0f) {
|
||||||
|
angle -= 0.02f;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
|
||||||
|
|
||||||
// determine if we are in range of the transition
|
factor = CruiseControlAngleToFactor(angle);
|
||||||
|
} else { // if thrust > 0 set factor from angle; else
|
||||||
// calculate the actual proportion of change in thrust
|
factor = 1.0f;
|
||||||
switch (cruise_control_inverted_power_switch) {
|
|
||||||
case -3:
|
|
||||||
case -2:
|
|
||||||
// CP heli case, stroke is max to -max
|
|
||||||
// thrust = (thrust + thrust) / (cruise_control_max_thrust + cruise_control_max_thrust);
|
|
||||||
thrust /= cruise_control_max_thrust;
|
|
||||||
break;
|
|
||||||
case -1:
|
|
||||||
// CP heli case, stroke is max to -stick
|
|
||||||
thrust = (thrust + actuatorDesired.Thrust) / (cruise_control_max_thrust + cruise_control_max_thrust);
|
|
||||||
break;
|
|
||||||
case 0:
|
|
||||||
// normal multi-copter case, stroke is max to min
|
|
||||||
thrust = (thrust - cruise_control_min_thrust) / (cruise_control_max_thrust - cruise_control_min_thrust);
|
|
||||||
break;
|
|
||||||
case 1:
|
|
||||||
// simply turn off boost, stroke is max to stick
|
|
||||||
thrust = (thrust - actuatorDesired.Thrust) / (cruise_control_max_thrust - cruise_control_min_thrust);
|
|
||||||
break;
|
|
||||||
case 2:
|
|
||||||
// CP heli case, no transition, stroke is zero
|
|
||||||
thrust = 0.0f;
|
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
// multiply this proportion of max stroke, times the max stroke time, to get this stroke time
|
|
||||||
// we only want half of this time before the transition (and half after the transition)
|
|
||||||
thrust *= cruise_control_half_power_delay;
|
|
||||||
// times angular rate gives angle that this stroke will take to execute
|
|
||||||
thrust *= fabsf(rate);
|
|
||||||
// if the transition is within range we use it, else we just use the current calculated thrust
|
|
||||||
if (cruise_control_max_angle - thrust < angle
|
|
||||||
&& angle < cruise_control_max_angle + thrust) {
|
|
||||||
// default to a little above max angle
|
|
||||||
angle = cruise_control_max_angle + 0.01f;
|
|
||||||
// if roll direction is downward then thrust value is taken from below max angle
|
|
||||||
if (rate < 0.0f) {
|
|
||||||
angle -= 0.02f;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// avoid singularity
|
|
||||||
if (angle > 89.999f && angle < 90.001f) {
|
|
||||||
factor = cruise_control_max_power_factor;
|
|
||||||
} else {
|
|
||||||
// the simple bank angle boost calculation that Cruise Control revolves around
|
|
||||||
factor = 1.0f / fabsf(cos_lookup_deg(angle));
|
|
||||||
// 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;
|
|
||||||
// limit to user specified max power multiplier
|
|
||||||
if (factor > cruise_control_max_power_factor) {
|
|
||||||
factor = cruise_control_max_power_factor;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
convert these to enums? something like
|
convert these to enums? something like
|
||||||
@ -709,47 +717,46 @@ inverted thrust direction: unchanged, reversed
|
|||||||
inverted power: zero, normal, boosted
|
inverted power: zero, normal, boosted
|
||||||
inverted yaw/pitch reverse: off, on
|
inverted yaw/pitch reverse: off, on
|
||||||
*/
|
*/
|
||||||
// if past max angle and so needing to go into an inverted mode
|
inverted_flight_mode = false;
|
||||||
if (angle >= cruise_control_max_angle) {
|
// if past max angle and so needing to go into an inverted mode
|
||||||
// -3 inverted mode, -2 boosted reverse, -1 normal reverse, 0 zero power, 1 normal power, 2 boosted power
|
if (angle >= cruise_control_max_angle) {
|
||||||
switch (cruise_control_inverted_power_switch) {
|
// -3 inverted mode, -2 boosted reverse, -1 normal reverse, 0 zero power, 1 normal power, 2 boosted power
|
||||||
case -3: // reversed boosted thrust with pitch/yaw reverse
|
switch (cruise_control_inverted_power_switch) {
|
||||||
actuatorDesired.Pitch = -actuatorDesired.Pitch;
|
case -3: // reversed boosted thrust with pitch/yaw reverse
|
||||||
actuatorDesired.Yaw = -actuatorDesired.Yaw;
|
inverted_flight_mode = true;
|
||||||
factor = -factor;
|
factor = -factor;
|
||||||
break;
|
break;
|
||||||
case -2: // reversed boosted thrust
|
case -2: // reversed boosted thrust
|
||||||
factor = -factor;
|
factor = -factor;
|
||||||
break;
|
break;
|
||||||
case -1: // reversed normal thrust
|
case -1: // reversed normal thrust
|
||||||
factor = -1.0f;
|
factor = -1.0f;
|
||||||
break;
|
break;
|
||||||
case 0: // no thrust
|
case 0: // no thrust
|
||||||
factor = -0.0f;
|
factor = 0.0f;
|
||||||
break;
|
break;
|
||||||
case 1: // normal thrust
|
case 1: // normal thrust
|
||||||
factor = 1.0f;
|
factor = 1.0f;
|
||||||
break;
|
break;
|
||||||
case 2: // normal boosted thrust
|
case 2: // normal boosted thrust
|
||||||
// no change to factor
|
// no change to factor
|
||||||
break;
|
break;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
} // if previous_time_valid i.e. we've got a rate; else leave (angle and) factor alone
|
||||||
}
|
previous_time = time;
|
||||||
|
previous_time_valid = true;
|
||||||
|
previous_angle = angle;
|
||||||
|
} // every 8th time
|
||||||
|
|
||||||
// also don't adjust thrust if <= 0, leaves neg alone and zero thrust stops motors
|
// don't touch thrust if it's less than min_thrust
|
||||||
if (actuatorDesired.Thrust > cruise_control_min_thrust) {
|
// without this test, props will spin up to min thrust even at zero throttle stick
|
||||||
actuatorDesired.Thrust *= factor;
|
actuatorDesired.Thrust = CruiseControlFactorToThrust(factor, actuatorDesired.Thrust);
|
||||||
// limit to user specified absolute max thrust
|
if (inverted_flight_mode) {
|
||||||
if (actuatorDesired.Thrust > cruise_control_max_thrust) {
|
actuatorDesired.Pitch = -actuatorDesired.Pitch;
|
||||||
actuatorDesired.Thrust = cruise_control_max_thrust;
|
actuatorDesired.Yaw = -actuatorDesired.Yaw;
|
||||||
} else if (actuatorDesired.Thrust < cruise_control_min_thrust) {
|
|
||||||
actuatorDesired.Thrust = cruise_control_min_thrust;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
} // if Cruise Control is enabled on this flight switch position
|
||||||
|
|
||||||
previous_flight_mode_switch_position = flight_mode_switch_position;
|
|
||||||
|
|
||||||
if (flightStatus.ControlChain.Stabilization == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
|
if (flightStatus.ControlChain.Stabilization == FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
|
||||||
ActuatorDesiredSet(&actuatorDesired);
|
ActuatorDesiredSet(&actuatorDesired);
|
||||||
@ -920,6 +927,54 @@ static void BankUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
static float CruiseControlAngleToFactor(float angle)
|
||||||
|
{
|
||||||
|
float factor;
|
||||||
|
// avoid singularity
|
||||||
|
if (angle > 89.999f && angle < 90.001f) {
|
||||||
|
factor = cruise_control_max_power_factor;
|
||||||
|
} else {
|
||||||
|
// the simple bank angle boost calculation that Cruise Control revolves around
|
||||||
|
factor = 1.0f / fabsf(cos_lookup_deg(angle));
|
||||||
|
// 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;
|
||||||
|
// limit to user specified max power multiplier
|
||||||
|
if (factor > cruise_control_max_power_factor) {
|
||||||
|
factor = cruise_control_max_power_factor;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return (factor);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// assumes 1.0 <= factor <= 100.0
|
||||||
|
// a factor of less than 1.0 could make it return a value less than cruise_control_min_thrust
|
||||||
|
static float CruiseControlFactorToThrust(float factor, float thrust)
|
||||||
|
{
|
||||||
|
// don't touch if below min_thrust or we never get to full down stick
|
||||||
|
// e.g. multicopter motors always spin
|
||||||
|
if (thrust > cruise_control_min_thrust) {
|
||||||
|
thrust = CruiseControlLimitThrust(thrust * factor);
|
||||||
|
}
|
||||||
|
return (thrust);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// we don't want to limit it to cruise_control_min_thrust on the low side
|
||||||
|
// because that is always close to zero, and CP helis need to run down to -1
|
||||||
|
// but CP needs to have min=-1
|
||||||
|
static float CruiseControlLimitThrust(float thrust)
|
||||||
|
{
|
||||||
|
// limit to user specified absolute max thrust
|
||||||
|
if (thrust > cruise_control_max_thrust) {
|
||||||
|
thrust = cruise_control_max_thrust;
|
||||||
|
// } else if (thrust < cruise_control_min_thrust) {
|
||||||
|
// thrust = cruise_control_min_thrust;
|
||||||
|
}
|
||||||
|
return (thrust);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||||
{
|
{
|
||||||
StabilizationSettingsGet(&settings);
|
StabilizationSettingsGet(&settings);
|
||||||
@ -957,10 +1012,10 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
|||||||
cur_flight_mode = -1;
|
cur_flight_mode = -1;
|
||||||
|
|
||||||
// Rattitude stick angle where the attitude to rate transition happens
|
// Rattitude stick angle where the attitude to rate transition happens
|
||||||
if (settings.RattitudeModeTransistion < (uint8_t) 10) {
|
if (settings.RattitudeModeTransition < (uint8_t) 10) {
|
||||||
rattitude_mode_transition_stick_position = 10.0f / 100.0f;
|
rattitude_mode_transition_stick_position = 10.0f / 100.0f;
|
||||||
} else {
|
} else {
|
||||||
rattitude_mode_transition_stick_position = (float)settings.RattitudeModeTransistion / 100.0f;
|
rattitude_mode_transition_stick_position = (float)settings.RattitudeModeTransition / 100.0f;
|
||||||
}
|
}
|
||||||
|
|
||||||
cruise_control_min_thrust = (float)settings.CruiseControlMinThrust / 100.0f;
|
cruise_control_min_thrust = (float)settings.CruiseControlMinThrust / 100.0f;
|
||||||
@ -971,6 +1026,7 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
|||||||
cruise_control_inverted_power_switch = settings.CruiseControlInvertedPowerSwitch;
|
cruise_control_inverted_power_switch = settings.CruiseControlInvertedPowerSwitch;
|
||||||
cruise_control_half_power_delay = settings.CruiseControlPowerDelayComp / 2.0f;
|
cruise_control_half_power_delay = settings.CruiseControlPowerDelayComp / 2.0f;
|
||||||
cruise_control_max_power_factor_angle = RAD2DEG(acosf(1.0f / settings.CruiseControlMaxPowerFactor));
|
cruise_control_max_power_factor_angle = RAD2DEG(acosf(1.0f / settings.CruiseControlMaxPowerFactor));
|
||||||
|
cruise_control_thrust_difference = cruise_control_max_thrust - cruise_control_min_thrust;
|
||||||
|
|
||||||
memcpy(
|
memcpy(
|
||||||
cruise_control_flight_mode_switch_pos_enable,
|
cruise_control_flight_mode_switch_pos_enable,
|
||||||
|
@ -13377,6 +13377,7 @@ border-radius: 5;</string>
|
|||||||
</property>
|
</property>
|
||||||
<property name="decimals">
|
<property name="decimals">
|
||||||
<number>5</number>
|
<number>5</number>
|
||||||
|
|
||||||
</property>
|
</property>
|
||||||
<property name="singleStep">
|
<property name="singleStep">
|
||||||
<double>0.000100000000000</double>
|
<double>0.000100000000000</double>
|
||||||
@ -19417,6 +19418,7 @@ border-radius: 5;</string>
|
|||||||
</property>
|
</property>
|
||||||
<property name="autoFillBackground">
|
<property name="autoFillBackground">
|
||||||
<bool>false</bool>
|
<bool>false</bool>
|
||||||
|
|
||||||
</property>
|
</property>
|
||||||
<property name="styleSheet">
|
<property name="styleSheet">
|
||||||
<string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
|
<string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
|
||||||
@ -19941,6 +19943,7 @@ border-radius: 5;</string>
|
|||||||
<red>39</red>
|
<red>39</red>
|
||||||
<green>39</green>
|
<green>39</green>
|
||||||
<blue>39</blue>
|
<blue>39</blue>
|
||||||
|
|
||||||
</color>
|
</color>
|
||||||
</brush>
|
</brush>
|
||||||
</colorrole>
|
</colorrole>
|
||||||
@ -21975,6 +21978,7 @@ border-radius: 5;</string>
|
|||||||
</size>
|
</size>
|
||||||
</property>
|
</property>
|
||||||
</spacer>
|
</spacer>
|
||||||
|
|
||||||
</item>
|
</item>
|
||||||
<item row="1" column="3">
|
<item row="1" column="3">
|
||||||
<widget class="QDoubleSpinBox" name="AccelKp">
|
<widget class="QDoubleSpinBox" name="AccelKp">
|
||||||
@ -24004,7 +24008,7 @@ border-radius: 5;</string>
|
|||||||
</spacer>
|
</spacer>
|
||||||
</item>
|
</item>
|
||||||
<item row="1" column="1">
|
<item row="1" column="1">
|
||||||
<widget class="QDoubleSpinBox" name="RattitudeModeTransistion">
|
<widget class="QDoubleSpinBox" name="RattitudeModeTransition">
|
||||||
<property name="sizePolicy">
|
<property name="sizePolicy">
|
||||||
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
|
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
|
||||||
<horstretch>0</horstretch>
|
<horstretch>0</horstretch>
|
||||||
@ -24050,7 +24054,7 @@ border-radius: 5;</string>
|
|||||||
<property name="objrelation" stdset="0">
|
<property name="objrelation" stdset="0">
|
||||||
<stringlist>
|
<stringlist>
|
||||||
<string>objname:StabilizationSettings</string>
|
<string>objname:StabilizationSettings</string>
|
||||||
<string>fieldname:RattitudeModeTransistion</string>
|
<string>fieldname:RattitudeModeTransition</string>
|
||||||
<string>haslimits:no</string>
|
<string>haslimits:no</string>
|
||||||
<string>scale:1</string>
|
<string>scale:1</string>
|
||||||
<string>buttongroup:15</string>
|
<string>buttongroup:15</string>
|
||||||
|
@ -28,7 +28,7 @@
|
|||||||
<field name="WeakLevelingKp" units="(deg/s)/deg" type="float" elements="1" defaultvalue="0.1"/>
|
<field name="WeakLevelingKp" units="(deg/s)/deg" type="float" elements="1" defaultvalue="0.1"/>
|
||||||
<field name="MaxWeakLevelingRate" units="deg/s" type="uint8" elements="1" defaultvalue="5"/>
|
<field name="MaxWeakLevelingRate" units="deg/s" type="uint8" elements="1" defaultvalue="5"/>
|
||||||
|
|
||||||
<field name="RattitudeModeTransistion" units="%" type="uint8" elements="1" defaultvalue="80"/>
|
<field name="RattitudeModeTransition" units="%" type="uint8" elements="1" defaultvalue="80"/>
|
||||||
|
|
||||||
<field name="CruiseControlMinThrust" units="%" type="uint8" elements="1" defaultvalue="5"/>
|
<field name="CruiseControlMinThrust" units="%" type="uint8" elements="1" defaultvalue="5"/>
|
||||||
<field name="CruiseControlMaxThrust" units="%" type="uint8" elements="1" defaultvalue="90"/>
|
<field name="CruiseControlMaxThrust" units="%" type="uint8" elements="1" defaultvalue="90"/>
|
||||||
|
Loading…
x
Reference in New Issue
Block a user