diff --git a/ArduPlane/APM_Config.h.reference b/ArduPlane/APM_Config.h.reference index 4c560ef062..fed8532b2e 100644 --- a/ArduPlane/APM_Config.h.reference +++ b/ArduPlane/APM_Config.h.reference @@ -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) // diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 3c50743d16..d0072ef63d 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -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; diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 8564690453..5a7f9a57db 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -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--; } } - diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 8247641c98..ca4a759acc 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -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")), diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 66597206c3..52ab2661e7 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -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); diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 5a16637f21..ee97b9065d 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -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 // diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index 2b6620009b..776886da01 100644 --- a/ArduPlane/navigation.pde +++ b/ArduPlane/navigation.pde @@ -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){ diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/Resources/MAVParam.txt b/Tools/ArdupilotMegaPlanner/bin/Release/Resources/MAVParam.txt index 9a19416a3b..d65fe939d5 100644 --- a/Tools/ArdupilotMegaPlanner/bin/Release/Resources/MAVParam.txt +++ b/Tools/ArdupilotMegaPlanner/bin/Release/Resources/MAVParam.txt @@ -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||