mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-30 20:48:33 -04:00
Plane: added GROUND_STEER_ALT and GROUND_STEER_DPS
this uses the new AP_SteerController steering controller developed for the rover code to allow for ground steering of planes
This commit is contained in:
Andrew Tridgell
parent
081510d37a
commit
d275f50539
@ -44,7 +44,6 @@
|
||||
#include <AP_ADC_AnalogSource.h>
|
||||
#include <AP_InertialSensor.h> // Inertial Sensor Library
|
||||
#include <AP_AHRS.h> // ArduPilot Mega DCM Library
|
||||
#include <PID.h> // PID library
|
||||
#include <RC_Channel.h> // RC Channel Library
|
||||
#include <AP_RangeFinder.h> // Range finder library
|
||||
#include <Filter.h> // Filter library
|
||||
@ -266,6 +265,7 @@ static AP_TECS TECS_controller(ahrs, aparm);
|
||||
static AP_RollController rollController(ahrs, aparm);
|
||||
static AP_PitchController pitchController(ahrs, aparm);
|
||||
static AP_YawController yawController(ahrs, aparm);
|
||||
static AP_SteerController steerController(ahrs);
|
||||
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
|
||||
@ -418,11 +418,6 @@ static bool have_position;
|
||||
// Location & Navigation
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
// Direction held during phases of takeoff and landing
|
||||
// deg * 100 dir of plane, A value of -1 indicates the course has not been set/is not in use
|
||||
// this is a 0..36000 value, or -1 for disabled
|
||||
static int32_t hold_course_cd = -1; // deg * 100 dir of plane
|
||||
|
||||
// There may be two active commands in Auto mode.
|
||||
// This indicates the active navigation command by index number
|
||||
static uint8_t nav_command_index;
|
||||
@ -492,6 +487,24 @@ static struct {
|
||||
uint32_t lock_timer_ms;
|
||||
} cruise_state;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// ground steering controller state
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
static struct {
|
||||
// Direction held during phases of takeoff and landing centidegrees
|
||||
// A value of -1 indicates the course has not been set/is not in use
|
||||
// this is a 0..36000 value, or -1 for disabled
|
||||
int32_t hold_course_cd;
|
||||
|
||||
// locked_course and locked_course_cd are used in stabilize mode
|
||||
// when ground steering is active
|
||||
bool locked_course;
|
||||
float locked_course_err;
|
||||
} steer_state = {
|
||||
hold_course_cd : -1,
|
||||
};
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// flight mode specific
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
@ -1053,7 +1066,7 @@ static void handle_auto_mode(void)
|
||||
{
|
||||
switch(nav_command_ID) {
|
||||
case MAV_CMD_NAV_TAKEOFF:
|
||||
if (hold_course_cd == -1) {
|
||||
if (steer_state.hold_course_cd == -1) {
|
||||
// we don't yet have a heading to hold - just level
|
||||
// the wings until we get up enough speed to get a GPS heading
|
||||
nav_roll_cd = 0;
|
||||
@ -1107,7 +1120,7 @@ static void handle_auto_mode(void)
|
||||
default:
|
||||
// we are doing normal AUTO flight, the special cases
|
||||
// are for takeoff and landing
|
||||
hold_course_cd = -1;
|
||||
steer_state.hold_course_cd = -1;
|
||||
land_complete = false;
|
||||
calc_nav_roll();
|
||||
calc_nav_pitch();
|
||||
@ -1128,7 +1141,7 @@ static void update_flight_mode(void)
|
||||
|
||||
if (effective_mode != AUTO) {
|
||||
// hold_course is only used in takeoff and landing
|
||||
hold_course_cd = -1;
|
||||
steer_state.hold_course_cd = -1;
|
||||
}
|
||||
|
||||
switch (effective_mode)
|
||||
|
||||
@ -190,26 +190,23 @@ static void stabilize_stick_mixing_fbw()
|
||||
|
||||
|
||||
/*
|
||||
stabilize the yaw axis
|
||||
stabilize the yaw axis. There are 3 modes of operation:
|
||||
|
||||
- hold a specific heading with ground steering
|
||||
- rate controlled with ground steering
|
||||
- yaw control for coordinated flight
|
||||
*/
|
||||
static void stabilize_yaw(float speed_scaler)
|
||||
{
|
||||
float ch4_inf = 1.0;
|
||||
bool ground_steering = fabsf(relative_altitude()) < g.ground_steer_alt;
|
||||
|
||||
if (stick_mixing_enabled()) {
|
||||
// stick mixing performed for rudder for all cases including FBW
|
||||
// important for steering on the ground during landing
|
||||
// -----------------------------------------------
|
||||
ch4_inf = (float)channel_rudder->radio_in - (float)channel_rudder->radio_trim;
|
||||
ch4_inf = fabsf(ch4_inf);
|
||||
ch4_inf = min(ch4_inf, 400.0);
|
||||
ch4_inf = ((400.0 - ch4_inf) /400.0);
|
||||
if (steer_state.hold_course_cd != -1 && ground_steering) {
|
||||
calc_nav_yaw_course();
|
||||
} else if (ground_steering) {
|
||||
calc_nav_yaw_ground();
|
||||
} else {
|
||||
calc_nav_yaw_coordinated(speed_scaler);
|
||||
}
|
||||
|
||||
// Apply output to Rudder
|
||||
calc_nav_yaw(speed_scaler, ch4_inf);
|
||||
channel_rudder->servo_out *= ch4_inf;
|
||||
channel_rudder->servo_out += channel_rudder->pwm_to_angle();
|
||||
}
|
||||
|
||||
|
||||
@ -374,19 +371,11 @@ static void calc_throttle()
|
||||
* Calculate desired roll/pitch/yaw angles (in medium freq loop)
|
||||
*****************************************/
|
||||
|
||||
// Yaw is separated into a function for heading hold on rolling take-off
|
||||
// ----------------------------------------------------------------------
|
||||
static void calc_nav_yaw(float speed_scaler, float ch4_inf)
|
||||
/*
|
||||
calculate yaw control for coordinated flight
|
||||
*/
|
||||
static void calc_nav_yaw_coordinated(float speed_scaler)
|
||||
{
|
||||
if (hold_course_cd != -1) {
|
||||
// steering on or close to ground
|
||||
int32_t bearing_error_cd = nav_controller->bearing_error_cd();
|
||||
channel_rudder->servo_out = g.pidWheelSteer.get_pid_4500(bearing_error_cd, speed_scaler) +
|
||||
g.kff_rudder_mix * channel_roll->servo_out;
|
||||
channel_rudder->servo_out = constrain_int16(channel_rudder->servo_out, -4500, 4500);
|
||||
return;
|
||||
}
|
||||
|
||||
bool disable_integrator = false;
|
||||
if (control_mode == STABILIZE && channel_rudder->control_in != 0) {
|
||||
disable_integrator = true;
|
||||
@ -398,6 +387,47 @@ static void calc_nav_yaw(float speed_scaler, float ch4_inf)
|
||||
channel_rudder->servo_out = constrain_int16(channel_rudder->servo_out, -4500, 4500);
|
||||
}
|
||||
|
||||
/*
|
||||
calculate yaw control for ground steering with specific course
|
||||
*/
|
||||
static void calc_nav_yaw_course(void)
|
||||
{
|
||||
// holding a specific navigation course on the ground. Used in
|
||||
// auto-takeoff and landing
|
||||
int32_t bearing_error_cd = nav_controller->bearing_error_cd();
|
||||
channel_rudder->servo_out = steerController.get_steering_out_angle_error(bearing_error_cd);
|
||||
if (stick_mixing_enabled()) {
|
||||
channel_rudder->servo_out += channel_rudder->pwm_to_angle();
|
||||
}
|
||||
channel_rudder->servo_out = constrain_int16(channel_rudder->servo_out, -4500, 4500);
|
||||
}
|
||||
|
||||
/*
|
||||
calculate yaw control for ground steering
|
||||
*/
|
||||
static void calc_nav_yaw_ground(void)
|
||||
{
|
||||
float steer_rate = (channel_rudder->control_in/4500.0f) * g.ground_steer_dps;
|
||||
if (steer_rate != 0) {
|
||||
// pilot is giving rudder input
|
||||
steer_state.locked_course = false;
|
||||
} else if (!steer_state.locked_course ||
|
||||
(g_gps->ground_speed_cm < 50 &&
|
||||
channel_throttle->control_in == 0)) {
|
||||
// pilot has released the rudder stick or we are still - lock the course
|
||||
steer_state.locked_course = true;
|
||||
steer_state.locked_course_err = 0;
|
||||
}
|
||||
if (!steer_state.locked_course) {
|
||||
channel_rudder->servo_out = steerController.get_steering_out_rate(steer_rate);
|
||||
} else {
|
||||
steer_state.locked_course_err += ahrs.get_gyro().z * 0.02f;
|
||||
int32_t yaw_error_cd = -ToDeg(steer_state.locked_course_err)*100;
|
||||
channel_rudder->servo_out = steerController.get_steering_out_angle_error(yaw_error_cd);
|
||||
}
|
||||
channel_rudder->servo_out = constrain_int16(channel_rudder->servo_out, -4500, 4500);
|
||||
}
|
||||
|
||||
|
||||
static void calc_nav_pitch()
|
||||
{
|
||||
@ -567,10 +597,10 @@ static bool suppress_throttle(void)
|
||||
if (control_mode==AUTO && takeoff_complete == false && auto_takeoff_check()) {
|
||||
// we're in auto takeoff
|
||||
throttle_suppressed = false;
|
||||
if (hold_course_cd != -1) {
|
||||
if (steer_state.hold_course_cd != -1) {
|
||||
// update takeoff course hold, if already initialised
|
||||
hold_course_cd = ahrs.yaw_sensor;
|
||||
gcs_send_text_fmt(PSTR("Holding course %ld"), hold_course_cd);
|
||||
steer_state.hold_course_cd = ahrs.yaw_sensor;
|
||||
gcs_send_text_fmt(PSTR("Holding course %ld"), steer_state.hold_course_cd);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
@ -94,6 +94,8 @@ public:
|
||||
k_param_skip_gyro_cal,
|
||||
k_param_auto_fbw_steer,
|
||||
k_param_waypoint_max_radius,
|
||||
k_param_ground_steer_alt,
|
||||
k_param_ground_steer_dps,
|
||||
|
||||
// 110: Telemetry control
|
||||
//
|
||||
@ -242,8 +244,8 @@ public:
|
||||
k_param_L1_controller,
|
||||
k_param_rcmap,
|
||||
k_param_TECS_controller,
|
||||
|
||||
k_param_rally_total,
|
||||
k_param_steerController,
|
||||
|
||||
//
|
||||
// 240: PID Controllers
|
||||
@ -254,7 +256,7 @@ public:
|
||||
k_param_pidServoRudder, // unused
|
||||
k_param_pidTeThrottle, // unused
|
||||
k_param_pidNavPitchAltitude, // unused
|
||||
k_param_pidWheelSteer,
|
||||
k_param_pidWheelSteer, // unused
|
||||
|
||||
// 254,255: reserved
|
||||
};
|
||||
@ -275,6 +277,8 @@ public:
|
||||
AP_Float kff_rudder_mix;
|
||||
AP_Float kff_pitch_to_throttle;
|
||||
AP_Float kff_throttle_to_pitch;
|
||||
AP_Float ground_steer_alt;
|
||||
AP_Int16 ground_steer_dps;
|
||||
|
||||
// speed used for speed scaling
|
||||
AP_Float scaling_speed;
|
||||
@ -411,9 +415,7 @@ public:
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
||||
RC_Channel_aux rc_12;
|
||||
#endif
|
||||
|
||||
// PID controllers
|
||||
PID pidWheelSteer;
|
||||
uint8_t _dummy;
|
||||
|
||||
Parameters() :
|
||||
// variable default
|
||||
@ -436,11 +438,7 @@ public:
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
||||
rc_12 (CH_12),
|
||||
#endif
|
||||
|
||||
// PID controller initial P initial I initial D initial imax
|
||||
//-----------------------------------------------------------------------------------
|
||||
pidWheelSteer (0, 0, 0, 0)
|
||||
|
||||
_dummy(0)
|
||||
{}
|
||||
};
|
||||
|
||||
|
||||
@ -525,6 +525,24 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||
// @User: Standard
|
||||
GSCALAR(acro_locking, "ACRO_LOCKING", 0),
|
||||
|
||||
// @Param: GROUND_STEER_ALT
|
||||
// @DisplayName: Ground steer altitude
|
||||
// @Description: Altitude at which to use the ground steering controller on the rudder. If non-zero then the STEER2SRV controller will be used to control the rudder for altitudes within this limit of the home altitude.
|
||||
// @Units: Meters
|
||||
// @Range: -100 100
|
||||
// @Increment: 0.1
|
||||
// @User: Standard
|
||||
GSCALAR(ground_steer_alt, "GROUND_STEER_ALT", 0),
|
||||
|
||||
// @Param: GROUND_STEER_DPS
|
||||
// @DisplayName: Ground steer rate
|
||||
// @Description: Ground steering rate in degrees per second for full rudder stick deflection
|
||||
// @Units: Meters
|
||||
// @Range: 10 360
|
||||
// @Increment: 1
|
||||
// @User: Advanced
|
||||
GSCALAR(ground_steer_dps, "GROUND_STEER_DPS", 90),
|
||||
|
||||
// @Param: TRIM_AUTO
|
||||
// @DisplayName: Automatic trim adjustment
|
||||
// @Description: Set RC trim PWM levels to current levels when switching away from manual mode. When this option is enabled and you change from MANUAL to any other mode then the APM will take the current position of the control sticks as the trim values for aileron, elevator and rudder. It will use those to set RC1_TRIM, RC2_TRIM and RC4_TRIM. This option is disabled by default as if a pilot is not aware of this option and changes from MANUAL to another mode while control inputs are not centered then the trim could be changed to a dangerously bad value. You can enable this option to assist with trimming your plane, by enabling it before takeoff then switching briefly to MANUAL in flight, and seeing how the plane reacts. You can then switch back to FBWA, trim the surfaces then again test MANUAL mode. Each time you switch from MANUAL the APM will take your control inputs as the new trim. After you have good trim on your aircraft you can disable TRIM_AUTO for future flights.
|
||||
@ -785,8 +803,6 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||
GGROUP(rc_12, "RC12_", RC_Channel_aux),
|
||||
#endif
|
||||
|
||||
GGROUP(pidWheelSteer, "WHEELSTEER_",PID),
|
||||
|
||||
// @Group: RLL2SRV_
|
||||
// @Path: ../libraries/APM_Control/AP_RollController.cpp
|
||||
GOBJECT(rollController, "RLL2SRV_", AP_RollController),
|
||||
@ -799,6 +815,10 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||
// @Path: ../libraries/APM_Control/AP_YawController.cpp
|
||||
GOBJECT(yawController, "YAW2SRV_", AP_YawController),
|
||||
|
||||
// @Group: STEER2SRV_
|
||||
// @Path: ../libraries/APM_Control/AP_SteerController.cpp
|
||||
GOBJECT(steerController, "STEER2SRV_", AP_SteerController),
|
||||
|
||||
// variables not in the g class which contain EEPROM saved variables
|
||||
|
||||
// @Group: COMPASS_
|
||||
|
||||
@ -317,23 +317,23 @@ static void do_loiter_time()
|
||||
static bool verify_takeoff()
|
||||
{
|
||||
if (ahrs.yaw_initialised()) {
|
||||
if (hold_course_cd == -1) {
|
||||
if (steer_state.hold_course_cd == -1) {
|
||||
// save our current course to take off
|
||||
hold_course_cd = ahrs.yaw_sensor;
|
||||
gcs_send_text_fmt(PSTR("Holding course %ld"), hold_course_cd);
|
||||
steer_state.hold_course_cd = ahrs.yaw_sensor;
|
||||
gcs_send_text_fmt(PSTR("Holding course %ld"), steer_state.hold_course_cd);
|
||||
}
|
||||
}
|
||||
|
||||
if (hold_course_cd != -1) {
|
||||
if (steer_state.hold_course_cd != -1) {
|
||||
// call navigation controller for heading hold
|
||||
nav_controller->update_heading_hold(hold_course_cd);
|
||||
nav_controller->update_heading_hold(steer_state.hold_course_cd);
|
||||
} else {
|
||||
nav_controller->update_level_flight();
|
||||
}
|
||||
|
||||
// see if we have reached takeoff altitude
|
||||
if (adjusted_altitude_cm() > takeoff_altitude_cm) {
|
||||
hold_course_cd = -1;
|
||||
steer_state.hold_course_cd = -1;
|
||||
takeoff_complete = true;
|
||||
next_WP = prev_WP = current_loc;
|
||||
return true;
|
||||
@ -356,18 +356,18 @@ static bool verify_land()
|
||||
|
||||
land_complete = true;
|
||||
|
||||
if (hold_course_cd == -1) {
|
||||
if (steer_state.hold_course_cd == -1) {
|
||||
// we have just reached the threshold of to flare for landing.
|
||||
// We now don't want to do any radical
|
||||
// turns, as rolling could put the wings into the runway.
|
||||
// To prevent further turns we set hold_course_cd to the
|
||||
// To prevent further turns we set steer_state.hold_course_cd to the
|
||||
// current heading. Previously we set this to
|
||||
// crosstrack_bearing, but the xtrack bearing can easily
|
||||
// be quite large at this point, and that could induce a
|
||||
// sudden large roll correction which is very nasty at
|
||||
// this point in the landing.
|
||||
hold_course_cd = ahrs.yaw_sensor;
|
||||
gcs_send_text_fmt(PSTR("Land Complete - Hold course %ld"), hold_course_cd);
|
||||
steer_state.hold_course_cd = ahrs.yaw_sensor;
|
||||
gcs_send_text_fmt(PSTR("Land Complete - Hold course %ld"), steer_state.hold_course_cd);
|
||||
}
|
||||
|
||||
if (g_gps->ground_speed_cm*0.01f < 3.0) {
|
||||
@ -381,9 +381,9 @@ static bool verify_land()
|
||||
}
|
||||
}
|
||||
|
||||
if (hold_course_cd != -1) {
|
||||
if (steer_state.hold_course_cd != -1) {
|
||||
// recalc bearing error with hold_course;
|
||||
nav_controller->update_heading_hold(hold_course_cd);
|
||||
nav_controller->update_heading_hold(steer_state.hold_course_cd);
|
||||
} else {
|
||||
nav_controller->update_waypoint(prev_WP, next_WP);
|
||||
}
|
||||
@ -392,7 +392,7 @@ static bool verify_land()
|
||||
|
||||
static bool verify_nav_wp()
|
||||
{
|
||||
hold_course_cd = -1;
|
||||
steer_state.hold_course_cd = -1;
|
||||
|
||||
nav_controller->update_waypoint(prev_WP, next_WP);
|
||||
|
||||
|
||||
@ -438,16 +438,6 @@ setup_compass(uint8_t argc, const Menu::arg *argv)
|
||||
// CLI reports
|
||||
/***************************************************************************/
|
||||
|
||||
static void report_batt_monitor()
|
||||
{
|
||||
//print_blanks(2);
|
||||
cliSerial->printf_P(PSTR("Batt Mointor\n"));
|
||||
print_divider();
|
||||
if(battery.monitoring() == AP_BATT_MONITOR_DISABLED) cliSerial->printf_P(PSTR("Batt monitoring disabled"));
|
||||
if(battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_ONLY) cliSerial->printf_P(PSTR("Monitoring batt volts"));
|
||||
if(battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_AND_CURRENT) cliSerial->printf_P(PSTR("Monitoring volts and current"));
|
||||
print_blanks(2);
|
||||
}
|
||||
static void report_radio()
|
||||
{
|
||||
//print_blanks(2);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user