GCS_MAVLink: remove CLI
This commit is contained in:
parent
1a25087dd5
commit
bff31e8b42
@ -89,8 +89,7 @@ class GCS_MAVLINK
|
|||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
GCS_MAVLINK();
|
GCS_MAVLINK();
|
||||||
FUNCTOR_TYPEDEF(run_cli_fn, void, AP_HAL::UARTDriver*);
|
void update(uint32_t max_time_us=1000);
|
||||||
void update(run_cli_fn run_cli, uint32_t max_time_us=1000);
|
|
||||||
void init(AP_HAL::UARTDriver *port, mavlink_channel_t mav_chan);
|
void init(AP_HAL::UARTDriver *port, mavlink_channel_t mav_chan);
|
||||||
void setup_uart(const AP_SerialManager& serial_manager, AP_SerialManager::SerialProtocol protocol, uint8_t instance);
|
void setup_uart(const AP_SerialManager& serial_manager, AP_SerialManager::SerialProtocol protocol, uint8_t instance);
|
||||||
void send_message(enum ap_message id);
|
void send_message(enum ap_message id);
|
||||||
@ -133,9 +132,6 @@ public:
|
|||||||
// see if we should send a stream now. Called at 50Hz
|
// see if we should send a stream now. Called at 50Hz
|
||||||
bool stream_trigger(enum streams stream_num);
|
bool stream_trigger(enum streams stream_num);
|
||||||
|
|
||||||
// call to reset the timeout window for entering the cli
|
|
||||||
void reset_cli_timeout();
|
|
||||||
|
|
||||||
bool is_high_bandwidth() { return chan == MAVLINK_COMM_0; }
|
bool is_high_bandwidth() { return chan == MAVLINK_COMM_0; }
|
||||||
// return true if this channel has hardware flow control
|
// return true if this channel has hardware flow control
|
||||||
bool have_flow_control();
|
bool have_flow_control();
|
||||||
@ -336,9 +332,6 @@ private:
|
|||||||
///
|
///
|
||||||
uint16_t packet_drops;
|
uint16_t packet_drops;
|
||||||
|
|
||||||
// this allows us to detect the user wanting the CLI to start
|
|
||||||
uint8_t crlf_count;
|
|
||||||
|
|
||||||
// waypoints
|
// waypoints
|
||||||
uint16_t waypoint_dest_sysid; // where to send requests
|
uint16_t waypoint_dest_sysid; // where to send requests
|
||||||
uint16_t waypoint_dest_compid; // "
|
uint16_t waypoint_dest_compid; // "
|
||||||
@ -353,10 +346,6 @@ private:
|
|||||||
// number of extra ticks to add to slow things down for the radio
|
// number of extra ticks to add to slow things down for the radio
|
||||||
uint8_t stream_slowdown;
|
uint8_t stream_slowdown;
|
||||||
|
|
||||||
// millis value to calculate cli timeout relative to.
|
|
||||||
// exists so we can separate the cli entry time from the system start time
|
|
||||||
uint32_t _cli_timeout;
|
|
||||||
|
|
||||||
// perf counters
|
// perf counters
|
||||||
static AP_HAL::Util::perf_counter_t _perf_packet;
|
static AP_HAL::Util::perf_counter_t _perf_packet;
|
||||||
static AP_HAL::Util::perf_counter_t _perf_update;
|
static AP_HAL::Util::perf_counter_t _perf_update;
|
||||||
@ -465,7 +454,6 @@ public:
|
|||||||
virtual GCS_MAVLINK &chan(const uint8_t ofs) = 0;
|
virtual GCS_MAVLINK &chan(const uint8_t ofs) = 0;
|
||||||
virtual const GCS_MAVLINK &chan(const uint8_t ofs) const = 0;
|
virtual const GCS_MAVLINK &chan(const uint8_t ofs) const = 0;
|
||||||
virtual uint8_t num_gcs() const = 0;
|
virtual uint8_t num_gcs() const = 0;
|
||||||
void reset_cli_timeout();
|
|
||||||
void send_message(enum ap_message id);
|
void send_message(enum ap_message id);
|
||||||
void send_mission_item_reached_message(uint16_t mission_index);
|
void send_mission_item_reached_message(uint16_t mission_index);
|
||||||
void send_home(const Location &home) const;
|
void send_home(const Location &home) const;
|
||||||
@ -474,11 +462,6 @@ public:
|
|||||||
void data_stream_send();
|
void data_stream_send();
|
||||||
void update();
|
void update();
|
||||||
virtual void setup_uarts(AP_SerialManager &serial_manager);
|
virtual void setup_uarts(AP_SerialManager &serial_manager);
|
||||||
void handle_interactive_setup();
|
|
||||||
|
|
||||||
FUNCTOR_TYPEDEF(run_cli_fn, void, AP_HAL::UARTDriver*);
|
|
||||||
run_cli_fn _run_cli;
|
|
||||||
void set_run_cli_func(run_cli_fn run_cli) { _run_cli = run_cli; }
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
set a dataflash pointer for logging
|
set a dataflash pointer for logging
|
||||||
@ -505,9 +488,6 @@ private:
|
|||||||
|
|
||||||
static GCS *_singleton;
|
static GCS *_singleton;
|
||||||
|
|
||||||
virtual bool cli_enabled() const = 0;
|
|
||||||
virtual AP_HAL::BetterStream* cliSerial() = 0;
|
|
||||||
|
|
||||||
struct statustext_t {
|
struct statustext_t {
|
||||||
uint8_t bitmask;
|
uint8_t bitmask;
|
||||||
mavlink_statustext_t msg;
|
mavlink_statustext_t msg;
|
||||||
|
@ -59,7 +59,6 @@ GCS_MAVLINK::init(AP_HAL::UARTDriver *port, mavlink_channel_t mav_chan)
|
|||||||
mavlink_comm_port[chan] = _port;
|
mavlink_comm_port[chan] = _port;
|
||||||
initialised = true;
|
initialised = true;
|
||||||
_queued_parameter = nullptr;
|
_queued_parameter = nullptr;
|
||||||
reset_cli_timeout();
|
|
||||||
|
|
||||||
if (!_perf_packet) {
|
if (!_perf_packet) {
|
||||||
_perf_packet = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "GCS_Packet");
|
_perf_packet = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "GCS_Packet");
|
||||||
@ -169,10 +168,6 @@ GCS_MAVLINK::queued_waypoint_send()
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void GCS_MAVLINK::reset_cli_timeout() {
|
|
||||||
_cli_timeout = AP_HAL::millis();
|
|
||||||
}
|
|
||||||
|
|
||||||
void GCS_MAVLINK::send_meminfo(void)
|
void GCS_MAVLINK::send_meminfo(void)
|
||||||
{
|
{
|
||||||
unsigned __brkval = 0;
|
unsigned __brkval = 0;
|
||||||
@ -821,8 +816,8 @@ void GCS_MAVLINK::send_message(enum ap_message id)
|
|||||||
void GCS_MAVLINK::packetReceived(const mavlink_status_t &status,
|
void GCS_MAVLINK::packetReceived(const mavlink_status_t &status,
|
||||||
mavlink_message_t &msg)
|
mavlink_message_t &msg)
|
||||||
{
|
{
|
||||||
// we exclude radio packets to make it possible to use the
|
// we exclude radio packets because we historically used this to
|
||||||
// CLI over the radio
|
// make it possible to use the CLI over the radio
|
||||||
if (msg.msgid != MAVLINK_MSG_ID_RADIO && msg.msgid != MAVLINK_MSG_ID_RADIO_STATUS) {
|
if (msg.msgid != MAVLINK_MSG_ID_RADIO && msg.msgid != MAVLINK_MSG_ID_RADIO_STATUS) {
|
||||||
mavlink_active |= (1U<<(chan-MAVLINK_COMM_0));
|
mavlink_active |= (1U<<(chan-MAVLINK_COMM_0));
|
||||||
}
|
}
|
||||||
@ -849,7 +844,7 @@ void GCS_MAVLINK::packetReceived(const mavlink_status_t &status,
|
|||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
GCS_MAVLINK::update(run_cli_fn run_cli, uint32_t max_time_us)
|
GCS_MAVLINK::update(uint32_t max_time_us)
|
||||||
{
|
{
|
||||||
// receive new packets
|
// receive new packets
|
||||||
mavlink_message_t msg;
|
mavlink_message_t msg;
|
||||||
@ -866,22 +861,6 @@ GCS_MAVLINK::update(run_cli_fn run_cli, uint32_t max_time_us)
|
|||||||
{
|
{
|
||||||
uint8_t c = comm_receive_ch(chan);
|
uint8_t c = comm_receive_ch(chan);
|
||||||
|
|
||||||
if (run_cli) {
|
|
||||||
/* allow CLI to be started by hitting enter 3 times, if no
|
|
||||||
* heartbeat packets have been received */
|
|
||||||
if ((mavlink_active==0) && (AP_HAL::millis() - _cli_timeout) < 20000 &&
|
|
||||||
comm_is_idle(chan)) {
|
|
||||||
if (c == '\n' || c == '\r') {
|
|
||||||
crlf_count++;
|
|
||||||
} else {
|
|
||||||
crlf_count = 0;
|
|
||||||
}
|
|
||||||
if (crlf_count == 3) {
|
|
||||||
run_cli(_port);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
bool parsed_packet = false;
|
bool parsed_packet = false;
|
||||||
|
|
||||||
// Try to get a new message
|
// Try to get a new message
|
||||||
@ -1235,13 +1214,6 @@ void GCS::service_statustext(void)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void GCS::reset_cli_timeout()
|
|
||||||
{
|
|
||||||
for (uint8_t i=0; i<num_gcs(); i++) {
|
|
||||||
chan(i).reset_cli_timeout();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void GCS::send_message(enum ap_message id)
|
void GCS::send_message(enum ap_message id)
|
||||||
{
|
{
|
||||||
for (uint8_t i=0; i<num_gcs(); i++) {
|
for (uint8_t i=0; i<num_gcs(); i++) {
|
||||||
@ -1274,7 +1246,7 @@ void GCS::update(void)
|
|||||||
{
|
{
|
||||||
for (uint8_t i=0; i<num_gcs(); i++) {
|
for (uint8_t i=0; i<num_gcs(); i++) {
|
||||||
if (chan(i).initialised) {
|
if (chan(i).initialised) {
|
||||||
chan(i).update(_run_cli);
|
chan(i).update();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -1296,21 +1268,6 @@ void GCS::setup_uarts(AP_SerialManager &serial_manager)
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void GCS::handle_interactive_setup()
|
|
||||||
{
|
|
||||||
if (cli_enabled()) {
|
|
||||||
AP_HAL::BetterStream* _cliSerial = cliSerial();
|
|
||||||
const char *msg = "\nPress ENTER 3 times to start interactive setup\n";
|
|
||||||
_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);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// report battery2 state
|
// report battery2 state
|
||||||
void GCS_MAVLINK::send_battery2(const AP_BattMonitor &battery)
|
void GCS_MAVLINK::send_battery2(const AP_BattMonitor &battery)
|
||||||
{
|
{
|
||||||
|
@ -37,8 +37,6 @@ class GCS_Dummy : public GCS
|
|||||||
uint8_t num_gcs() const override { return 1; }
|
uint8_t num_gcs() const override { return 1; }
|
||||||
GCS_MAVLINK_Dummy &chan(const uint8_t ofs) override { return dummy_backend; }
|
GCS_MAVLINK_Dummy &chan(const uint8_t ofs) override { return dummy_backend; }
|
||||||
const GCS_MAVLINK_Dummy &chan(const uint8_t ofs) const override { return dummy_backend; };
|
const GCS_MAVLINK_Dummy &chan(const uint8_t ofs) const override { return dummy_backend; };
|
||||||
bool cli_enabled() const override { return false; }
|
|
||||||
AP_HAL::BetterStream* cliSerial() { return nullptr; }
|
|
||||||
|
|
||||||
void send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const char *text) { hal.console->printf("TOGCS: %s\n", text); }
|
void send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const char *text) { hal.console->printf("TOGCS: %s\n", text); }
|
||||||
};
|
};
|
||||||
|
Loading…
Reference in New Issue
Block a user