Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Tracker: create GCS subclass, use inheritted methods

This commit is contained in:
Peter Barker 2017-02-14 12:30:32 +11:00 committed by Francisco Ferreira
parent cfeb449979
commit b81e5a8562
7 changed files with 112 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
AntennaTracker/GCS_Mavlink.cpp
View File

@ -58,7 +58,7 @@ void Tracker::send_heartbeat(mavlink_channel_t chan)
base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
}
gcs_chan[chan-MAVLINK_COMM_0].send_heartbeat(MAV_TYPE_ANTENNA_TRACKER,
gcs().chan(chan-MAVLINK_COMM_0).send_heartbeat(MAV_TYPE_ANTENNA_TRACKER,
base_mode,
custom_mode,
system_status);
@ -111,7 +111,7 @@ void Tracker::send_hwstatus(mavlink_channel_t chan)
void Tracker::send_waypoint_request(mavlink_channel_t chan)
{
gcs_chan[chan-MAVLINK_COMM_0].queued_waypoint_send();
gcs().chan(chan-MAVLINK_COMM_0).queued_waypoint_send();
}
void Tracker::send_nav_controller_output(mavlink_channel_t chan)
@ -485,30 +485,8 @@ void Tracker::mavlink_check_target(const mavlink_message_t* msg)
// send data stream request to target on all channels
// Note: this doesn't check success for all sends meaning it's not guaranteed the vehicle's positions will be sent at 1hz
for (uint8_t i=0; i < num_gcs; i++) {
if (gcs_chan[i].initialised) {
// request position
if (HAVE_PAYLOAD_SPACE((mavlink_channel_t)i, DATA_STREAM)) {
mavlink_msg_request_data_stream_send(
(mavlink_channel_t)i,
msg->sysid,
msg->compid,
MAV_DATA_STREAM_POSITION,
g.mavlink_update_rate,
1); // start streaming
}
// request air pressure
if (HAVE_PAYLOAD_SPACE((mavlink_channel_t)i, DATA_STREAM)) {
mavlink_msg_request_data_stream_send(
(mavlink_channel_t)i,
msg->sysid,
msg->compid,
MAV_DATA_STREAM_RAW_SENSORS,
g.mavlink_update_rate,
1); // start streaming
}
}
}
gcs().request_datastream_position(msg->sysid, msg->compid);
gcs().request_datastream_airpressure(msg->sysid, msg->compid);
// flag target has been set
target_set = true;
@ -869,7 +847,9 @@ mission_failed:
void Tracker::mavlink_delay_cb()
{
static uint32_t last_1hz, last_50hz, last_5s;
if (!gcs_chan[0].initialised) return;
if (!gcs().chan(0).initialised) {
return;
}
tracker.in_mavlink_delay = true;
DataFlash.EnableWrites(false);
@ -899,11 +879,7 @@ void Tracker::mavlink_delay_cb()
*/
void Tracker::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);
}
/*
@ -911,11 +887,7 @@ void Tracker::gcs_send_message(enum ap_message id)
*/
void Tracker::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();
}
/*
@ -923,11 +895,7 @@ void Tracker::gcs_data_stream_send(void)
*/
void Tracker::gcs_update(void)
{
for (uint8_t i=0; i<num_gcs; i++) {
if (gcs_chan[i].initialised) {
gcs_chan[i].update(nullptr);
}
}
gcs().update();
}
void Tracker::gcs_send_text(MAV_SEVERITY severity, const char *str)

61
AntennaTracker/GCS_Tracker.cpp Normal file
View File

@ -0,0 +1,61 @@
#include "GCS_Tracker.h"
#include "Tracker.h"
bool GCS_Tracker::cli_enabled() const
{
return false;
}
AP_HAL::BetterStream* GCS_Tracker::cliSerial() {
return nullptr;
}
static void mavlink_snoop_static(const mavlink_message_t* msg)
{
tracker.mavlink_snoop(msg);
}
void GCS_Tracker::setup_uarts(AP_SerialManager &serial_manager)
{
GCS::setup_uarts(serial_manager);
for (uint8_t i = 1; i < num_gcs(); i++) {
gcs().chan(i).set_snoop(mavlink_snoop_static);
}
}
void GCS_Tracker::request_datastream_position(const uint8_t sysid, const uint8_t compid)
{
for (uint8_t i=0; i < num_gcs(); i++) {
if (gcs().chan(i).initialised) {
// request position
if (HAVE_PAYLOAD_SPACE((mavlink_channel_t)i, DATA_STREAM)) {
mavlink_msg_request_data_stream_send(
(mavlink_channel_t)i,
sysid,
compid,
MAV_DATA_STREAM_POSITION,
tracker.g.mavlink_update_rate,
1); // start streaming
}
}
}
}
void GCS_Tracker::request_datastream_airpressure(const uint8_t sysid, const uint8_t compid)
{
for (uint8_t i=0; i < num_gcs(); i++) {
if (gcs().chan(i).initialised) {
// request air pressure
if (HAVE_PAYLOAD_SPACE((mavlink_channel_t)i, DATA_STREAM)) {
mavlink_msg_request_data_stream_send(
(mavlink_channel_t)i,
sysid,
compid,
MAV_DATA_STREAM_RAW_SENSORS,
tracker.g.mavlink_update_rate,
1); // start streaming
}
}
}
}

30
AntennaTracker/GCS_Tracker.h Normal file
View File

@ -0,0 +1,30 @@
#pragma once
#include <GCS_MAVLink/GCS.h>
#include "GCS_Mavlink.h"
class GCS_Tracker : public GCS
{
friend class Tracker; // 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_Tracker &chan(const uint8_t ofs) override { return _chan[ofs]; };
void setup_uarts(AP_SerialManager &serial_manager) override;
private:
void request_datastream_position(uint8_t sysid, uint8_t compid);
void request_datastream_airpressure(uint8_t sysid, uint8_t compid);
GCS_MAVLINK_Tracker _chan[MAVLINK_COMM_NUM_BUFFERS];
bool cli_enabled() const override;
AP_HAL::BetterStream* cliSerial() override;
};

4
AntennaTracker/Log.cpp
View File

@ -96,9 +96,7 @@ void Tracker::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();

8
AntennaTracker/Parameters.cpp
View File

@ -253,19 +253,19 @@ const AP_Param::Info Tracker::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),
// @Param: LOG_BITMASK
// @DisplayName: Log bitmask

8
AntennaTracker/Tracker.h
View File

@ -69,6 +69,7 @@
#include "Parameters.h"
#include "GCS_Mavlink.h"
#include "GCS_Tracker.h"
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <SITL/SITL.h>
@ -77,6 +78,7 @@
class Tracker : public AP_HAL::HAL::Callbacks {
public:
friend class GCS_MAVLINK_Tracker;
friend class GCS_Tracker;
friend class Parameters;
Tracker(void);
@ -136,10 +138,8 @@ private:
bool pitch_servo_out_filt_init = false;
AP_SerialManager serial_manager;
const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS;
GCS_MAVLINK_Tracker gcs_chan[MAVLINK_COMM_NUM_BUFFERS];
GCS _gcs; // avoid using this; use GCS::instance()
GCS &gcs() { return _gcs; }
GCS_Tracker _gcs; // avoid using this; use gcs()
GCS_Tracker &gcs() { return _gcs; }
AP_BoardConfig BoardConfig;

12
AntennaTracker/system.cpp
View File

@ -4,11 +4,6 @@
// mission storage
static const StorageAccess wp_storage(StorageManager::StorageMission);
static void mavlink_snoop_static(const mavlink_message_t* msg)
{
tracker.mavlink_snoop(msg);
}
static void mavlink_delay_cb_static()
{
tracker.mavlink_delay_cb();
@ -34,7 +29,7 @@ void Tracker::init_tracker()
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
@ -60,10 +55,7 @@ void Tracker::init_tracker()
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_chan[i].set_snoop(mavlink_snoop_static);
}
gcs().setup_uarts(serial_manager);
#if LOGGING_ENABLED == ENABLED
log_init();