Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: home is set becomes three state

This commit is contained in:
Randy Mackay 2015-02-09 20:23:49 +09:00
parent a4fd36a775
commit e081b9d1c7
5 changed files with 172 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
ArduCopter/AP_State.pde
View File

@ -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)
{

57
ArduCopter/ArduCopter.pde
View File

@ -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();

151
ArduCopter/commands.pde
View File

@ -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);
// 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;
}
}

4
ArduCopter/config.h
View File

@ -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

8
ArduCopter/defines.h
View File

@ -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