From 97ed733adaf2c42eeb4ebc2d0afe9043260bc6c3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 17 Jun 2013 09:50:53 +1000 Subject: [PATCH] Rover: use L1 controller for navigation this uses the AP_L1_Control library for rover navigation --- APMrover2/APMrover2.pde | 49 ++++++++++-------------------- APMrover2/GCS_Mavlink.pde | 9 +++--- APMrover2/Log.pde | 14 ++++----- APMrover2/Parameters.h | 9 +++--- APMrover2/Parameters.pde | 40 ++++++++++++++----------- APMrover2/Steering.pde | 58 ++++++++++++++++++++++++++---------- APMrover2/commands.pde | 12 -------- APMrover2/commands_logic.pde | 6 ---- APMrover2/control_modes.pde | 4 +-- APMrover2/navigation.pde | 34 --------------------- APMrover2/setup.pde | 17 ----------- 11 files changed, 97 insertions(+), 155 deletions(-) diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index db893f1939..a881acd991 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -90,6 +90,8 @@ #include #include // main loop scheduler #include +#include +#include #include #include @@ -242,6 +244,11 @@ AP_InertialSensor_Oilpan ins( &adc ); AP_AHRS_DCM ahrs(&ins, g_gps); +static AP_L1_Control L1_controller(ahrs); + +// selected navigation controller +static AP_Navigation *nav_controller = &L1_controller; + #if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL SITL sitl; #endif @@ -372,17 +379,6 @@ const float radius_of_earth = 6378100; // meters // true if we have a position estimate from AHRS static bool have_position; -// This is the currently calculated direction to fly. -// deg * 100 : 0 to 360 -static int32_t nav_bearing; -// This is the direction to the next waypoint -// deg * 100 : 0 to 360 -static int32_t target_bearing; -//This is the direction from the last waypoint to the next waypoint -// deg * 100 : 0 to 360 -static int32_t crosstrack_bearing; -// A gain scaler to account for ground speed/headwind/tailwind -static float nav_gain_scaler = 1.0f; static bool rtl_complete = false; // There may be two active commands in Auto mode. @@ -427,15 +423,6 @@ static bool auto_triggered; static float ground_speed = 0; static int16_t throttle_last = 0, throttle = 500; -//////////////////////////////////////////////////////////////////////////////// -// Location Errors -//////////////////////////////////////////////////////////////////////////////// -// Difference between current bearing and desired bearing. in centi-degrees -static int32_t bearing_error_cd; - -// Distance perpandicular to the course line that we are off trackline. Meters -static float crosstrack_error; - //////////////////////////////////////////////////////////////////////////////// // CH7 control //////////////////////////////////////////////////////////////////////////////// @@ -462,8 +449,8 @@ static float current_total1; //////////////////////////////////////////////////////////////////////////////// // Navigation control variables //////////////////////////////////////////////////////////////////////////////// -// The instantaneous desired steering angle. Hundredths of a degree -static int32_t nav_steer_cd; +// The instantaneous desired lateral acceleration in m/s/s +static float lateral_acceleration; //////////////////////////////////////////////////////////////////////////////// // Waypoint distances @@ -663,9 +650,6 @@ static void fast_loop() read_sonars(); - // uses the yaw from the DCM to give more accurate turns - calc_bearing_error(); - if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) Log_Write_Attitude(); @@ -865,23 +849,22 @@ static void update_current_mode(void) case AUTO: case RTL: case GUIDED: + calc_lateral_acceleration(); calc_nav_steer(); calc_throttle(g.speed_cruise); break; - case STEERING: + case STEERING: { /* - in steering mode we control the bearing error, which gives - the same type of steering control as auto mode. The throttle - controls the target speed, in proportion to the throttle + in steering mode we control lateral acceleration directly */ - bearing_error_cd = channel_steer->pwm_to_angle(); + lateral_acceleration = g.turn_max_g * GRAVITY_MSS * (channel_steer->pwm_to_angle()/4500.0f); calc_nav_steer(); - /* we need to reset the I term or it will build up */ - g.pidNavSteer.reset_I(); + // and throttle gives speed in proportion to cruise speed calc_throttle(channel_throttle->pwm_to_angle() * 0.01 * g.speed_cruise); break; + } case LEARNING: case MANUAL: @@ -923,8 +906,8 @@ static void update_navigation() case RTL: case GUIDED: // no loitering around the wp with the rover, goes direct to the wp position + calc_lateral_acceleration(); calc_nav_steer(); - calc_bearing_error(); if (verify_RTL()) { channel_throttle->servo_out = g.throttle_min.get(); set_mode(HOLD); diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index 8f838ff970..a2aa37a5e7 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -237,17 +237,16 @@ static void NOINLINE send_location(mavlink_channel_t chan) static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) { - int16_t bearing = nav_bearing / 100; mavlink_msg_nav_controller_output_send( chan, - nav_steer_cd / 1.0e2, + lateral_acceleration, 0, - bearing, - target_bearing / 100, + nav_controller->nav_bearing_cd() * 0.01f, + nav_controller->target_bearing_cd() * 0.01f, wp_distance, 0, groundspeed_error, - crosstrack_error); + nav_controller->crosstrack_error()); } static void NOINLINE send_gps_raw(mavlink_channel_t chan) diff --git a/APMrover2/Log.pde b/APMrover2/Log.pde index 14cb16f9b8..cca16dd5d5 100644 --- a/APMrover2/Log.pde +++ b/APMrover2/Log.pde @@ -305,7 +305,6 @@ struct PACKED log_Nav_Tuning { float wp_distance; uint16_t target_bearing_cd; uint16_t nav_bearing_cd; - int16_t nav_gain_scalar; int8_t throttle; }; @@ -316,9 +315,8 @@ static void Log_Write_Nav_Tuning() LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG), yaw : (uint16_t)ahrs.yaw_sensor, wp_distance : wp_distance, - target_bearing_cd : (uint16_t)target_bearing, - nav_bearing_cd : (uint16_t)nav_bearing, - nav_gain_scalar : (int16_t)(nav_gain_scaler*1000), + target_bearing_cd : (uint16_t)nav_controller->target_bearing_cd(), + nav_bearing_cd : (uint16_t)nav_controller->nav_bearing_cd(), throttle : (int8_t)(100 * channel_throttle->norm_output()) }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); @@ -364,7 +362,7 @@ static void Log_Write_Mode() struct PACKED log_Sonar { LOG_PACKET_HEADER; - int16_t nav_steer; + float lateral_accel; uint16_t sonar1_distance; uint16_t sonar2_distance; uint16_t detected_count; @@ -383,7 +381,7 @@ static void Log_Write_Sonar() } struct log_Sonar pkt = { LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG), - nav_steer : (int16_t)nav_steer_cd, + lateral_accel : lateral_acceleration, sonar1_distance : (uint16_t)sonar.distance_cm(), sonar2_distance : (uint16_t)sonar2.distance_cm(), detected_count : obstacle.detected_count, @@ -466,9 +464,9 @@ static const struct LogStructure log_structure[] PROGMEM = { { LOG_CTUN_MSG, sizeof(log_Control_Tuning), "CTUN", "hcchf", "Steer,Roll,Pitch,ThrOut,AccY" }, { LOG_NTUN_MSG, sizeof(log_Nav_Tuning), - "NTUN", "HfHHhb", "Yaw,WpDist,TargBrg,NavBrg,NavGain,Thr" }, + "NTUN", "HfHHb", "Yaw,WpDist,TargBrg,NavBrg,Thr" }, { LOG_SONAR_MSG, sizeof(log_Sonar), - "SONR", "hHHHbHCb", "NavStr,S1Dist,S2Dist,DCnt,TAng,TTim,Spd,Thr" }, + "SONR", "fHHHbHCb", "LatAcc,S1Dist,S2Dist,DCnt,TAng,TTim,Spd,Thr" }, { LOG_CURRENT_MSG, sizeof(log_Current), "CURR", "hhhHf", "Thr,Volt,Curr,Vcc,CurrTot" }, { LOG_MODE_MSG, sizeof(log_Mode), diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 461084e6a9..c6b8d5c921 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -74,6 +74,8 @@ public: k_param_ch7_option, k_param_auto_trigger_pin, k_param_auto_kickstart, + k_param_turn_circle, + k_param_turn_max_g, // // 160: Radio settings @@ -156,6 +158,7 @@ public: k_param_ins, k_param_compass, k_param_rcmap, + k_param_L1_controller, // 254,255: reserved }; @@ -194,14 +197,14 @@ public: // navigation parameters // - AP_Float crosstrack_gain; - AP_Int16 crosstrack_entry_angle; AP_Float speed_cruise; AP_Int8 speed_turn_gain; AP_Float speed_turn_dist; AP_Int8 ch7_option; AP_Int8 auto_trigger_pin; AP_Float auto_kickstart; + AP_Float turn_circle; + AP_Float turn_max_g; // RC channels RC_Channel rc_1; @@ -265,7 +268,6 @@ public: // PID controllers // - PID pidNavSteer; PID pidServoSteer; PID pidSpeedThrottle; @@ -292,7 +294,6 @@ public: // PID controller initial P initial I initial D initial imax //----------------------------------------------------------------------------------- - pidNavSteer (0.7, 0.1, 0.2, 2000), pidServoSteer (0.5, 0.1, 0.2, 2000), pidSpeedThrottle (0.7, 0.2, 0.2, 4000) {} diff --git a/APMrover2/Parameters.pde b/APMrover2/Parameters.pde index 320c5f15f6..816f32a266 100644 --- a/APMrover2/Parameters.pde +++ b/APMrover2/Parameters.pde @@ -123,23 +123,6 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Standard GSCALAR(pack_capacity, "BATT_CAPACITY", HIGH_DISCHARGE), - // @Param: XTRK_GAIN_SC - // @DisplayName: Crosstrack Gain - // @Description: This controls how hard the Rover tries to follow the lines between waypoints, as opposed to driving directly to the next waypoint. The value is the scale between distance off the line and angle to meet the line (in Degrees * 100) - // @Range: 0 2000 - // @Increment: 1 - // @User: Standard - GSCALAR(crosstrack_gain, "XTRK_GAIN_SC", XTRACK_GAIN_SCALED), - - // @Param: XTRK_ANGLE_CD - // @DisplayName: Crosstrack Entry Angle - // @Description: Maximum angle used to correct for track following. - // @Units: centi-Degrees - // @Range: 0 9000 - // @Increment: 1 - // @User: Standard - GSCALAR(crosstrack_entry_angle, "XTRK_ANGLE_CD", XTRACK_ENTRY_ANGLE_CENTIDEGREE), - // @Param: AUTO_TRIGGER_PIN // @DisplayName: Auto mode trigger pin // @Description: pin number to use to enable the throttle in auto mode. If set to -1 then don't use a trigger, otherwise this is a pin number which if held low in auto mode will enable the motor to run. If the switch is released while in AUTO then the motor will stop again. This can be used in combination with INITIAL_MODE to give a 'press button to start' rover with no receiver. @@ -431,7 +414,24 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Standard GSCALAR(waypoint_radius, "WP_RADIUS", 2.0f), - GGROUP(pidNavSteer, "HDNG2STEER_", PID), + // @Param: TURN_CIRCLE + // @DisplayName: Turning circle + // @Description: The diameter of the turning circle in meters of the rover at low speed when wheels are turned to the maximum turn angle + // @Units: meters + // @Range: 0.5 50 + // @Increment: 0.1 + // @User: Standard + GSCALAR(turn_circle, "TURN_CIRCLE", 1.5f), + + // @Param: TURN_MAX_G + // @DisplayName: Turning maximum G force + // @Description: The maximum turning acceleration (in units of gravities) that the rover can handle while remaining stable. The navigation code will keep the lateral acceleration below this level to avoid rolling over or slipping the wheels in turns + // @Units: gravities + // @Range: 0.2 10 + // @Increment: 0.1 + // @User: Standard + GSCALAR(turn_max_g, "TURN_MAX_G", 2.0f), + GGROUP(pidServoSteer, "STEER2SRV_", PID), GGROUP(pidSpeedThrottle, "SPEED2THR_", PID), @@ -456,6 +456,10 @@ const AP_Param::Info var_info[] PROGMEM = { GOBJECT(gcs0, "SR0_", GCS_MAVLINK), GOBJECT(gcs3, "SR3_", GCS_MAVLINK), + // @Group: NAVL1_ + // @Path: ../libraries/AP_L1_Control/AP_L1_Control.cpp + GOBJECT(L1_controller, "NAVL1_", AP_L1_Control), + // @Group: SONAR_ // @Path: ../libraries/AP_RangeFinder/AP_RangeFinder_analog.cpp GOBJECT(sonar, "SONAR_", AP_RangeFinder_analog), diff --git a/APMrover2/Steering.pde b/APMrover2/Steering.pde index 6e5eeafc23..a97d57cc19 100644 --- a/APMrover2/Steering.pde +++ b/APMrover2/Steering.pde @@ -87,7 +87,7 @@ static void calc_throttle(float target_speed) reduce target speed in proportion to turning rate, up to the SPEED_TURN_GAIN percentage. */ - float steer_rate = fabsf((nav_steer_cd/nav_gain_scaler) / (float)SERVO_MAX); + float steer_rate = fabsf(lateral_acceleration / (g.turn_max_g*GRAVITY_MSS)); steer_rate = constrain_float(steer_rate, 0.0, 1.0); float reduction = 1.0 - steer_rate*(100 - g.speed_turn_gain)*0.01; @@ -117,27 +117,55 @@ static void calc_throttle(float target_speed) * Calculate desired turn angles (in medium freq loop) *****************************************/ -static void calc_nav_steer() +static void calc_lateral_acceleration() { - // Adjust gain based on ground speed - if (ground_speed < 0.01) { - nav_gain_scaler = 1.4f; - } else { - nav_gain_scaler = g.speed_cruise / ground_speed; - } - nav_gain_scaler = constrain_float(nav_gain_scaler, 0.2f, 1.4f); + switch (control_mode) { + case AUTO: + nav_controller->update_waypoint(prev_WP, next_WP); + break; - // Calculate the required turn of the wheels rover - // ---------------------------------------- + case RTL: + case GUIDED: + case STEERING: + nav_controller->update_waypoint(current_loc, next_WP); + break; + default: + return; + } + + // Calculate the required turn of the wheels // negative error = left turn // positive error = right turn - nav_steer_cd = g.pidNavSteer.get_pid_4500(bearing_error_cd, nav_gain_scaler); + lateral_acceleration = nav_controller->lateral_acceleration(); +} - // avoid obstacles, if any - nav_steer_cd += obstacle.turn_angle*100; +/* + calculate steering angle given lateral_acceleration + */ +static void calc_nav_steer() +{ + float speed = g_gps->ground_speed_cm * 0.01f; + float steer; - channel_steer->servo_out = nav_steer_cd; + if (speed < 1.0f) { + // gps speed isn't very accurate at low speed + speed = 1.0f; + } + + // add in obstacle avoidance + lateral_acceleration += (obstacle.turn_angle/45.0f) * g.turn_max_g; + + // constrain to max G force + lateral_acceleration = constrain_float(lateral_acceleration, -g.turn_max_g*GRAVITY_MSS, g.turn_max_g*GRAVITY_MSS); + + // this is a linear approximation of the inverse steering + // equation for a rover. It returns steering as a proportion + // from -1 to 1 + steer = 0.5 * lateral_acceleration * g.turn_circle / (speed*speed); + steer = constrain_float(steer, -1, 1); + + channel_steer->servo_out = steer * 4500; } /***************************************** diff --git a/APMrover2/commands.pde b/APMrover2/commands.pde index 15fc7dd5d5..a1da28d295 100644 --- a/APMrover2/commands.pde +++ b/APMrover2/commands.pde @@ -123,12 +123,6 @@ static void set_next_WP(const struct Location *wp) // this is handy for the groundstation wp_totalDistance = get_distance(current_loc, next_WP); wp_distance = wp_totalDistance; - target_bearing = get_bearing_cd(current_loc, next_WP); - nav_bearing = target_bearing; - - // set a new crosstrack bearing - // ---------------------------- - reset_crosstrack(); } static void set_guided_WP(void) @@ -144,11 +138,6 @@ static void set_guided_WP(void) // this is handy for the groundstation wp_totalDistance = get_distance(current_loc, next_WP); wp_distance = wp_totalDistance; - target_bearing = get_bearing_cd(current_loc, next_WP); - - // set a new crosstrack bearing - // ---------------------------- - reset_crosstrack(); } // run this at setup on the ground @@ -185,7 +174,6 @@ void init_home() static void restart_nav() { - g.pidNavSteer.reset_I(); g.pidSpeedThrottle.reset_I(); prev_WP = current_loc; nav_command_ID = NO_COMMAND; diff --git a/APMrover2/commands_logic.pde b/APMrover2/commands_logic.pde index c9b9c3477f..d9a05668a1 100644 --- a/APMrover2/commands_logic.pde +++ b/APMrover2/commands_logic.pde @@ -6,10 +6,6 @@ static void handle_process_nav_cmd() { - // reset navigation integrators - // ------------------------- - g.pidNavSteer.reset_I(); - gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nav_command.id); switch(next_nav_command.id){ @@ -211,8 +207,6 @@ static bool verify_takeoff() static bool verify_nav_wp() { - update_crosstrack(); - if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) { gcs_send_text_fmt(PSTR("Reached Waypoint #%i dist %um"), (unsigned)nav_command_index, diff --git a/APMrover2/control_modes.pde b/APMrover2/control_modes.pde index 4d5b30cfd5..1f57286da9 100644 --- a/APMrover2/control_modes.pde +++ b/APMrover2/control_modes.pde @@ -24,9 +24,7 @@ static void read_control_switch() oldSwitchPosition = switchPosition; prev_WP = current_loc; - // reset navigation and speed integrators - // ------------------------- - g.pidNavSteer.reset_I(); + // reset speed integrator g.pidSpeedThrottle.reset_I(); } diff --git a/APMrover2/navigation.pde b/APMrover2/navigation.pde index 3b11014f6b..c3ab4b608e 100644 --- a/APMrover2/navigation.pde +++ b/APMrover2/navigation.pde @@ -24,46 +24,12 @@ static void navigate() return; } - // target_bearing is where we should be heading - // -------------------------------------------- - target_bearing = get_bearing_cd(current_loc, next_WP); - - // nav_bearing will includes xtrac correction - // ------------------------------------------ - nav_bearing = target_bearing; - // control mode specific updates to nav_bearing // -------------------------------------------- update_navigation(); } -static void calc_bearing_error() -{ - static butter10hz1_6 butter; - - bearing_error_cd = wrap_180_cd(nav_bearing - ahrs.yaw_sensor); - bearing_error_cd = butter.filter(bearing_error_cd); -} - -static void update_crosstrack(void) -{ - // Crosstrack Error - // ---------------- - - // If we are too far off or too close we don't do track following - if (abs(wrap_180_cd(target_bearing - crosstrack_bearing)) < 4500 && wp_distance >= 3.0f) { - crosstrack_error = sinf(radians((target_bearing - crosstrack_bearing) / (float)100)) * wp_distance; // Meters we are off track line - nav_bearing += constrain_float(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get()); - nav_bearing = wrap_360_cd(nav_bearing); - } -} - -static void reset_crosstrack() -{ - crosstrack_bearing = get_bearing_cd(prev_WP, next_WP); // Used for track following -} - void reached_waypoint() { diff --git a/APMrover2/setup.pde b/APMrover2/setup.pde index 4969936563..852bf6f931 100644 --- a/APMrover2/setup.pde +++ b/APMrover2/setup.pde @@ -109,7 +109,6 @@ setup_show(uint8_t argc, const Menu::arg *argv) report_radio(); report_batt_monitor(); report_gains(); - report_xtrack(); report_throttle(); report_modes(); report_compass(); @@ -510,28 +509,12 @@ static void report_gains() cliSerial->printf_P(PSTR("servo steer:\n")); print_PID(&g.pidServoSteer); - cliSerial->printf_P(PSTR("nav steer:\n")); - print_PID(&g.pidNavSteer); - cliSerial->printf_P(PSTR("speed throttle:\n")); print_PID(&g.pidSpeedThrottle); print_blanks(2); } -static void report_xtrack() -{ - //print_blanks(2); - cliSerial->printf_P(PSTR("Crosstrack\n")); - print_divider(); - // radio - cliSerial->printf_P(PSTR("XTRACK: %4.2f\n" - "XTRACK angle: %d\n"), - (float)g.crosstrack_gain, - (int)g.crosstrack_entry_angle); - print_blanks(2); -} - static void report_throttle() { //print_blanks(2);