mirror of https://github.com/ArduPilot/ardupilot
Copter: home is set becomes three state
This commit is contained in:
parent
a4fd36a775
commit
e081b9d1c7
|
@ -1,18 +1,27 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
|
||||
void set_home_is_set(bool b)
|
||||
// set_home_state - update home state
|
||||
void set_home_state(enum HomeState new_home_state)
|
||||
{
|
||||
// if no change, exit immediately
|
||||
if( ap.home_is_set == b )
|
||||
if (ap.home_state == new_home_state)
|
||||
return;
|
||||
|
||||
ap.home_is_set = b;
|
||||
if(b) {
|
||||
// update state
|
||||
ap.home_state = new_home_state;
|
||||
|
||||
// log if home has been set
|
||||
if (new_home_state == HOME_SET_NOT_LOCKED || new_home_state == HOME_SET_AND_LOCKED) {
|
||||
Log_Write_Event(DATA_SET_HOME);
|
||||
}
|
||||
}
|
||||
|
||||
// home_is_set - returns true if home positions has been set (to GPS location, armed location or EKF origin)
|
||||
bool home_is_set()
|
||||
{
|
||||
return (ap.home_state == HOME_SET_NOT_LOCKED || ap.home_state == HOME_SET_AND_LOCKED);
|
||||
}
|
||||
|
||||
// ---------------------------------------------
|
||||
void set_auto_armed(bool b)
|
||||
{
|
||||
|
|
|
@ -356,7 +356,7 @@ static bool sonar_enabled = true; // enable user switch for sonar
|
|||
//Documentation of GLobals:
|
||||
static union {
|
||||
struct {
|
||||
uint8_t home_is_set : 1; // 0
|
||||
uint8_t unused1 : 1; // 0
|
||||
uint8_t simple_mode : 2; // 1,2 // This is the state of simple mode : 0 = disabled ; 1 = SIMPLE ; 2 = SUPERSIMPLE
|
||||
uint8_t pre_arm_rc_check : 1; // 3 // true if rc input pre-arm checks have been completed successfully
|
||||
uint8_t pre_arm_check : 1; // 4 // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed
|
||||
|
@ -373,6 +373,9 @@ static union {
|
|||
uint8_t initialised : 1; // 17 // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes
|
||||
uint8_t land_complete_maybe : 1; // 18 // true if we may have landed (less strict version of land_complete)
|
||||
uint8_t throttle_zero : 1; // 19 // true if the throttle stick is at zero, debounced
|
||||
uint8_t system_time_set : 1; // 20 // true if the system time has been set from the GPS
|
||||
uint8_t gps_base_pos_set : 1; // 21 // true when the gps base position has been set (used for RTK gps only)
|
||||
enum HomeState home_state : 2; // 22,23 - home status (unset, set, locked)
|
||||
};
|
||||
uint32_t value;
|
||||
} ap;
|
||||
|
@ -1153,7 +1156,6 @@ static void one_hz_loop()
|
|||
static void update_GPS(void)
|
||||
{
|
||||
static uint32_t last_gps_reading[GPS_MAX_INSTANCES]; // time of last gps message
|
||||
static uint8_t ground_start_count = 10; // counter used to grab at least 10 reads before commiting the Home location
|
||||
bool report_gps_glitch;
|
||||
bool gps_updated = false;
|
||||
|
||||
|
@ -1175,7 +1177,7 @@ static void update_GPS(void)
|
|||
|
||||
if (gps_updated) {
|
||||
// run glitch protection and update AP_Notify if home has been initialised
|
||||
if (ap.home_is_set) {
|
||||
if (home_is_set()) {
|
||||
gps_glitch.check_position();
|
||||
report_gps_glitch = (gps_glitch.glitching() && !ap.usb_connected && hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
|
||||
if (AP_Notify::flags.gps_glitching != report_gps_glitch) {
|
||||
|
@ -1188,49 +1190,18 @@ static void update_GPS(void)
|
|||
}
|
||||
}
|
||||
|
||||
// set system time if necessary
|
||||
set_system_time_from_GPS();
|
||||
|
||||
// update home from GPS location if necessary
|
||||
update_home_from_GPS();
|
||||
|
||||
// check gps base position (used for RTK only)
|
||||
check_gps_base_pos();
|
||||
|
||||
// checks to initialise home and take location based pictures
|
||||
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
|
||||
|
||||
// check if we can initialise home yet
|
||||
if (!ap.home_is_set) {
|
||||
// if we have a 3d lock and valid location
|
||||
if(gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.location().lat != 0) {
|
||||
if (ground_start_count > 0 ) {
|
||||
ground_start_count--;
|
||||
} else {
|
||||
// after 10 successful reads store home location
|
||||
// ap.home_is_set will be true so this will only happen once
|
||||
ground_start_count = 0;
|
||||
init_home();
|
||||
|
||||
// set system clock for log timestamps
|
||||
hal.util->set_system_clock(gps.time_epoch_usec());
|
||||
|
||||
if (g.compass_enabled) {
|
||||
// Set compass declination automatically
|
||||
compass.set_initial_location(gps.location().lat, gps.location().lng);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// start again if we lose 3d lock
|
||||
ground_start_count = 10;
|
||||
}
|
||||
} else {
|
||||
// update home position when not armed
|
||||
if (!motors.armed()) {
|
||||
update_home();
|
||||
}
|
||||
}
|
||||
|
||||
//If we are not currently armed, and we're ready to
|
||||
//enter RTK mode, then capture current state as home,
|
||||
//and enter RTK fixes!
|
||||
if (!motors.armed() && gps.can_calculate_base_pos()) {
|
||||
|
||||
gps.calculate_base_pos();
|
||||
|
||||
}
|
||||
|
||||
#if CAMERA == ENABLED
|
||||
if (camera.update_location(current_loc) == true) {
|
||||
do_take_picture();
|
||||
|
|
|
@ -1,17 +1,102 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
// run this at setup on the ground
|
||||
// -------------------------------
|
||||
static void init_home()
|
||||
/*
|
||||
* the home_state has a number of possible values (see enum HomeState in defines.h's)
|
||||
* HOME_UNSET = home is not set, no GPS positions yet received
|
||||
* HOME_SET_NOT_LOCKED = home is set to EKF origin or armed location (can be moved)
|
||||
* HOME_SET_AND_LOCKED = home has been set by user, cannot be moved except by user initiated do-set-home command
|
||||
*/
|
||||
|
||||
static uint8_t ground_start_count = 10; // counter used to grab at least 10 reads before accepting a GPS location as home location
|
||||
|
||||
// checks if we should update ahrs/RTL home position from GPS
|
||||
static void update_home_from_GPS()
|
||||
{
|
||||
set_home_is_set(true);
|
||||
// exit immediately if counter has run down and home already set
|
||||
if (ground_start_count == 0 && ap.home_state != HOME_UNSET) {
|
||||
return;
|
||||
}
|
||||
|
||||
// copter uses 0 home altitude
|
||||
Location loc = gps.location();
|
||||
// if counter has not run down
|
||||
if (ground_start_count > 0) {
|
||||
|
||||
// reset counter if we do not have GPS lock
|
||||
if (gps.status() < AP_GPS::GPS_OK_FIX_3D) {
|
||||
ground_start_count = 10;
|
||||
|
||||
// count down for 10 consecutive locks
|
||||
} else {
|
||||
ground_start_count--;
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
// move home to current gps location (this will set home_state to HOME_SET)
|
||||
set_home(gps.location());
|
||||
}
|
||||
|
||||
// set_home_to_current_location - set home to current GPS location
|
||||
static bool set_home_to_current_location() {
|
||||
// exit with failure if we haven't had 10 good GPS position
|
||||
if (ground_start_count > 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// set home to latest gps location
|
||||
return set_home(gps.location());
|
||||
}
|
||||
|
||||
// set_home_to_current_location_and_lock - set home to current location and lock so it cannot be moved
|
||||
static bool set_home_to_current_location_and_lock()
|
||||
{
|
||||
if (set_home_to_current_location()) {
|
||||
set_home_state(HOME_SET_AND_LOCKED);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// set_home_and_lock - sets ahrs home (used for RTL) to specified location and locks so it cannot be moved
|
||||
// unless this function is called again
|
||||
static bool set_home_and_lock(const Location& loc)
|
||||
{
|
||||
if (set_home(loc)) {
|
||||
set_home_state(HOME_SET_AND_LOCKED);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// set_home - sets ahrs home (used for RTL) to specified location
|
||||
// initialises inertial nav and compass on first call
|
||||
// returns true if home location set successfully
|
||||
static bool set_home(const Location& loc)
|
||||
{
|
||||
// check location is valid
|
||||
if (loc.lat == 0 && loc.lng == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// set ahrs home (used for RTL)
|
||||
ahrs.set_home(loc);
|
||||
|
||||
inertial_nav.setup_home_position();
|
||||
// init inav and compass declination
|
||||
if (ap.home_state == HOME_UNSET) {
|
||||
// Set compass declination automatically
|
||||
if (g.compass_enabled) {
|
||||
compass.set_initial_location(gps.location().lat, gps.location().lng);
|
||||
}
|
||||
// set inertial nav home
|
||||
inertial_nav.setup_home_position();
|
||||
// update navigation scalers. used to offset the shrinking longitude as we go towards the poles
|
||||
scaleLongDown = longitude_scale(loc);
|
||||
scaleLongUp = 1.0f/scaleLongDown;
|
||||
// record home is set
|
||||
set_home_state(HOME_SET_NOT_LOCKED);
|
||||
}
|
||||
|
||||
// To-Do: doing the stuff below constantly while armed could lead to lots of logging or performance hit?
|
||||
|
||||
// log new home position which mission library will pull from ahrs
|
||||
if (should_log(MASK_LOG_CMD)) {
|
||||
|
@ -21,24 +106,50 @@ static void init_home()
|
|||
}
|
||||
}
|
||||
|
||||
// update navigation scalers. used to offset the shrinking longitude as we go towards the poles
|
||||
scaleLongDown = longitude_scale(loc);
|
||||
scaleLongUp = 1.0f/scaleLongDown;
|
||||
// return success
|
||||
return true;
|
||||
}
|
||||
|
||||
// update_home - reset home to current location
|
||||
// should be called only when vehicle is disarmed
|
||||
static void update_home()
|
||||
// far_from_EKF_origin - checks if a location is too far from the EKF origin
|
||||
// returns true if too far
|
||||
static bool far_from_EKF_origin(const Location& loc)
|
||||
{
|
||||
// copter uses 0 home altitude
|
||||
Location loc = gps.location();
|
||||
// check distance to EKF origin
|
||||
const struct Location &ekf_origin = inertial_nav.get_origin();
|
||||
if (get_distance(ekf_origin, loc) > EKF_ORIGIN_MAX_DIST_M) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// set ahrs object's home position
|
||||
ahrs.set_home(loc);
|
||||
|
||||
// update navigation scalers. used to offset the shrinking longitude as we go towards the poles
|
||||
scaleLongDown = longitude_scale(loc);
|
||||
scaleLongUp = 1.0f/scaleLongDown;
|
||||
// close enough to origin
|
||||
return false;
|
||||
}
|
||||
|
||||
// checks if we should update ahrs/RTL home position from GPS
|
||||
static void set_system_time_from_GPS()
|
||||
{
|
||||
// exit immediately if system time already set
|
||||
if (ap.system_time_set) {
|
||||
return;
|
||||
}
|
||||
|
||||
// if we have a 3d lock and valid location
|
||||
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
|
||||
// set system clock for log timestamps
|
||||
hal.util->set_system_clock(gps.time_epoch_usec());
|
||||
ap.system_time_set = true;
|
||||
Log_Write_Event(DATA_SYSTEM_TIME_SET);
|
||||
}
|
||||
}
|
||||
|
||||
// check_gps_base_pos - sets gps base position (used for RTK only)
|
||||
static void check_gps_base_pos()
|
||||
{
|
||||
if (!ap.gps_base_pos_set && !motors.armed() && home_is_set()) {
|
||||
// if we're ready to enter RTK mode, then capture current state as home,
|
||||
// and enter RTK fixes
|
||||
if (gps.can_calculate_base_pos()) {
|
||||
gps.calculate_base_pos();
|
||||
}
|
||||
ap.gps_base_pos_set = true;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -334,6 +334,10 @@
|
|||
# define DCMCHECK_THRESHOLD_DEFAULT 0.8f // DCM checker's default yaw error threshold above which we will abandon horizontal position hold. The units are sin(angle) so 0.8 = about 60degrees of error
|
||||
#endif
|
||||
|
||||
#ifndef EKF_ORIGIN_MAX_DIST_M
|
||||
# define EKF_ORIGIN_MAX_DIST_M 50000 // EKF origin and waypoints (including home) must be within 50km
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// MAGNETOMETER
|
||||
#ifndef MAGNETOMETER
|
||||
|
|
|
@ -171,6 +171,13 @@
|
|||
#define WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL 2 // auto pilot will face next waypoint except when doing RTL at which time it will stay in it's last
|
||||
#define WP_YAW_BEHAVIOR_LOOK_AHEAD 3 // auto pilot will look ahead during missions and rtl (primarily meant for traditional helicotpers)
|
||||
|
||||
// home states (held in ap.home_state flags)
|
||||
enum HomeState {
|
||||
HOME_UNSET, // home is unset, no GPS positions yet received
|
||||
HOME_SET_NOT_LOCKED, // home is set to EKF origin or armed location (can be moved)
|
||||
HOME_SET_AND_LOCKED // home has been set by user, cannot be moved except by user initiated do-set-home command
|
||||
};
|
||||
|
||||
// Auto modes
|
||||
enum AutoMode {
|
||||
Auto_TakeOff,
|
||||
|
@ -261,6 +268,7 @@ enum FlipState {
|
|||
#define DATA_MAVLINK_INT16 3
|
||||
#define DATA_MAVLINK_INT8 4
|
||||
#define DATA_AP_STATE 7
|
||||
#define DATA_SYSTEM_TIME_SET 8
|
||||
#define DATA_INIT_SIMPLE_BEARING 9
|
||||
#define DATA_ARMED 10
|
||||
#define DATA_DISARMED 11
|
||||
|
|
Loading…
Reference in New Issue