Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: Implement NAV_CMD_LOITER_TURNS

This commit is contained in:
jaxxzer 2017-03-04 14:41:26 -05:00 committed by Randy Mackay
parent ca38a344f8
commit 1ec592a2c5
3 changed files with 26 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduSub/Sub.h
View File

@ -838,6 +838,7 @@ private:
void auto_terrain_recover_run(void);
void translate_wpnav_rp(float &lateral_out, float &forward_out);
void translate_circle_nav_rp(float &lateral_out, float &forward_out);
bool surface_init(bool ignore_flags);
void surface_run();

9
ArduSub/control_auto.cpp
View File

@ -371,11 +371,18 @@ void Sub::auto_circle_run()
// call circle controller
circle_nav.update();
float lateral_out, forward_out;
translate_circle_nav_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
pos_control.update_z_controller();
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true, get_smoothing_gain());
attitude_control.input_euler_angle_roll_pitch_yaw(channel_roll->get_control_in(), channel_pitch->get_control_in(), circle_nav.get_yaw(), true, get_smoothing_gain());
}
#if NAV_GUIDED == ENABLED

17
ArduSub/motors.cpp
View File

@ -220,3 +220,20 @@ void Sub::translate_wpnav_rp(float &lateral_out, float &forward_out)
lateral_out = (float)lateral/(float)aparm.angle_max;
forward_out = (float)forward/(float)aparm.angle_max;
}
// translate wpnav roll/pitch outputs to lateral/forward
void Sub::translate_circle_nav_rp(float &lateral_out, float &forward_out)
{
// get roll and pitch targets in centidegrees
int32_t lateral = circle_nav.get_roll();
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;
}