Rover: create GCS subclass, use inheritted methods

This commit is contained in:
Peter Barker 2017-02-13 22:00:19 +11:00 committed by Francisco Ferreira
parent 5b3b61a2e4
commit cfeb449979
8 changed files with 65 additions and 59 deletions

View File

@ -63,7 +63,7 @@ void Rover::send_heartbeat(mavlink_channel_t chan)
// indicate we have set a custom mode
base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
gcs_chan[chan-MAVLINK_COMM_0].send_heartbeat(MAV_TYPE_GROUND_ROVER,
gcs().chan(chan-MAVLINK_COMM_0).send_heartbeat(MAV_TYPE_GROUND_ROVER,
base_mode,
custom_mode,
system_status);
@ -1562,7 +1562,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
void Rover::mavlink_delay_cb()
{
static uint32_t last_1hz, last_50hz, last_5s;
if (!gcs_chan[0].initialised || in_mavlink_delay) {
if (!gcs().chan(0).initialised || in_mavlink_delay) {
return;
}
@ -1597,11 +1597,7 @@ void Rover::mavlink_delay_cb()
*/
void Rover::gcs_send_message(enum ap_message id)
{
for (uint8_t i=0; i < num_gcs; i++) {
if (gcs_chan[i].initialised) {
gcs_chan[i].send_message(id);
}
}
gcs().send_message(id);
}
/*
@ -1609,12 +1605,7 @@ void Rover::gcs_send_message(enum ap_message id)
*/
void Rover::gcs_send_mission_item_reached_message(uint16_t mission_index)
{
for (uint8_t i=0; i < num_gcs; i++) {
if (gcs_chan[i].initialised) {
gcs_chan[i].mission_item_reached_index = mission_index;
gcs_chan[i].send_message(MSG_MISSION_ITEM_REACHED);
}
}
gcs().send_mission_item_reached_message(mission_index);
}
/*
@ -1622,11 +1613,7 @@ void Rover::gcs_send_mission_item_reached_message(uint16_t mission_index)
*/
void Rover::gcs_data_stream_send(void)
{
for (uint8_t i=0; i < num_gcs; i++) {
if (gcs_chan[i].initialised) {
gcs_chan[i].data_stream_send();
}
}
gcs().data_stream_send();
}
/*
@ -1634,15 +1621,7 @@ void Rover::gcs_data_stream_send(void)
*/
void Rover::gcs_update(void)
{
for (uint8_t i=0; i < num_gcs; i++) {
if (gcs_chan[i].initialised) {
#if CLI_ENABLED == ENABLED
gcs_chan[i].update(g.cli_enabled == 1 ? FUNCTOR_BIND_MEMBER(&Rover::run_cli, void, AP_HAL::UARTDriver *) : nullptr);
#else
gcs_chan[i].update(nullptr);
#endif
}
}
gcs().update();
}
void Rover::gcs_send_text(MAV_SEVERITY severity, const char *str)

15
APMrover2/GCS_Rover.cpp Normal file
View File

@ -0,0 +1,15 @@
#include "GCS_Rover.h"
#include "Rover.h"
bool GCS_Rover::cli_enabled() const
{
#if CLI_ENABLED == ENABLED
return rover.g.cli_enabled;
#else
return false;
#endif
}
AP_HAL::BetterStream* GCS_Rover::cliSerial() {
return rover.cliSerial;
}

25
APMrover2/GCS_Rover.h Normal file
View File

@ -0,0 +1,25 @@
#pragma once
#include <GCS_MAVLink/GCS.h>
#include "GCS_Mavlink.h"
class GCS_Rover : public GCS
{
friend class Rover; // for access to _chan in parameter declarations
public:
// return the number of valid GCS objects
uint8_t num_gcs() const override { return ARRAY_SIZE(_chan); };
// return GCS link at offset ofs
GCS_MAVLINK_Rover &chan(const uint8_t ofs) override { return _chan[ofs]; };
private:
GCS_MAVLINK_Rover _chan[MAVLINK_COMM_NUM_BUFFERS];
bool cli_enabled() const override;
AP_HAL::BetterStream* cliSerial() override;
};

View File

@ -473,9 +473,7 @@ void Rover::log_init(void)
{
DataFlash.Init(log_structure, ARRAY_SIZE(log_structure));
for (uint8_t i=0; i < num_gcs; i++) {
gcs_chan[i].reset_cli_timeout();
}
gcs().reset_cli_timeout();
if (g.log_bitmask != 0) {
start_logging();

View File

@ -402,19 +402,19 @@ const AP_Param::Info Rover::var_info[] = {
// @Group: SR0_
// @Path: GCS_Mavlink.cpp
GOBJECTN(gcs_chan[0], gcs0, "SR0_", GCS_MAVLINK),
GOBJECTN(_gcs._chan[0], gcs0, "SR0_", GCS_MAVLINK),
// @Group: SR1_
// @Path: GCS_Mavlink.cpp
GOBJECTN(gcs_chan[1], gcs1, "SR1_", GCS_MAVLINK),
GOBJECTN(_gcs._chan[1], gcs1, "SR1_", GCS_MAVLINK),
// @Group: SR2_
// @Path: GCS_Mavlink.cpp
GOBJECTN(gcs_chan[2], gcs2, "SR2_", GCS_MAVLINK),
GOBJECTN(_gcs._chan[2], gcs2, "SR2_", GCS_MAVLINK),
// @Group: SR3_
// @Path: GCS_Mavlink.cpp
GOBJECTN(gcs_chan[3], gcs3, "SR3_", GCS_MAVLINK),
GOBJECTN(_gcs._chan[3], gcs3, "SR3_", GCS_MAVLINK),
// @Group: SERIAL
// @Path: ../libraries/AP_SerialManager/AP_SerialManager.cpp

View File

@ -33,7 +33,6 @@ Rover::Rover(void) :
FUNCTOR_BIND_MEMBER(&Rover::start_command, bool, const AP_Mission::Mission_Command&),
FUNCTOR_BIND_MEMBER(&Rover::verify_command_callback, bool, const AP_Mission::Mission_Command&),
FUNCTOR_BIND_MEMBER(&Rover::exit_mission, void)),
num_gcs(MAVLINK_COMM_NUM_BUFFERS),
ServoRelayEvents(relay),
#if CAMERA == ENABLED
camera(&relay),

View File

@ -86,6 +86,7 @@
#endif
#include "Parameters.h"
#include "GCS_Mavlink.h"
#include "GCS_Rover.h"
#include <AP_Declination/AP_Declination.h> // ArduPilot Mega Declination Helper Library
@ -102,6 +103,7 @@ public:
#if ADVANCED_FAILSAFE == ENABLED
friend class AP_AdvancedFailsafe_Rover;
#endif
friend class GCS_Rover;
Rover(void);
@ -189,10 +191,8 @@ private:
// GCS handling
AP_SerialManager serial_manager;
const uint8_t num_gcs;
GCS_MAVLINK_Rover gcs_chan[MAVLINK_COMM_NUM_BUFFERS];
GCS _gcs; // avoid using this; use gcs()
GCS &gcs() { return _gcs; }
GCS_Rover _gcs; // avoid using this; use gcs()
GCS_Rover &gcs() { return _gcs; }
// relay support
AP_Relay relay;

View File

@ -100,12 +100,19 @@ void Rover::init_ardupilot()
serial_manager.init();
// setup first port early to allow BoardConfig to report errors
gcs_chan[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0);
gcs().chan(0).setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0);
// Register mavlink_delay_cb, which will run anytime you have
// more than 5ms remaining in your call to hal.scheduler->delay
hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5);
// specify callback function for CLI menu system
#if CLI_ENABLED == ENABLED
if (gcs().cli_enabled()) {
gcs().set_run_cli_func(FUNCTOR_BIND_MEMBER(&Rover::run_cli, void, AP_HAL::UARTDriver *));
}
#endif
BoardConfig.init();
#if HAL_WITH_UAVCAN
BoardConfig_CAN.init();
@ -135,9 +142,7 @@ void Rover::init_ardupilot()
check_usb_mux();
// setup telem slots with serial ports
for (uint8_t i = 1; i < MAVLINK_COMM_NUM_BUFFERS; i++) {
gcs_chan[i].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, i);
}
gcs().setup_uarts(serial_manager);
// setup frsky telemetry
#if FRSKY_TELEM_ENABLED == ENABLED
@ -193,22 +198,7 @@ void Rover::init_ardupilot()
#if CLI_ENABLED == ENABLED
// If the switch is in 'menu' mode, run the main menu.
//
// Since we can't be sure that the setup or test mode won't leave
// the system in an odd state, we don't let the user exit the top
// menu; they must reset in order to fly.
//
if (g.cli_enabled == 1) {
const char *msg = "\nPress ENTER 3 times to start interactive setup\n";
cliSerial->printf("%s\n", msg);
if (gcs_chan[1].initialised && (gcs_chan[1].get_uart() != nullptr)) {
gcs_chan[1].get_uart()->printf("%s\n", msg);
}
if (num_gcs > 2 && gcs_chan[2].initialised && (gcs_chan[2].get_uart() != nullptr)) {
gcs_chan[2].get_uart()->printf("%s\n", msg);
}
}
gcs().handle_interactive_setup();
#endif
init_capabilities();