diff --git a/ArduCopter/AP_State.pde b/ArduCopter/AP_State.pde index fcba1c36c3..1ad9031f7f 100644 --- a/ArduCopter/AP_State.pde +++ b/ArduCopter/AP_State.pde @@ -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) { diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 64c8855ba5..e0c49bd2c6 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -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(); diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index 3dc10b5e71..c1a6df3da6 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -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; + } +} diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 1e56b8f640..fb7fde8a4e 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -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 diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 972d2eba7c..67092c7566 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -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