aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Mount: use AHRS singleton to get current position

Browse Source
This commit is contained in:
Peter Barker 2019-03-26 22:36:36 +11:00 committed by Peter Barker
parent 58d2c9669e
commit 47ad331135
3 changed files with 9 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_Mount/AP_Mount.cpp
View File

@ -393,8 +393,7 @@ const AP_Param::GroupInfo AP_Mount::var_info[] = {
AP_GROUPEND
};
AP_Mount::AP_Mount(const struct Location &current_loc) :
_current_loc(current_loc)
AP_Mount::AP_Mount()
{
if (_singleton != nullptr) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL

5
libraries/AP_Mount/AP_Mount.h
View File

@ -51,7 +51,7 @@ class AP_Mount
friend class AP_Mount_SToRM32_serial;
public:
AP_Mount(const struct Location &current_loc);
AP_Mount();
/* Do not allow copies */
AP_Mount(const AP_Mount &other) = delete;
@ -129,9 +129,6 @@ protected:
static AP_Mount *_singleton;
// private members
const struct Location &_current_loc; // reference to the vehicle's current location
// frontend parameters
AP_Int8 _joystick_speed; // joystick gain

10
libraries/AP_Mount/AP_Mount_Backend.cpp
View File

@ -134,9 +134,13 @@ float AP_Mount_Backend::angle_input_rad(const RC_Channel* rc, int16_t angle_min,
// 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, 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;
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).
Location current_loc;
if (!AP::ahrs().get_position(current_loc)) {
// completely ignore this error; ahrs will give us its best guess
}
const float GPS_vector_x = (target.lng-current_loc.lng)*cosf(ToRad((current_loc.lat+target.lat)*0.00000005f))*0.01113195f;
const float GPS_vector_y = (target.lat-current_loc.lat)*0.01113195f;
const float GPS_vector_z = (target.alt-current_loc.alt); // baro altitude(IN CM) should be adjusted to known home elevation before take off (Set altimeter).
float target_distance = 100.0f*norm(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