Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-29 20:18:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: create GCS_Plane subclass

Browse Source
This commit is contained in:
Peter Barker 2016-08-25 17:48:27 +10:00 committed by Andrew Tridgell
parent 65a182a068
commit 1d7994e9ba
8 changed files with 140 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
ArduPlane/GCS_Mavlink.cpp
View File

@ -91,7 +91,7 @@ void Plane::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_FIXED_WING,
gcs().chan(chan-MAVLINK_COMM_0).send_heartbeat(MAV_TYPE_FIXED_WING,
base_mode,
custom_mode,
system_status);
@ -2099,7 +2099,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
void Plane::mavlink_delay_cb()
{
static uint32_t last_1hz, last_50hz, last_5s;
if (!gcs_chan[0].initialised || in_mavlink_delay) return;
if (!gcs().chan(0).initialised || in_mavlink_delay) return;
in_mavlink_delay = true;
@ -2129,11 +2129,7 @@ void Plane::mavlink_delay_cb()
*/
void Plane::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);
}
/*
@ -2141,12 +2137,7 @@ void Plane::gcs_send_message(enum ap_message id)
*/
void Plane::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);
}
/*
@ -2154,11 +2145,7 @@ void Plane::gcs_send_mission_item_reached_message(uint16_t mission_index)
*/
void Plane::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();
}
/*
@ -2166,15 +2153,7 @@ void Plane::gcs_data_stream_send(void)
*/
void Plane::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(&Plane::run_cli, void, AP_HAL::UARTDriver *):nullptr);
#else
gcs_chan[i].update(nullptr);
#endif
}
}
gcs().update();
}
void Plane::gcs_send_text(MAV_SEVERITY severity, const char *str)
@ -2204,13 +2183,7 @@ void Plane::gcs_send_text_fmt(MAV_SEVERITY severity, const char *fmt, ...)
*/
void Plane::gcs_send_airspeed_calibration(const Vector3f &vg)
{
for (uint8_t i=0; i<num_gcs; i++) {
if (gcs_chan[i].initialised) {
if (HAVE_PAYLOAD_SPACE((mavlink_channel_t)i, AIRSPEED_AUTOCAL)) {
airspeed.log_mavlink_send((mavlink_channel_t)i, vg);
}
}
}
gcs().send_airspeed_calibration(vg);
}
/**

78
ArduPlane/GCS_Plane.cpp Normal file
View File

@ -0,0 +1,78 @@
#include "GCS_Plane.h"
#include "Plane.h"
void GCS_Plane::reset_cli_timeout()
{
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);
}
}
}
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);
}
}
}
void GCS_Plane::send_airspeed_calibration(const Vector3f &vg)
{
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);
}
}
}
}
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);
}
}
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);
}
}
}

37
ArduPlane/GCS_Plane.h Normal file
View File

@ -0,0 +1,37 @@
#pragma once
#include <GCS_MAVLink/GCS.h>
#include "GCS_Mavlink.h"
class GCS_Plane : public GCS
{
friend class Plane; // for temporary access to num_gcs and links
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; };
// return GCS link at offset ofs
GCS_MAVLINK_Plane &chan(const uint8_t ofs) { 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);
void handle_interactive_setup();
private:
uint8_t _num_gcs = MAVLINK_COMM_NUM_BUFFERS;
GCS_MAVLINK_Plane _chan[MAVLINK_COMM_NUM_BUFFERS];
run_cli_fn _run_cli;
};

4
ArduPlane/Log.cpp
View File

@ -542,9 +542,7 @@ void Plane::log_init(void)
gcs_send_text(MAV_SEVERITY_INFO, "Preparing log system");
DataFlash.Prep();
gcs_send_text(MAV_SEVERITY_INFO, "Prepared log system");
for (uint8_t i=0; i<num_gcs; i++) {
gcs_chan[i].reset_cli_timeout();
}
gcs().reset_cli_timeout();
}
}

8
ArduPlane/Parameters.cpp
View File

@ -1070,19 +1070,19 @@ const AP_Param::Info Plane::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: INS_
// @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp

4
ArduPlane/Plane.cpp
View File

@ -27,6 +27,10 @@ Plane::Plane(void)
auto_state.takeoff_complete = true;
auto_state.next_wp_no_crosstrack = true;
auto_state.no_crosstrack = true;
if (plane.g.cli_enabled) {
gcs().set_run_cli_func(FUNCTOR_BIND_MEMBER(&Plane::run_cli, void, AP_HAL::UARTDriver *));
}
}
Plane plane;

8
ArduPlane/Plane.h
View File

@ -95,6 +95,7 @@
#include <AP_Landing/AP_Landing.h>
#include "GCS_Mavlink.h"
#include "GCS_Plane.h"
#include "quadplane.h"
#include "tuning.h"
@ -144,6 +145,7 @@ public:
friend class AP_Tuning_Plane;
friend class AP_AdvancedFailsafe_Plane;
friend class AP_Avoidance_Plane;
friend class GCS_Plane;
Plane(void);
@ -250,10 +252,8 @@ private:
// GCS selection
AP_SerialManager serial_manager;
const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS;
GCS_MAVLINK_Plane gcs_chan[MAVLINK_COMM_NUM_BUFFERS];
GCS _gcs; // avoid using this; use gcs()
GCS &gcs() { return _gcs; }
GCS_Plane _gcs; // avoid using this; use gcs()
GCS_Plane &gcs() { return _gcs; }
// selected navigation controller
AP_Navigation *nav_controller = &L1_controller;

21
ArduPlane/system.cpp
View File

@ -122,7 +122,7 @@ void Plane::init_ardupilot()
// initialise serial ports
serial_manager.init();
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
@ -161,9 +161,7 @@ void Plane::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
#if FRSKY_TELEM_ENABLED == ENABLED
@ -229,16 +227,7 @@ void Plane::init_ardupilot()
hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);
#if CLI_ENABLED == ENABLED
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 // CLI_ENABLED
init_capabilities();
@ -571,8 +560,8 @@ void Plane::check_long_failsafe()
(tnow - failsafe.last_heartbeat_ms) > g.long_fs_timeout*1000) {
failsafe_long_on_event(FAILSAFE_GCS, MODE_REASON_GCS_FAILSAFE);
} else if (g.gcs_heartbeat_fs_enabled == GCS_FAILSAFE_HB_RSSI &&
gcs_chan[0].last_radio_status_remrssi_ms != 0 &&
(tnow - gcs_chan[0].last_radio_status_remrssi_ms) > g.long_fs_timeout*1000) {
gcs().chan(0).last_radio_status_remrssi_ms != 0 &&
(tnow - gcs().chan(0).last_radio_status_remrssi_ms) > g.long_fs_timeout*1000) {
failsafe_long_on_event(FAILSAFE_GCS, MODE_REASON_GCS_FAILSAFE);
}
} else {