Browse Source

Plane: move GCS functions up to superclass GCS

master
Peter Barker 8 years ago committed by Francisco Ferreira
parent
commit
5b3b61a2e4
  1. 72
      ArduPlane/GCS_Plane.cpp
  2. 26
      ArduPlane/GCS_Plane.h

72
ArduPlane/GCS_Plane.cpp

@ -1,53 +1,22 @@ @@ -1,53 +1,22 @@
#include "GCS_Plane.h"
#include "Plane.h"
void GCS_Plane::reset_cli_timeout()
bool GCS_Plane::cli_enabled() const
{
for (uint8_t i=0; i<_num_gcs; i++) {
_chan[i].reset_cli_timeout();
}
}
void GCS_Plane::send_message(enum ap_message id)
{
for (uint8_t i=0; i<_num_gcs; i++) {
if (_chan[i].initialised) {
_chan[i].send_message(id);
}
}
}
void GCS_Plane::data_stream_send()
{
for (uint8_t i=0; i<_num_gcs; i++) {
if (_chan[i].initialised) {
_chan[i].data_stream_send();
}
}
}
void GCS_Plane::update(void)
{
for (uint8_t i=0; i<_num_gcs; i++) {
if (_chan[i].initialised) {
_chan[i].update(_run_cli);
}
}
#if CLI_ENABLED == ENABLED
return plane.g.cli_enabled;
#else
return false;
#endif
}
void GCS_Plane::send_mission_item_reached_message(uint16_t mission_index)
{
for (uint8_t i=0; i<_num_gcs; i++) {
if (_chan[i].initialised) {
_chan[i].mission_item_reached_index = mission_index;
_chan[i].send_message(MSG_MISSION_ITEM_REACHED);
}
}
AP_HAL::BetterStream* GCS_Plane::cliSerial() {
return plane.cliSerial;
}
void GCS_Plane::send_airspeed_calibration(const Vector3f &vg)
{
for (uint8_t i=0; i<_num_gcs; i++) {
for (uint8_t i=0; i<num_gcs(); i++) {
if (_chan[i].initialised) {
if (HAVE_PAYLOAD_SPACE((mavlink_channel_t)i, AIRSPEED_AUTOCAL)) {
plane.airspeed.log_mavlink_send((mavlink_channel_t)i, vg);
@ -55,26 +24,3 @@ void GCS_Plane::send_airspeed_calibration(const Vector3f &vg) @@ -55,26 +24,3 @@ void GCS_Plane::send_airspeed_calibration(const Vector3f &vg)
}
}
}
void GCS_Plane::setup_uarts(AP_SerialManager &serial_manager)
{
for (uint8_t i = 1; i < MAVLINK_COMM_NUM_BUFFERS; i++) {
_chan[i].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, i);
}
}
#if CLI_ENABLED == ENABLED
void GCS_Plane::handle_interactive_setup()
{
if (plane.g.cli_enabled == 1) {
const char *msg = "\nPress ENTER 3 times to start interactive setup\n";
plane.cliSerial->printf("%s\n", msg);
if (_chan[1].initialised && (_chan[1].get_uart() != NULL)) {
_chan[1].get_uart()->printf("%s\n", msg);
}
if (num_gcs() > 2 && _chan[2].initialised && (_chan[2].get_uart() != NULL)) {
_chan[2].get_uart()->printf("%s\n", msg);
}
}
}
#endif

26
ArduPlane/GCS_Plane.h

@ -6,35 +6,25 @@ @@ -6,35 +6,25 @@
class GCS_Plane : public GCS
{
friend class Plane; // for temporary access to num_gcs and links
friend class Plane; // for access to _chan in parameter declarations
public:
FUNCTOR_TYPEDEF(run_cli_fn, void, AP_HAL::UARTDriver*);
// return the number of valid GCS objects
uint8_t num_gcs() const { return _num_gcs; };
uint8_t num_gcs() const override { return ARRAY_SIZE(_chan); };
// return GCS link at offset ofs
GCS_MAVLINK_Plane &chan(const uint8_t ofs) { return _chan[ofs]; };
GCS_MAVLINK_Plane &chan(const uint8_t ofs) override {
return _chan[ofs];
};
void reset_cli_timeout();
void send_message(enum ap_message id);
void send_mission_item_reached_message(uint16_t mission_index);
void data_stream_send();
void update();
void send_airspeed_calibration(const Vector3f &vg);
void set_run_cli_func(run_cli_fn run_cli) { _run_cli = run_cli; }
void setup_uarts(AP_SerialManager &serial_manager);
#if CLI_ENABLED == ENABLED
void handle_interactive_setup();
#endif
private:
uint8_t _num_gcs = MAVLINK_COMM_NUM_BUFFERS;
GCS_MAVLINK_Plane _chan[MAVLINK_COMM_NUM_BUFFERS];
run_cli_fn _run_cli;
bool cli_enabled() const override;
AP_HAL::BetterStream* cliSerial() override;
};

Loading…
Cancel
Save