Browse Source

Copter: move LAND state variables to be members rather than static

Also rename one of them as we may be using (e.g.) OF to control position rather
than GPS.
zr-v5.1
Peter Barker 5 years ago committed by Peter Barker
parent
commit
a274697776
  1. 10
      ArduCopter/mode.h
  2. 19
      ArduCopter/mode_land.cpp

10
ArduCopter/mode.h

@ -912,6 +912,11 @@ public: @@ -912,6 +912,11 @@ public:
void do_not_use_GPS();
// returns true if LAND mode is trying to control X/Y position
bool controlling_position() const { return control_position; }
void set_land_pause(bool new_value) { land_pause = new_value; }
protected:
const char *name() const override { return "LAND"; }
@ -921,6 +926,11 @@ private: @@ -921,6 +926,11 @@ private:
void gps_run();
void nogps_run();
bool control_position; // true if we are using an external reference to control position
uint32_t land_start_time;
bool land_pause;
};

19
ArduCopter/mode_land.cpp

@ -1,17 +1,11 @@ @@ -1,17 +1,11 @@
#include "Copter.h"
// FIXME? why are these static?
static bool land_with_gps;
static uint32_t land_start_time;
static bool land_pause;
// land_init - initialise land controller
bool ModeLand::init(bool ignore_checks)
{
// check if we have GPS and decide which LAND we're going to do
land_with_gps = copter.position_ok();
if (land_with_gps) {
control_position = copter.position_ok();
if (control_position) {
// set target to stopping point
Vector3f stopping_point;
loiter_nav->get_stopping_point_xy(stopping_point);
@ -54,7 +48,7 @@ bool ModeLand::init(bool ignore_checks) @@ -54,7 +48,7 @@ bool ModeLand::init(bool ignore_checks)
// should be called at 100hz or more
void ModeLand::run()
{
if (land_with_gps) {
if (control_position) {
gps_run();
} else {
nogps_run();
@ -150,7 +144,7 @@ void ModeLand::nogps_run() @@ -150,7 +144,7 @@ void ModeLand::nogps_run()
// has no effect if we are not already in LAND mode
void ModeLand::do_not_use_GPS()
{
land_with_gps = false;
control_position = false;
}
// set_mode_land_with_pause - sets mode to LAND and triggers 4 second delay before descent starts
@ -158,7 +152,7 @@ void ModeLand::do_not_use_GPS() @@ -158,7 +152,7 @@ void ModeLand::do_not_use_GPS()
void Copter::set_mode_land_with_pause(ModeReason reason)
{
set_mode(Mode::Number::LAND, reason);
land_pause = true;
mode_land.set_land_pause(true);
// alert pilot to mode change
AP_Notify::events.failsafe_mode_change = 1;
@ -167,5 +161,6 @@ void Copter::set_mode_land_with_pause(ModeReason reason) @@ -167,5 +161,6 @@ void Copter::set_mode_land_with_pause(ModeReason reason)
// landing_with_GPS - returns true if vehicle is landing using GPS
bool Copter::landing_with_GPS()
{
return (flightmode->mode_number() == Mode::Number::LAND && land_with_gps);
return (flightmode->mode_number() == Mode::Number::LAND &&
mode_land.controlling_position());
}

Loading…
Cancel
Save