From d000cd232010a80b258f4bb90da3843c697a95d2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Sep 2013 10:18:31 +1000 Subject: [PATCH] Rover: switched to new steering controller this uses a steering rate controller, based upon the planes roll controller --- APMrover2/APMrover2.pde | 11 +++------ APMrover2/GCS_Mavlink.pde | 2 +- APMrover2/Parameters.h | 11 +++------ APMrover2/Parameters.pde | 21 +++------------- APMrover2/Steering.pde | 51 +-------------------------------------- APMrover2/setup.pde | 3 --- 6 files changed, 14 insertions(+), 85 deletions(-) diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index b4b00f2723..26e7dd6f88 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -91,6 +91,7 @@ #include // main loop scheduler #include #include +#include #include #include @@ -249,6 +250,9 @@ static AP_L1_Control L1_controller(ahrs); // selected navigation controller static AP_Navigation *nav_controller = &L1_controller; +// steering controller +static AP_SteerController steerController(ahrs); + #if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL SITL sitl; #endif @@ -541,10 +545,6 @@ static uint8_t delta_ms_fast_loop; // Counter of main loop executions. Used for performance monitoring and failsafe processing static uint16_t mainLoop_count; -static struct { - float last_saved_value; -} learning; - //////////////////////////////////////////////////////////////////////////////// // Top-level logic //////////////////////////////////////////////////////////////////////////////// @@ -868,9 +868,6 @@ static void update_current_mode(void) // and throttle gives speed in proportion to cruise speed throttle_nudge = 0; calc_throttle(channel_throttle->pwm_to_angle() * 0.01 * g.speed_cruise); - if (g.steering_learn) { - steering_learning(); - } break; } diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index 80a0f86070..d8d80c4b51 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -244,7 +244,7 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) nav_controller->nav_bearing_cd() * 0.01f, nav_controller->target_bearing_cd() * 0.01f, wp_distance, - g.turn_circle.get(), + 0, groundspeed_error, nav_controller->crosstrack_error()); } diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 2079b2d77c..b8fdb0c68a 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -55,7 +55,7 @@ public: // 130: Sensor parameters // k_param_compass_enabled = 130, - k_param_steering_learn, + k_param_steering_learn, // unused // 140: battery controls k_param_battery_monitoring = 140, @@ -75,7 +75,7 @@ public: k_param_ch7_option, k_param_auto_trigger_pin, k_param_auto_kickstart, - k_param_turn_circle, + k_param_turn_circle, // unused k_param_turn_max_g, // @@ -144,7 +144,7 @@ public: // // 240: PID Controllers k_param_pidNavSteer = 230, - k_param_pidServoSteer, + k_param_pidServoSteer, // unused k_param_pidSpeedThrottle, // high RC channels @@ -160,6 +160,7 @@ public: k_param_compass, k_param_rcmap, k_param_L1_controller, + k_param_steerController, // 254,255: reserved }; @@ -204,9 +205,7 @@ public: AP_Int8 ch7_option; AP_Int8 auto_trigger_pin; AP_Float auto_kickstart; - AP_Float turn_circle; AP_Float turn_max_g; - AP_Int8 steering_learn; // RC channels RC_Channel rc_1; @@ -270,7 +269,6 @@ public: // PID controllers // - PID pidServoSteer; PID pidSpeedThrottle; Parameters() : @@ -296,7 +294,6 @@ public: // PID controller initial P initial I initial D initial imax //----------------------------------------------------------------------------------- - 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 023c318db5..6a68205f5b 100644 --- a/APMrover2/Parameters.pde +++ b/APMrover2/Parameters.pde @@ -414,22 +414,6 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Standard GSCALAR(waypoint_radius, "WP_RADIUS", 2.0f), - // @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: STEERING_LEARN - // @DisplayName: Steering learn enable - // @Description: When this option is enabled in STEERING mode the APM will try to learn the right tuning parameters for steering mode automatically while you are driving - // @User: Standard - // @Values: 0:Disabled,1:Enabled - GSCALAR(steering_learn, "STEERING_LEARN", 0), - // @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 @@ -439,7 +423,10 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Standard GSCALAR(turn_max_g, "TURN_MAX_G", 2.0f), - GGROUP(pidServoSteer, "STEER2SRV_", PID), + // @Group: STEER2SRV_ + // @Path: ../libraries/APM_Control/AP_SteerController.cpp + GOBJECT(steerController, "STEER2SRV_", AP_SteerController), + GGROUP(pidSpeedThrottle, "SPEED2THR_", PID), // variables not in the g class which contain EEPROM saved variables diff --git a/APMrover2/Steering.pde b/APMrover2/Steering.pde index 1f6f1dfef7..3b73733494 100644 --- a/APMrover2/Steering.pde +++ b/APMrover2/Steering.pde @@ -146,7 +146,6 @@ static void calc_lateral_acceleration() static void calc_nav_steer() { float speed = g_gps->ground_speed_cm * 0.01f; - float steer; if (speed < 1.0f) { // gps speed isn't very accurate at low speed @@ -159,13 +158,7 @@ static void calc_nav_steer() // 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; + channel_steer->servo_out = steerController.get_steering_out(lateral_acceleration); } /***************************************** @@ -269,45 +262,3 @@ static void demo_servos(uint8_t i) { } } - -/* - learning of TURN_CIRCLE in STEERING mode - */ -static void steering_learning(void) -{ - /* - only do learning when we are moving at least at 2m/s, and do not - have saturated steering - */ - if (abs(channel_steer->servo_out) >= 4490 || - abs(channel_steer->servo_out) < 100 || - g_gps->status() < GPS::GPS_OK_FIX_3D || - g_gps->ground_speed_cm < 100) { - return; - } - /* - the idea is to slowly adjust the turning circle to bring the - actual and desired turn rates into line - */ - float demanded = lateral_acceleration; - /* - for Y accel use the gyro times the velocity, as that is less - noise sensitive, and is a more direct measure of steering rate, - which is what we are really trying to control - */ - float actual = ins.get_gyro().z * 0.01f * g_gps->ground_speed_cm; - if (fabsf(actual) < 0.1f) { - // too little acceleration to really measure accurately - return; - } - float ratio = demanded/actual; - if (ratio > 1.0f) { - g.turn_circle.set(g.turn_circle * 1.0005f); - } else { - g.turn_circle.set(g.turn_circle * 0.9995f); - } - if (fabs(learning.last_saved_value - g.turn_circle) > 0.05f) { - learning.last_saved_value = g.turn_circle; - g.turn_circle.save(); - } -} diff --git a/APMrover2/setup.pde b/APMrover2/setup.pde index 852bf6f931..a059653071 100644 --- a/APMrover2/setup.pde +++ b/APMrover2/setup.pde @@ -506,9 +506,6 @@ static void report_gains() cliSerial->printf_P(PSTR("Gains\n")); print_divider(); - cliSerial->printf_P(PSTR("servo steer:\n")); - print_PID(&g.pidServoSteer); - cliSerial->printf_P(PSTR("speed throttle:\n")); print_PID(&g.pidSpeedThrottle);