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

Plane: move home state into AP_AHRS

This commit is contained in:
Peter Barker 2018-03-16 12:49:06 +11:00 committed by Randy Mackay
parent 45f2312bfe
commit 7fd859da65
11 changed files with 13 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
APMrover2/commands_logic.cpp
View File

@ -4,7 +4,7 @@
void Rover::update_mission(void)
{
if (control_mode == &mode_auto) {
if (home_is_set != HOME_UNSET && mission.num_commands() > 1) {
if (ahrs.home_is_set() && mission.num_commands() > 1) {
mission.update();
}
}

2
APMrover2/mode_rtl.cpp
View File

@ -4,7 +4,7 @@
bool ModeRTL::_enter()
{
// refuse RTL if home has not been set
if (rover.home_is_set == HOME_UNSET) {
if (!AP::ahrs().home_is_set()) {
return false;
}

5
ArduPlane/AP_Arming.cpp
View File

@ -18,11 +18,6 @@ const AP_Param::GroupInfo AP_Arming_Plane::var_info[] = {
AP_GROUPEND
};
enum HomeState AP_Arming_Plane::home_status() const
{
return plane.home_is_set;
}
/*
additional arming checks for plane

1
ArduPlane/AP_Arming.h
View File

@ -34,7 +34,6 @@ public:
protected:
bool ins_checks(bool report);
enum HomeState home_status() const override;
// parameters
AP_Int8 rudder_arming_value;

2
ArduPlane/ArduPlane.cpp
View File

@ -756,7 +756,7 @@ void Plane::update_navigation()
switch(control_mode) {
case AUTO:
if (home_is_set != HOME_UNSET) {
if (ahrs.home_is_set()) {
mission.update();
}
break;

10
ArduPlane/GCS_Mavlink.cpp
View File

@ -900,14 +900,14 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
new_home_loc.alt = packet.z * 100;
// handle relative altitude
if (packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT || packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
if (plane.home_is_set == HOME_UNSET) {
if (!AP::ahrs().home_is_set()) {
// cannot use relative altitude if home is not set
break;
}
new_home_loc.alt += plane.ahrs.get_home().alt;
}
plane.ahrs.set_home(new_home_loc);
plane.home_is_set = HOME_SET_NOT_LOCKED;
AP::ahrs().set_home_status(HOME_SET_NOT_LOCKED);
plane.Log_Write_Home_And_Origin();
gcs().send_home(new_home_loc);
result = MAV_RESULT_ACCEPTED;
@ -1194,7 +1194,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
break;
case MAV_CMD_GET_HOME_POSITION:
if (plane.home_is_set != HOME_UNSET) {
if (AP::ahrs().home_is_set()) {
send_home(plane.ahrs.get_home());
Location ekf_origin;
if (plane.ahrs.get_origin(ekf_origin)) {
@ -1300,7 +1300,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
new_home_loc.alt = (int32_t)(packet.param7 * 100.0f);
plane.ahrs.set_home(new_home_loc);
plane.home_is_set = HOME_SET_NOT_LOCKED;
AP::ahrs().set_home_status(HOME_SET_NOT_LOCKED);
plane.Log_Write_Home_And_Origin();
gcs().send_home(new_home_loc);
result = MAV_RESULT_ACCEPTED;
@ -1712,7 +1712,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
new_home_loc.lng = packet.longitude;
new_home_loc.alt = packet.altitude / 10;
plane.ahrs.set_home(new_home_loc);
plane.home_is_set = HOME_SET_NOT_LOCKED;
plane.ahrs.set_home_status(HOME_SET_NOT_LOCKED);
plane.Log_Write_Home_And_Origin();
gcs().send_home(new_home_loc);
gcs().send_text(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %um",

2
ArduPlane/Log.cpp
View File

@ -325,7 +325,7 @@ void Plane::Log_Write_Home_And_Origin()
#endif
// 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());
}
}

3
ArduPlane/Plane.h
View File

@ -684,9 +684,6 @@ private:
// Location structure defined in AP_Common
const struct Location &home = ahrs.get_home();
// Flag for if we have g_gps lock and have set the home location in AHRS
enum HomeState home_is_set = HOME_UNSET;
// The location of the previous waypoint. Used for track following and altitude ramp calculations
Location prev_WP_loc {};

2
ArduPlane/altitude.cpp
View File

@ -574,7 +574,7 @@ void Plane::rangefinder_height_update(void)
{
float distance = rangefinder.distance_cm_orient(ROTATION_PITCH_270)*0.01f;
if ((rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::RangeFinder_Good) && home_is_set != HOME_UNSET) {
if ((rangefinder.status_orient(ROTATION_PITCH_270) == RangeFinder::RangeFinder_Good) && ahrs.home_is_set()) {
if (!rangefinder_state.have_initial_reading) {
rangefinder_state.have_initial_reading = true;
rangefinder_state.initial_range = distance;

4
ArduPlane/commands.cpp
View File

@ -110,7 +110,7 @@ void Plane::init_home()
gcs().send_text(MAV_SEVERITY_INFO, "Init HOME");
ahrs.set_home(gps.location());
home_is_set = HOME_SET_NOT_LOCKED;
ahrs.set_home_status(HOME_SET_NOT_LOCKED);
Log_Write_Home_And_Origin();
gcs().send_home(gps.location());
@ -138,7 +138,7 @@ void Plane::update_home()
// significantly
return;
}
if (home_is_set == HOME_SET_NOT_LOCKED) {
if (ahrs.home_status() == HOME_SET_NOT_LOCKED) {
Location loc;
if(ahrs.get_position(loc)) {
ahrs.set_home(loc);

2
ArduPlane/commands_logic.cpp
View File

@ -937,7 +937,7 @@ void Plane::do_set_home(const AP_Mission::Mission_Command& cmd)
init_home();
} else {
ahrs.set_home(cmd.content.location);
home_is_set = HOME_SET_NOT_LOCKED;
ahrs.set_home_status(HOME_SET_NOT_LOCKED);
Log_Write_Home_And_Origin();
gcs().send_home(cmd.content.location);
// send ekf origin if set