Rover: use AR_AttitudeControl for steering control

APMrover2/GCS_Mavlink.cpp

@@ -229,7 +229,7 @@ void Rover::send_pid_tuning(mavlink_channel_t chan)
     const Vector3f &gyro = ahrs.get_gyro();
     const DataFlash_Class::PID_Info *pid_info;
     if (g.gcs_pid_mask & 1) {
-        pid_info = &steerController.get_pid_info();
+        pid_info = &g2.attitude_control.get_steering_rate_pid().get_pid_info();
         mavlink_msg_pid_tuning_send(chan, PID_TUNING_STEER,
                                     pid_info->desired,
                                     degrees(gyro.z),

APMrover2/Log.cpp

@@ -153,8 +153,8 @@ void Rover::Log_Write_Attitude()
 #endif
     DataFlash.Log_Write_POS(ahrs);
 
-    DataFlash.Log_Write_PID(LOG_PIDS_MSG, steerController.get_pid_info());
-
+    // log steering rate controller
+    DataFlash.Log_Write_PID(LOG_PIDS_MSG, g2.attitude_control.get_steering_rate_pid().get_pid_info());
     DataFlash.Log_Write_PID(LOG_PIDA_MSG, g.pidSpeedThrottle.get_pid_info());
 }
 

APMrover2/Parameters.cpp

@@ -341,10 +341,6 @@ const AP_Param::Info Rover::var_info[] = {
     // @User: Standard
     GSCALAR(turn_max_g,             "TURN_MAX_G",      2.0f),
 
-    // @Group: STEER2SRV_
-    // @Path: ../libraries/APM_Control/AP_SteerController.cpp
-    GOBJECT(steerController,        "STEER2SRV_",   AP_SteerController),
-
     // @Group: SPEED2THR_
     // @Path: ../libraries/PID/PID.cpp
     GGROUP(pidSpeedThrottle,        "SPEED2THR_", PID),
@@ -532,6 +528,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Path: ../libraries/AP_WheelEncoder/AP_WheelEncoder.cpp
     AP_SUBGROUPINFO(wheel_encoder, "WENC", 9, ParametersG2, AP_WheelEncoder),
 
+    // @Group: ATC
+    // @Path: ../libraries/APM_Control/AR_AttitudeControl.cpp
+    AP_SUBGROUPINFO(attitude_control, "ATC", 10, ParametersG2, AR_AttitudeControl),
+
     AP_GROUPEND
 };
 
@@ -542,7 +542,8 @@ ParametersG2::ParametersG2(void)
     afs(rover.mission, rover.barometer, rover.gps, rover.rcmap),
 #endif
     beacon(rover.serial_manager),
-    motors(rover.ServoRelayEvents)
+    motors(rover.ServoRelayEvents),
+    attitude_control(rover.ahrs)
 {
     AP_Param::setup_object_defaults(this, var_info);
 }

APMrover2/Parameters.h

@@ -195,7 +195,7 @@ public:
         k_param_compass,
         k_param_rcmap,
         k_param_L1_controller,
-        k_param_steerController,
+        k_param_steerController_old,    // unused
         k_param_barometer,
         k_param_notify,
         k_param_button,
@@ -323,6 +323,9 @@ public:
 
     // wheel encoders
     AP_WheelEncoder wheel_encoder;
+
+    // steering and throttle controller
+    AR_AttitudeControl attitude_control;
 };
 
 extern const AP_Param::Info var_info[];

APMrover2/Rover.cpp

@@ -28,7 +28,6 @@ Rover::Rover(void) :
     modes(&g.mode1),
     L1_controller(ahrs, nullptr),
     nav_controller(&L1_controller),
-    steerController(ahrs),
     mission(ahrs,
             FUNCTOR_BIND_MEMBER(&Rover::start_command, bool, const AP_Mission::Mission_Command&),
             FUNCTOR_BIND_MEMBER(&Rover::verify_command_callback, bool, const AP_Mission::Mission_Command&),

APMrover2/Rover.h

@@ -39,6 +39,8 @@
 #include <AP_Rally/AP_Rally.h>
 #include <AP_Terrain/AP_Terrain.h>
 #include <PID/PID.h>                                // PID library
+#include <AC_PID/AC_P.h>
+#include <AC_PID/AC_PID.h>
 #include <RC_Channel/RC_Channel.h>                  // RC Channel Library
 #include <AP_RangeFinder/AP_RangeFinder.h>          // Range finder library
 #include <Filter/Filter.h>                          // Filter library
@@ -57,8 +59,8 @@
 #include <AP_RCMapper/AP_RCMapper.h>                // RC input mapping library
 #include <AP_Scheduler/AP_Scheduler.h>              // main loop scheduler
 #include <AP_Navigation/AP_Navigation.h>
-#include <APM_Control/APM_Control.h>
 #include <AP_L1_Control/AP_L1_Control.h>
+#include <APM_Control/AR_AttitudeControl.h>
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_BoardConfig/AP_BoardConfig_CAN.h>
 #include <AP_Frsky_Telem/AP_Frsky_Telem.h>
@@ -180,9 +182,6 @@ private:
     // selected navigation controller
     AP_Navigation *nav_controller;
 
-    // steering controller
-    AP_SteerController steerController;
-
     // Mission library
     AP_Mission mission;
 

APMrover2/mode.cpp

@@ -7,7 +7,8 @@ Mode::Mode() :
     g2(rover.g2),
     channel_steer(rover.channel_steer),
     channel_throttle(rover.channel_throttle),
-    mission(rover.mission)
+    mission(rover.mission),
+    attitude_control(rover.g2.attitude_control)
 { }
 
 void Mode::exit()
@@ -177,9 +178,7 @@ void Mode::calc_nav_steer(bool reversed)
     // constrain to max G force
     lateral_acceleration = constrain_float(lateral_acceleration, -g.turn_max_g * GRAVITY_MSS, g.turn_max_g * GRAVITY_MSS);
 
-    // set controller reversal
-    rover.steerController.set_reverse(reversed);
-
     // send final steering command to motor library
-    g2.motors.set_steering(rover.steerController.get_steering_out_lat_accel(lateral_acceleration));
+    float steering_out = attitude_control.get_steering_out_lat_accel(lateral_acceleration, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right);
+    g2.motors.set_steering(steering_out * 4500.0f);
 }

APMrover2/mode.h

@@ -109,6 +109,8 @@ protected:
     class RC_Channel *&channel_steer;
     class RC_Channel *&channel_throttle;
     class AP_Mission &mission;
+    class AR_AttitudeControl &attitude_control;
+
 
     // private members for waypoint navigation
     Location _origin;           // origin Location (vehicle will travel from the origin to the destination)

APMrover2/mode_auto.cpp

@@ -66,11 +66,12 @@ void ModeAuto::update()
         {
             if (!_reached_heading) {
                 // run steering and throttle controllers
-                const float yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
-                g2.motors.set_steering(rover.steerController.get_steering_out_angle_error(yaw_error_cd));
+                const float yaw_error = wrap_PI(radians((_desired_yaw_cd - ahrs.yaw_sensor) * 0.01f));
+                const float steering_out = attitude_control.get_steering_out_angle_error(yaw_error, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right);
+                g2.motors.set_steering(steering_out * 4500.0f);
                 calc_throttle(_desired_speed);
                 // check if we have reached target
-                _reached_heading = (fabsf(yaw_error_cd) < 500);
+                _reached_heading = (fabsf(yaw_error) < radians(5));
             } else {
                 g2.motors.set_throttle(g.throttle_min.get());
                 g2.motors.set_steering(0.0f);

APMrover2/mode_guided.cpp

@@ -52,8 +52,9 @@ void ModeGuided::update()
             }
             if (have_attitude_target) {
                 // run steering and throttle controllers
-                const float yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
-                g2.motors.set_steering(rover.steerController.get_steering_out_angle_error(yaw_error_cd));
+                const float yaw_error = wrap_PI(radians((_desired_yaw_cd - ahrs.yaw_sensor) * 0.01f));
+                const float steering_out = attitude_control.get_steering_out_angle_error(yaw_error, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right);
+                g2.motors.set_steering(steering_out * 4500.0f);
                 calc_throttle(_desired_speed);
             } else {
                 g2.motors.set_throttle(g.throttle_min.get());
@@ -71,7 +72,8 @@ void ModeGuided::update()
             }
             if (have_attitude_target) {
                 // run steering and throttle controllers
-                g2.motors.set_steering(rover.steerController.get_steering_out_rate(_desired_yaw_rate_cds / 100.0f));
+                float steering_out = attitude_control.get_steering_out_rate(radians(_desired_yaw_rate_cds / 100.0f), g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right);
+                g2.motors.set_steering(steering_out * 4500.0f);
                 calc_throttle(_desired_speed);
             } else {
                 g2.motors.set_throttle(g.throttle_min.get());