mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-02 22:18:28 -04:00
Rover: set home from ekf position
This commit is contained in:
parent
0da6e73d76
commit
584974fd74
@ -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,44 +402,23 @@ 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();
|
||||
// get ground speed estimate from AHRS
|
||||
ground_speed = ahrs.groundspeed();
|
||||
|
||||
#if CAMERA == ENABLED
|
||||
if (camera.update_location(current_loc, rover.ahrs) == true) {
|
||||
do_take_picture();
|
||||
}
|
||||
if (camera.update_location(current_loc, rover.ahrs) == true) {
|
||||
do_take_picture();
|
||||
}
|
||||
#endif
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -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);
|
||||
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));
|
||||
if (rover.set_home(new_home_loc, true)) {
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
@ -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),
|
||||
|
@ -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);
|
||||
|
@ -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());
|
||||
home_is_set = HOME_SET_NOT_LOCKED;
|
||||
Log_Write_Home_And_Origin();
|
||||
GCS_MAVLINK::send_home_all(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 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()
|
||||
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -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;
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user