mirror of https://github.com/ArduPilot/ardupilot
Rover: subclass GCS_MAVLink in place of defining its functions for it
This commit is contained in:
parent
bb19c57615
commit
a1c97f4585
|
@ -3,6 +3,8 @@
|
||||||
#include "Rover.h"
|
#include "Rover.h"
|
||||||
#include "version.h"
|
#include "version.h"
|
||||||
|
|
||||||
|
#include "GCS_Mavlink.h"
|
||||||
|
|
||||||
// default sensors are present and healthy: gyro, accelerometer, rate_control, attitude_stabilization, yaw_position, altitude control, x/y position control, motor_control
|
// default sensors are present and healthy: gyro, accelerometer, rate_control, attitude_stabilization, yaw_position, altitude control, x/y position control, motor_control
|
||||||
#define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS | MAV_SYS_STATUS_AHRS)
|
#define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS | MAV_SYS_STATUS_AHRS)
|
||||||
|
|
||||||
|
@ -399,7 +401,7 @@ bool Rover::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_Rover::try_send_message(enum ap_message id)
|
||||||
{
|
{
|
||||||
if (rover.telemetry_delayed(chan)) {
|
if (rover.telemetry_delayed(chan)) {
|
||||||
return false;
|
return false;
|
||||||
|
@ -692,7 +694,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_Rover::adjust_rate_for_stream_trigger(enum streams stream_num)
|
||||||
{
|
{
|
||||||
if ((stream_num != STREAM_PARAMS) &&
|
if ((stream_num != STREAM_PARAMS) &&
|
||||||
(waypoint_receiving || _queued_parameter != NULL)) {
|
(waypoint_receiving || _queued_parameter != NULL)) {
|
||||||
|
@ -703,7 +705,7 @@ float GCS_MAVLINK::adjust_rate_for_stream_trigger(enum streams stream_num)
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
GCS_MAVLINK::data_stream_send(void)
|
GCS_MAVLINK_Rover::data_stream_send(void)
|
||||||
{
|
{
|
||||||
rover.gcs_out_of_time = false;
|
rover.gcs_out_of_time = false;
|
||||||
|
|
||||||
|
@ -810,7 +812,7 @@ GCS_MAVLINK::data_stream_send(void)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
bool GCS_MAVLINK::handle_guided_request(AP_Mission::Mission_Command &cmd)
|
bool GCS_MAVLINK_Rover::handle_guided_request(AP_Mission::Mission_Command &cmd)
|
||||||
{
|
{
|
||||||
if (rover.control_mode != GUIDED) {
|
if (rover.control_mode != GUIDED) {
|
||||||
// only accept position updates when in GUIDED mode
|
// only accept position updates when in GUIDED mode
|
||||||
|
@ -825,12 +827,12 @@ bool GCS_MAVLINK::handle_guided_request(AP_Mission::Mission_Command &cmd)
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
void GCS_MAVLINK::handle_change_alt_request(AP_Mission::Mission_Command &cmd)
|
void GCS_MAVLINK_Rover::handle_change_alt_request(AP_Mission::Mission_Command &cmd)
|
||||||
{
|
{
|
||||||
// nothing to do
|
// nothing to do
|
||||||
}
|
}
|
||||||
|
|
||||||
void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
||||||
{
|
{
|
||||||
switch (msg->msgid) {
|
switch (msg->msgid) {
|
||||||
|
|
||||||
|
|
|
@ -0,0 +1,22 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <GCS_MAVLink/GCS.h>
|
||||||
|
|
||||||
|
class GCS_MAVLINK_Rover : 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;
|
||||||
|
|
||||||
|
};
|
|
@ -52,7 +52,6 @@
|
||||||
#include <AP_ServoRelayEvents/AP_ServoRelayEvents.h>
|
#include <AP_ServoRelayEvents/AP_ServoRelayEvents.h>
|
||||||
#include <AP_Mount/AP_Mount.h> // Camera/Antenna mount
|
#include <AP_Mount/AP_Mount.h> // Camera/Antenna mount
|
||||||
#include <AP_Camera/AP_Camera.h> // Camera triggering
|
#include <AP_Camera/AP_Camera.h> // Camera triggering
|
||||||
#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_Airspeed/AP_Airspeed.h> // needed for AHRS build
|
#include <AP_Airspeed/AP_Airspeed.h> // needed for AHRS build
|
||||||
#include <AP_Vehicle/AP_Vehicle.h> // needed for AHRS build
|
#include <AP_Vehicle/AP_Vehicle.h> // needed for AHRS build
|
||||||
|
@ -80,7 +79,7 @@
|
||||||
// Local modules
|
// Local modules
|
||||||
#include "defines.h"
|
#include "defines.h"
|
||||||
#include "Parameters.h"
|
#include "Parameters.h"
|
||||||
#include <GCS_MAVLink/GCS.h>
|
#include "GCS_Mavlink.h"
|
||||||
|
|
||||||
#include <AP_Declination/AP_Declination.h> // ArduPilot Mega Declination Helper Library
|
#include <AP_Declination/AP_Declination.h> // ArduPilot Mega Declination Helper Library
|
||||||
|
|
||||||
|
@ -90,7 +89,7 @@
|
||||||
|
|
||||||
class Rover : public AP_HAL::HAL::Callbacks {
|
class Rover : public AP_HAL::HAL::Callbacks {
|
||||||
public:
|
public:
|
||||||
friend class GCS_MAVLINK;
|
friend class GCS_MAVLINK_Rover;
|
||||||
friend class Parameters;
|
friend class Parameters;
|
||||||
friend class AP_Arming;
|
friend class AP_Arming;
|
||||||
|
|
||||||
|
@ -176,7 +175,7 @@ private:
|
||||||
// GCS handling
|
// GCS handling
|
||||||
AP_SerialManager serial_manager;
|
AP_SerialManager serial_manager;
|
||||||
const uint8_t num_gcs;
|
const uint8_t num_gcs;
|
||||||
GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS];
|
GCS_MAVLINK_Rover gcs[MAVLINK_COMM_NUM_BUFFERS];
|
||||||
|
|
||||||
// relay support
|
// relay support
|
||||||
AP_Relay relay;
|
AP_Relay relay;
|
||||||
|
|
Loading…
Reference in New Issue