Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: Guided Yaw Fix

Browse Source
This commit is contained in:
Leonard Hall 2023-03-04 21:32:41 +10:30 committed by Randy Mackay
parent f12777f369
commit 715bb6e705
2 changed files with 40 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
ArduCopter/autoyaw.cpp
View File

@ -92,7 +92,7 @@ void Mode::AutoYaw::set_mode(Mode yaw_mode)
case Mode::RATE:
// initialise target yaw rate to zero
_yaw_rate_cds = 0.0f;
_yaw_rate_cds = 0.0;
break;
case Mode::CIRCLE:
@ -108,9 +108,6 @@ void Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_ds, int8_t di
{
_last_update_ms = millis();
_yaw_angle_cd = copter.attitude_control->get_att_target_euler_cd().z;
_yaw_rate_cds = 0.0;
// calculate final angle as relative to vehicle heading or absolute
if (relative_angle) {
_fixed_yaw_offset_cd = angle_deg * 100.0 * (direction >= 0 ? 1.0 : -1.0);
@ -211,14 +208,15 @@ bool Mode::AutoYaw::reached_fixed_yaw_target()
return (fabsf(wrap_180_cd(copter.ahrs.yaw_sensor-_yaw_angle_cd)) <= 200);
}
// yaw - returns target heading depending upon auto_yaw.mode()
float Mode::AutoYaw::yaw()
// yaw_cd - returns target heading depending upon auto_yaw.mode()
float Mode::AutoYaw::yaw_cd()
{
switch (_mode) {
case Mode::ROI:
// point towards a location held in roi
return roi_yaw();
_yaw_angle_cd = roi_yaw();
break;
case Mode::FIXED: {
// keep heading pointing in the direction held in fixed_yaw
@ -229,45 +227,55 @@ float Mode::AutoYaw::yaw()
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;
_yaw_angle_cd += yaw_angle_step;
return _yaw_angle_cd;
break;
}
case Mode::LOOK_AHEAD:
// Commanded Yaw to automatically look ahead.
return look_ahead_yaw();
_yaw_angle_cd = look_ahead_yaw();
break;
case Mode::RESETTOARMEDYAW:
// changes yaw to be same as when quad was armed
return copter.initial_armed_bearing;
_yaw_angle_cd = copter.initial_armed_bearing;
break;
case Mode::CIRCLE:
#if MODE_CIRCLE_ENABLED
if (copter.circle_nav->is_active()) {
return copter.circle_nav->get_yaw();
_yaw_angle_cd = copter.circle_nav->get_yaw();
}
#endif
// return the current attitude target
return wrap_360_cd(copter.attitude_control->get_att_target_euler_cd().z);
break;
case Mode::ANGLE_RATE:{
const uint32_t now_ms = millis();
float dt = (now_ms - _last_update_ms) * 0.001;
_last_update_ms = now_ms;
_yaw_angle_cd += _yaw_rate_cds * dt;
return _yaw_angle_cd;
break;
}
case Mode::RATE:
case Mode::WEATHERVANE:
case Mode::PILOT_RATE:
_yaw_angle_cd = copter.attitude_control->get_att_target_euler_cd().z;
break;
case Mode::LOOK_AT_NEXT_WP:
default:
// point towards next waypoint.
// we don't use wp_bearing because we don't want the copter to turn too much during flight
return copter.pos_control->get_yaw_cd();
_yaw_angle_cd = copter.pos_control->get_yaw_cd();
break;
}
return _yaw_angle_cd;
}
// returns yaw rate normally set by SET_POSITION_TARGET mavlink
// messages (positive is clockwise, negative is counter clockwise)
float Mode::AutoYaw::rate_cds() const
float Mode::AutoYaw::rate_cds()
{
switch (_mode) {
@ -277,22 +285,25 @@ float Mode::AutoYaw::rate_cds() const
case Mode::LOOK_AHEAD:
case Mode::RESETTOARMEDYAW:
case Mode::CIRCLE:
return 0.0f;
_yaw_rate_cds = 0.0f;
break;
case Mode::LOOK_AT_NEXT_WP:
_yaw_rate_cds = copter.pos_control->get_yaw_rate_cds();
break;
case Mode::PILOT_RATE:
_yaw_rate_cds = _pilot_yaw_rate_cds;
break;
case Mode::ANGLE_RATE:
case Mode::RATE:
case Mode::WEATHERVANE:
return _yaw_rate_cds;
case Mode::LOOK_AT_NEXT_WP:
return copter.pos_control->get_yaw_rate_cds();
case Mode::PILOT_RATE:
return _pilot_yaw_rate_cds;
break;
}
// return zero turn rate (this should never happen)
return 0.0f;
return _yaw_rate_cds;
}
AC_AttitudeControl::HeadingCommand Mode::AutoYaw::get_heading()
@ -315,7 +326,7 @@ AC_AttitudeControl::HeadingCommand Mode::AutoYaw::get_heading()
#endif
AC_AttitudeControl::HeadingCommand heading;
heading.yaw_angle_cd = yaw();
heading.yaw_angle_cd = auto_yaw.yaw_cd();
heading.yaw_rate_cds = auto_yaw.rate_cds();
switch (auto_yaw.mode()) {

7
ArduCopter/mode.h
View File

@ -257,7 +257,6 @@ public:
void set_mode(Mode new_mode);
Mode default_mode(bool rtl) const;
void set_rate(float new_rate_cds);
// set_roi(...): set a "look at" location:
@ -280,11 +279,11 @@ public:
private:
// yaw(): main product of AutoYaw; the heading:
float yaw();
// yaw_cd(): main product of AutoYaw; the heading:
float yaw_cd();
// rate_cds(): desired yaw rate in centidegrees/second:
float rate_cds() const;
float rate_cds();
float look_ahead_yaw();
float roi_yaw() const;