From 375cef5813bbd4ea99a67d9f046833e1c492f439 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 5 Jun 2025 23:22:22 +1000 Subject: [PATCH] Sub: use get_yaw_rad to get heading --- ArduSub/Sub.h | 2 +- ArduSub/mode_althold.cpp | 10 +++++----- ArduSub/mode_poshold.cpp | 10 +++++----- ArduSub/mode_stabilize.cpp | 10 +++++----- ArduSub/system.cpp | 2 +- 5 files changed, 17 insertions(+), 17 deletions(-) diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 0b09d8bb242..cdc8df618cc 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -391,7 +391,7 @@ private: // setup the var_info table AP_Param param_loader; - uint32_t last_pilot_heading; + float last_pilot_heading_rad; uint32_t last_pilot_yaw_input_ms; uint32_t fs_terrain_recover_start_ms; diff --git a/ArduSub/mode_althold.cpp b/ArduSub/mode_althold.cpp index 125c248b3b7..5484c54a1db 100644 --- a/ArduSub/mode_althold.cpp +++ b/ArduSub/mode_althold.cpp @@ -15,7 +15,7 @@ bool ModeAlthold::init(bool ignore_checks) { // initialise position and desired velocity position_control->D_init_controller(); - sub.last_pilot_heading = ahrs.yaw_sensor; + sub.last_pilot_heading_rad = ahrs.get_yaw_rad(); return true; } @@ -43,7 +43,7 @@ void ModeAlthold::run_pre() attitude_control->set_throttle_out(0.5,true,g.throttle_filt); attitude_control->relax_attitude_controllers(); position_control->D_relax_controller(motors.get_throttle_hover()); - sub.last_pilot_heading = ahrs.yaw_sensor; + sub.last_pilot_heading_rad = ahrs.get_yaw_rad(); return; } @@ -79,7 +79,7 @@ void ModeAlthold::run_pre() // call attitude controller if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll, target_pitch, target_yaw_rate); - sub.last_pilot_heading = ahrs.yaw_sensor; + sub.last_pilot_heading_rad = ahrs.get_yaw_rad(); sub.last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading } else { // hold current heading @@ -91,10 +91,10 @@ void ModeAlthold::run_pre() // call attitude controller with target yaw rate = 0 to decelerate on yaw axis attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll, target_pitch, target_yaw_rate); - sub.last_pilot_heading = ahrs.yaw_sensor; // update heading to hold + sub.last_pilot_heading_rad = ahrs.get_yaw_rad(); // update heading to hold } else { // call attitude controller holding absolute bearing - attitude_control->input_euler_angle_roll_pitch_yaw_cd(target_roll, target_pitch, sub.last_pilot_heading, true); + attitude_control->input_euler_angle_roll_pitch_yaw_cd(target_roll, target_pitch, rad_to_cd(sub.last_pilot_heading_rad), true); } } } diff --git a/ArduSub/mode_poshold.cpp b/ArduSub/mode_poshold.cpp index ba51e4d4bd7..794c953304b 100644 --- a/ArduSub/mode_poshold.cpp +++ b/ArduSub/mode_poshold.cpp @@ -30,7 +30,7 @@ bool ModePoshold::init(bool ignore_checks) attitude_control->relax_attitude_controllers(); position_control->D_relax_controller(0.5f); - sub.last_pilot_heading = ahrs.yaw_sensor; + sub.last_pilot_heading_rad = ahrs.get_yaw_rad(); return true; } @@ -48,7 +48,7 @@ void ModePoshold::run() attitude_control->relax_attitude_controllers(); position_control->NE_init_controller_stopping_point(); position_control->D_relax_controller(0.5f); - sub.last_pilot_heading = ahrs.yaw_sensor; + sub.last_pilot_heading_rad = ahrs.get_yaw_rad(); return; } @@ -70,7 +70,7 @@ void ModePoshold::run() // update attitude controller targets if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll, target_pitch, target_yaw_rate); - sub.last_pilot_heading = ahrs.yaw_sensor; + sub.last_pilot_heading_rad = ahrs.get_yaw_rad(); sub.last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading } else { // hold current heading @@ -82,10 +82,10 @@ void ModePoshold::run() // call attitude controller with target yaw rate = 0 to decelerate on yaw axis attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll, target_pitch, target_yaw_rate); - sub.last_pilot_heading = ahrs.yaw_sensor; // update heading to hold + sub.last_pilot_heading_rad = ahrs.get_yaw_rad(); // update heading to hold } else { // call attitude controller holding absolute bearing - attitude_control->input_euler_angle_roll_pitch_yaw_cd(target_roll, target_pitch, sub.last_pilot_heading, true); + attitude_control->input_euler_angle_roll_pitch_yaw_cd(target_roll, target_pitch, rad_to_cd(sub.last_pilot_heading_rad), true); } } diff --git a/ArduSub/mode_stabilize.cpp b/ArduSub/mode_stabilize.cpp index c5474f66d7b..5b95377baa4 100644 --- a/ArduSub/mode_stabilize.cpp +++ b/ArduSub/mode_stabilize.cpp @@ -4,7 +4,7 @@ bool ModeStabilize::init(bool ignore_checks) { // set target altitude to zero for reporting position_control->set_pos_desired_U_cm(0); - sub.last_pilot_heading = ahrs.yaw_sensor; + sub.last_pilot_heading_rad = ahrs.get_yaw_rad(); return true; return true; @@ -20,7 +20,7 @@ void ModeStabilize::run() motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); attitude_control->set_throttle_out(0,true,g.throttle_filt); attitude_control->relax_attitude_controllers(); - sub.last_pilot_heading = ahrs.yaw_sensor; + sub.last_pilot_heading_rad = ahrs.get_yaw_rad(); return; } @@ -40,7 +40,7 @@ void ModeStabilize::run() if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll, target_pitch, target_yaw_rate); - sub.last_pilot_heading = ahrs.yaw_sensor; + sub.last_pilot_heading_rad = ahrs.get_yaw_rad(); sub.last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading } else { // hold current heading @@ -52,10 +52,10 @@ void ModeStabilize::run() // call attitude controller with target yaw rate = 0 to decelerate on yaw axis attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll, target_pitch, target_yaw_rate); - sub.last_pilot_heading = ahrs.yaw_sensor; // update heading to hold + sub.last_pilot_heading_rad = ahrs.get_yaw_rad(); // update heading to hold } else { // call attitude controller holding absolute absolute bearing - attitude_control->input_euler_angle_roll_pitch_yaw_cd(target_roll, target_pitch, sub.last_pilot_heading, true); + attitude_control->input_euler_angle_roll_pitch_yaw_cd(target_roll, target_pitch, rad_to_cd(sub.last_pilot_heading_rad), true); } } diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp index 2417f93d864..7e439b71b91 100644 --- a/ArduSub/system.cpp +++ b/ArduSub/system.cpp @@ -130,7 +130,7 @@ void Sub::init_ardupilot() leak_detector.init(); - last_pilot_heading = ahrs.yaw_sensor; + last_pilot_heading_rad = ahrs.get_yaw_rad(); // initialise rangefinder #if AP_RANGEFINDER_ENABLED