mirror of https://github.com/ArduPilot/ardupilot
AP_Mount: allow computation of gps point target in earth fixed frame
This commit is contained in:
parent
fc5f4c20cd
commit
6d65b351cd
|
@ -132,7 +132,7 @@ float AP_Mount_Backend::angle_input_rad(RC_Channel* rc, int16_t angle_min, int16
|
||||||
}
|
}
|
||||||
|
|
||||||
// calc_angle_to_location - calculates the earth-frame roll, tilt and pan angles (and radians) to point at the given target
|
// calc_angle_to_location - calculates the earth-frame roll, tilt and pan angles (and radians) to point at the given target
|
||||||
void AP_Mount_Backend::calc_angle_to_location(const struct Location &target, Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan)
|
void AP_Mount_Backend::calc_angle_to_location(const struct Location &target, Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan, bool relative_pan)
|
||||||
{
|
{
|
||||||
float GPS_vector_x = (target.lng-_frontend._current_loc.lng)*cosf(ToRad((_frontend._current_loc.lat+target.lat)*0.00000005f))*0.01113195f;
|
float GPS_vector_x = (target.lng-_frontend._current_loc.lng)*cosf(ToRad((_frontend._current_loc.lat+target.lat)*0.00000005f))*0.01113195f;
|
||||||
float GPS_vector_y = (target.lat-_frontend._current_loc.lat)*0.01113195f;
|
float GPS_vector_y = (target.lat-_frontend._current_loc.lat)*0.01113195f;
|
||||||
|
@ -150,6 +150,9 @@ void AP_Mount_Backend::calc_angle_to_location(const struct Location &target, Vec
|
||||||
// pan calcs
|
// pan calcs
|
||||||
if (calc_pan) {
|
if (calc_pan) {
|
||||||
// calc absolute heading and then onvert to vehicle relative yaw
|
// calc absolute heading and then onvert to vehicle relative yaw
|
||||||
angles_to_target_rad.z = wrap_PI(atan2f(GPS_vector_x, GPS_vector_y) - _frontend._ahrs.yaw);
|
angles_to_target_rad.z = atan2f(GPS_vector_x, GPS_vector_y);
|
||||||
|
if (relative_pan) {
|
||||||
|
angles_to_target_rad.z = wrap_PI(angles_to_target_rad.z - _frontend._ahrs.yaw);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -87,7 +87,7 @@ protected:
|
||||||
float angle_input_rad(RC_Channel* rc, int16_t angle_min, int16_t angle_max);
|
float angle_input_rad(RC_Channel* rc, int16_t angle_min, int16_t angle_max);
|
||||||
|
|
||||||
// calc_angle_to_location - calculates the earth-frame roll, tilt and pan angles (and radians) to point at the given target
|
// calc_angle_to_location - calculates the earth-frame roll, tilt and pan angles (and radians) to point at the given target
|
||||||
void calc_angle_to_location(const struct Location &target, Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan);
|
void calc_angle_to_location(const struct Location &target, Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan, bool relative_pan = true);
|
||||||
|
|
||||||
// get the mount mode from frontend
|
// get the mount mode from frontend
|
||||||
MAV_MOUNT_MODE get_mode(void) const { return _frontend.get_mode(_instance); }
|
MAV_MOUNT_MODE get_mode(void) const { return _frontend.get_mode(_instance); }
|
||||||
|
|
|
@ -70,7 +70,7 @@ void AP_Mount_Servo::update()
|
||||||
case MAV_MOUNT_MODE_GPS_POINT:
|
case MAV_MOUNT_MODE_GPS_POINT:
|
||||||
{
|
{
|
||||||
if(_frontend._ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
|
if(_frontend._ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
|
||||||
calc_angle_to_location(_state._roi_target, _angle_ef_target_rad, _flags.tilt_control, _flags.pan_control);
|
calc_angle_to_location(_state._roi_target, _angle_ef_target_rad, _flags.tilt_control, _flags.pan_control, false);
|
||||||
stabilize();
|
stabilize();
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
|
Loading…
Reference in New Issue