aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Rover: use AR_AttitudeControl for steering control

Browse Source
This commit is contained in:
Randy Mackay 2017-08-08 14:37:21 +09:00
parent c56d8cb6b1
commit 9097269d6d
10 changed files with 31 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
APMrover2/GCS_Mavlink.cpp
View File

@ -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),

4
APMrover2/Log.cpp
View File

@ -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());
}

11
APMrover2/Parameters.cpp
View File

@ -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);
}

5
APMrover2/Parameters.h
View File

@ -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[];

1
APMrover2/Rover.cpp
View File

@ -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&),

7
APMrover2/Rover.h
View File

@ -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;

9
APMrover2/mode.cpp
View File

@ -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);
}

2
APMrover2/mode.h
View File

@ -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)

7
APMrover2/mode_auto.cpp
View File

@ -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);

8
APMrover2/mode_guided.cpp
View File

@ -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());