2015-05-29 23:12:49 -03:00
|
|
|
#include "Copter.h"
|
|
|
|
|
2013-02-18 01:58:24 -04:00
|
|
|
// 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
|
2015-05-29 23:12:49 -03:00
|
|
|
void Copter::run_nav_updates(void)
|
2013-01-08 01:45:12 -04:00
|
|
|
{
|
2017-12-05 23:34:33 -04:00
|
|
|
update_super_simple_bearing(false);
|
2013-10-29 01:28:27 -03:00
|
|
|
|
2017-12-05 21:28:32 -04:00
|
|
|
flightmode->update_navigation();
|
2014-04-17 10:22:05 -03:00
|
|
|
}
|
|
|
|
|
2017-12-14 07:40:46 -04:00
|
|
|
// distance between vehicle and home in cm
|
2017-12-05 23:34:33 -04:00
|
|
|
uint32_t Copter::home_distance()
|
2014-04-17 10:22:05 -03:00
|
|
|
{
|
2015-01-02 07:43:50 -04:00
|
|
|
if (position_ok()) {
|
2019-02-24 20:11:25 -04:00
|
|
|
_home_distance = current_loc.get_distance(ahrs.get_home()) * 100;
|
2017-12-05 23:34:33 -04:00
|
|
|
}
|
|
|
|
return _home_distance;
|
|
|
|
}
|
2012-12-08 01:23:32 -04:00
|
|
|
|
2017-12-14 07:40:46 -04:00
|
|
|
// The location of home in relation to the vehicle in centi-degrees
|
2017-12-05 23:34:33 -04:00
|
|
|
int32_t Copter::home_bearing()
|
|
|
|
{
|
|
|
|
if (position_ok()) {
|
2019-04-05 10:02:44 -03:00
|
|
|
_home_bearing = current_loc.get_bearing_to(ahrs.get_home());
|
2013-02-18 01:58:24 -04:00
|
|
|
}
|
2017-12-05 23:34:33 -04:00
|
|
|
return _home_bearing;
|
2012-04-19 01:06:15 -03:00
|
|
|
}
|