mirror of https://github.com/ArduPilot/ardupilot
79 lines
2.5 KiB
Plaintext
79 lines
2.5 KiB
Plaintext
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
// 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
|
|
static void run_nav_updates(void)
|
|
{
|
|
// fetch position from inertial navigation
|
|
calc_position();
|
|
|
|
// calculate distance and bearing for reporting and autopilot decisions
|
|
calc_distance_and_bearing();
|
|
|
|
// run autopilot to make high level decisions about control modes
|
|
run_autopilot();
|
|
}
|
|
|
|
// calc_position - get lat and lon positions from inertial nav library
|
|
static void calc_position(){
|
|
if( inertial_nav.position_ok() ) {
|
|
// pull position from interial nav library
|
|
current_loc.lng = inertial_nav.get_longitude();
|
|
current_loc.lat = inertial_nav.get_latitude();
|
|
}
|
|
}
|
|
|
|
// calc_distance_and_bearing - calculate distance and direction to waypoints for reporting and autopilot decisions
|
|
static void calc_distance_and_bearing()
|
|
{
|
|
Vector3f curr = inertial_nav.get_position();
|
|
|
|
// get target from loiter or wpinav controller
|
|
if (control_mode == LOITER || control_mode == CIRCLE) {
|
|
wp_distance = wp_nav.get_loiter_distance_to_target();
|
|
wp_bearing = wp_nav.get_loiter_bearing_to_target();
|
|
}else if (control_mode == AUTO) {
|
|
wp_distance = wp_nav.get_wp_distance_to_destination();
|
|
wp_bearing = wp_nav.get_wp_bearing_to_destination();
|
|
}else{
|
|
wp_distance = 0;
|
|
wp_bearing = 0;
|
|
}
|
|
|
|
// calculate home distance and bearing
|
|
if(GPS_ok()) {
|
|
home_distance = pythagorous2(curr.x, curr.y);
|
|
home_bearing = pv_get_bearing_cd(curr,Vector3f(0,0,0));
|
|
|
|
// update super simple bearing (if required) because it relies on home_bearing
|
|
update_super_simple_bearing(false);
|
|
}
|
|
}
|
|
|
|
// run_autopilot - highest level call to process mission commands
|
|
static void run_autopilot()
|
|
{
|
|
if (control_mode == AUTO) {
|
|
// load the next command if the command queues are empty
|
|
update_commands();
|
|
|
|
// process the active navigation and conditional commands
|
|
verify_commands();
|
|
}
|
|
}
|
|
|
|
// Keeps old data out of our calculation / logs
|
|
static void reset_nav_params(void)
|
|
{
|
|
// Will be set by new command
|
|
wp_bearing = 0;
|
|
|
|
// Will be set by new command
|
|
wp_distance = 0;
|
|
|
|
// Will be set by nav or loiter controllers
|
|
lon_error = 0;
|
|
lat_error = 0;
|
|
}
|