Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Mount_Backend: move calc_angle_to_lcoation to backend

Browse Source
This commit is contained in:
Randy Mackay 2015-01-15 12:58:28 +09:00 committed by Andrew Tridgell
parent 8fabacf5ff
commit b3044ced1f
4 changed files with 30 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_Mount/AP_Mount_Backend.cpp Normal file
View File

@ -0,0 +1,25 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_Mount_Backend.h>
// 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)
{
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_z = (target.alt-_frontend._current_loc.alt); // baro altitude(IN CM) should be adjusted to known home elevation before take off (Set altimeter).
float target_distance = 100.0f*pythagorous2(GPS_vector_x, GPS_vector_y); // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters.
// initialise all angles to zero
angles_to_target_rad.zero();
// tilt calcs
if (calc_tilt) {
angles_to_target_rad.y = atan2f(GPS_vector_z, target_distance);
}
// pan calcs
if (calc_pan) {
angles_to_target_rad.z = atan2f(GPS_vector_x, GPS_vector_y);
}
}

4
libraries/AP_Mount/AP_Mount_Backend.h
View File

@ -62,6 +62,10 @@ public:
virtual void status_msg(mavlink_channel_t chan) {};
protected:
// 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);
AP_Mount &_frontend; // reference to the front end which holds parameters
uint8_t _instance; // this instance's number
};

24
libraries/AP_Mount/AP_Mount_Servo.cpp
View File

@ -101,7 +101,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(_frontend.state[_instance]._roi_target, _angle_ef_target_rad);
calc_angle_to_location(_frontend.state[_instance]._roi_target, _angle_ef_target_rad, _flags.pan_control, _flags.tilt_control);
stabilize();
}
break;
@ -153,28 +153,6 @@ void AP_Mount_Servo::check_servo_map()
_flags.pan_control = RC_Channel_aux::function_assigned(_pan_idx);
}
// calc_angle_to_location - calculates the earth-frame roll, tilt and pan angles (and radians) to point at the given target
void AP_Mount_Servo::calc_angle_to_location(const struct Location &target, Vector3f& angles_to_target_rad)
{
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_z = (target.alt-_frontend._current_loc.alt); // baro altitude(IN CM) should be adjusted to known home elevation before take off (Set altimeter).
float target_distance = 100.0f*pythagorous2(GPS_vector_x, GPS_vector_y); // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters.
// initialise all angles to zero
angles_to_target_rad.zero();
// tilt calcs
if (_flags.tilt_control) {
angles_to_target_rad.y = atan2f(GPS_vector_z, target_distance);
}
// pan calcs
if (_flags.pan_control) {
angles_to_target_rad.z = atan2f(GPS_vector_x, GPS_vector_y);
}
}
// configure_msg - process MOUNT_CONFIGURE messages received from GCS
void AP_Mount_Servo::configure_msg(mavlink_message_t* msg)
{

3
libraries/AP_Mount/AP_Mount_Servo.h
View File

@ -70,9 +70,6 @@ private:
// should be called periodically (i.e. 1hz or less)
void check_servo_map();
// 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);
// stabilize - stabilizes the mount relative to the Earth's frame
void stabilize();