mirror of https://github.com/ArduPilot/ardupilot
AP_RCProtocol: add and use AP_RCPROTOCOL_ENABLED
This commit is contained in:
parent
49879ab28c
commit
2c00813aab
|
@ -15,8 +15,12 @@
|
||||||
* Code by Andrew Tridgell and Siddharth Bharat Purohit
|
* Code by Andrew Tridgell and Siddharth Bharat Purohit
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <AP_Vehicle/AP_Vehicle_Type.h>
|
#include "AP_RCProtocol_config.h"
|
||||||
|
|
||||||
#include "AP_RCProtocol.h"
|
#include "AP_RCProtocol.h"
|
||||||
|
|
||||||
|
#if AP_RCPROTOCOL_ENABLED
|
||||||
|
|
||||||
#include "AP_RCProtocol_PPMSum.h"
|
#include "AP_RCProtocol_PPMSum.h"
|
||||||
#include "AP_RCProtocol_DSM.h"
|
#include "AP_RCProtocol_DSM.h"
|
||||||
#include "AP_RCProtocol_IBUS.h"
|
#include "AP_RCProtocol_IBUS.h"
|
||||||
|
@ -31,6 +35,8 @@
|
||||||
#include <AP_Math/AP_Math.h>
|
#include <AP_Math/AP_Math.h>
|
||||||
#include <RC_Channel/RC_Channel.h>
|
#include <RC_Channel/RC_Channel.h>
|
||||||
|
|
||||||
|
#include <AP_Vehicle/AP_Vehicle_Type.h>
|
||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
void AP_RCProtocol::init()
|
void AP_RCProtocol::init()
|
||||||
|
@ -405,6 +411,8 @@ void AP_RCProtocol::start_bind(void)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif // AP_RCPROTOCOL_ENABLED
|
||||||
|
|
||||||
/*
|
/*
|
||||||
return protocol name
|
return protocol name
|
||||||
*/
|
*/
|
||||||
|
@ -459,6 +467,7 @@ const char *AP_RCProtocol::protocol_name_from_protocol(rcprotocol_t protocol)
|
||||||
return nullptr;
|
return nullptr;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#if AP_RCPROTOCOL_ENABLED
|
||||||
/*
|
/*
|
||||||
return protocol name
|
return protocol name
|
||||||
*/
|
*/
|
||||||
|
@ -493,3 +502,5 @@ namespace AP {
|
||||||
return rcprot;
|
return rcprot;
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
#endif // AP_RCPROTOCOL_ENABLED
|
||||||
|
|
|
@ -28,9 +28,6 @@ class AP_RCProtocol_Backend;
|
||||||
|
|
||||||
class AP_RCProtocol {
|
class AP_RCProtocol {
|
||||||
public:
|
public:
|
||||||
AP_RCProtocol() {}
|
|
||||||
~AP_RCProtocol();
|
|
||||||
friend class AP_RCProtocol_Backend;
|
|
||||||
|
|
||||||
enum rcprotocol_t {
|
enum rcprotocol_t {
|
||||||
PPM = 0,
|
PPM = 0,
|
||||||
|
@ -64,6 +61,16 @@ public:
|
||||||
#endif
|
#endif
|
||||||
NONE //last enum always is None
|
NONE //last enum always is None
|
||||||
};
|
};
|
||||||
|
|
||||||
|
// return protocol name as a string
|
||||||
|
static const char *protocol_name_from_protocol(rcprotocol_t protocol);
|
||||||
|
|
||||||
|
#if AP_RCPROTOCOL_ENABLED
|
||||||
|
|
||||||
|
AP_RCProtocol() {}
|
||||||
|
~AP_RCProtocol();
|
||||||
|
friend class AP_RCProtocol_Backend;
|
||||||
|
|
||||||
void init();
|
void init();
|
||||||
bool valid_serial_prot() const
|
bool valid_serial_prot() const
|
||||||
{
|
{
|
||||||
|
@ -134,9 +141,6 @@ public:
|
||||||
int16_t get_RSSI(void) const;
|
int16_t get_RSSI(void) const;
|
||||||
int16_t get_rx_link_quality(void) const;
|
int16_t get_rx_link_quality(void) const;
|
||||||
|
|
||||||
// return protocol name as a string
|
|
||||||
static const char *protocol_name_from_protocol(rcprotocol_t protocol);
|
|
||||||
|
|
||||||
// return protocol name as a string
|
// return protocol name as a string
|
||||||
const char *protocol_name(void) const;
|
const char *protocol_name(void) const;
|
||||||
|
|
||||||
|
@ -196,10 +200,15 @@ private:
|
||||||
|
|
||||||
// allowed RC protocols mask (first bit means "all")
|
// allowed RC protocols mask (first bit means "all")
|
||||||
uint32_t rc_protocols_mask;
|
uint32_t rc_protocols_mask;
|
||||||
|
|
||||||
|
#endif // AP_RCPROTCOL_ENABLED
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
|
#if AP_RCPROTOCOL_ENABLED
|
||||||
namespace AP {
|
namespace AP {
|
||||||
AP_RCProtocol &RC();
|
AP_RCProtocol &RC();
|
||||||
};
|
};
|
||||||
|
|
||||||
#include "AP_RCProtocol_Backend.h"
|
#include "AP_RCProtocol_Backend.h"
|
||||||
|
#endif // AP_RCProtocol_enabled
|
||||||
|
|
|
@ -15,6 +15,10 @@
|
||||||
* Code by Andrew Tridgell and Siddharth Bharat Purohit
|
* Code by Andrew Tridgell and Siddharth Bharat Purohit
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
#include "AP_RCProtocol_config.h"
|
||||||
|
|
||||||
|
#if AP_RCPROTOCOL_ENABLED
|
||||||
|
|
||||||
#include "AP_RCProtocol.h"
|
#include "AP_RCProtocol.h"
|
||||||
#include <AP_Math/AP_Math.h>
|
#include <AP_Math/AP_Math.h>
|
||||||
#include <RC_Channel/RC_Channel.h>
|
#include <RC_Channel/RC_Channel.h>
|
||||||
|
@ -207,3 +211,5 @@ void AP_RCProtocol_Backend::log_data(AP_RCProtocol::rcprotocol_t prot, uint32_t
|
||||||
}
|
}
|
||||||
#endif // HAL_LOGGING_ENABLED
|
#endif // HAL_LOGGING_ENABLED
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif // AP_RCPROTOCOL_ENABLED
|
||||||
|
|
|
@ -18,6 +18,9 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "AP_RCProtocol.h"
|
#include "AP_RCProtocol.h"
|
||||||
|
|
||||||
|
#if AP_RCPROTOCOL_ENABLED
|
||||||
|
|
||||||
#include <AP_HAL/utility/sparse-endian.h>
|
#include <AP_HAL/utility/sparse-endian.h>
|
||||||
#include <AP_VideoTX/AP_VideoTX_config.h>
|
#include <AP_VideoTX/AP_VideoTX_config.h>
|
||||||
|
|
||||||
|
@ -131,3 +134,5 @@ private:
|
||||||
int16_t rssi = -1;
|
int16_t rssi = -1;
|
||||||
int16_t rx_link_quality = -1;
|
int16_t rx_link_quality = -1;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
#endif // AP_RCPROTOCOL_ENABLED
|
||||||
|
|
|
@ -17,7 +17,11 @@
|
||||||
/*
|
/*
|
||||||
with thanks to PX4 dsm.c for DSM decoding approach
|
with thanks to PX4 dsm.c for DSM decoding approach
|
||||||
*/
|
*/
|
||||||
#include <AP_Vehicle/AP_Vehicle_Type.h>
|
|
||||||
|
#include "AP_RCProtocol_config.h"
|
||||||
|
|
||||||
|
#if AP_RCPROTOCOL_ENABLED
|
||||||
|
|
||||||
#include "AP_RCProtocol_DSM.h"
|
#include "AP_RCProtocol_DSM.h"
|
||||||
#include <AP_VideoTX/AP_VideoTX_config.h>
|
#include <AP_VideoTX/AP_VideoTX_config.h>
|
||||||
|
|
||||||
|
@ -534,3 +538,5 @@ void AP_RCProtocol_DSM::process_byte(uint8_t b, uint32_t baudrate)
|
||||||
}
|
}
|
||||||
_process_byte(AP_HAL::millis(), b);
|
_process_byte(AP_HAL::millis(), b);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif // AP_RCPROTOCOL_ENABLED
|
||||||
|
|
|
@ -17,7 +17,12 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "AP_RCProtocol.h"
|
#include "AP_RCProtocol_config.h"
|
||||||
|
|
||||||
|
#if AP_RCPROTOCOL_ENABLED
|
||||||
|
|
||||||
|
#include "AP_RCProtocol_Backend.h"
|
||||||
|
|
||||||
#include "SoftSerial.h"
|
#include "SoftSerial.h"
|
||||||
|
|
||||||
#define AP_DSM_MAX_CHANNELS 12
|
#define AP_DSM_MAX_CHANNELS 12
|
||||||
|
@ -77,3 +82,5 @@ private:
|
||||||
|
|
||||||
SoftSerial ss{115200, SoftSerial::SERIAL_CONFIG_8N1};
|
SoftSerial ss{115200, SoftSerial::SERIAL_CONFIG_8N1};
|
||||||
};
|
};
|
||||||
|
|
||||||
|
#endif // AP_RCPROTOCOL_ENABLED
|
||||||
|
|
|
@ -14,6 +14,10 @@
|
||||||
*
|
*
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
#include "AP_RCProtocol_config.h"
|
||||||
|
|
||||||
|
#if AP_RCPROTOCOL_ENABLED
|
||||||
|
|
||||||
#include "AP_RCProtocol_IBUS.h"
|
#include "AP_RCProtocol_IBUS.h"
|
||||||
|
|
||||||
// constructor
|
// constructor
|
||||||
|
@ -105,3 +109,5 @@ void AP_RCProtocol_IBUS::process_byte(uint8_t b, uint32_t baudrate)
|
||||||
}
|
}
|
||||||
_process_byte(AP_HAL::micros(), b);
|
_process_byte(AP_HAL::micros(), b);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif // AP_RCPROTOCOL_ENABLED
|
||||||
|
|
|
@ -16,10 +16,15 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include "AP_RCProtocol_config.h"
|
||||||
|
|
||||||
|
#if AP_RCPROTOCOL_ENABLED
|
||||||
|
|
||||||
#define IBUS_FRAME_SIZE 32
|
#define IBUS_FRAME_SIZE 32
|
||||||
#define IBUS_INPUT_CHANNELS 14
|
#define IBUS_INPUT_CHANNELS 14
|
||||||
|
|
||||||
#include "AP_RCProtocol.h"
|
#include "AP_RCProtocol_Backend.h"
|
||||||
|
|
||||||
#include "SoftSerial.h"
|
#include "SoftSerial.h"
|
||||||
|
|
||||||
class AP_RCProtocol_IBUS : public AP_RCProtocol_Backend
|
class AP_RCProtocol_IBUS : public AP_RCProtocol_Backend
|
||||||
|
@ -40,3 +45,5 @@ private:
|
||||||
uint32_t last_byte_us;
|
uint32_t last_byte_us;
|
||||||
} byte_input;
|
} byte_input;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
#endif // AP_RCPROTOCOL_ENABLED
|
||||||
|
|
|
@ -17,6 +17,8 @@
|
||||||
|
|
||||||
#include "AP_RCProtocol_PPMSum.h"
|
#include "AP_RCProtocol_PPMSum.h"
|
||||||
|
|
||||||
|
#if AP_RCPROTOCOL_ENABLED
|
||||||
|
|
||||||
/*
|
/*
|
||||||
process a PPM-sum pulse of the given width
|
process a PPM-sum pulse of the given width
|
||||||
*/
|
*/
|
||||||
|
@ -63,3 +65,5 @@ void AP_RCProtocol_PPMSum::process_pulse(uint32_t width_s0, uint32_t width_s1)
|
||||||
ppm_state._channel_counter = -1;
|
ppm_state._channel_counter = -1;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif // AP_RCPROTOCOL_ENABLED
|
||||||
|
|
|
@ -16,7 +16,11 @@
|
||||||
*/
|
*/
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "AP_RCProtocol.h"
|
#include "AP_RCProtocol_config.h"
|
||||||
|
|
||||||
|
#if AP_RCPROTOCOL_ENABLED
|
||||||
|
|
||||||
|
#include "AP_RCProtocol_Backend.h"
|
||||||
|
|
||||||
class AP_RCProtocol_PPMSum : public AP_RCProtocol_Backend {
|
class AP_RCProtocol_PPMSum : public AP_RCProtocol_Backend {
|
||||||
public:
|
public:
|
||||||
|
@ -29,3 +33,5 @@ private:
|
||||||
uint16_t _pulse_capt[MAX_RCIN_CHANNELS];
|
uint16_t _pulse_capt[MAX_RCIN_CHANNELS];
|
||||||
} ppm_state;
|
} ppm_state;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
#endif // AP_RCPROTOCOL_ENABLED
|
||||||
|
|
|
@ -42,6 +42,10 @@
|
||||||
*
|
*
|
||||||
* @author Marco Bauer <marco@wtns.de>
|
* @author Marco Bauer <marco@wtns.de>
|
||||||
*/
|
*/
|
||||||
|
#include "AP_RCProtocol_config.h"
|
||||||
|
|
||||||
|
#if AP_RCPROTOCOL_ENABLED
|
||||||
|
|
||||||
#include "AP_RCProtocol_ST24.h"
|
#include "AP_RCProtocol_ST24.h"
|
||||||
|
|
||||||
// #define SUMD_DEBUG
|
// #define SUMD_DEBUG
|
||||||
|
@ -233,3 +237,5 @@ void AP_RCProtocol_ST24::process_byte(uint8_t byte, uint32_t baudrate)
|
||||||
}
|
}
|
||||||
_process_byte(byte);
|
_process_byte(byte);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif // AP_RCPROTOCOL_ENABLED
|
||||||
|
|
|
@ -17,7 +17,11 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "AP_RCProtocol.h"
|
#include "AP_RCProtocol_config.h"
|
||||||
|
|
||||||
|
#if AP_RCPROTOCOL_ENABLED
|
||||||
|
|
||||||
|
#include "AP_RCProtocol_Backend.h"
|
||||||
#include "SoftSerial.h"
|
#include "SoftSerial.h"
|
||||||
|
|
||||||
#define ST24_DATA_LEN_MAX 64
|
#define ST24_DATA_LEN_MAX 64
|
||||||
|
@ -149,3 +153,5 @@ private:
|
||||||
|
|
||||||
SoftSerial ss{115200, SoftSerial::SERIAL_CONFIG_8N1};
|
SoftSerial ss{115200, SoftSerial::SERIAL_CONFIG_8N1};
|
||||||
};
|
};
|
||||||
|
|
||||||
|
#endif // AP_RCPROTOCOL_ENABLED
|
||||||
|
|
|
@ -42,7 +42,12 @@
|
||||||
*
|
*
|
||||||
* @author Marco Bauer <marco@wtns.de>
|
* @author Marco Bauer <marco@wtns.de>
|
||||||
*/
|
*/
|
||||||
|
#include "AP_RCProtocol_config.h"
|
||||||
|
|
||||||
|
#if AP_RCPROTOCOL_ENABLED
|
||||||
|
|
||||||
#include "AP_RCProtocol_SUMD.h"
|
#include "AP_RCProtocol_SUMD.h"
|
||||||
|
|
||||||
#include <AP_Math/crc.h>
|
#include <AP_Math/crc.h>
|
||||||
|
|
||||||
#define SUMD_HEADER_LENGTH 3
|
#define SUMD_HEADER_LENGTH 3
|
||||||
|
@ -332,3 +337,5 @@ void AP_RCProtocol_SUMD::process_byte(uint8_t byte, uint32_t baudrate)
|
||||||
}
|
}
|
||||||
_process_byte(AP_HAL::micros(), byte);
|
_process_byte(AP_HAL::micros(), byte);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#endif // AP_RCPROTOCOL_ENABLED
|
||||||
|
|
|
@ -17,7 +17,11 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "AP_RCProtocol.h"
|
#include "AP_RCProtocol_config.h"
|
||||||
|
|
||||||
|
#if AP_RCPROTOCOL_ENABLED
|
||||||
|
|
||||||
|
#include "AP_RCProtocol_Backend.h"
|
||||||
#include "SoftSerial.h"
|
#include "SoftSerial.h"
|
||||||
|
|
||||||
#define SUMD_MAX_CHANNELS 32
|
#define SUMD_MAX_CHANNELS 32
|
||||||
|
@ -68,3 +72,5 @@ private:
|
||||||
|
|
||||||
SoftSerial ss{115200, SoftSerial::SERIAL_CONFIG_8N1};
|
SoftSerial ss{115200, SoftSerial::SERIAL_CONFIG_8N1};
|
||||||
};
|
};
|
||||||
|
|
||||||
|
#endif // AP_RCPROTOCOL_ENABLED
|
||||||
|
|
|
@ -3,8 +3,12 @@
|
||||||
#include <AP_HAL/AP_HAL_Boards.h>
|
#include <AP_HAL/AP_HAL_Boards.h>
|
||||||
#include <AP_Frsky_Telem/AP_Frsky_config.h>
|
#include <AP_Frsky_Telem/AP_Frsky_config.h>
|
||||||
|
|
||||||
|
#ifndef AP_RCPROTOCOL_ENABLED
|
||||||
|
#define AP_RCPROTOCOL_ENABLED 1
|
||||||
|
#endif
|
||||||
|
|
||||||
#ifndef AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED
|
#ifndef AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED
|
||||||
#define AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED 1
|
#define AP_RCPROTOCOL_BACKEND_DEFAULT_ENABLED AP_RCPROTOCOL_ENABLED
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifndef AP_RCPROTOCOL_CRSF_ENABLED
|
#ifndef AP_RCPROTOCOL_CRSF_ENABLED
|
||||||
|
|
Loading…
Reference in New Issue