// calculate the lateral acceleration target to cause the vehicle to drive along the path from origin to destination
// this function updates the _yaw_error_cd value
void Mode::calc_steering_to_waypoint(const struct Location &origin, const struct Location &destination, bool reversed)
{
// record system time of call
last_steer_to_wp_ms = AP_HAL::millis();
// Calculate the required turn of the wheels
// negative error = left turn
// positive error = right turn
rover.nav_controller->set_reverse(reversed);
rover.nav_controller->update_waypoint(origin, destination, g.waypoint_radius);
float desired_lat_accel = rover.nav_controller->lateral_acceleration();
float desired_heading = rover.nav_controller->target_bearing_cd();
if (reversed) {
_yaw_error_cd = wrap_180_cd(desired_heading - ahrs.yaw_sensor + 18000);
} else {
_yaw_error_cd = wrap_180_cd(desired_heading - ahrs.yaw_sensor);
}
if (rover.use_pivot_steering(_yaw_error_cd)) {
// for pivot turns use heading controller
calc_steering_to_heading(desired_heading, reversed);
} else {
// call lateral acceleration to steering controller
calc_steering_from_lateral_acceleration(desired_lat_accel, reversed);
}
}
void Tracker::calc_angle_error(float pitch, float yaw, bool direction_reversed)
{
// Pitch angle error in centidegrees
// Positive error means the target is above current pitch
// Negative error means the target is below current pitch
float ahrs_pitch = ahrs.pitch_sensor;
int32_t ef_pitch_angle_error = pitch - ahrs_pitch;
// Yaw angle error in centidegrees
// Positive error means the target is right of current yaw
// Negative error means the target is left of current yaw
int32_t ahrs_yaw_cd = wrap_180_cd(ahrs.yaw_sensor);
int32_t ef_yaw_angle_error = wrap_180_cd(yaw - ahrs_yaw_cd);
if (direction_reversed) {
if (ef_yaw_angle_error > 0) {
ef_yaw_angle_error = (yaw - ahrs_yaw_cd) - 36000;
} else {
ef_yaw_angle_error = 36000 + (yaw - ahrs_yaw_cd);
}
}
// earth frame to body frame angle error conversion
float bf_pitch_err;
float bf_yaw_err;
convert_ef_to_bf(ef_pitch_angle_error, ef_yaw_angle_error, bf_pitch_err, bf_yaw_err);
nav_status.angle_error_pitch = bf_pitch_err;
nav_status.angle_error_yaw = bf_yaw_err;
}
void ModeLoiter::update()
{
// get distance (in meters) to destination
_distance_to_destination = get_distance(rover.current_loc, _destination);
// if within waypoint radius slew desired speed towards zero and use existing desired heading
if (_distance_to_destination <= g.waypoint_radius) {
_desired_speed = attitude_control.get_desired_speed_accel_limited(0.0f, rover.G_Dt);
_yaw_error_cd = 0.0f;
} else {
// P controller with hard-coded gain to convert distance to desired speed
// To-Do: make gain configurable or calculate from attitude controller's maximum accelearation
_desired_speed = MIN((_distance_to_destination - g.waypoint_radius) * 0.5f, g.speed_cruise);
// calculate bearing to destination
_desired_yaw_cd = get_bearing_cd(rover.current_loc, _destination);
_yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
// if destination is behind vehicle, reverse towards it
if (fabsf(_yaw_error_cd) > 9000 && g2.loit_type == 0) {
_desired_yaw_cd = wrap_180_cd(_desired_yaw_cd + 18000);
_yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
_desired_speed = -_desired_speed;
}
// reduce desired speed if yaw_error is large
// 45deg of error reduces speed to 75%, 90deg of error reduces speed to 50%
float yaw_error_ratio = 1.0f - constrain_float(fabsf(_yaw_error_cd / 9000.0f), 0.0f, 1.0f) * 0.5f;
_desired_speed *= yaw_error_ratio;
}
// run steering and throttle controllers
calc_steering_to_heading(_desired_yaw_cd);
calc_throttle(_desired_speed, false, true);
}
// guided_angle_control_run - runs the guided angle controller
// called from guided_run
void Copter::guided_angle_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || (ap.land_complete && guided_angle_state.climb_rate_cms <= 0.0f)) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.set_yaw_target_to_current_heading();
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain());
attitude_control.set_throttle_out(0.0f,false,g.throttle_filt);
#else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out_unstabilized(0.0f,true,g.throttle_filt);
#endif
pos_control.relax_alt_hold_controllers(0.0f);
return;
}
// constrain desired lean angles
float roll_in = guided_angle_state.roll_cd;
float pitch_in = guided_angle_state.pitch_cd;
float total_in = norm(roll_in, pitch_in);
float angle_max = MIN(attitude_control.get_althold_lean_angle_max(), aparm.angle_max);
if (total_in > angle_max) {
float ratio = angle_max / total_in;
roll_in *= ratio;
pitch_in *= ratio;
}
// wrap yaw request
float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd);
float yaw_rate_in = wrap_180_cd(guided_angle_state.yaw_rate_cds);
// constrain climb rate
float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabsf(wp_nav.get_speed_down()), wp_nav.get_speed_up());
// check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds
uint32_t tnow = millis();
if (tnow - guided_angle_state.update_time_ms > GUIDED_ATTITUDE_TIMEOUT_MS) {
roll_in = 0.0f;
pitch_in = 0.0f;
climb_rate_cms = 0.0f;
yaw_rate_in = 0.0f;
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// call attitude controller
if (guided_angle_state.use_yaw_rate) {
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(roll_in, pitch_in, yaw_rate_in, get_smoothing_gain());
} else {
attitude_control.input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true, get_smoothing_gain());
}
// call position controller
pos_control.set_alt_target_from_climb_rate_ff(climb_rate_cms, G_Dt, false);
pos_control.update_z_controller();
}
/**
update the yaw continuous rotation servo
*/
void Tracker::update_yaw_cr_servo(float yaw)
{
int32_t ahrs_yaw_cd = wrap_180_cd(ahrs.yaw_sensor);
float yaw_cd = wrap_180_cd(yaw*100.0f);
float err_cd = wrap_180_cd(yaw_cd - (float)ahrs_yaw_cd);
g.pidYaw2Srv.set_input_filter_all(err_cd);
channel_yaw.set_servo_out(g.pidYaw2Srv.get_pid());
}
// guided_angle_control_run - runs the guided angle controller
// called from guided_run
void Copter::ModeGuided::angle_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || (ap.land_complete && guided_angle_state.climb_rate_cms <= 0.0f)) {
#if FRAME_CONFIG == HELI_FRAME
attitude_control->set_yaw_target_to_current_heading();
#endif
zero_throttle_and_relax_ac();
pos_control->relax_alt_hold_controllers(0.0f);
return;
}
// constrain desired lean angles
float roll_in = guided_angle_state.roll_cd;
float pitch_in = guided_angle_state.pitch_cd;
float total_in = norm(roll_in, pitch_in);
float angle_max = MIN(attitude_control->get_althold_lean_angle_max(), copter.aparm.angle_max);
if (total_in > angle_max) {
float ratio = angle_max / total_in;
roll_in *= ratio;
pitch_in *= ratio;
}
// wrap yaw request
float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd);
float yaw_rate_in = wrap_180_cd(guided_angle_state.yaw_rate_cds);
// constrain climb rate
float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabsf(wp_nav->get_default_speed_down()), wp_nav->get_default_speed_up());
// get avoidance adjusted climb rate
climb_rate_cms = get_avoidance_adjusted_climbrate(climb_rate_cms);
// check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds
uint32_t tnow = millis();
if (tnow - guided_angle_state.update_time_ms > GUIDED_ATTITUDE_TIMEOUT_MS) {
roll_in = 0.0f;
pitch_in = 0.0f;
climb_rate_cms = 0.0f;
yaw_rate_in = 0.0f;
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// call attitude controller
if (guided_angle_state.use_yaw_rate) {
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(roll_in, pitch_in, yaw_rate_in);
} else {
attitude_control->input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true);
}
// call position controller
pos_control->set_alt_target_from_climb_rate_ff(climb_rate_cms, G_Dt, false);
pos_control->update_z_controller();
}
// update_ef_roll_angle_and_error - update _angle_ef_target.x using an earth frame roll rate request
void AC_AttitudeControl::update_ef_roll_angle_and_error(float roll_rate_ef, Vector3f &angle_ef_error, float overshoot_max)
{
// calculate angle error with maximum of +- max angle overshoot
angle_ef_error.x = wrap_180_cd(_angle_ef_target.x - _ahrs.roll_sensor);
angle_ef_error.x = constrain_float(angle_ef_error.x, -overshoot_max, overshoot_max);
// update roll angle target to be within max angle overshoot of our roll angle
_angle_ef_target.x = angle_ef_error.x + _ahrs.roll_sensor;
// increment the roll angle target
_angle_ef_target.x += roll_rate_ef * _dt;
_angle_ef_target.x = wrap_180_cd(_angle_ef_target.x);
}
// update_ef_roll_angle_and_error - update _angle_ef_target.x using an earth frame roll rate request
void AC_AttitudeControl::update_ef_roll_angle_and_error(float roll_rate_ef, Vector3f &angle_ef_error)
{
// calculate angle error with maximum of +- max angle overshoot
angle_ef_error.x = wrap_180_cd(_angle_ef_target.x - _ahrs.roll_sensor);
angle_ef_error.x = constrain_float(angle_ef_error.x, -AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX);
// update roll angle target to be within max angle overshoot of our roll angle
_angle_ef_target.x = angle_ef_error.x + _ahrs.roll_sensor;
// increment the roll angle target
_angle_ef_target.x += roll_rate_ef * _dt;
_angle_ef_target.x = wrap_180_cd(_angle_ef_target.x);
}
// update_ef_roll_angle_and_error - update _angle_ef_target.x using an earth frame roll rate request
void AC_AttitudeControl::update_ef_roll_angle_and_error(float roll_rate_ef, Vector3f &angle_ef_error, float overshoot_max)
{
// calculate angle error with maximum of +- max angle overshoot
angle_ef_error.x = wrap_180_cd(_angle_ef_target.x - _ahrs.roll_sensor);//把误差转化到180度之内，此误差为上一个目标点与实际角度的误差
angle_ef_error.x = constrain_float(angle_ef_error.x, -overshoot_max, overshoot_max);
// update roll angle target to be within max angle overshoot of our roll angle误差加传感器的值为目标值
_angle_ef_target.x = angle_ef_error.x + _ahrs.roll_sensor;//修正当前目标点（猜测在修正传感器误差）
// increment the roll angle target，把roll角目标更新到下一时刻
_angle_ef_target.x += roll_rate_ef * _dt;//更新下一个目标点
_angle_ef_target.x = wrap_180_cd(_angle_ef_target.x);
}
// update_ef_pitch_angle_and_error - update _angle_ef_target.y using an earth frame pitch rate request
void AC_AttitudeControl::update_ef_pitch_angle_and_error(float pitch_rate_ef, Vector3f &angle_ef_error)
{
// calculate angle error with maximum of +- max angle overshoot
// To-Do: should we do something better as we cross 90 degrees?
angle_ef_error.y = wrap_180_cd(_angle_ef_target.y - _ahrs.pitch_sensor);
angle_ef_error.y = constrain_float(angle_ef_error.y, -AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX, AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX);
// update pitch angle target to be within max angle overshoot of our pitch angle
_angle_ef_target.y = angle_ef_error.y + _ahrs.pitch_sensor;
// increment the pitch angle target
_angle_ef_target.y += pitch_rate_ef * _dt;
_angle_ef_target.y = wrap_180_cd(_angle_ef_target.y);
}
// return value is true if taking the long road to the target, false if normal, shortest direction should be used
bool Tracker::get_ef_yaw_direction()
{
// calculating distances from current pitch/yaw to lower and upper limits in centi-degrees
float yaw_angle_limit_lower = (-g.yaw_range * 100.0f / 2.0f) - yaw_servo_out_filt.get();
float yaw_angle_limit_upper = (g.yaw_range * 100.0f / 2.0f) - yaw_servo_out_filt.get();
float pitch_angle_limit_lower = (g.pitch_min * 100.0f) - pitch_servo_out_filt.get();
float pitch_angle_limit_upper = (g.pitch_max * 100.0f) - pitch_servo_out_filt.get();
// distances to earthframe angle limits in centi-degrees
float ef_yaw_limit_lower = yaw_angle_limit_lower;
float ef_yaw_limit_upper = yaw_angle_limit_upper;
float ef_pitch_limit_lower = pitch_angle_limit_lower;
float ef_pitch_limit_upper = pitch_angle_limit_upper;
convert_bf_to_ef(pitch_angle_limit_lower, yaw_angle_limit_lower, ef_pitch_limit_lower, ef_yaw_limit_lower);
convert_bf_to_ef(pitch_angle_limit_upper, yaw_angle_limit_upper, ef_pitch_limit_upper, ef_yaw_limit_upper);
float ef_yaw_current = wrap_180_cd(ahrs.yaw_sensor);
float ef_yaw_target = wrap_180_cd((nav_status.bearing+g.yaw_trim)*100);
float ef_yaw_angle_error = wrap_180_cd(ef_yaw_target - ef_yaw_current);
// calculate angle error to target in both directions (i.e. moving up/right or lower/left)
float yaw_angle_error_upper;
float yaw_angle_error_lower;
if (ef_yaw_angle_error >= 0) {
yaw_angle_error_upper = ef_yaw_angle_error;
yaw_angle_error_lower = ef_yaw_angle_error - 36000;
} else {
yaw_angle_error_upper = 36000 + ef_yaw_angle_error;
yaw_angle_error_lower = ef_yaw_angle_error;
}
// checks that the vehicle is outside the tracker's range
if ((yaw_angle_error_lower < ef_yaw_limit_lower) && (yaw_angle_error_upper > ef_yaw_limit_upper)) {
// if the tracker is trying to move clockwise to reach the vehicle,
// but the tracker could get closer to the vehicle by moving counter-clockwise then set direction_reversed to true
if (ef_yaw_angle_error>0 && ((ef_yaw_limit_lower - yaw_angle_error_lower) < (yaw_angle_error_upper - ef_yaw_limit_upper))) {
return true;
}
// if the tracker is trying to move counter-clockwise to reach the vehicle,
// but the tracker could get closer to the vehicle by moving then set direction_reversed to true
if (ef_yaw_angle_error<0 && ((ef_yaw_limit_lower - yaw_angle_error_lower) > (yaw_angle_error_upper - ef_yaw_limit_upper))) {
return true;
}
}
// if we get this far we can use the regular, shortest path to the target
return false;
}
// set guided mode angle target
void Copter::ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads)
{
// check we are in velocity control mode
if (guided_mode != Guided_Angle) {
angle_control_start();
}
// convert quaternion to euler angles
q.to_euler(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd);
guided_angle_state.roll_cd = ToDeg(guided_angle_state.roll_cd) * 100.0f;
guided_angle_state.pitch_cd = ToDeg(guided_angle_state.pitch_cd) * 100.0f;
guided_angle_state.yaw_cd = wrap_180_cd(ToDeg(guided_angle_state.yaw_cd) * 100.0f);
guided_angle_state.yaw_rate_cds = ToDeg(yaw_rate_rads) * 100.0f;
guided_angle_state.use_yaw_rate = use_yaw_rate;
guided_angle_state.climb_rate_cms = climb_rate_cms;
guided_angle_state.update_time_ms = millis();
// interpret positive climb rate as triggering take-off
if (motors->armed() && !ap.auto_armed && (guided_angle_state.climb_rate_cms > 0.0f)) {
copter.set_auto_armed(true);
}
// log target
copter.Log_Write_GuidedTarget(guided_mode,
Vector3f(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd),
Vector3f(0.0f, 0.0f, guided_angle_state.climb_rate_cms));
}
// set desired location
void Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
{
// set origin to last destination if waypoint controller active
if ((AP_HAL::millis() - last_steer_to_wp_ms < 100) && _reached_destination) {
_origin = _destination;
} else {
// otherwise use reasonable stopping point
calc_stopping_location(_origin);
}
_destination = destination;
// initialise distance
_distance_to_destination = get_distance(_origin, _destination);
_reached_destination = false;
// set final desired speed
_desired_speed_final = 0.0f;
if (!is_equal(next_leg_bearing_cd, MODE_NEXT_HEADING_UNKNOWN)) {
const float curr_leg_bearing_cd = get_bearing_cd(_origin, _destination);
const float turn_angle_cd = wrap_180_cd(next_leg_bearing_cd - curr_leg_bearing_cd);
if (is_zero(turn_angle_cd)) {
// if not turning can continue at full speed
_desired_speed_final = _desired_speed;
} else if (rover.use_pivot_steering_at_next_WP(turn_angle_cd)) {
// pivoting so we will stop
_desired_speed_final = 0.0f;
} else {
// calculate maximum speed that keeps overshoot within bounds
const float radius_m = fabsf(g.waypoint_overshoot / (cosf(radians(turn_angle_cd * 0.01f)) - 1.0f));
_desired_speed_final = MIN(_desired_speed, safe_sqrt(g.turn_max_g * GRAVITY_MSS * radius_m));
}
}
}
/*
update the total angle we have covered in a loiter. Used to support
commands to do N circles of loiter
*/
void Plane::loiter_angle_update(void)
{
static const int32_t lap_check_interval_cd = 3*36000;
const int32_t target_bearing_cd = nav_controller->target_bearing_cd();
int32_t loiter_delta_cd;
if (loiter.sum_cd == 0 && !reached_loiter_target()) {
// we don't start summing until we are doing the real loiter
loiter_delta_cd = 0;
} else if (loiter.sum_cd == 0) {
// use 1 cd for initial delta
loiter_delta_cd = 1;
loiter.start_lap_alt_cm = current_loc.alt;
loiter.next_sum_lap_cd = lap_check_interval_cd;
} else {
loiter_delta_cd = target_bearing_cd - loiter.old_target_bearing_cd;
}
loiter.old_target_bearing_cd = target_bearing_cd;
loiter_delta_cd = wrap_180_cd(loiter_delta_cd);
loiter.sum_cd += loiter_delta_cd * loiter.direction;
if (labs(current_loc.alt - next_WP_loc.alt) < 500) {
loiter.reached_target_alt = true;
loiter.unable_to_acheive_target_alt = false;
loiter.next_sum_lap_cd = loiter.sum_cd + lap_check_interval_cd;
} else if (!loiter.reached_target_alt && labs(loiter.sum_cd) >= loiter.next_sum_lap_cd) {
// check every few laps for scenario where up/downdrafts inhibit you from loitering up/down for too long
loiter.unable_to_acheive_target_alt = labs(current_loc.alt - loiter.start_lap_alt_cm) < 500;
loiter.start_lap_alt_cm = current_loc.alt;
loiter.next_sum_lap_cd += lap_check_interval_cd;
}
}
static void test_wrap_cd(void)
{
for (uint8_t i=0; i < ARRAY_SIZE(wrap_180_tests); i++) {
int32_t r = wrap_180_cd(wrap_180_tests[i].v);
if (r != wrap_180_tests[i].wv) {
hal.console->printf("wrap_180: v=%ld wv=%ld r=%ld\n",
(long)wrap_180_tests[i].v,
(long)wrap_180_tests[i].wv,
(long)r);
}
}
for (uint8_t i=0; i < ARRAY_SIZE(wrap_360_tests); i++) {
int32_t r = wrap_360_cd(wrap_360_tests[i].v);
if (r != wrap_360_tests[i].wv) {
hal.console->printf("wrap_360: v=%ld wv=%ld r=%ld\n",
(long)wrap_360_tests[i].v,
(long)wrap_360_tests[i].wv,
(long)r);
}
}
for (uint8_t i=0; i < ARRAY_SIZE(wrap_PI_tests); i++) {
float r = wrap_PI(wrap_PI_tests[i].v);
if (fabsf(r - wrap_PI_tests[i].wv) > 0.001f) {
hal.console->printf("wrap_PI: v=%f wv=%f r=%f\n",
wrap_PI_tests[i].v,
wrap_PI_tests[i].wv,
r);
}
}
hal.console->printf("wrap_cd tests done\n");
}
void Rover::calc_lateral_acceleration() {
switch (control_mode) {
case AUTO:
// If we have reached the waypoint previously navigate
// back to it from our current position
if (previously_reached_wp && (loiter_duration > 0)) {
nav_controller->update_waypoint(current_loc, next_WP);
} else {
nav_controller->update_waypoint(prev_WP, next_WP);
}
break;
case RTL:
case GUIDED:
case STEERING:
nav_controller->update_waypoint(current_loc, next_WP);
break;
default:
return;
}
// Calculate the required turn of the wheels
// negative error = left turn
// positive error = right turn
lateral_acceleration = nav_controller->lateral_acceleration();
if (use_pivot_steering()) {
const int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100;
if (bearing_error > 0) {
lateral_acceleration = g.turn_max_g * GRAVITY_MSS;
} else {
lateral_acceleration = -g.turn_max_g * GRAVITY_MSS;
}
}
}
// set desired location
void Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
{
// record targets
_origin = rover.current_loc;
_destination = destination;
// initialise distance
_distance_to_destination = get_distance(_origin, _destination);
_reached_destination = false;
// set final desired speed
_desired_speed_final = 0.0f;
if (!is_equal(next_leg_bearing_cd, MODE_NEXT_HEADING_UNKNOWN)) {
// if not turning can continue at full speed
if (is_zero(next_leg_bearing_cd)) {
_desired_speed_final = _desired_speed;
} else {
// calculate maximum speed that keeps overshoot within bounds
const float curr_leg_bearing_cd = get_bearing_cd(_origin, _destination);
const float turn_angle_cd = wrap_180_cd(next_leg_bearing_cd - curr_leg_bearing_cd);
const float radius_m = fabsf(g.waypoint_overshoot / (cosf(radians(turn_angle_cd * 0.01f)) - 1.0f));
_desired_speed_final = MIN(_desired_speed, safe_sqrt(g.turn_max_g * GRAVITY_MSS * radius_m));
}
}
}
/*
Wrap AHRS yaw sensor if in reverse - centi-degress
*/
float AP_L1_Control::get_yaw_sensor()
{
if (_reverse) {
return wrap_180_cd(18000 + _ahrs.yaw_sensor);
}
return _ahrs.yaw_sensor;
}
// set_auto_yaw_look_at_heading - sets the yaw look at heading for auto mode
void Copter::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle)
{
// get current yaw target
int32_t curr_yaw_target = attitude_control.get_att_target_euler_cd().z;
// get final angle, 1 = Relative, 0 = Absolute
if (relative_angle == 0) {
// absolute angle
yaw_look_at_heading = wrap_360_cd(angle_deg * 100);
} else {
// relative angle
if (direction < 0) {
angle_deg = -angle_deg;
}
yaw_look_at_heading = wrap_360_cd((angle_deg*100+curr_yaw_target));
}
// get turn speed
if (is_zero(turn_rate_dps)) {
// default to regular auto slew rate
yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE;
}else{
int32_t turn_rate = (wrap_180_cd(yaw_look_at_heading - curr_yaw_target) / 100) / turn_rate_dps;
yaw_look_at_heading_slew = constrain_int32(turn_rate, 1, 360); // deg / sec
}
// set yaw mode
set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING);
// TO-DO: restore support for clockwise and counter clockwise rotation held in cmd.content.yaw.direction. 1 = clockwise, -1 = counterclockwise
}
// return true if we should use the compass for yaw correction
bool AP_AHRS_DCM::use_compass(void)
{
if (!_compass || !_compass->use_for_yaw()) {
// no compass available
return false;
}
if (!_flags.fly_forward || !have_gps()) {
// we don't have any alterative to the compass
return true;
}
if (_gps.ground_speed() < GPS_SPEED_MIN) {
// we are not going fast enough to use the GPS
return true;
}
// if the current yaw differs from the GPS yaw by more than 45
// degrees and the estimated wind speed is less than 80% of the
// ground speed, then switch to GPS navigation. This will help
// prevent flyaways with very bad compass offsets
int32_t error = abs(wrap_180_cd(yaw_sensor - _gps.ground_course_cd()));
if (error > 4500 && _wind.length() < _gps.ground_speed()*0.8f) {
if (AP_HAL::millis() - _last_consistent_heading > 2000) {
// start using the GPS for heading if the compass has been
// inconsistent with the GPS for 2 seconds
return false;
}
} else {
_last_consistent_heading = AP_HAL::millis();
}
// use the compass
return true;
}
// Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing
void AC_AttitudeControl_Heli::input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw)
{
if (_inverted_flight) {
euler_roll_angle_cd = wrap_180_cd(euler_roll_angle_cd + 18000);
}
AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(euler_roll_angle_cd, euler_pitch_angle_cd, euler_yaw_angle_cd, slew_yaw);
}
// calculate steering output to drive along line from origin to destination waypoint
// relies on update_distance_and_bearing_to_destination being called first so _wp_bearing_cd has been updated
void AR_WPNav::update_steering(const Location& current_loc, float current_speed)
{
// calculate yaw error for determining if vehicle should pivot towards waypoint
float yaw_cd = _reversed ? wrap_360_cd(_wp_bearing_cd + 18000) : _wp_bearing_cd;
float yaw_error_cd = wrap_180_cd(yaw_cd - AP::ahrs().yaw_sensor);
// calculate desired turn rate and update desired heading
if (use_pivot_steering(yaw_error_cd)) {
_cross_track_error = 0.0f;
_desired_heading_cd = yaw_cd;
_desired_lat_accel = 0.0f;
_desired_turn_rate_rads = _atc.get_turn_rate_from_heading(radians(yaw_cd * 0.01f), radians(_pivot_rate));
} else {
// run L1 controller
_nav_controller.set_reverse(_reversed);
_nav_controller.update_waypoint((_reached_destination ? current_loc : _origin), _destination, _radius);
// retrieve lateral acceleration, heading back towards line and crosstrack error
_desired_lat_accel = constrain_float(_nav_controller.lateral_acceleration(), -_turn_max_mss, _turn_max_mss);
_desired_heading_cd = wrap_360_cd(_nav_controller.nav_bearing_cd());
if (_reversed) {
_desired_lat_accel *= -1.0f;
_desired_heading_cd = wrap_360_cd(_desired_heading_cd + 18000);
}
_cross_track_error = _nav_controller.crosstrack_error();
_desired_turn_rate_rads = _atc.get_turn_rate_from_lat_accel(_desired_lat_accel, current_speed);
}
}
// rtl_climb_return_descent_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller
// called by rtl_run at 100hz or more
void Copter::rtl_loiterathome_run()
{
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
attitude_control.set_throttle_out(0,false,g.throttle_filt);
#else
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
// reset attitude control targets
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// To-Do: re-initialise wpnav targets
return;
}
// process pilot's yaw input
float target_yaw_rate = 0;
if (!failsafe.radio) {
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
if (!is_zero(target_yaw_rate)) {
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
}
// set motors to full range
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// run waypoint controller
failsafe_terrain_set_status(wp_nav.update_wpnav());
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control.update_z_controller();
// call attitude controller
if (auto_yaw_mode == AUTO_YAW_HOLD) {
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
}else{
// roll, pitch from waypoint controller, yaw heading from auto_heading()
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true, get_smoothing_gain());
}
// check if we've completed this stage of RTL
if ((millis() - rtl_loiter_start_time) >= (uint32_t)g.rtl_loiter_time.get()) {
if (auto_yaw_mode == AUTO_YAW_RESETTOARMEDYAW) {
// check if heading is within 2 degrees of heading when vehicle was armed
if (labs(wrap_180_cd(ahrs.yaw_sensor-initial_armed_bearing)) <= 200) {
rtl_state_complete = true;
}
} else {
// we have loitered long enough
rtl_state_complete = true;
}
}
}
/*
verify a LOITER_TO_ALT command. This involves checking we have
reached both the desired altitude and desired heading. The desired
altitude only needs to be reached once.
*/
bool Plane::verify_loiter_to_alt()
{
update_loiter();
//has target altitude been reached?
if (condition_value != 0) {
if (labs(condition_value - current_loc.alt) < 500) {
//Only have to reach the altitude once -- that's why I need
//this global condition variable.
//
//This is in case of altitude oscillation when still trying
//to reach the target heading.
condition_value = 0;
} else {
return false;
}
}
//has target heading been reached?
if (condition_value2 != 0) {
//Get the lat/lon of next Nav waypoint after this one:
AP_Mission::Mission_Command next_nav_cmd;
if (! mission.get_next_nav_cmd(mission.get_current_nav_index() + 1,
next_nav_cmd)) {
//no next waypoint to shoot for -- go ahead and break out of loiter
return true;
}
// Bearing in radians
int32_t bearing_cd = get_bearing_cd(current_loc,next_nav_cmd.content.location);
// get current heading. We should probably be using the ground
// course instead to improve the accuracy in wind
int32_t heading_cd = ahrs.yaw_sensor;
int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd);
/*
Check to see if the the plane is heading toward the land
waypoint
We use 10 degrees of slop so that we can handle 100
degrees/second of yaw
*/
if (abs(heading_err_cd) <= 1000) {
//Want to head in a straight line from _here_ to the next waypoint.
//DON'T want to head in a line from the center of the loiter to
//the next waypoint.
//Therefore: mark the "last" (next_wp_loc is about to be updated)
//wp lat/lon as the current location.
next_WP_loc = current_loc;
return true;
} else {
return false;
}
}
return true;
}
void ModeGuided::set_desired_heading_delta_and_speed(float yaw_delta_cd, float target_speed)
{
// handle initialisation
if (_guided_mode != ModeGuided::Guided_HeadingAndSpeed) {
_guided_mode = ModeGuided::Guided_HeadingAndSpeed;
_desired_yaw_cd = ahrs.yaw_sensor;
}
set_desired_heading_and_speed(wrap_180_cd(_desired_yaw_cd + yaw_delta_cd), target_speed);
}
/*
work out if we are going to use pivot steering
*/
bool Rover::use_pivot_steering(void) {
if (control_mode >= AUTO && g.skid_steer_out && g.pivot_turn_angle != 0) {
const int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100;
if (abs(bearing_error) > g.pivot_turn_angle) {
return true;
}
}
return false;
}
// calculated desired speed(in m/s) based on yaw error and lateral acceleration and/or distance to a waypoint
// relies on update_distance_and_bearing_to_destination and update_steering being run so these internal members
// have been updated: _wp_bearing_cd, _cross_track_error, _distance_to_destination
void AR_WPNav::update_desired_speed(float dt)
{
// reduce speed to zero during pivot turns
if (_pivot_active) {
// decelerate to zero
_desired_speed_limited = _atc.get_desired_speed_accel_limited(0.0f, dt);
return;
}
// accelerate desired speed towards max
float des_speed_lim = _atc.get_desired_speed_accel_limited(_reversed ? -_desired_speed : _desired_speed, dt);
// reduce speed to limit overshoot from line between origin and destination
// calculate number of degrees vehicle must turn to face waypoint
float ahrs_yaw_sensor = AP::ahrs().yaw_sensor;
const float heading_cd = is_negative(des_speed_lim) ? wrap_180_cd(ahrs_yaw_sensor + 18000) : ahrs_yaw_sensor;
const float wp_yaw_diff_cd = wrap_180_cd(_wp_bearing_cd - heading_cd);
const float turn_angle_rad = fabsf(radians(wp_yaw_diff_cd * 0.01f));
// calculate distance from vehicle to line + wp_overshoot
const float line_yaw_diff = wrap_180_cd(_origin.get_bearing_to(_destination) - heading_cd);
const float dist_from_line = fabsf(_cross_track_error);
const bool heading_away = is_positive(line_yaw_diff) == is_positive(_cross_track_error);
const float wp_overshoot_adj = heading_away ? -dist_from_line : dist_from_line;
// calculate radius of circle that touches vehicle's current position and heading and target position and heading
float radius_m = 999.0f;
const float radius_calc_denom = fabsf(1.0f - cosf(turn_angle_rad));
if (!is_zero(radius_calc_denom)) {
radius_m = MAX(0.0f, _overshoot + wp_overshoot_adj) / radius_calc_denom;
}
// calculate and limit speed to allow vehicle to stay on circle
const float overshoot_speed_max = safe_sqrt(_turn_max_mss * MAX(_turn_radius, radius_m));
des_speed_lim = constrain_float(des_speed_lim, -overshoot_speed_max, overshoot_speed_max);
// limit speed based on distance to waypoint and max acceleration/deceleration
if (is_positive(_distance_to_destination) && is_positive(_atc.get_decel_max())) {
const float dist_speed_max = safe_sqrt(2.0f * _distance_to_destination * _atc.get_decel_max() + sq(_desired_speed_final));
des_speed_lim = constrain_float(des_speed_lim, -dist_speed_max, dist_speed_max);
}
_desired_speed_limited = des_speed_lim;
}
bool Plane::verify_loiter_heading(bool init)
{
if (quadplane.in_vtol_auto()) {
// skip heading verify if in VTOL auto
return true;
}
//Get the lat/lon of next Nav waypoint after this one:
AP_Mission::Mission_Command next_nav_cmd;
if (! mission.get_next_nav_cmd(mission.get_current_nav_index() + 1,
next_nav_cmd)) {
//no next waypoint to shoot for -- go ahead and break out of loiter
return true;
}
if (get_distance(next_WP_loc, next_nav_cmd.content.location) < abs(aparm.loiter_radius)) {
/* Whenever next waypoint is within the loiter radius,
maintaining loiter would prevent us from ever pointing toward the next waypoint.
Hence break out of loiter immediately
*/
return true;
}
// Bearing in degrees
int32_t bearing_cd = get_bearing_cd(current_loc,next_nav_cmd.content.location);
// get current heading.
int32_t heading_cd = gps.ground_course_cd();
int32_t heading_err_cd = wrap_180_cd(bearing_cd - heading_cd);
if (init) {
loiter.total_cd = wrap_360_cd(bearing_cd - heading_cd);
loiter.sum_cd = 0;
}
/*
Check to see if the the plane is heading toward the land
waypoint. We use 20 degrees (+/-10 deg) of margin so that
we can handle 200 degrees/second of yaw. Allow turn count
to stop it too to ensure we don't loop around forever in
case high winds are forcing us beyond 200 deg/sec at this
particular moment.
*/
if (labs(heading_err_cd) <= 1000 ||
loiter.sum_cd > loiter.total_cd) {
// Want to head in a straight line from _here_ to the next waypoint instead of center of loiter wp
// 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location
if (next_WP_loc.flags.loiter_xtrack) {
next_WP_loc = current_loc;
}
return true;
}
return false;
}
// verify_yaw - return true if we have reached the desired heading
bool Sub::verify_yaw()
{
// set yaw mode if it has been changed (the waypoint controller often retakes control of yaw as it executes a new waypoint command)
if (auto_yaw_mode != AUTO_YAW_LOOK_AT_HEADING) {
set_auto_yaw_mode(AUTO_YAW_LOOK_AT_HEADING);
}
// check if we are within 2 degrees of the target heading
return (fabsf(wrap_180_cd(ahrs.yaw_sensor-yaw_look_at_heading)) <= 200);
}
void ModeLoiter::update()
{
// get distance (in meters) to destination
_distance_to_destination = get_distance(rover.current_loc, _destination);
// if within waypoint radius slew desired speed towards zero and use existing desired heading
if (_distance_to_destination <= g.waypoint_radius) {
if (is_negative(_desired_speed)) {
_desired_speed = MIN(_desired_speed + attitude_control.get_accel_max() * rover.G_Dt, 0.0f);
} else {
_desired_speed = MAX(_desired_speed - attitude_control.get_accel_max() * rover.G_Dt, 0.0f);
}
_yaw_error_cd = 0.0f;
} else {
// P controller with hard-coded gain to convert distance to desired speed
// To-Do: make gain configurable or calculate from attitude controller's maximum accelearation
_desired_speed = MIN((_distance_to_destination - g.waypoint_radius) * 0.5f, g.speed_cruise);
// calculate bearing to destination
_desired_yaw_cd = get_bearing_cd(rover.current_loc, _destination);
_yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
// if destination is behind vehicle, reverse towards it
if (fabsf(_yaw_error_cd) > 9000) {
_desired_yaw_cd = wrap_180_cd(_desired_yaw_cd + 18000);
_yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);
_desired_speed = -_desired_speed;
}
// reduce desired speed if yaw_error is large
// note: copied from calc_reduced_speed_for_turn_or_distance
float yaw_error_ratio = 1.0f - constrain_float(fabsf(_yaw_error_cd / 9000.0f), 0.0f, 1.0f) * ((100.0f - g.speed_turn_gain) * 0.01f);
_desired_speed *= yaw_error_ratio;
}
// run steering and throttle controllers
calc_steering_to_heading(_desired_yaw_cd, _desired_speed < 0);
calc_throttle(_desired_speed, false, true);
// mark us as in_reverse when using a negative throttle
// To-Do: only in reverse if vehicle is actually travelling backwards?
rover.set_reverse(_desired_speed < 0);
}
