mirror of https://github.com/ArduPilot/ardupilot
APMrover v2.1.6 - Added Speed Booster between wp and soft start
Signed-off-by: Jean-Louis Naudin <jlnaudin@gmail.com>
This commit is contained in:
Jean-Louis Naudin
parent
9d221b3625
commit
408d3dfa98
|
|
@ -20,8 +20,8 @@
|
|||
|
||||
#define TRACE DISABLED
|
||||
|
||||
//#include "APM_Config_HILmode.h" // for test in HIL mode with AeroSIM Rc 3.83
|
||||
#include "APM_Config_Rover.h" // to be used with the real Traxxas model Monster Jam Grinder
|
||||
#include "APM_Config_HILmode.h" // for test in HIL mode with AeroSIM Rc 3.83
|
||||
//#include "APM_Config_Rover.h" // to be used with the real Traxxas model Monster Jam Grinder
|
||||
|
||||
// Radio setup:
|
||||
// APM INPUT (Rec = receiver)
|
||||
|
|
|
|||
|
|
@ -53,6 +53,11 @@
|
|||
#define MAX_DIST 50 //300 // max distance (in m) for the HEADALT mode
|
||||
#define SARSEC_BRANCH 50 // Long branch of the SARSEC pattern
|
||||
|
||||
#define BOOSTER 2 // booster factor x2
|
||||
#define AUTO_WP_RADIUS DISABLED
|
||||
#define AIRSPEED_CRUISE 3 // 4m/s
|
||||
#define THROTTLE_SLEW_LIMIT 2 // set to 2 for a smooth acceleration by 0.2 step
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// FLIGHT AND NAVIGATION CONTROL
|
||||
|
|
@ -65,7 +70,6 @@
|
|||
// is 10m/s, which is a conservative value suitable for relatively small,
|
||||
// light aircraft.
|
||||
//
|
||||
#define AIRSPEED_CRUISE 3
|
||||
#define GSBOOST 0 // 60 // boost the throttle if ground speed is too low in case of windy conditions // 100
|
||||
#define NUDGE_OFFSET 0 //1603 // nudge_offset to get a good sustained speed in autonomous flight
|
||||
#define MIN_GNDSPEED 3
|
||||
|
|
@ -373,7 +377,6 @@
|
|||
#define THROTTLE_TE_I 0.0
|
||||
#define THROTTLE_TE_D 0.0
|
||||
#define THROTTLE_TE_INT_MAX 20
|
||||
#define THROTTLE_SLEW_LIMIT 0
|
||||
#define P_TO_T 0.0
|
||||
#define T_TO_P 0
|
||||
|
||||
|
|
|
|||
|
|
@ -33,7 +33,6 @@
|
|||
#define GPS_PROTOCOL GPS_PROTOCOL_AUTO
|
||||
|
||||
#define CH7_OPTION CH7_SAVE_WP
|
||||
#define TUNING_OPTION TUN_NONE
|
||||
|
||||
#define FLIGHT_MODE_1 AUTO // pos 0 ---
|
||||
#define FLIGHT_MODE_2 AUTO // pos 1
|
||||
|
|
@ -49,10 +48,21 @@
|
|||
#define TURN_GAIN 5
|
||||
|
||||
#define CLOSED_LOOP_NAV ENABLED // set to ENABLED if closed loop navigation else set to DISABLED (Return To Lauch)
|
||||
#define AUTO_WP_RADIUS DISABLED
|
||||
|
||||
#define MAX_DIST 50 //300 // max distance (in m) for the HEADALT mode
|
||||
#define SARSEC_BRANCH 50 // Long branch of the SARSEC pattern
|
||||
/*
|
||||
During straight lines if the speed booster is enabled, after passing the Wp,
|
||||
the speed is multplied by a speed factor ROV_BOOSTER = 2
|
||||
(i.e. this is my tested value... so the required speed will be 2 x 4 = 8 m/s in straight lines),
|
||||
the when the rover approach the wp, it slow down to 4 m/s (TRIM_ARSPD_CM)...
|
||||
This feature works only if the ROV_AWPR_NAV is set to 0
|
||||
*/
|
||||
|
||||
#define BOOSTER 2 // booster factor x2
|
||||
#define AUTO_WP_RADIUS DISABLED
|
||||
#define AIRSPEED_CRUISE 4 // 4m/s
|
||||
#define THROTTLE_SLEW_LIMIT 2 // set to 2 for a smooth acceleration by 0.2 step
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
|
@ -66,7 +76,6 @@
|
|||
// is 10m/s, which is a conservative value suitable for relatively small,
|
||||
// light aircraft.
|
||||
//
|
||||
#define AIRSPEED_CRUISE 3
|
||||
#define GSBOOST 0
|
||||
#define NUDGE_OFFSET 0
|
||||
#define MIN_GNDSPEED 3
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#define THISFIRMWARE "APMrover v2.14 JL NAUDIN" //New version of the APMrover for the APM v1 or APM v2 and magnetometer + SONAR
|
||||
#define THISFIRMWARE "APMrover v2.16 JL NAUDIN" //New version of the APMrover for the APM v1 or APM v2 and magnetometer + SONAR
|
||||
|
||||
// This is a full version of Arduplane v2.32 specially adapted for a Rover by Jean-Louis Naudin (JLN)
|
||||
|
||||
|
|
@ -18,7 +18,7 @@ License as published by the Free Software Foundation; either
|
|||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
//
|
||||
// JLN updates: last update 2012-05-15
|
||||
// JLN updates: last update 2012-05-17
|
||||
//
|
||||
// DOLIST:
|
||||
//
|
||||
|
|
@ -26,6 +26,9 @@ version 2.1 of the License, or (at your option) any later version.
|
|||
//-------------------------------------------------------------------------------------------------------------------------
|
||||
// Dev Startup : 2012-04-21
|
||||
//
|
||||
// 2012-05-17: added speed_boost during straight line
|
||||
// 2012-05-17: New update about the throttle rate control based on the field test done by Franco Borasio (Thanks Franco..)
|
||||
// 2012-05-15: The Throttle rate can be controlled by the THROTTLE_SLEW_LIMIT (the value give the step increase, 1 = 0.1)
|
||||
// 2012-05-14: Update about mavlink library (now compatible with the latest version of mavlink)
|
||||
// 2012-05-14: Added option (hold roll to full right + SW7 ON/OFF) to init_home during the wp_list reset
|
||||
// 2012-05-13: Add ROV_SONAR_TRIG (default = 200 cm)
|
||||
|
|
@ -505,6 +508,7 @@ static int16_t sonar_dist;
|
|||
// The amount current ground speed is below min ground speed. Centimeters per second
|
||||
static long groundspeed_undershoot = 0;
|
||||
static long ground_speed = 0;
|
||||
static int throttle_last = 0, throttle = 500;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Location Errors
|
||||
|
|
@ -583,6 +587,7 @@ static bool final = false;
|
|||
// JLN Update
|
||||
unsigned long timesw = 0;
|
||||
static long ground_course = 0; // deg * 100 dir of plane
|
||||
static bool speed_boost = false;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Loiter management
|
||||
|
|
@ -1023,9 +1028,7 @@ static void slow_loop()
|
|||
|
||||
#if TRACE == ENABLED
|
||||
Serial.printf_P(PSTR("NAV->gnd_crs=%3.0f, nav_brg=%3.0f, tgt_brg=%3.0f, brg_err=%3.0f, nav_rll=%3.1f rsvo=%3.1f\n"),
|
||||
(float)ground_course/100, (float)nav_bearing/100, (float)target_bearing/100, (float)bearing_error/100, (float)nav_roll/100, (float)g.channel_roll.servo_out/100);
|
||||
// Serial.printf_P(PSTR("WPL->g.command_total=%d, g.command_index=%d, nav_command_index=%d\n"),
|
||||
// g.command_total, g.command_index, nav_command_index);
|
||||
(float)ground_course/100, (float)nav_bearing/100, (float)target_bearing/100, (float)bearing_error/100, (float)nav_roll/100, (float)g.channel_roll.servo_out/100);
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
|
|
@ -1165,7 +1168,6 @@ static void update_current_flight_mode(void)
|
|||
g.channel_pitch.servo_out = g.channel_pitch.pwm_to_angle();
|
||||
g.channel_rudder.servo_out = g.channel_roll.pwm_to_angle();
|
||||
break;
|
||||
//roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000
|
||||
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -25,14 +25,33 @@ static void crash_checker()
|
|||
}
|
||||
|
||||
static void calc_throttle()
|
||||
{
|
||||
int throttle_target = g.throttle_cruise + throttle_nudge + 50;
|
||||
|
||||
// Normal airspeed target
|
||||
target_airspeed = g.airspeed_cruise;
|
||||
groundspeed_error = target_airspeed - (float)ground_speed;
|
||||
g.channel_throttle.servo_out = throttle_target + g.pidTeThrottle.get_pid(groundspeed_error, dTnav);
|
||||
g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out, g.throttle_min.get(), g.throttle_max.get());
|
||||
{ int rov_speed;
|
||||
|
||||
int throttle_target = g.throttle_cruise + throttle_nudge + 50;
|
||||
|
||||
target_airspeed = g.airspeed_cruise;
|
||||
|
||||
if(speed_boost)
|
||||
rov_speed = g.booster * target_airspeed;
|
||||
else
|
||||
rov_speed = target_airspeed;
|
||||
|
||||
groundspeed_error = rov_speed - (float)ground_speed;
|
||||
|
||||
int throttle_req = (throttle_target + g.pidTeThrottle.get_pid(groundspeed_error, dTnav)) * 10;
|
||||
|
||||
if(g.throttle_slewrate > 0)
|
||||
{ if (throttle_req > throttle_last)
|
||||
throttle = throttle + g.throttle_slewrate;
|
||||
else if (throttle_req < throttle_last) {
|
||||
throttle = throttle - g.throttle_slewrate;
|
||||
}
|
||||
throttle = constrain(throttle, 500, throttle_req);
|
||||
throttle_last = throttle;
|
||||
} else {
|
||||
throttle = throttle_req;
|
||||
}
|
||||
g.channel_throttle.servo_out = constrain(((float)throttle / 10.0f), 0, g.throttle_max.get());
|
||||
}
|
||||
|
||||
/*****************************************
|
||||
|
|
@ -74,6 +93,7 @@ float roll_slew_limit(float servo)
|
|||
*****************************************/
|
||||
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
|
||||
|
||||
|
|
@ -81,9 +101,9 @@ static void throttle_slew_limit()
|
|||
g.channel_throttle.radio_out = constrain(g.channel_throttle.radio_out, last - (int)temp, last + (int)temp);
|
||||
last = g.channel_throttle.radio_out;
|
||||
}
|
||||
*/
|
||||
}
|
||||
|
||||
|
||||
// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
|
||||
// Keeps outdated data out of our calculations
|
||||
static void reset_I(void)
|
||||
|
|
@ -125,19 +145,12 @@ static void set_servos(void)
|
|||
g.channel_rudder.calc_pwm();
|
||||
|
||||
g.channel_throttle.radio_out = g.channel_throttle.radio_in;
|
||||
|
||||
// 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());
|
||||
|
||||
// g.channel_throttle.calc_pwm();
|
||||
|
||||
/* TO DO - fix this for RC_Channel library
|
||||
#if THROTTLE_REVERSE == 1
|
||||
radio_out[CH_THROTTLE] = radio_max(CH_THROTTLE) + radio_min(CH_THROTTLE) - radio_out[CH_THROTTLE];
|
||||
#endif
|
||||
*/
|
||||
}
|
||||
|
||||
if (control_mode >= FLY_BY_WIRE_B) {
|
||||
// convert 0 to 100% into PWM
|
||||
g.channel_throttle.calc_pwm();
|
||||
/* only do throttle slew limiting in modes where throttle
|
||||
control is automatic */
|
||||
|
|
|
|||
|
|
@ -133,6 +133,7 @@ public:
|
|||
k_param_auto_wp_radius,
|
||||
k_param_sonar_trigger,
|
||||
k_param_turn_gain,
|
||||
k_param_booster,
|
||||
|
||||
// ************************************************************
|
||||
//
|
||||
|
|
@ -351,7 +352,8 @@ public:
|
|||
AP_Int8 auto_wp_radius;
|
||||
AP_Int16 sonar_trigger;
|
||||
AP_Int16 turn_gain;
|
||||
|
||||
AP_Int8 booster;
|
||||
|
||||
// ************************************************************
|
||||
|
||||
// RC channels
|
||||
|
|
@ -482,7 +484,8 @@ public:
|
|||
closed_loop_nav (CLOSED_LOOP_NAV),
|
||||
auto_wp_radius (AUTO_WP_RADIUS),
|
||||
sonar_trigger (SONAR_TRIGGER),
|
||||
turn_gain (TURN_GAIN),
|
||||
turn_gain (TURN_GAIN),
|
||||
booster (BOOSTER),
|
||||
|
||||
// ************************************************************
|
||||
|
||||
|
|
|
|||
|
|
@ -124,6 +124,8 @@ static const AP_Param::Info var_info[] PROGMEM = {
|
|||
GSCALAR(auto_wp_radius, "ROV_AWPR_NAV"),
|
||||
GSCALAR(sonar_trigger, "ROV_SONAR_TRIG"),
|
||||
GSCALAR(turn_gain, "ROV_GAIN"),
|
||||
GSCALAR(booster, "ROV_BOOSTER"),
|
||||
|
||||
// ************************************************************
|
||||
|
||||
GGROUP(channel_roll, "RC1_", RC_Channel),
|
||||
|
|
|
|||
|
|
@ -351,7 +351,9 @@ static void set_mode(byte mode)
|
|||
|
||||
control_mode = mode;
|
||||
crash_timer = 0;
|
||||
|
||||
throttle_last = 0;
|
||||
throttle = 500;
|
||||
|
||||
switch(control_mode)
|
||||
{
|
||||
case MANUAL:
|
||||
|
|
|
|||
Loading…
Reference in New Issue