mirror of https://github.com/ArduPilot/ardupilot
AP_Frsky_Telem: move get_bearing_cd to Location and rename to get_bearing_to
This commit is contained in:
parent
6faf1d2849
commit
1abe8c04f2
|
@ -27,6 +27,7 @@
|
||||||
#include <AP_RangeFinder/AP_RangeFinder.h>
|
#include <AP_RangeFinder/AP_RangeFinder.h>
|
||||||
#include <AP_Common/AP_FWVersion.h>
|
#include <AP_Common/AP_FWVersion.h>
|
||||||
#include <GCS_MAVLink/GCS.h>
|
#include <GCS_MAVLink/GCS.h>
|
||||||
|
#include <AP_Common/Location.h>
|
||||||
|
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
|
|
||||||
|
@ -733,7 +734,7 @@ uint32_t AP_Frsky_Telem::calc_home(void)
|
||||||
// distance between vehicle and home_loc in meters
|
// distance between vehicle and home_loc in meters
|
||||||
home = prep_number(roundf(home_loc.get_distance(loc)), 3, 2);
|
home = prep_number(roundf(home_loc.get_distance(loc)), 3, 2);
|
||||||
// angle from front of vehicle to the direction of home_loc in 3 degree increments (just in case, limit to 127 (0x7F) since the value is stored on 7 bits)
|
// angle from front of vehicle to the direction of home_loc in 3 degree increments (just in case, limit to 127 (0x7F) since the value is stored on 7 bits)
|
||||||
home |= (((uint8_t)roundf(get_bearing_cd(loc,home_loc) * 0.00333f)) & HOME_BEARING_LIMIT)<<HOME_BEARING_OFFSET;
|
home |= (((uint8_t)roundf(loc.get_bearing_to(home_loc) * 0.00333f)) & HOME_BEARING_LIMIT)<<HOME_BEARING_OFFSET;
|
||||||
}
|
}
|
||||||
// altitude between vehicle and home_loc
|
// altitude between vehicle and home_loc
|
||||||
_relative_home_altitude = loc.alt;
|
_relative_home_altitude = loc.alt;
|
||||||
|
|
Loading…
Reference in New Issue