Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: allow computation of gps point target in earth fixed frame

This commit is contained in:
Przemek Lekston 2016-10-27 23:46:27 +02:00 committed by Randy Mackay
parent fc5f4c20cd
commit 6d65b351cd
3 changed files with 7 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_Mount/AP_Mount_Backend.cpp
View File

@ -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
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_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
if (calc_pan) {
// 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);
}
}
}

2
libraries/AP_Mount/AP_Mount_Backend.h
View File

@ -87,7 +87,7 @@ protected:
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
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
MAV_MOUNT_MODE get_mode(void) const { return _frontend.get_mode(_instance); }

2
libraries/AP_Mount/AP_Mount_Servo.cpp
View File

@ -70,7 +70,7 @@ void AP_Mount_Servo::update()
case MAV_MOUNT_MODE_GPS_POINT:
{
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();
}
break;