aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Rover: move home state into AP_AHRS

Browse Source
This commit is contained in:
Peter Barker 2018-03-16 12:06:46 +11:00 committed by Randy Mackay
parent cc5af90d1a
commit 9d0da4a71f
6 changed files with 8 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
APMrover2/AP_Arming.cpp
View File

@ -1,11 +1,6 @@
#include "AP_Arming.h" #include "AP_Arming.h"
#include "Rover.h" #include "Rover.h"
enum HomeState AP_Arming_Rover::home_status() const
{
return rover.home_is_set;
}
// perform pre_arm_rc_checks checks // perform pre_arm_rc_checks checks
bool AP_Arming_Rover::pre_arm_rc_checks(const bool display_failure) bool AP_Arming_Rover::pre_arm_rc_checks(const bool display_failure)
{ {

1
APMrover2/AP_Arming.h
View File

@ -25,7 +25,6 @@ public:
bool gps_checks(bool display_failure) override; bool gps_checks(bool display_failure) override;
protected: protected:
enum HomeState home_status() const override;
bool fence_checks(bool report); bool fence_checks(bool report);
bool proximity_check(bool report); bool proximity_check(bool report);

4
APMrover2/GCS_Mavlink.cpp
View File

@ -691,7 +691,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
new_home_loc.alt = packet.z * 100; new_home_loc.alt = packet.z * 100;
// handle relative altitude // handle relative altitude
if (packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT || packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { if (packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT || packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
if (rover.home_is_set == HOME_UNSET) { if (!rover.ahrs.home_is_set()) {
// cannot use relative altitude if home is not set // cannot use relative altitude if home is not set
break; break;
} }
@ -892,7 +892,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
break; break;
case MAV_CMD_GET_HOME_POSITION: case MAV_CMD_GET_HOME_POSITION:
if (rover.home_is_set != HOME_UNSET) { if (AP::ahrs().home_is_set()) {
send_home(rover.ahrs.get_home()); send_home(rover.ahrs.get_home());
Location ekf_origin; Location ekf_origin;
if (rover.ahrs.get_origin(ekf_origin)) { if (rover.ahrs.get_origin(ekf_origin)) {

2
APMrover2/Log.cpp
View File

@ -231,7 +231,7 @@ void Rover::Log_Write_Home_And_Origin()
#endif #endif
// log ahrs home if set // log ahrs home if set
if (home_is_set != HOME_UNSET) { if (ahrs.home_is_set()) {
DataFlash.Log_Write_Origin(LogOriginType::ahrs_home, ahrs.get_home()); DataFlash.Log_Write_Origin(LogOriginType::ahrs_home, ahrs.get_home());
} }
} }

3
APMrover2/Rover.h
View File

@ -316,9 +316,6 @@ private:
// The home location used for RTL. The location is set when we first get stable GPS lock // The home location used for RTL. The location is set when we first get stable GPS lock
const struct Location &home; const struct Location &home;
// 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 // true if the system time has been set from the GPS
bool system_time_set; bool system_time_set;

10
APMrover2/commands.cpp
View File

@ -4,7 +4,7 @@
void Rover::update_home_from_EKF() void Rover::update_home_from_EKF()
{ {
// exit immediately if home already set // exit immediately if home already set
if (home_is_set != HOME_UNSET) { if (ahrs.home_is_set()) {
return; return;
} }
@ -50,9 +50,9 @@ bool Rover::set_home(const Location& loc, bool lock)
ahrs.set_home(loc); ahrs.set_home(loc);
// init compass declination // init compass declination
if (home_is_set == HOME_UNSET) { if (!ahrs.home_is_set()) {
// record home is set // record home is set
home_is_set = HOME_SET_NOT_LOCKED; ahrs.set_home_status(HOME_SET_NOT_LOCKED);
// log new home position which mission library will pull from ahrs // log new home position which mission library will pull from ahrs
if (should_log(MASK_LOG_CMD)) { if (should_log(MASK_LOG_CMD)) {
@ -65,7 +65,7 @@ bool Rover::set_home(const Location& loc, bool lock)
// lock home position // lock home position
if (lock) { if (lock) {
home_is_set = HOME_SET_AND_LOCKED; ahrs.set_home_status(HOME_SET_AND_LOCKED);
} }
// Save Home to EEPROM // Save Home to EEPROM
@ -143,7 +143,7 @@ void Rover::set_system_time_from_GPS()
*/ */
void Rover::update_home() void Rover::update_home()
{ {
if (home_is_set == HOME_SET_NOT_LOCKED) { if (ahrs.home_status() == HOME_SET_NOT_LOCKED) {
Location loc; Location loc;
if (ahrs.get_position(loc)) { if (ahrs.get_position(loc)) {
if (get_distance(loc, ahrs.get_home()) > DISTANCE_HOME_MAX) { if (get_distance(loc, ahrs.get_home()) > DISTANCE_HOME_MAX) {