Browse Source

Rover: GCS can report simple/supersimple input modes

master
Peter Barker 6 years ago committed by Peter Barker
parent
commit
903b2e381b
  1. 16
      APMrover2/GCS_Rover.cpp
  2. 3
      APMrover2/GCS_Rover.h
  3. 4
      APMrover2/mode.h

16
APMrover2/GCS_Rover.cpp

@ -4,6 +4,22 @@ @@ -4,6 +4,22 @@
#include <AP_RangeFinder/RangeFinder_Backend.h>
bool GCS_Rover::simple_input_active() const
{
if (rover.control_mode != &rover.mode_simple) {
return false;
}
return (rover.g2.simple_type == ModeSimple::Simple_InitialHeading);
}
bool GCS_Rover::supersimple_input_active() const
{
if (rover.control_mode != &rover.mode_simple) {
return false;
}
return (rover.g2.simple_type == ModeSimple::Simple_CardinalDirections);
}
// update error mask of sensors and subsystems. The mask
// uses the MAV_SYS_STATUS_* values from mavlink. If a bit is set
// then it indicates that the sensor or subsystem is present but

3
APMrover2/GCS_Rover.h

@ -24,6 +24,9 @@ public: @@ -24,6 +24,9 @@ public:
void update_sensor_status_flags(void) override;
bool simple_input_active() const override;
bool supersimple_input_active() const override;
private:
GCS_MAVLINK_Rover _chan[MAVLINK_COMM_NUM_BUFFERS];

4
APMrover2/mode.h

@ -615,14 +615,14 @@ public: @@ -615,14 +615,14 @@ public:
void update() override;
void init_heading();
private:
// simple type enum used for SIMPLE_TYPE parameter
enum simple_type {
Simple_InitialHeading = 0,
Simple_CardinalDirections = 1,
};
private:
float _initial_heading_cd; // vehicle heading (in centi-degrees) at moment vehicle was armed
float _desired_heading_cd; // latest desired heading (in centi-degrees) from pilot
};

Loading…
Cancel
Save