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
|
|
|
*/
|
2011-10-25 22:27:23 -03:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
#include "Plane.h"
|
|
|
|
|
2011-09-08 22:29:39 -03:00
|
|
|
/*
|
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
|
|
|
*/
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::set_next_WP(const struct Location &loc)
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2017-11-21 15:41:45 -04:00
|
|
|
if (auto_state.next_wp_crosstrack) {
|
|
|
|
// copy the current WP into the OldWP slot
|
|
|
|
prev_WP_loc = next_WP_loc;
|
|
|
|
auto_state.crosstrack = true;
|
|
|
|
} else {
|
2014-08-04 07:39:15 -03:00
|
|
|
// we should not try to cross-track for this waypoint
|
|
|
|
prev_WP_loc = current_loc;
|
|
|
|
// use cross-track for the next waypoint
|
2017-11-21 15:41:45 -04:00
|
|
|
auto_state.next_wp_crosstrack = true;
|
|
|
|
auto_state.crosstrack = false;
|
2014-08-04 07:39:15 -03:00
|
|
|
}
|
2011-09-08 22:29:39 -03:00
|
|
|
|
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;
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-08-13 20:40:23 -03:00
|
|
|
// if lat and lon is zero, then use current lat/lon
|
|
|
|
// this allows a mission to contain a "loiter on the spot"
|
|
|
|
// command
|
2014-03-16 01:53:10 -03:00
|
|
|
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;
|
2012-08-13 20:40:23 -03:00
|
|
|
// additionally treat zero altitude as current altitude
|
2014-03-16 01:53:10 -03:00
|
|
|
if (next_WP_loc.alt == 0) {
|
|
|
|
next_WP_loc.alt = current_loc.alt;
|
|
|
|
next_WP_loc.flags.relative_alt = false;
|
2014-07-24 21:23:12 -03:00
|
|
|
next_WP_loc.flags.terrain_alt = false;
|
2012-08-13 20:40:23 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2014-03-04 00:54:46 -04:00
|
|
|
// convert relative alt to absolute alt
|
2014-03-16 01:53:10 -03:00
|
|
|
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
|
|
|
}
|
2012-08-13 20:40:23 -03:00
|
|
|
|
2012-07-21 07:32:07 -03: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
|
2014-03-16 01:53:10 -03:00
|
|
|
if (location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
|
2017-07-08 22:15:58 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_NOTICE, "Resetting previous waypoint");
|
2014-03-16 01:53:10 -03:00
|
|
|
prev_WP_loc = current_loc;
|
2012-07-21 07:32:07 -03:00
|
|
|
}
|
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
// used to control FBW and limit the rate of climb
|
|
|
|
// -----------------------------------------------
|
2014-08-04 03:55:15 -03:00
|
|
|
set_target_altitude_location(next_WP_loc);
|
2012-08-21 23:19:51 -03:00
|
|
|
|
|
|
|
// zero out our loiter vals to watch for missed waypoints
|
2013-04-11 21:25:46 -03:00
|
|
|
loiter_angle_reset();
|
2012-08-21 23:19:51 -03:00
|
|
|
|
2013-07-05 01:56:58 -03:00
|
|
|
setup_glide_slope();
|
2014-07-24 03:21:30 -03:00
|
|
|
setup_turn_angle();
|
2012-08-21 23:19:51 -03:00
|
|
|
|
2013-04-11 21:25:46 -03:00
|
|
|
loiter_angle_reset();
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::set_guided_WP(void)
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2017-01-30 13:44:35 -04:00
|
|
|
if (aparm.loiter_radius < 0 || guided_WP_loc.flags.loiter_ccw) {
|
2013-04-21 07:52:50 -03:00
|
|
|
loiter.direction = -1;
|
|
|
|
} else {
|
|
|
|
loiter.direction = 1;
|
|
|
|
}
|
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
// copy the current location into the OldWP slot
|
|
|
|
// ---------------------------------------
|
2014-03-16 01:53:10 -03:00
|
|
|
prev_WP_loc = current_loc;
|
2012-08-21 23:19:51 -03:00
|
|
|
|
|
|
|
// Load the next_WP slot
|
|
|
|
// ---------------------
|
2014-03-16 01:53:10 -03:00
|
|
|
next_WP_loc = guided_WP_loc;
|
2012-08-21 23:19:51 -03:00
|
|
|
|
|
|
|
// used to control FBW and limit the rate of climb
|
|
|
|
// -----------------------------------------------
|
2014-07-24 03:21:30 -03:00
|
|
|
set_target_altitude_current();
|
2012-08-21 23:19:51 -03:00
|
|
|
|
2013-07-05 01:56:58 -03:00
|
|
|
setup_glide_slope();
|
2014-07-24 03:21:30 -03:00
|
|
|
setup_turn_angle();
|
2012-08-21 23:19:51 -03:00
|
|
|
|
2017-11-21 15:44:04 -04:00
|
|
|
// disable crosstrack, head directly to the point
|
|
|
|
auto_state.crosstrack = false;
|
|
|
|
|
2016-05-05 22:28:26 -03:00
|
|
|
// reset loiter start time.
|
|
|
|
loiter.start_time_ms = 0;
|
|
|
|
|
|
|
|
// start in non-VTOL mode
|
2016-05-11 02:57:41 -03:00
|
|
|
auto_state.vtol_loiter = false;
|
2016-05-05 22:28:26 -03:00
|
|
|
|
2013-04-11 21:25:46 -03:00
|
|
|
loiter_angle_reset();
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// run this at setup on the ground
|
|
|
|
// -------------------------------
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::init_home()
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2017-07-08 22:15:58 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "Init HOME");
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2014-03-28 16:52:48 -03:00
|
|
|
ahrs.set_home(gps.location());
|
2015-02-20 19:14:18 -04:00
|
|
|
home_is_set = HOME_SET_NOT_LOCKED;
|
2015-07-05 23:01:17 -03:00
|
|
|
Log_Write_Home_And_Origin();
|
2017-07-08 01:46:24 -03:00
|
|
|
gcs().send_home(gps.location());
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2014-03-03 09:50:45 -04:00
|
|
|
// Save Home to EEPROM
|
|
|
|
mission.write_home_to_storage();
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
// Save prev loc
|
|
|
|
// -------------
|
2014-03-16 01:53:10 -03:00
|
|
|
next_WP_loc = prev_WP_loc = home;
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2013-10-05 05:44:44 -03:00
|
|
|
/*
|
|
|
|
update home location from GPS
|
|
|
|
this is called as long as we have 3D lock and the arming switch is
|
|
|
|
not pushed
|
|
|
|
*/
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::update_home()
|
2013-10-05 05:44:44 -03:00
|
|
|
{
|
2017-08-28 19:20:13 -03:00
|
|
|
if ((g2.home_reset_threshold == -1) ||
|
|
|
|
((g2.home_reset_threshold > 0) &&
|
|
|
|
(fabsf(barometer.get_altitude()) > g2.home_reset_threshold))) {
|
2016-07-19 21:05:12 -03:00
|
|
|
// don't auto-update if we have changed barometer altitude
|
|
|
|
// significantly. This allows us to cope with slow baro drift
|
|
|
|
// but not re-do home and the baro if we have changed height
|
|
|
|
// significantly
|
|
|
|
return;
|
|
|
|
}
|
2015-02-20 19:14:18 -04:00
|
|
|
if (home_is_set == HOME_SET_NOT_LOCKED) {
|
2017-02-27 15:14:29 -04:00
|
|
|
Location loc;
|
|
|
|
if(ahrs.get_position(loc)) {
|
|
|
|
ahrs.set_home(loc);
|
|
|
|
Log_Write_Home_And_Origin();
|
2017-07-08 01:46:24 -03:00
|
|
|
gcs().send_home(loc);
|
2017-02-27 15:14:29 -04:00
|
|
|
}
|
2015-02-20 19:14:18 -04:00
|
|
|
}
|
2013-10-05 05:44:44 -03:00
|
|
|
barometer.update_calibration();
|
|
|
|
}
|
2017-09-18 02:39:00 -03:00
|
|
|
|
|
|
|
// sets ekf_origin if it has not been set.
|
|
|
|
// should only be used when there is no GPS to provide an absolute position
|
|
|
|
void Plane::set_ekf_origin(const Location& loc)
|
|
|
|
{
|
|
|
|
// check location is valid
|
|
|
|
if (!check_latlng(loc)) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// check if EKF origin has already been set
|
|
|
|
Location ekf_origin;
|
|
|
|
if (ahrs.get_origin(ekf_origin)) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (!ahrs.set_origin(loc)) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// log ahrs home and ekf origin dataflash
|
|
|
|
Log_Write_Home_And_Origin();
|
|
|
|
|
|
|
|
// send ekf origin to GCS
|
|
|
|
gcs().send_ekf_origin(loc);
|
|
|
|
}
|