diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index a061c8a934..26ba6da2dc 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/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), diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp index 75a2fb15a3..5584f9955e 100644 --- a/APMrover2/Log.cpp +++ b/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()); } diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index 82727d71de..d20d549698 100644 --- a/APMrover2/Parameters.cpp +++ b/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); } diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 67d67104d1..7fc90f7657 100644 --- a/APMrover2/Parameters.h +++ b/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[]; diff --git a/APMrover2/Rover.cpp b/APMrover2/Rover.cpp index 8bd66f2044..21685b29be 100644 --- a/APMrover2/Rover.cpp +++ b/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&), diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index eaf8afca2c..2457233a57 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -39,6 +39,8 @@ #include #include #include // PID library +#include +#include #include // RC Channel Library #include // Range finder library #include // Filter library @@ -57,8 +59,8 @@ #include // RC input mapping library #include // main loop scheduler #include -#include #include +#include #include #include #include @@ -180,9 +182,6 @@ private: // selected navigation controller AP_Navigation *nav_controller; - // steering controller - AP_SteerController steerController; - // Mission library AP_Mission mission; diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp index d1fb18e55a..cc9bed6f62 100644 --- a/APMrover2/mode.cpp +++ b/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); } diff --git a/APMrover2/mode.h b/APMrover2/mode.h index 4cfe843e1a..f145f6efd9 100644 --- a/APMrover2/mode.h +++ b/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) diff --git a/APMrover2/mode_auto.cpp b/APMrover2/mode_auto.cpp index 2542560d54..6d6eeaaf6b 100644 --- a/APMrover2/mode_auto.cpp +++ b/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); diff --git a/APMrover2/mode_guided.cpp b/APMrover2/mode_guided.cpp index a093123551..ab2ddc38ce 100644 --- a/APMrover2/mode_guided.cpp +++ b/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());