Skip to content

Sub: use get_yaw_rad to get heading #31745

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
peterbarker wants to merge 1 commit into
base: master
Choose a base branch
from
+17 −17
Open

Sub: use get_yaw_rad to get heading #31745

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion ArduSub/Sub.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -391,7 +391,7 @@ class Sub : public AP_Vehicle {
// 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;

Expand Down
10 changes: 5 additions & 5 deletions ArduSub/mode_althold.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -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
Expand All @@ -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);
}
}
}
Expand Down
10 changes: 5 additions & 5 deletions ArduSub/mode_poshold.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand All @@ -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;
}

Expand All @@ -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
Expand All @@ -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);
}
}

Expand Down
10 changes: 5 additions & 5 deletions ArduSub/mode_stabilize.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
}

Expand All @@ -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
Expand All @@ -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);
}
}

Expand Down
2 changes: 1 addition & 1 deletion ArduSub/system.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Loading