Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-06 19:00:27 +08:00
Code Issues Packages Projects Releases Wiki Activity

Sub: use get_yaw_rad to get heading

Browse Source
This commit is contained in:
Peter Barker
2026-01-19 12:27:27 -03:00
committed by Willian Galvani
parent 56005f8ca8
commit 375cef5813
5 changed files with 17 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ArduSub/Sub.h
+1 -1
View File
@@ -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;
ArduSub/mode_althold.cpp
+5 -5
View File
@@ -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);
}
}
}
ArduSub/mode_poshold.cpp
+5 -5
View File
@@ -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);
}
}
ArduSub/mode_stabilize.cpp
+5 -5
View File
@@ -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);
}
}
ArduSub/system.cpp
+1 -1
View File
@@ -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