2015-06-01 02:04:25 -03:00
|
|
|
#include "Tracker.h"
|
|
|
|
|
2015-04-17 21:45:26 -03:00
|
|
|
/*
|
2016-07-25 15:52:50 -03:00
|
|
|
* Auto control mode
|
2015-04-17 21:45:26 -03:00
|
|
|
*/
|
|
|
|
|
|
|
|
/*
|
|
|
|
* update_auto - runs the auto controller
|
2019-09-13 12:41:55 -03:00
|
|
|
* called at 50hz while control mode is 'AUTO'
|
2015-04-17 21:45:26 -03:00
|
|
|
*/
|
2019-09-13 12:41:55 -03:00
|
|
|
void Mode::update_auto(void)
|
2015-04-17 21:45:26 -03:00
|
|
|
{
|
2019-09-13 12:41:55 -03:00
|
|
|
struct Tracker::NavStatus &nav_status = tracker.nav_status;
|
|
|
|
|
|
|
|
Parameters &g = tracker.g;
|
|
|
|
|
2016-07-13 23:40:49 -03:00
|
|
|
float yaw = wrap_180_cd((nav_status.bearing+g.yaw_trim)*100); // target yaw in centidegrees
|
2019-03-10 07:49:28 -03:00
|
|
|
float pitch = constrain_float(nav_status.pitch+g.pitch_trim, g.pitch_min, g.pitch_max) * 100; // target pitch in centidegrees
|
2016-07-13 23:40:49 -03:00
|
|
|
|
2016-07-27 23:19:54 -03:00
|
|
|
bool direction_reversed = get_ef_yaw_direction();
|
|
|
|
|
|
|
|
calc_angle_error(pitch, yaw, direction_reversed);
|
2016-07-13 23:40:49 -03:00
|
|
|
|
|
|
|
float bf_pitch;
|
|
|
|
float bf_yaw;
|
2016-07-27 23:31:48 -03:00
|
|
|
convert_ef_to_bf(pitch, yaw, bf_pitch, bf_yaw);
|
2015-04-26 23:24:47 -03:00
|
|
|
|
2019-03-10 07:49:28 -03:00
|
|
|
// only move servos if target is at least distance_min away if we have a target
|
2019-09-13 12:41:55 -03:00
|
|
|
if ((g.distance_min <= 0) || (nav_status.distance >= g.distance_min) || !tracker.vehicle.location_valid) {
|
|
|
|
tracker.update_pitch_servo(bf_pitch);
|
|
|
|
tracker.update_yaw_servo(bf_yaw);
|
2015-04-26 23:24:47 -03:00
|
|
|
}
|
2015-04-17 21:45:26 -03:00
|
|
|
}
|
2016-07-27 23:31:48 -03:00
|
|
|
|
2019-09-13 12:41:55 -03:00
|
|
|
void Mode::calc_angle_error(float pitch, float yaw, bool direction_reversed)
|
2016-07-13 23:40:49 -03:00
|
|
|
{
|
2016-07-27 23:19:54 -03:00
|
|
|
// Pitch angle error in centidegrees
|
|
|
|
// Positive error means the target is above current pitch
|
2016-07-13 23:40:49 -03:00
|
|
|
// Negative error means the target is below current pitch
|
2019-09-13 12:41:55 -03:00
|
|
|
const AP_AHRS &ahrs = AP::ahrs();
|
2016-07-13 23:40:49 -03:00
|
|
|
float ahrs_pitch = ahrs.pitch_sensor;
|
2016-07-27 23:19:54 -03:00
|
|
|
int32_t ef_pitch_angle_error = pitch - ahrs_pitch;
|
2016-07-13 23:40:49 -03:00
|
|
|
|
|
|
|
// Yaw angle error in centidegrees
|
2016-07-27 23:19:54 -03:00
|
|
|
// Positive error means the target is right of current yaw
|
2016-07-13 23:40:49 -03:00
|
|
|
// Negative error means the target is left of current yaw
|
|
|
|
int32_t ahrs_yaw_cd = wrap_180_cd(ahrs.yaw_sensor);
|
2016-07-27 23:19:54 -03:00
|
|
|
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);
|
|
|
|
}
|
|
|
|
}
|
2016-07-13 23:40:49 -03:00
|
|
|
|
|
|
|
// earth frame to body frame angle error conversion
|
2016-07-27 23:19:54 -03:00
|
|
|
float bf_pitch_err;
|
|
|
|
float bf_yaw_err;
|
2016-07-27 23:31:48 -03:00
|
|
|
convert_ef_to_bf(ef_pitch_angle_error, ef_yaw_angle_error, bf_pitch_err, bf_yaw_err);
|
2019-09-13 12:41:55 -03:00
|
|
|
struct Tracker::NavStatus &nav_status = tracker.nav_status;
|
2016-07-13 23:40:49 -03:00
|
|
|
nav_status.angle_error_pitch = bf_pitch_err;
|
|
|
|
nav_status.angle_error_yaw = bf_yaw_err;
|
2019-03-10 07:39:23 -03:00
|
|
|
|
|
|
|
// set actual and desired for logging, note we are using angles not rates
|
2019-09-13 12:41:55 -03:00
|
|
|
Parameters &g = tracker.g;
|
2019-06-27 06:36:31 -03:00
|
|
|
g.pidPitch2Srv.set_target_rate(pitch * 0.01);
|
2019-03-10 07:39:23 -03:00
|
|
|
g.pidPitch2Srv.set_actual_rate(ahrs_pitch * 0.01);
|
2019-06-27 06:36:31 -03:00
|
|
|
g.pidYaw2Srv.set_target_rate(yaw * 0.01);
|
2019-03-10 07:39:23 -03:00
|
|
|
g.pidYaw2Srv.set_actual_rate(ahrs_yaw_cd * 0.01);
|
2016-07-13 23:40:49 -03:00
|
|
|
}
|
2016-07-27 23:31:48 -03:00
|
|
|
|
2019-09-13 12:41:55 -03:00
|
|
|
void Mode::convert_ef_to_bf(float pitch, float yaw, float& bf_pitch, float& bf_yaw)
|
2016-07-13 23:40:49 -03:00
|
|
|
{
|
|
|
|
// earth frame to body frame pitch and yaw conversion
|
2019-09-13 12:41:55 -03:00
|
|
|
const AP_AHRS &ahrs = AP::ahrs();
|
2016-07-27 23:31:48 -03:00
|
|
|
bf_pitch = ahrs.cos_roll() * pitch + ahrs.sin_roll() * ahrs.cos_pitch() * yaw;
|
|
|
|
bf_yaw = -ahrs.sin_roll() * pitch + ahrs.cos_pitch() * ahrs.cos_roll() * yaw;
|
2016-07-13 23:40:49 -03:00
|
|
|
}
|
2016-07-27 23:19:54 -03:00
|
|
|
|
2019-09-13 12:41:55 -03:00
|
|
|
bool Mode::convert_bf_to_ef(float pitch, float yaw, float& ef_pitch, float& ef_yaw)
|
2016-07-27 23:19:54 -03:00
|
|
|
{
|
2019-09-13 12:41:55 -03:00
|
|
|
const AP_AHRS &ahrs = AP::ahrs();
|
2016-07-27 23:19:54 -03:00
|
|
|
// avoid divide by zero
|
|
|
|
if (is_zero(ahrs.cos_pitch())) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
// convert earth frame angle or rates to body frame
|
2016-07-27 23:31:48 -03:00
|
|
|
ef_pitch = ahrs.cos_roll() * pitch - ahrs.sin_roll() * yaw;
|
|
|
|
ef_yaw = (ahrs.sin_roll() / ahrs.cos_pitch()) * pitch + (ahrs.cos_roll() / ahrs.cos_pitch()) * yaw;
|
2016-07-27 23:19:54 -03:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2016-07-27 23:31:48 -03:00
|
|
|
// return value is true if taking the long road to the target, false if normal, shortest direction should be used
|
2019-09-13 12:41:55 -03:00
|
|
|
bool Mode::get_ef_yaw_direction()
|
2016-07-27 23:19:54 -03:00
|
|
|
{
|
|
|
|
// calculating distances from current pitch/yaw to lower and upper limits in centi-degrees
|
2019-09-13 12:41:55 -03:00
|
|
|
Parameters &g = tracker.g;
|
|
|
|
float yaw_angle_limit_lower = (-g.yaw_range * 100.0f / 2.0f) - tracker.yaw_servo_out_filt.get();
|
|
|
|
float yaw_angle_limit_upper = (g.yaw_range * 100.0f / 2.0f) - tracker.yaw_servo_out_filt.get();
|
|
|
|
float pitch_angle_limit_lower = (g.pitch_min * 100.0f) - tracker.pitch_servo_out_filt.get();
|
|
|
|
float pitch_angle_limit_upper = (g.pitch_max * 100.0f) - tracker.pitch_servo_out_filt.get();
|
2016-07-27 23:19:54 -03:00
|
|
|
|
|
|
|
// 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);
|
|
|
|
|
2019-09-13 12:41:55 -03:00
|
|
|
const AP_AHRS &ahrs = AP::ahrs();
|
2016-07-27 23:19:54 -03:00
|
|
|
float ef_yaw_current = wrap_180_cd(ahrs.yaw_sensor);
|
2019-09-13 12:41:55 -03:00
|
|
|
struct Tracker::NavStatus &nav_status = tracker.nav_status;
|
2016-07-27 23:19:54 -03:00
|
|
|
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,
|
2016-08-04 14:09:27 -03:00
|
|
|
// but the tracker could get closer to the vehicle by moving counter-clockwise then set direction_reversed to true
|
2016-07-27 23:19:54 -03:00
|
|
|
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,
|
2016-08-04 14:09:27 -03:00
|
|
|
// but the tracker could get closer to the vehicle by moving then set direction_reversed to true
|
2016-07-27 23:19:54 -03:00
|
|
|
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;
|
|
|
|
}
|