Sub: minimal support for inheritting from RC_Channel

This commit is contained in:
Peter Barker 2018-04-27 12:26:38 +10:00 committed by Randy Mackay
parent 9ce02bc504
commit 1ba0901e51
6 changed files with 57 additions and 3 deletions

View File

@ -158,7 +158,7 @@ void Sub::fifty_hz_loop()
failsafe_sensors_check();
// Update rc input/output
RC_Channels::read_input();
rc().read_input();
SRV_Channels::output_ch_all();
}

View File

@ -327,7 +327,7 @@ public:
#endif
// RC input channels
RC_Channels rc_channels;
RC_Channels_Sub rc_channels;
// control over servo output ranges
SRV_Channels servo_channels;

17
ArduSub/RC_Channel.cpp Normal file
View File

@ -0,0 +1,17 @@
#include "Sub.h"
#include "RC_Channel.h"
// defining these two macros and including the RC_Channels_VarInfo
// header defines the parameter information common to all vehicle
// types
#define RC_CHANNELS_SUBCLASS RC_Channels_Sub
#define RC_CHANNEL_SUBCLASS RC_Channel_Sub
#include <RC_Channel/RC_Channels_VarInfo.h>
// note that this callback is not presently used on Plane:
int8_t RC_Channels_Sub::flight_mode_channel_number() const
{
return 1; // sub does not have a flight mode channel
}

33
ArduSub/RC_Channel.h Normal file
View File

@ -0,0 +1,33 @@
#pragma once
#include <RC_Channel/RC_Channel.h>
class RC_Channel_Sub : public RC_Channel
{
public:
protected:
private:
};
class RC_Channels_Sub : public RC_Channels
{
public:
RC_Channel_Sub obj_channels[NUM_RC_CHANNELS];
RC_Channel_Sub *channel(const uint8_t chan) override {
if (chan > NUM_RC_CHANNELS) {
return nullptr;
}
return &obj_channels[chan];
}
protected:
// note that these callbacks are not presently used on Plane:
int8_t flight_mode_channel_number() const override;
};

View File

@ -54,7 +54,6 @@
#include <AC_PID/AC_PID_2D.h> // PID library (2-axis)
#include <AC_AttitudeControl/AC_AttitudeControl_Sub.h> // Attitude control library
#include <AC_AttitudeControl/AC_PosControl_Sub.h> // Position control library
#include <RC_Channel/RC_Channel.h> // RC Channel Library
#include <AP_Motors/AP_Motors.h> // AP Motors library
#include <AP_RangeFinder/AP_RangeFinder.h> // Range finder library
#include <Filter/Filter.h> // Filter library
@ -82,6 +81,7 @@
#include "defines.h"
#include "config.h"
#include "GCS_Mavlink.h"
#include "RC_Channel.h" // RC Channel Library
#include "Parameters.h"
#include "AP_Arming_Sub.h"
#include "GCS_Sub.h"
@ -130,6 +130,7 @@ public:
friend class Parameters;
friend class ParametersG2;
friend class AP_Arming_Sub;
friend class RC_Channels_Sub;
Sub(void);

View File

@ -87,6 +87,9 @@ void Sub::init_ardupilot()
gcs().set_dataflash(&DataFlash);
// initialise rc channels including setting mode
rc().init();
init_rc_in(); // sets up rc channels from radio
init_rc_out(); // sets up motors and output to escs
init_joystick(); // joystick initialization