From 8dc09440d8efc0e8f4c997a51ae603978c70e3cb Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Mon, 6 Mar 2017 21:29:03 -0500 Subject: [PATCH] Sub: Implement guided mode --- ArduSub/Sub.h | 1 + ArduSub/control_guided.cpp | 53 ++++++++++++++++++++++++++------------ ArduSub/motors.cpp | 17 +++++++++++- 3 files changed, 53 insertions(+), 18 deletions(-) diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 30749e78ed..acf8bc32a5 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -839,6 +839,7 @@ private: void translate_wpnav_rp(float &lateral_out, float &forward_out); void translate_circle_nav_rp(float &lateral_out, float &forward_out); + void translate_pos_control_rp(float &lateral_out, float &forward_out); bool surface_init(bool ignore_flags); void surface_run(); diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp index 8d93b2e75f..37c52a62ba 100644 --- a/ArduSub/control_guided.cpp +++ b/ArduSub/control_guided.cpp @@ -36,17 +36,15 @@ struct Guided_Limit { // guided_init - initialise guided controller bool Sub::guided_init(bool ignore_checks) { - return false; // Not implemented - - // if (position_ok() || ignore_checks) { - // // initialise yaw - // set_auto_yaw_mode(get_default_auto_yaw_mode(false)); - // // start in position control mode - // guided_pos_control_start(); - // return true; - // }else{ - // return false; - // } + if (position_ok() || ignore_checks) { + // initialise yaw + set_auto_yaw_mode(get_default_auto_yaw_mode(false)); + // start in position control mode + guided_pos_control_start(); + return true; + }else{ + return false; + } } // initialise guided mode's position controller @@ -304,16 +302,23 @@ void Sub::guided_pos_control_run() // run waypoint controller failsafe_terrain_set_status(wp_nav.update_wpnav()); + float lateral_out, forward_out; + translate_wpnav_rp(lateral_out, forward_out); + + // Send to forward/lateral outputs + motors.set_lateral(lateral_out); + motors.set_forward(forward_out); + // call z-axis position controller (wpnav should have already updated it's alt target) pos_control.update_z_controller(); // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate, get_smoothing_gain()); } else { // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(), true, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true, get_smoothing_gain()); } } @@ -354,13 +359,20 @@ void Sub::guided_vel_control_run() // call velocity controller which includes z axis controller pos_control.update_vel_controller_xyz(ekfNavVelGainScaler); + float lateral_out, forward_out; + translate_pos_control_rp(lateral_out, forward_out); + + // Send to forward/lateral outputs + motors.set_lateral(lateral_out); + motors.set_forward(forward_out); + // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(pos_control.get_roll(), pos_control.get_pitch(), target_yaw_rate, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate, get_smoothing_gain()); } else { // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control.input_euler_angle_roll_pitch_yaw(pos_control.get_roll(), pos_control.get_pitch(), get_auto_heading(), true, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true, get_smoothing_gain()); } } @@ -421,15 +433,22 @@ void Sub::guided_posvel_control_run() pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_AND_VEL_FF, ekfNavVelGainScaler, false); } + float lateral_out, forward_out; + translate_pos_control_rp(lateral_out, forward_out); + + // Send to forward/lateral outputs + motors.set_lateral(lateral_out); + motors.set_forward(forward_out); + pos_control.update_z_controller(); // call attitude controller if (auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(pos_control.get_roll(), pos_control.get_pitch(), target_yaw_rate, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_yaw_rate, get_smoothing_gain()); } else { // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control.input_euler_angle_roll_pitch_yaw(pos_control.get_roll(), pos_control.get_pitch(), get_auto_heading(), true, get_smoothing_gain()); + attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), get_auto_heading(), true, get_smoothing_gain()); } } diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp index b6cc94fc94..f7b1137320 100644 --- a/ArduSub/motors.cpp +++ b/ArduSub/motors.cpp @@ -229,7 +229,22 @@ void Sub::translate_circle_nav_rp(float &lateral_out, float &forward_out) int32_t forward = -circle_nav.get_pitch(); // output is reversed // constrain target forward/lateral values - // The outputs of wp_nav.get_roll and get_pitch should already be constrained to these values + lateral = constrain_int16(lateral, -aparm.angle_max, aparm.angle_max); + forward = constrain_int16(forward, -aparm.angle_max, aparm.angle_max); + + // Normalize + lateral_out = (float)lateral/(float)aparm.angle_max; + forward_out = (float)forward/(float)aparm.angle_max; +} + +// translate pos_control roll/pitch outputs to lateral/forward +void Sub::translate_pos_control_rp(float &lateral_out, float &forward_out) +{ + // get roll and pitch targets in centidegrees + int32_t lateral = pos_control.get_roll(); + int32_t forward = -pos_control.get_pitch(); // output is reversed + + // constrain target forward/lateral values lateral = constrain_int16(lateral, -aparm.angle_max, aparm.angle_max); forward = constrain_int16(forward, -aparm.angle_max, aparm.angle_max);