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

Copter: Auto Yaw variable names and comments

This commit is contained in:
Leonard Hall 2021-07-10 10:55:52 +09:30 committed by Randy Mackay
parent 703f09638f
commit 0ad493fdf7
2 changed files with 11 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
ArduCopter/autoyaw.cpp
View File

@ -97,18 +97,14 @@ void Mode::AutoYaw::set_mode(autopilot_yaw_mode yaw_mode)
// set_fixed_yaw - sets the yaw look at heading for auto mode // set_fixed_yaw - sets the yaw look at heading for auto mode
void Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_ds, int8_t direction, bool relative_angle) void Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_ds, int8_t direction, bool relative_angle)
{ {
fixed_last_update = millis(); _last_update_ms = millis();
_yaw_angle_cd = copter.attitude_control->get_att_target_euler_cd().z; _yaw_angle_cd = copter.attitude_control->get_att_target_euler_cd().z;
_yaw_rate_cds = 0.0; _yaw_rate_cds = 0.0;
// calculate final angle as relative to vehicle heading or absolute // calculate final angle as relative to vehicle heading or absolute
if (relative_angle) { if (relative_angle) {
if (direction == 0) { _fixed_yaw_offset_cd = angle_deg * 100.0 * (direction >= 0 ? 1.0 : -1.0);
_fixed_yaw_offset_cd = angle_deg;
} else {
_fixed_yaw_offset_cd = angle_deg * direction;
}
} else { } else {
// absolute angle // absolute angle
_fixed_yaw_offset_cd = wrap_180_cd(angle_deg * 100.0 - _yaw_angle_cd); _fixed_yaw_offset_cd = wrap_180_cd(angle_deg * 100.0 - _yaw_angle_cd);
@ -134,7 +130,7 @@ void Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_ds, int8_t di
// set_fixed_yaw - sets the yaw look at heading for auto mode // set_fixed_yaw - sets the yaw look at heading for auto mode
void Mode::AutoYaw::set_yaw_angle_rate(float yaw_angle_d, float yaw_rate_ds) void Mode::AutoYaw::set_yaw_angle_rate(float yaw_angle_d, float yaw_rate_ds)
{ {
fixed_last_update = millis(); _last_update_ms = millis();
_yaw_angle_cd = yaw_angle_d * 100.0; _yaw_angle_cd = yaw_angle_d * 100.0;
_yaw_rate_cds = yaw_rate_ds * 100.0; _yaw_rate_cds = yaw_rate_ds * 100.0;
@ -201,8 +197,9 @@ float Mode::AutoYaw::yaw()
case AUTO_YAW_FIXED: { case AUTO_YAW_FIXED: {
// keep heading pointing in the direction held in fixed_yaw // keep heading pointing in the direction held in fixed_yaw
// with no pilot input allowed // with no pilot input allowed
float dt = millis() - fixed_last_update; const uint32_t now_ms = millis();
fixed_last_update = millis(); float dt = now_ms - _last_update_ms;
_last_update_ms = now_ms;
float yaw_angle_step = constrain_float(_fixed_yaw_offset_cd, - dt * _fixed_yaw_slewrate_cds, dt * _fixed_yaw_slewrate_cds); float yaw_angle_step = constrain_float(_fixed_yaw_offset_cd, - dt * _fixed_yaw_slewrate_cds, dt * _fixed_yaw_slewrate_cds);
_fixed_yaw_offset_cd -= yaw_angle_step; _fixed_yaw_offset_cd -= yaw_angle_step;
_yaw_angle_cd += yaw_angle_step; _yaw_angle_cd += yaw_angle_step;
@ -227,8 +224,9 @@ float Mode::AutoYaw::yaw()
return wrap_360_cd(copter.attitude_control->get_att_target_euler_cd().z); return wrap_360_cd(copter.attitude_control->get_att_target_euler_cd().z);
case AUTO_YAW_ANGLE_RATE:{ case AUTO_YAW_ANGLE_RATE:{
float dt = millis() - fixed_last_update; const uint32_t now_ms = millis();
fixed_last_update = millis(); float dt = now_ms - _last_update_ms;
_last_update_ms = now_ms;
_yaw_angle_cd += _yaw_rate_cds * dt; _yaw_angle_cd += _yaw_rate_cds * dt;
return _yaw_angle_cd; return _yaw_angle_cd;
} }

3
ArduCopter/mode.h
View File

@ -226,7 +226,8 @@ public:
// Deg/s we should turn // Deg/s we should turn
float _fixed_yaw_slewrate_cds; float _fixed_yaw_slewrate_cds;
uint32_t fixed_last_update; // time of the last yaw update
uint32_t _last_update_ms;
// heading when in yaw_look_ahead_yaw // heading when in yaw_look_ahead_yaw
float _look_ahead_yaw; float _look_ahead_yaw;