ardupilot/ArduPlane/commands.pde

128 lines
3.4 KiB
Plaintext
Raw Normal View History

// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
2012-02-13 17:53:54 -04:00
/*
2012-08-21 23:19:51 -03:00
* logic for dealing with the current command in the mission and home location
2012-02-13 17:53:54 -04:00
*/
static int32_t read_alt_to_hold()
{
2012-08-21 23:19:51 -03:00
if (g.RTL_altitude_cm < 0) {
return current_loc.alt;
}
2014-01-03 07:33:40 -04:00
return g.RTL_altitude_cm + home.alt;
}
/*
2014-03-17 04:45:45 -03:00
* set_next_WP - sets the target location the vehicle should fly to
2012-08-21 23:19:51 -03:00
*/
2014-03-17 04:45:45 -03:00
static void set_next_WP(const struct Location& loc)
{
2012-08-21 23:19:51 -03:00
// copy the current WP into the OldWP slot
// ---------------------------------------
prev_WP_loc = next_WP_loc;
2012-08-21 23:19:51 -03:00
// Load the next_WP slot
// ---------------------
2014-03-17 04:45:45 -03:00
next_WP_loc = loc;
// if lat and lon is zero, then use current lat/lon
// this allows a mission to contain a "loiter on the spot"
// command
if (next_WP_loc.lat == 0 && next_WP_loc.lng == 0) {
next_WP_loc.lat = current_loc.lat;
next_WP_loc.lng = current_loc.lng;
// additionally treat zero altitude as current altitude
if (next_WP_loc.alt == 0) {
next_WP_loc.alt = current_loc.alt;
next_WP_loc.flags.relative_alt = false;
}
}
2014-03-04 00:54:46 -04:00
// convert relative alt to absolute alt
if (next_WP_loc.flags.relative_alt) {
next_WP_loc.flags.relative_alt = false;
next_WP_loc.alt += home.alt;
2014-03-04 00:54:46 -04:00
}
// are we already past the waypoint? This happens when we jump
// waypoints, and it can cause us to skip a waypoint. If we are
// past the waypoint when we start on a leg, then use the current
// location as the previous waypoint, to prevent immediately
// considering the waypoint complete
if (location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
gcs_send_text_P(SEVERITY_LOW, PSTR("Resetting prev_WP"));
prev_WP_loc = current_loc;
}
2012-08-21 23:19:51 -03:00
// used to control FBW and limit the rate of climb
// -----------------------------------------------
target_altitude_cm = current_loc.alt;
// zero out our loiter vals to watch for missed waypoints
loiter_angle_reset();
2012-08-21 23:19:51 -03:00
setup_glide_slope();
2012-08-21 23:19:51 -03:00
loiter_angle_reset();
}
static void set_guided_WP(void)
{
if (g.loiter_radius < 0) {
loiter.direction = -1;
} else {
loiter.direction = 1;
}
2012-08-21 23:19:51 -03:00
// copy the current location into the OldWP slot
// ---------------------------------------
prev_WP_loc = current_loc;
2012-08-21 23:19:51 -03:00
// Load the next_WP slot
// ---------------------
next_WP_loc = guided_WP_loc;
2012-08-21 23:19:51 -03:00
// used to control FBW and limit the rate of climb
// -----------------------------------------------
target_altitude_cm = current_loc.alt;
setup_glide_slope();
2012-08-21 23:19:51 -03:00
loiter_angle_reset();
}
// run this at setup on the ground
// -------------------------------
static void init_home()
{
2012-08-21 23:19:51 -03:00
gcs_send_text_P(SEVERITY_LOW, PSTR("init home"));
2014-03-28 16:52:48 -03:00
ahrs.set_home(gps.location());
2012-08-21 23:19:51 -03:00
home_is_set = true;
2014-01-03 07:33:40 -04:00
gcs_send_text_fmt(PSTR("gps alt: %lu"), (unsigned long)home.alt);
2014-03-03 09:50:45 -04:00
// Save Home to EEPROM
mission.write_home_to_storage();
2012-08-21 23:19:51 -03:00
// Save prev loc
// -------------
next_WP_loc = prev_WP_loc = home;
2012-08-21 23:19:51 -03:00
// Load home for a default guided_WP
// -------------
guided_WP_loc = home;
guided_WP_loc.alt += g.RTL_altitude_cm;
}
/*
update home location from GPS
this is called as long as we have 3D lock and the arming switch is
not pushed
*/
static void update_home()
{
2014-03-28 16:52:48 -03:00
ahrs.set_home(gps.location());
barometer.update_calibration();
}