mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-30 04:28:30 -04:00
Rover: create GCS subclass, use inheritted methods
This commit is contained in:
parent
5b3b61a2e4
commit
cfeb449979
@ -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
15
APMrover2/GCS_Rover.cpp
Normal 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
25
APMrover2/GCS_Rover.h
Normal 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;
|
||||
|
||||
};
|
@ -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();
|
||||
|
@ -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
|
||||
|
@ -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),
|
||||
|
@ -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;
|
||||
|
@ -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();
|
||||
|
Loading…
Reference in New Issue
Block a user