You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
27 lines
1.0 KiB
27 lines
1.0 KiB
#include "Copter.h" |
|
|
|
// run_nav_updates - top level call for the autopilot |
|
// ensures calculations such as "distance to waypoint" are calculated before autopilot makes decisions |
|
// To-Do - rename and move this function to make it's purpose more clear |
|
void Copter::run_nav_updates(void) |
|
{ |
|
// calculate distance and bearing for reporting and autopilot decisions |
|
calc_home_distance_and_bearing(); |
|
|
|
flightmode->update_navigation(); |
|
} |
|
|
|
// calc_home_distance_and_bearing - calculate distance and bearing to home for reporting and autopilot decisions |
|
void Copter::calc_home_distance_and_bearing() |
|
{ |
|
// calculate home distance and bearing |
|
if (position_ok()) { |
|
Vector3f home = pv_location_to_vector(ahrs.get_home()); |
|
Vector3f curr = inertial_nav.get_position(); |
|
home_distance = get_horizontal_distance_cm(curr, home); |
|
home_bearing = get_bearing_cd(curr,home); |
|
|
|
// update super simple bearing (if required) because it relies on home_bearing |
|
update_super_simple_bearing(false); |
|
} |
|
}
|
|
|