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

Rover: set home from ekf position

This commit is contained in:
Randy Mackay 2017-06-05 16:55:24 +09:00
parent 0da6e73d76
commit 584974fd74
7 changed files with 131 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
APMrover2/APMrover2.cpp
View File

@ -173,6 +173,9 @@ void Rover::ahrs_update()
ahrs.update();
// update home from EKF if necessary
update_home_from_EKF();
// if using the EKF get a speed update now (from accelerometers)
Vector3f velocity;
if (ahrs.get_velocity_NED(velocity)) {
@ -399,36 +402,14 @@ void Rover::update_GPS_10Hz(void)
{
have_position = ahrs.get_position(current_loc);
if (gps.last_message_time_ms() != last_gps_msg_ms && gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
if (gps.last_message_time_ms() != last_gps_msg_ms) {
last_gps_msg_ms = gps.last_message_time_ms();
if (ground_start_count > 1) {
ground_start_count--;
// set system time if necessary
set_system_time_from_GPS();
} else if (ground_start_count == 1) {
// We countdown N number of good GPS fixes
// so that the altitude is more accurate
// -------------------------------------
if (current_loc.lat == 0 && current_loc.lng == 0) {
ground_start_count = 20;
} else {
init_home();
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
// set system clock for log timestamps
const uint64_t gps_timestamp = gps.time_epoch_usec();
hal.util->set_system_clock(gps_timestamp);
// update signing timestamp
GCS_MAVLINK::update_signing_timestamp(gps_timestamp);
if (g.compass_enabled) {
// Set compass declination automatically
compass.set_initial_location(gps.location().lat, gps.location().lng);
}
ground_start_count = 0;
}
}
// get ground speed estimate from AHRS
ground_speed = ahrs.groundspeed();
@ -439,6 +420,7 @@ void Rover::update_GPS_10Hz(void)
#endif
}
}
}
void Rover::update_current_mode(void)
{

22
APMrover2/GCS_Mavlink.cpp
View File

@ -1046,29 +1046,17 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
// param7 : altitude
result = MAV_RESULT_FAILED; // assume failure
if (is_equal(packet.param1, 1.0f)) {
rover.init_home();
if (rover.set_home_to_current_location(true)) {
result = MAV_RESULT_ACCEPTED;
}
} else {
if (is_zero(packet.param5) && is_zero(packet.param6) && is_zero(packet.param7)) {
// don't allow the 0,0 position
break;
}
// sanity check location
if (!check_latlng(packet.param5, packet.param6)) {
break;
}
Location new_home_loc {};
new_home_loc.lat = static_cast<int32_t>(packet.param5 * 1.0e7f);
new_home_loc.lng = static_cast<int32_t>(packet.param6 * 1.0e7f);
new_home_loc.alt = static_cast<int32_t>(packet.param7 * 100.0f);
rover.ahrs.set_home(new_home_loc);
rover.home_is_set = HOME_SET_NOT_LOCKED;
rover.Log_Write_Home_And_Origin();
GCS_MAVLINK::send_home_all(new_home_loc);
if (rover.set_home(new_home_loc, true)) {
result = MAV_RESULT_ACCEPTED;
rover.gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %.2fm",
static_cast<double>(new_home_loc.lat * 1.0e-7f),
static_cast<double>(new_home_loc.lng * 1.0e-7f),
static_cast<double>(new_home_loc.alt * 0.01f));
}
}
break;
}

1
APMrover2/Rover.cpp
View File

@ -43,7 +43,6 @@ Rover::Rover(void) :
camera_mount(ahrs, current_loc),
#endif
control_mode(INITIALISING),
ground_start_count(20),
throttle(500),
#if FRSKY_TELEM_ENABLED == ENABLED
frsky_telemetry(ahrs, battery, sonar),

12
APMrover2/Rover.h
View File

@ -243,10 +243,6 @@ private:
// notification object for LEDs, buzzers etc (parameter set to false disables external leds)
AP_Notify notify;
// A counter used to count down valid gps fixes to allow the gps estimate to settle
// before recording our home position (and executing a ground start if we booted with an air start)
uint8_t ground_start_count;
// true if we have a position estimate from AHRS
bool have_position;
@ -335,6 +331,9 @@ private:
// Flag for if we have g_gps lock and have set the home location in AHRS
enum HomeState home_is_set = HOME_UNSET;
// true if the system time has been set from the GPS
bool system_time_set;
// The location of the previous waypoint. Used for track following and altitude ramp calculations
struct Location prev_WP;
// The location of the current/active waypoint. Used for track following
@ -476,7 +475,10 @@ private:
void set_auto_WP(const struct Location& loc);
void set_guided_WP(const struct Location& loc);
void set_guided_velocity(float target_steer_speed, float target_speed);
void init_home();
void update_home_from_EKF();
bool set_home_to_current_location(bool lock);
bool set_home(const Location& loc, bool lock);
void set_system_time_from_GPS();
void restart_nav();
void exit_mission();
void do_RTL(void);

116
APMrover2/commands.cpp
View File

@ -60,32 +60,118 @@ void Rover::set_guided_velocity(float target_steer_speed, float target_speed)
rover.rtl_complete = false;
}
// run this at setup on the ground
// -------------------------------
void Rover::init_home()
// checks if we should update ahrs home position from the EKF's position
void Rover::update_home_from_EKF()
{
if (!have_position) {
// we need position information
// exit immediately if home already set
if (home_is_set != HOME_UNSET) {
return;
}
gcs_send_text(MAV_SEVERITY_INFO, "Init HOME");
// move home to current ekf location (this will set home_state to HOME_SET)
set_home_to_current_location(false);
}
ahrs.set_home(gps.location());
// set ahrs home to current location from EKF reported location or GPS
bool Rover::set_home_to_current_location(bool lock)
{
// use position from EKF if available otherwise use GPS
Location temp_loc;
if (ahrs.get_position(temp_loc)) {
return set_home(temp_loc, lock);
}
return false;
}
// sets ahrs home to specified location
// returns true if home location set successfully
bool Rover::set_home(const Location& loc, bool lock)
{
// check location is valid
if (loc.lat == 0 && loc.lng == 0 && loc.alt == 0) {
return false;
}
if (!check_latlng(loc)) {
return false;
}
// set EKF origin to home if it hasn't been set yet
Location ekf_origin;
if (!ahrs.get_origin(ekf_origin)) {
ahrs.set_origin(loc);
}
// set ahrs home
ahrs.set_home(loc);
// init compass declination
if (home_is_set == HOME_UNSET) {
// Set compass declination automatically
if (g.compass_enabled) {
compass.set_initial_location(loc.lat, loc.lng);
}
// record home is set
home_is_set = HOME_SET_NOT_LOCKED;
Log_Write_Home_And_Origin();
GCS_MAVLINK::send_home_all(gps.location());
// log new home position which mission library will pull from ahrs
if (should_log(MASK_LOG_CMD)) {
AP_Mission::Mission_Command temp_cmd;
if (mission.read_cmd_from_storage(0, temp_cmd)) {
DataFlash.Log_Write_Mission_Cmd(mission, temp_cmd);
}
}
// initialise navigation to home
next_WP = prev_WP = home;
// Load home for a default guided_WP
set_guided_WP(home);
}
// lock home position
if (lock) {
home_is_set = HOME_SET_AND_LOCKED;
}
// Save Home to EEPROM
mission.write_home_to_storage();
// Save prev loc
// -------------
next_WP = prev_WP = home;
// log ahrs home and ekf origin dataflash
Log_Write_Home_And_Origin();
// Load home for a default guided_WP
// -------------
set_guided_WP(home);
// send new home location to GCS
GCS_MAVLINK::send_home_all(loc);
// send text of home position to ground stations
rover.gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %.2fm",
static_cast<double>(loc.lat * 1.0e-7f),
static_cast<double>(loc.lng * 1.0e-7f),
static_cast<double>(loc.alt * 0.01f));
// return success
return true;
}
// checks if we should update ahrs/RTL home position from GPS
void Rover::set_system_time_from_GPS()
{
// exit immediately if system time already set
if (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
const uint64_t gps_timestamp = gps.time_epoch_usec();
hal.util->set_system_clock(gps_timestamp);
// update signing timestamp
GCS_MAVLINK::update_signing_timestamp(gps_timestamp);
system_time_set = true;
}
}
void Rover::restart_nav()

7
APMrover2/commands_logic.cpp
View File

@ -542,12 +542,9 @@ void Rover::do_change_speed(const AP_Mission::Mission_Command& cmd)
void Rover::do_set_home(const AP_Mission::Mission_Command& cmd)
{
if (cmd.p1 == 1 && have_position) {
init_home();
set_home_to_current_location(false);
} else {
ahrs.set_home(cmd.content.location);
home_is_set = HOME_SET_NOT_LOCKED;
Log_Write_Home_And_Origin();
GCS_MAVLINK::send_home_all(cmd.content.location);
set_home(cmd.content.location, false);
}
}

2
APMrover2/control_modes.cpp
View File

@ -99,7 +99,7 @@ void Rover::read_trim_switch()
mission.clear();
if (channel_steer->get_control_in() > 3000) {
// if roll is full right store the current location as home
init_home();
set_home_to_current_location(false);
}
break;