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

Sub: Implement guided mode

Browse Source
This commit is contained in:
Jacob Walser 2017-03-06 21:29:03 -05:00
parent c3b047cc5d
commit 8dc09440d8
3 changed files with 53 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduSub/Sub.h
View File

@ -839,6 +839,7 @@ private:
void translate_wpnav_rp(float &lateral_out, float &forward_out); void translate_wpnav_rp(float &lateral_out, float &forward_out);
void translate_circle_nav_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); bool surface_init(bool ignore_flags);
void surface_run(); void surface_run();

53
ArduSub/control_guided.cpp
View File

@ -36,17 +36,15 @@ struct Guided_Limit {
// guided_init - initialise guided controller // guided_init - initialise guided controller
bool Sub::guided_init(bool ignore_checks) bool Sub::guided_init(bool ignore_checks)
{ {
return false; // Not implemented if (position_ok() || ignore_checks) {
// initialise yaw
// if (position_ok() || ignore_checks) { set_auto_yaw_mode(get_default_auto_yaw_mode(false));
// // initialise yaw // start in position control mode
// set_auto_yaw_mode(get_default_auto_yaw_mode(false)); guided_pos_control_start();
// // start in position control mode return true;
// guided_pos_control_start(); }else{
// return true; return false;
// }else{ }
// return false;
// }
} }
// initialise guided mode's position controller // initialise guided mode's position controller
@ -304,16 +302,23 @@ void Sub::guided_pos_control_run()
// run waypoint controller // run waypoint controller
failsafe_terrain_set_status(wp_nav.update_wpnav()); 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) // call z-axis position controller (wpnav should have already updated it's alt target)
pos_control.update_z_controller(); pos_control.update_z_controller();
// call attitude controller // call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) { if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot // 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 { } else {
// roll, pitch from waypoint controller, yaw heading from auto_heading() // 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 // call velocity controller which includes z axis controller
pos_control.update_vel_controller_xyz(ekfNavVelGainScaler); 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 // call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) { if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot // 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 { } else {
// roll, pitch from waypoint controller, yaw heading from auto_heading() // 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); 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(); pos_control.update_z_controller();
// call attitude controller // call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) { if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot // 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 { } else {
// roll, pitch from waypoint controller, yaw heading from auto_heading() // 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());
} }
} }

17
ArduSub/motors.cpp
View File

@ -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 int32_t forward = -circle_nav.get_pitch(); // output is reversed
// constrain target forward/lateral values // 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); lateral = constrain_int16(lateral, -aparm.angle_max, aparm.angle_max);
forward = constrain_int16(forward, -aparm.angle_max, aparm.angle_max); forward = constrain_int16(forward, -aparm.angle_max, aparm.angle_max);