Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: additional yaw modes and fixes

Browse Source
This commit is contained in:
Leonard Hall 2021-07-09 18:14:07 +09:30 committed by Randy Mackay
parent 05f21d7665
commit 23ec88e90a
6 changed files with 79 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ArduCopter/GCS_Mavlink.cpp
View File

@ -714,11 +714,9 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_mount(const mavlink_command_long_t
// if vehicle has a camera mount but it doesn't do pan control then yaw the entire vehicle instead
if ((copter.camera_mount.get_mount_type() != copter.camera_mount.MountType::Mount_Type_None) &&
!copter.camera_mount.has_pan_control()) {
copter.flightmode->auto_yaw.set_fixed_yaw(
copter.flightmode->auto_yaw.set_yaw_angle_rate(
(float)packet.param3 * 0.01f,
0.0f,
0,
false);
0.0f);
}
break;
#endif
@ -984,11 +982,9 @@ void GCS_MAVLINK_Copter::handle_mount_message(const mavlink_message_t &msg)
// if vehicle has a camera mount but it doesn't do pan control then yaw the entire vehicle instead
if ((copter.camera_mount.get_mount_type() != copter.camera_mount.MountType::Mount_Type_None) &&
!copter.camera_mount.has_pan_control()) {
copter.flightmode->auto_yaw.set_fixed_yaw(
copter.flightmode->auto_yaw.set_yaw_angle_rate(
mavlink_msg_mount_control_get_input_c(&msg) * 0.01f,
0.0f,
0,
false);
0.0f);
break;
}

77
ArduCopter/autoyaw.cpp
View File

@ -85,7 +85,7 @@ void Mode::AutoYaw::set_mode(autopilot_yaw_mode yaw_mode)
case AUTO_YAW_RATE:
// initialise target yaw rate to zero
_rate_cds = 0.0f;
_yaw_rate_cds = 0.0f;
break;
case AUTO_YAW_CIRCLE:
@ -95,35 +95,52 @@ void Mode::AutoYaw::set_mode(autopilot_yaw_mode yaw_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_dps, 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)
{
const int32_t curr_yaw_target = copter.attitude_control->get_att_target_euler_cd().z;
fixed_last_update = 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) {
// absolute angle
_fixed_yaw = wrap_360_cd(angle_deg * 100);
} else {
// relative angle
if (direction < 0) {
angle_deg = -angle_deg;
if (relative_angle) {
if (direction == 0) {
_fixed_yaw_offset_cd = angle_deg;
} else {
_fixed_yaw_offset_cd = angle_deg * direction;
}
} else {
// absolute angle
_fixed_yaw_offset_cd = wrap_180_cd(angle_deg * 100.0 - _yaw_angle_cd);
if ( direction < 0 && is_positive(_fixed_yaw_offset_cd) ) {
_fixed_yaw_offset_cd -= 36000.0;
} else if ( direction > 0 && is_negative(_fixed_yaw_offset_cd) ) {
_fixed_yaw_offset_cd += 36000.0;
}
_fixed_yaw = wrap_360_cd((angle_deg * 100) + curr_yaw_target);
}
// get turn speed
if (is_zero(turn_rate_dps)) {
// default to regular auto slew rate
_fixed_yaw_slewrate = AUTO_YAW_SLEW_RATE;
if (!is_positive(turn_rate_ds)) {
// default to default slew rate
_fixed_yaw_slewrate_cds = copter.attitude_control->get_slew_yaw_cds();
} else {
const int32_t turn_rate = (wrap_180_cd(_fixed_yaw - curr_yaw_target) / 100) / turn_rate_dps;
_fixed_yaw_slewrate = constrain_int32(turn_rate, 1, 360); // deg / sec
_fixed_yaw_slewrate_cds = MIN(copter.attitude_control->get_slew_yaw_cds(), turn_rate_ds * 100.0);
}
// set yaw mode
set_mode(AUTO_YAW_FIXED);
}
// TO-DO: restore support for clockwise and counter clockwise rotation held in cmd.content.yaw.direction. 1 = clockwise, -1 = counterclockwise
// 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)
{
fixed_last_update = millis();
_yaw_angle_cd = yaw_angle_d * 100.0;
_yaw_rate_cds = yaw_rate_ds * 100.0;
// set yaw mode
set_mode(AUTO_YAW_ANGLE_RATE);
}
// set_roi - sets the yaw to look at roi for auto mode
@ -169,7 +186,7 @@ void Mode::AutoYaw::set_roi(const Location &roi_location)
void Mode::AutoYaw::set_rate(float turn_rate_cds)
{
set_mode(AUTO_YAW_RATE);
_rate_cds = turn_rate_cds;
_yaw_rate_cds = turn_rate_cds;
}
// yaw - returns target heading depending upon auto_yaw.mode()
@ -181,10 +198,16 @@ float Mode::AutoYaw::yaw()
// point towards a location held in roi
return roi_yaw();
case AUTO_YAW_FIXED:
case AUTO_YAW_FIXED: {
// keep heading pointing in the direction held in fixed_yaw
// with no pilot input allowed
return _fixed_yaw;
float dt = millis() - fixed_last_update;
fixed_last_update = millis();
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;
}
case AUTO_YAW_LOOK_AHEAD:
// Commanded Yaw to automatically look ahead.
@ -203,11 +226,18 @@ float Mode::AutoYaw::yaw()
// return the current attitude target
return wrap_360_cd(copter.attitude_control->get_att_target_euler_cd().z);
case AUTO_YAW_ANGLE_RATE:{
float dt = millis() - fixed_last_update;
fixed_last_update = millis();
_yaw_angle_cd += _yaw_rate_cds * dt;
return _yaw_angle_cd;
}
case AUTO_YAW_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.wp_nav->get_yaw();
return copter.pos_control->get_yaw_cd();
}
}
@ -225,11 +255,12 @@ float Mode::AutoYaw::rate_cds() const
case AUTO_YAW_CIRCLE:
return 0.0f;
case AUTO_YAW_ANGLE_RATE:
case AUTO_YAW_RATE:
return _rate_cds;
return _yaw_rate_cds;
case AUTO_YAW_LOOK_AT_NEXT_WP:
return copter.wp_nav->get_yaw_rate_cds();
return copter.pos_control->get_yaw_rate_cds();
}
// return zero turn rate (this should never happen)

5
ArduCopter/defines.h
View File

@ -18,8 +18,9 @@ enum autopilot_yaw_mode {
AUTO_YAW_FIXED = 3, // point towards a particular angle (no pilot input accepted)
AUTO_YAW_LOOK_AHEAD = 4, // point in the direction the copter is moving
AUTO_YAW_RESETTOARMEDYAW = 5, // point towards heading at time motors were armed
AUTO_YAW_RATE = 6, // turn at a specified rate (held in auto_yaw_rate)
AUTO_YAW_CIRCLE = 7, // use AC_Circle's provided yaw (used during Loiter-Turns commands)
AUTO_YAW_ANGLE_RATE = 6, // turn at a specified rate from a starting angle
AUTO_YAW_RATE = 7, // turn at a specified rate (held in auto_yaw_rate)
AUTO_YAW_CIRCLE = 8, // use AC_Circle's provided yaw (used during Loiter-Turns commands)
};
// Frame types

11
ArduCopter/mode.h
View File

@ -207,6 +207,8 @@ public:
int8_t direction,
bool relative_angle);
void set_yaw_angle_rate(float yaw_angle_d, float yaw_rate_ds);
private:
float look_ahead_yaw();
@ -219,16 +221,19 @@ public:
Vector3f roi;
// yaw used for YAW_FIXED yaw_mode
int32_t _fixed_yaw;
float _fixed_yaw_offset_cd;
// Deg/s we should turn
int16_t _fixed_yaw_slewrate;
float _fixed_yaw_slewrate_cds;
uint32_t fixed_last_update;
// heading when in yaw_look_ahead_yaw
float _look_ahead_yaw;
// turn rate (in cds) when auto_yaw_mode is set to AUTO_YAW_RATE
float _rate_cds;
float _yaw_angle_cd;
float _yaw_rate_cds;
};
static AutoYaw auto_yaw;

2
ArduCopter/mode_auto.cpp
View File

@ -1501,7 +1501,7 @@ void ModeAuto::do_mount_control(const AP_Mission::Mission_Command& cmd)
// if vehicle has a camera mount but it doesn't do pan control then yaw the entire vehicle instead
if ((copter.camera_mount.get_mount_type() != copter.camera_mount.MountType::Mount_Type_None) &&
!copter.camera_mount.has_pan_control()) {
auto_yaw.set_fixed_yaw(cmd.content.mount_control.yaw,0.0f,0,false);
auto_yaw.set_yaw_angle_rate(cmd.content.mount_control.yaw,0.0f);
}
// pass the target angles to the camera mount
copter.camera_mount.set_angle_targets(cmd.content.mount_control.roll, cmd.content.mount_control.pitch, cmd.content.mount_control.yaw);

14
ArduCopter/mode_guided.cpp
View File

@ -576,7 +576,7 @@ void ModeGuided::pos_control_run()
attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds());
} else {
// roll & pitch from position controller, yaw heading from GCS or auto_heading()
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw());
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds());
}
}
@ -639,7 +639,7 @@ void ModeGuided::accel_control_run()
attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds());
} else {
// roll & pitch from position controller, yaw heading from GCS or auto_heading()
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw());
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds());
}
}
@ -713,7 +713,7 @@ void ModeGuided::velaccel_control_run()
attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds());
} else {
// roll & pitch from position controller, yaw heading from GCS or auto_heading()
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw());
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds());
}
}
@ -794,7 +794,7 @@ void ModeGuided::posvelaccel_control_run()
attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds());
} else {
// roll & pitch from position controller, yaw heading from GCS or auto_heading()
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw());
attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds());
}
}
@ -887,8 +887,12 @@ void ModeGuided::set_desired_velocity_with_accel_and_fence_limits(const Vector3f
// helper function to set yaw state and targets
void ModeGuided::set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle)
{
if (use_yaw) {
if (use_yaw && relative_angle) {
auto_yaw.set_fixed_yaw(yaw_cd * 0.01f, 0.0f, 0, relative_angle);
} else if (use_yaw && use_yaw_rate) {
auto_yaw.set_yaw_angle_rate(yaw_cd * 0.01f, yaw_rate_cds * 0.01f);
} else if (use_yaw && !use_yaw_rate) {
auto_yaw.set_yaw_angle_rate(yaw_cd * 0.01f, 0.0f);
} else if (use_yaw_rate) {
auto_yaw.set_rate(yaw_rate_cds);
}