Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Minimum ground speed patch from Claudio Natoli. Thanks Claudio! 

This patch will boost the target airspeed as necessary to keep the ground speed above a parameter value - param_min_groundspeed.  Airspeed is still limited to FBW-max.  Setting min_groundspeed to zero (default) disables the feature.
This commit is contained in:
Doug Weibel 2011-12-09 16:40:56 -07:00
parent 2a7013f7e2
commit f34333d42b
8 changed files with 104 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduPlane/APM_Config.h.reference
View File

@ -516,6 +516,16 @@
//#define AIRSPEED_CRUISE 12
//
//////////////////////////////////////////////////////////////////////////////
// MIN_GNDSPEED OPTIONAL
//
// The minimum ground speed in metres per second to maintain during
// cruise. A value of 0 will disable any attempt to maintain a minumum
// speed over ground.
//
#define MIN_GNDSPEED 0
//
//////////////////////////////////////////////////////////////////////////////
// FLY_BY_WIRE_B airspeed control (also used for throttle "nudging" in AUTO)
//

44
ArduPlane/ArduPlane.pde
View File

@ -325,10 +325,14 @@ static byte non_nav_command_ID = NO_COMMAND; // active non-nav command ID
// --------
static int airspeed; // m/s * 100
static int airspeed_nudge; // m/s * 100 : additional airspeed based on throttle stick position in top 1/2 of range
static long target_airspeed; // m/s * 100 (used for Auto-flap deployment in FBW_B mode)
static float airspeed_error; // m/s * 100
static float airspeed_fbwB; // m/s * 100
static long energy_error; // energy state error (kinetic + potential) for altitude hold
static long airspeed_energy_error; // kinetic portion of energy error
static long airspeed_energy_error; // kinetic portion of energy error (m^2/s^2)
// Ground speed
static long groundspeed_undershoot = 0; // m/s * 100 (>=0, where > 0 => amount below min ground speed)
// Location Errors
// ---------------
@ -523,21 +527,21 @@ static void fast_loop()
read_radio();
// try to send any deferred messages if the serial port now has
// some space available
// some space available
gcs_send_message(MSG_RETRY_DEFERRED);
// check for loss of control signal failsafe condition
// ------------------------------------
check_short_failsafe();
// Read Airspeed
// -------------
// Read Airspeed
// -------------
if (g.airspeed_enabled == true) {
#if HIL_MODE != HIL_MODE_ATTITUDE
read_airspeed();
#endif
} else if (g.airspeed_enabled == true && HIL_MODE == HIL_MODE_ATTITUDE) {
#else
calc_airspeed_errors();
#endif
}
#if HIL_MODE == HIL_MODE_SENSORS
@ -599,7 +603,10 @@ static void medium_loop()
//-------------------------------
case 0:
medium_loopCounter++;
if(GPS_enabled) update_GPS();
if(GPS_enabled){
update_GPS();
calc_gndspeed_undershoot();
}
#if HIL_MODE != HIL_MODE_ATTITUDE
if(g.compass_enabled){
@ -888,23 +895,10 @@ static void update_current_flight_mode(void)
if ((current_loc.alt>=home.alt+g.FBWB_min_altitude) || (g.FBWB_min_altitude == -1)) {
altitude_error = g.channel_pitch.norm_input() * g.pitch_limit_min;
} else {
if (g.channel_pitch.norm_input()<0)
altitude_error =( (home.alt + g.FBWB_min_altitude) - current_loc.alt) + g.channel_pitch.norm_input() * g.pitch_limit_min ;
else altitude_error =( (home.alt + g.FBWB_min_altitude) - current_loc.alt) ;
if (g.channel_pitch.norm_input()<0)
altitude_error =( (home.alt + g.FBWB_min_altitude) - current_loc.alt) + g.channel_pitch.norm_input() * g.pitch_limit_min ;
else altitude_error =( (home.alt + g.FBWB_min_altitude) - current_loc.alt) ;
}
if (g.airspeed_enabled == true)
{
airspeed_fbwB = ((int)(g.flybywire_airspeed_max -
g.flybywire_airspeed_min) *
g.channel_throttle.servo_out) +
((int)g.flybywire_airspeed_min * 100);
airspeed_energy_error = (long)(((long)airspeed_fbwB *
(long)airspeed_fbwB) -
((long)airspeed * (long)airspeed))/20000;
airspeed_error = (airspeed_error - airspeed);
}
calc_throttle();
calc_nav_pitch();
break;

19
ArduPlane/Attitude.pde
View File

@ -127,6 +127,11 @@ static void calc_throttle()
if (g.airspeed_enabled == false) {
int throttle_target = g.throttle_cruise + throttle_nudge;
// TODO: think up an elegant way to bump throttle when
// groundspeed_undershoot > 0 in the no airspeed sensor case; PID
// control?
// no airspeed sensor, we use nav pitch to determine the proper throttle output
// AUTO, RTL, etc
// ---------------------------------------------------------------------------
@ -236,7 +241,7 @@ static void throttle_slew_limit()
{
static int last = 1000;
if(g.throttle_slewrate) { // if slew limit rate is set to zero then do not slew limit
float temp = g.throttle_slewrate * G_Dt * 10.f; // * 10 to scale % to pwm range of 1000 to 2000
Serial.print("radio "); Serial.print(g.channel_throttle.radio_out); Serial.print(" temp "); Serial.print(temp); Serial.print(" last "); Serial.println(last);
g.channel_throttle.radio_out = constrain(g.channel_throttle.radio_out, last - (int)temp, last + (int)temp);
@ -262,7 +267,7 @@ static void reset_I(void)
static void set_servos(void)
{
int flapSpeedSource = 0;
// vectorize the rc channels
RC_Channel_aux* rc_array[NUM_CHANNELS];
rc_array[CH_1] = NULL;
@ -325,7 +330,7 @@ static void set_servos(void)
#else
// convert 0 to 100% into PWM
g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out, g.throttle_min.get(), g.throttle_max.get());
// We want to supress the throttle if we think we are on the ground and in an autopilot controlled throttle mode.
/* Disable throttle if following conditions are met:
1 - We are in Circle mode (which we use for short term failsafe), or in FBW-B or higher
@ -336,7 +341,7 @@ static void set_servos(void)
OR
5 - Home location is not set
*/
if (
if (
(control_mode == CIRCLE || control_mode >= FLY_BY_WIRE_B) &&
(abs(home.alt - current_loc.alt) < 1000) &&
((g.airspeed_enabled ? airspeed : g_gps->ground_speed) < 500 ) &&
@ -345,7 +350,7 @@ static void set_servos(void)
g.channel_throttle.servo_out = 0;
g.channel_throttle.calc_pwm();
}
#endif
g.channel_throttle.calc_pwm();
@ -369,8 +374,9 @@ static void set_servos(void)
g_rc_function[RC_Channel_aux::k_flap_auto]->radio_out = g_rc_function[RC_Channel_aux::k_flap_auto]->radio_trim;
}
} else if (control_mode >= FLY_BY_WIRE_B) {
// FIXME: use target_airspeed in both FBW_B and g.airspeed_enabled cases - Doug?
if (control_mode == FLY_BY_WIRE_B) {
flapSpeedSource = airspeed_fbwB/100;
flapSpeedSource = ((float)target_airspeed)/100;
} else if (g.airspeed_enabled == true) {
flapSpeedSource = g.airspeed_cruise/100;
} else {
@ -417,4 +423,3 @@ static void demo_servos(byte i) {
i--;
}
}

3
ArduPlane/Parameters.h
View File

@ -116,6 +116,7 @@ public:
k_param_airspeed_cruise,
k_param_RTL_altitude,
k_param_inverted_flight_ch,
k_param_min_gndspeed,
//
// 170: Radio settings
@ -308,6 +309,7 @@ public:
AP_Int16 log_bitmask;
AP_Int16 log_last_filenumber;
AP_Int16 airspeed_cruise;
AP_Int16 min_gndspeed;
AP_Int16 pitch_trim;
AP_Int16 RTL_altitude;
AP_Int16 ground_temperature;
@ -414,6 +416,7 @@ public:
log_bitmask (DEFAULT_LOG_BITMASK, k_param_log_bitmask, PSTR("LOG_BITMASK")),
log_last_filenumber (0, k_param_log_last_filenumber, PSTR("LOG_LASTFILE")),
airspeed_cruise (AIRSPEED_CRUISE_CM, k_param_airspeed_cruise, PSTR("TRIM_ARSPD_CM")),
min_gndspeed (MIN_GNDSPEED_CM, k_param_min_gndspeed, PSTR("MIN_GNDSPD_CM")),
pitch_trim (0, k_param_pitch_trim, PSTR("TRIM_PITCH_CD")),
RTL_altitude (ALT_HOLD_HOME_CM, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")),
FBWB_min_altitude (ALT_HOLD_FBW_CM, k_param_FBWB_min_altitude, PSTR("ALT_HOLD_FBWCM")),

13
ArduPlane/commands_logic.pde
View File

@ -498,9 +498,16 @@ static void do_jump()
static void do_change_speed()
{
// Note: we have no implementation for commanded ground speed, only air speed and throttle
if(next_nonnav_command.alt > 0)
g.airspeed_cruise.set_and_save(next_nonnav_command.alt * 100);
switch (next_nonnav_command.p1)
{
case 0: // Airspeed
if(next_nonnav_command.alt > 0)
g.airspeed_cruise.set_and_save(next_nonnav_command.alt * 100);
break;
case 1: // Ground speed
g.min_gndspeed.set_and_save(next_nonnav_command.alt * 100);
break;
}
if(next_nonnav_command.lat > 0)
g.throttle_cruise.set_and_save(next_nonnav_command.lat);

10
ArduPlane/config.h
View File

@ -456,6 +456,16 @@
#endif
#define AIRSPEED_CRUISE_CM AIRSPEED_CRUISE*100
//////////////////////////////////////////////////////////////////////////////
// MIN_GNDSPEED
//
#ifndef MIN_GNDSPEED
# define MIN_GNDSPEED 0 // m/s (0 disables)
#endif
#define MIN_GNDSPEED_CM MIN_GNDSPEED*100
//////////////////////////////////////////////////////////////////////////////
// FLY_BY_WIRE_B airspeed control
//

47
ArduPlane/navigation.pde
View File

@ -64,14 +64,43 @@ void calc_distance_error()
static void calc_airspeed_errors()
{
// XXX excess casting here
if(control_mode>=AUTO && airspeed_nudge > 0) {
airspeed_error = g.airspeed_cruise + airspeed_nudge - airspeed;
airspeed_energy_error = (long)(((long)(g.airspeed_cruise + airspeed_nudge) * (long)(g.airspeed_cruise + airspeed_nudge)) - ((long)airspeed * (long)airspeed))/20000; //Changed 0.00005f * to / 20000 to avoid floating point calculation
} else {
airspeed_error = g.airspeed_cruise - airspeed;
airspeed_energy_error = (long)(((long)g.airspeed_cruise * (long)g.airspeed_cruise) - ((long)airspeed * (long)airspeed))/20000; //Changed 0.00005f * to / 20000 to avoid floating point calculation
}
// Normal airspeed target
target_airspeed = g.airspeed_cruise;
// FBW_B airspeed target
if (control_mode == FLY_BY_WIRE_B) {
target_airspeed = ((int)(g.flybywire_airspeed_max -
g.flybywire_airspeed_min) *
g.channel_throttle.servo_out) +
((int)g.flybywire_airspeed_min * 100);
}
// Set target to current airspeed + ground speed undershoot,
// but only when this is faster than the target airspeed commanded
// above.
if (control_mode >= FLY_BY_WIRE_B && (g.min_gndspeed > 0)) {
long min_gnd_target_airspeed = airspeed + groundspeed_undershoot;
if (min_gnd_target_airspeed > target_airspeed)
target_airspeed = min_gnd_target_airspeed;
}
// Bump up the target airspeed based on throttle nudging
if (control_mode >= AUTO && airspeed_nudge > 0) {
target_airspeed += airspeed_nudge;
}
// Apply airspeed limit
if (target_airspeed > (g.flybywire_airspeed_max * 100))
target_airspeed = (g.flybywire_airspeed_max * 100);
airspeed_error = target_airspeed - airspeed;
airspeed_energy_error = ((target_airspeed * target_airspeed) - ((long)airspeed * (long)airspeed))/20000; //Changed 0.00005f * to / 20000 to avoid floating point calculation
}
static void calc_gndspeed_undershoot()
{
// Function is overkill, but here in case we want to add filtering later
groundspeed_undershoot = (g.min_gndspeed > 0) ? (g.min_gndspeed - g_gps->ground_speed) : 0;
}
static void calc_bearing_error()
@ -156,7 +185,7 @@ static void update_loiter()
}else{
update_crosstrack();
loiter_time = millis(); // keep start time for loiter updating till we get within LOITER_RANGE of orbit
}
/*
if (wp_distance < g.loiter_radius){

3
Tools/ArdupilotMegaPlanner/bin/Release/Resources/MAVParam.txt
View File

@ -102,7 +102,8 @@ It includes both fixed wing (APM) and rotary wing (!ArduCopter) parameters. Some
||LOG_BITMASK||0||65535||334||1||1||LOG_BITMASK||
||TRIM_ELEVON||900||2100||1500||1||1||TRIM_ELEVON||
||THR_FS_VALUE||850||1000||950||1||1||THROTTLE_FS_VALUE - If the throttle failsafe is enabled, THROTTLE_FS_VALUE sets the channel value below which the failsafe engages. The default is 975ms, which is a very low throttle setting. Most transmitters will let you trim the manual throttle position up so that you cannot engage the failsafe with a regular stick movement. Configure your receiver's failsafe setting for the throttle channel to the absolute minimum, and use the !ArduPilotMega_demo program to check that you cannot reach that value with the throttle control. Leave a margin of at least 50 microseconds between the lowest throttle setting and THROTTLE_FS_VALUE.||
||TRIM_ARSPD_CM||500||5000||1200||100||1||AIRSPEED_CRUISE_CM - The speed in metres per second to maintain during cruise. The default is 10m/s, which is a conservative value suitable for relatively small, light aircraft.||
||TRIM_ARSPD_CM||500||5000||1200||100||1||AIRSPEED_CRUISE_CM - The speed in cm/s to maintain during cruise. The default is 12m/s, which is a conservative value suitable for relatively small, light aircraft.||
||MIN_GNDSPD_CM||0||5000||0||100||1||MIN_GNDSPEED_CM - The minimum ground in cm/s to maintain during cruise. The default value of 0 will disable any attempt to maintain a minimum speed over ground.||
||GND_TEMP||-10||50||28||1||1||GND_TEMP - Ground Temperature||
||AP_OFFSET|| || ||0|| || ||AP_OFFSET||
||TRIM_PITCH_CD|| || ||0|| || ||TRIM_PITCH_CD||