Browse Source

Plane: subclass GCS_MAVLink in place of defining its functions for it

master
Peter Barker 9 years ago committed by Andrew Tridgell
parent
commit
af5a52e2aa
  1. 14
      ArduPlane/GCS_Mavlink.cpp
  2. 22
      ArduPlane/GCS_Mavlink.h
  3. 6
      ArduPlane/Plane.h

14
ArduPlane/GCS_Mavlink.cpp

@ -1,5 +1,7 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "GCS_Mavlink.h"
#include "Plane.h" #include "Plane.h"
#include "version.h" #include "version.h"
@ -636,7 +638,7 @@ bool Plane::telemetry_delayed(mavlink_channel_t chan)
// try to send a message, return false if it won't fit in the serial tx buffer // try to send a message, return false if it won't fit in the serial tx buffer
bool GCS_MAVLINK::try_send_message(enum ap_message id) bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id)
{ {
if (plane.telemetry_delayed(chan)) { if (plane.telemetry_delayed(chan)) {
return false; return false;
@ -975,7 +977,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = {
AP_GROUPEND AP_GROUPEND
}; };
float GCS_MAVLINK::adjust_rate_for_stream_trigger(enum streams stream_num) float GCS_MAVLINK_Plane::adjust_rate_for_stream_trigger(enum streams stream_num)
{ {
// send at a much lower rate while handling waypoints and // send at a much lower rate while handling waypoints and
// parameter sends // parameter sends
@ -988,7 +990,7 @@ float GCS_MAVLINK::adjust_rate_for_stream_trigger(enum streams stream_num)
} }
void void
GCS_MAVLINK::data_stream_send(void) GCS_MAVLINK_Plane::data_stream_send(void)
{ {
plane.gcs_out_of_time = false; plane.gcs_out_of_time = false;
@ -1110,7 +1112,7 @@ GCS_MAVLINK::data_stream_send(void)
handle a request to switch to guided mode. This happens via a handle a request to switch to guided mode. This happens via a
callback from handle_mission_item() callback from handle_mission_item()
*/ */
bool GCS_MAVLINK::handle_guided_request(AP_Mission::Mission_Command &cmd) bool GCS_MAVLINK_Plane::handle_guided_request(AP_Mission::Mission_Command &cmd)
{ {
if (plane.control_mode != GUIDED) { if (plane.control_mode != GUIDED) {
// only accept position updates when in GUIDED mode // only accept position updates when in GUIDED mode
@ -1132,7 +1134,7 @@ bool GCS_MAVLINK::handle_guided_request(AP_Mission::Mission_Command &cmd)
handle a request to change current WP altitude. This happens via a handle a request to change current WP altitude. This happens via a
callback from handle_mission_item() callback from handle_mission_item()
*/ */
void GCS_MAVLINK::handle_change_alt_request(AP_Mission::Mission_Command &cmd) void GCS_MAVLINK_Plane::handle_change_alt_request(AP_Mission::Mission_Command &cmd)
{ {
plane.next_WP_loc.alt = cmd.content.location.alt; plane.next_WP_loc.alt = cmd.content.location.alt;
if (cmd.content.location.flags.relative_alt) { if (cmd.content.location.flags.relative_alt) {
@ -1143,7 +1145,7 @@ void GCS_MAVLINK::handle_change_alt_request(AP_Mission::Mission_Command &cmd)
plane.reset_offset_altitude(); plane.reset_offset_altitude();
} }
void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
{ {
switch (msg->msgid) { switch (msg->msgid) {

22
ArduPlane/GCS_Mavlink.h

@ -0,0 +1,22 @@
#pragma once
#include <GCS_MAVLink/GCS.h>
class GCS_MAVLINK_Plane : public GCS_MAVLINK
{
public:
void data_stream_send(void) override;
protected:
private:
float adjust_rate_for_stream_trigger(enum streams stream_num) override;
void handleMessage(mavlink_message_t * msg) override;
bool handle_guided_request(AP_Mission::Mission_Command &cmd) override;
void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override;
bool try_send_message(enum ap_message id) override;
};

6
ArduPlane/Plane.h

@ -56,7 +56,6 @@
#include <APM_OBC/APM_OBC.h> #include <APM_OBC/APM_OBC.h>
#include <APM_Control/APM_Control.h> #include <APM_Control/APM_Control.h>
#include <APM_Control/AP_AutoTune.h> #include <APM_Control/AP_AutoTune.h>
#include <GCS_MAVLink/GCS.h>
#include <GCS_MAVLink/GCS_MAVLink.h> // MAVLink GCS definitions #include <GCS_MAVLink/GCS_MAVLink.h> // MAVLink GCS definitions
#include <AP_SerialManager/AP_SerialManager.h> // Serial manager library #include <AP_SerialManager/AP_SerialManager.h> // Serial manager library
#include <AP_Mount/AP_Mount.h> // Camera/Antenna mount #include <AP_Mount/AP_Mount.h> // Camera/Antenna mount
@ -90,6 +89,7 @@
#include <AP_Parachute/AP_Parachute.h> #include <AP_Parachute/AP_Parachute.h>
#include <AP_ADSB/AP_ADSB.h> #include <AP_ADSB/AP_ADSB.h>
#include "GCS_Mavlink.h"
#include "quadplane.h" #include "quadplane.h"
#include "tuning.h" #include "tuning.h"
@ -129,7 +129,7 @@ protected:
*/ */
class Plane : public AP_HAL::HAL::Callbacks { class Plane : public AP_HAL::HAL::Callbacks {
public: public:
friend class GCS_MAVLINK; friend class GCS_MAVLINK_Plane;
friend class Parameters; friend class Parameters;
friend class AP_Arming_Plane; friend class AP_Arming_Plane;
friend class QuadPlane; friend class QuadPlane;
@ -254,7 +254,7 @@ private:
// GCS selection // GCS selection
AP_SerialManager serial_manager; AP_SerialManager serial_manager;
const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS; const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS;
GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS]; GCS_MAVLINK_Plane gcs[MAVLINK_COMM_NUM_BUFFERS];
// selected navigation controller // selected navigation controller
AP_Navigation *nav_controller = &L1_controller; AP_Navigation *nav_controller = &L1_controller;

Loading…
Cancel
Save