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

Copter: Guided Angle: Initialize yaw to current yaw.

This commit is contained in:
Leonard Hall 2024-03-17 11:15:42 +10:30 committed by Randy Mackay
parent 785c3a1e26
commit da6e4b8813
1 changed files with 2 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduCopter/mode_guided.cpp
View File

@ -313,14 +313,11 @@ void ModeGuided::angle_control_start()
// initialise targets // initialise targets
guided_angle_state.update_time_ms = millis(); guided_angle_state.update_time_ms = millis();
guided_angle_state.attitude_quat.initialise(); guided_angle_state.attitude_quat.from_euler(Vector3f(0.0, 0.0, attitude_control->get_att_target_euler_rad().z));
guided_angle_state.ang_vel.zero(); guided_angle_state.ang_vel.zero();
guided_angle_state.climb_rate_cms = 0.0f; guided_angle_state.climb_rate_cms = 0.0f;
guided_angle_state.yaw_rate_cds = 0.0f; guided_angle_state.yaw_rate_cds = 0.0f;
guided_angle_state.use_yaw_rate = false; guided_angle_state.use_yaw_rate = false;
// pilot always controls yaw
auto_yaw.set_mode(AutoYaw::Mode::HOLD);
} }
// set_destination - sets guided mode's target destination // set_destination - sets guided mode's target destination
@ -939,7 +936,7 @@ void ModeGuided::angle_control_run()
// check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds // check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds
uint32_t tnow = millis(); uint32_t tnow = millis();
if (tnow - guided_angle_state.update_time_ms > get_timeout_ms()) { if (tnow - guided_angle_state.update_time_ms > get_timeout_ms()) {
guided_angle_state.attitude_quat.initialise(); guided_angle_state.attitude_quat.from_euler(Vector3f(0.0, 0.0, attitude_control->get_att_target_euler_rad().z));
guided_angle_state.ang_vel.zero(); guided_angle_state.ang_vel.zero();
climb_rate_cms = 0.0f; climb_rate_cms = 0.0f;
if (guided_angle_state.use_thrust) { if (guided_angle_state.use_thrust) {