ardupilot/ArduCopter/navigation.pde

77 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) {
// update state of mission
// may call commands_process.pde's start_command and verify_command functions
mission.update();
}
}
// 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;
}