ardupilot/libraries/AP_RCProtocol/AP_RCProtocol_DSM.cpp

327 lines
10 KiB
C++
Raw Normal View History

/*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*
* Code by Andrew Tridgell and Siddharth Bharat Purohit
*/
/*
* See https://www.spektrumrc.com/ProdInfo/Files/Remote%20Receiver%20Interfacing%20Rev%20A.pdf for official
* Spektrum documentation on the format.
*/
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include "AP_RCProtocol_DSM.h"
#if !APM_BUILD_TYPE(APM_BUILD_iofirmware)
#include "AP_RCProtocol_SRXL2.h"
#endif
#include <stdio.h>
2018-04-10 03:14:54 -03:00
extern const AP_HAL::HAL& hal;
// #define DSM_DEBUG
#ifdef DSM_DEBUG
# define debug(fmt, args...) printf(fmt "\n", ##args)
#else
# define debug(fmt, args...) do {} while(0)
#endif
#define DSM_FRAME_SIZE 16 /**<DSM frame size in bytes*/
#define DSM_FRAME_CHANNELS 7 /**<Max supported DSM channels*/
#define DSM2_1024_22MS 0x01
#define DSM2_2048_11MS 0x12
#define DSMX_2048_22MS 0xa2
#define DSMX_2048_11MS 0xb2
#define SPEKTRUM_VTX_CONTROL_FRAME_MASK 0xf000f000
#define SPEKTRUM_VTX_CONTROL_FRAME 0xe000e000
#define SPEKTRUM_VTX_BAND_MASK 0x00e00000
#define SPEKTRUM_VTX_CHANNEL_MASK 0x000f0000
#define SPEKTRUM_VTX_PIT_MODE_MASK 0x00000010
#define SPEKTRUM_VTX_POWER_MASK 0x00000007
#define SPEKTRUM_VTX_BAND_SHIFT 21
#define SPEKTRUM_VTX_CHANNEL_SHIFT 16
#define SPEKTRUM_VTX_PIT_MODE_SHIFT 4
#define SPEKTRUM_VTX_POWER_SHIFT 0
void AP_RCProtocol_DSM::process_pulse(uint32_t width_s0, uint32_t width_s1)
{
2018-11-04 23:56:00 -04:00
uint8_t b;
if (ss.process_pulse(width_s0, width_s1, b)) {
_process_byte(ss.get_byte_timestamp_us(), b);
}
}
/**
* Decode the entire dsm frame (all contained channels)
*
*/
bool AP_RCProtocol_DSM::dsm_decode(uint32_t frame_time_us, const uint8_t dsm_frame[16],
uint16_t *values, uint16_t *num_values, uint16_t max_values)
{
/* we have received something we think is a dsm_frame */
last_frame_time_us = frame_time_us;
// Get the VTX control bytes in a frame
uint32_t vtxControl = ((dsm_frame[AP_DSM_FRAME_SIZE-4] << 24)
| (dsm_frame[AP_DSM_FRAME_SIZE-3] << 16)
| (dsm_frame[AP_DSM_FRAME_SIZE-2] << 8)
| (dsm_frame[AP_DSM_FRAME_SIZE-1] << 0));
uint8_t dsm_frame_data_size;
// Handle VTX control frame.
if ((vtxControl & SPEKTRUM_VTX_CONTROL_FRAME_MASK) == SPEKTRUM_VTX_CONTROL_FRAME
&& (dsm_frame[2] & 0x80) == 0) {
dsm_frame_data_size = AP_DSM_FRAME_SIZE - 4;
#if !APM_BUILD_TYPE(APM_BUILD_iofirmware)
AP_RCProtocol_SRXL2::configure_vtx(
(vtxControl & SPEKTRUM_VTX_BAND_MASK) >> SPEKTRUM_VTX_BAND_SHIFT,
(vtxControl & SPEKTRUM_VTX_CHANNEL_MASK) >> SPEKTRUM_VTX_CHANNEL_SHIFT,
(vtxControl & SPEKTRUM_VTX_POWER_MASK) >> SPEKTRUM_VTX_POWER_SHIFT,
(vtxControl & SPEKTRUM_VTX_PIT_MODE_MASK) >> SPEKTRUM_VTX_PIT_MODE_SHIFT);
#endif
} else {
dsm_frame_data_size = AP_DSM_FRAME_SIZE;
}
// Get the RC control channel inputs
for (uint8_t b = 3; b < dsm_frame_data_size; b += 2) {
uint8_t channel = 0x0F & (dsm_frame[b - 1] >> channel_shift);
uint32_t value = ((uint32_t)(dsm_frame[b - 1] & channel_mask) << 8) + dsm_frame[b];
/* ignore channels out of range */
if (channel >= max_values) {
continue;
}
/* update the decoded channel count */
if (channel >= *num_values) {
*num_values = channel + 1;
}
/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding. */
if (channel_shift == 2) {
value *= 2;
}
/* Spektrum scaling is defined as (see reference):
2048: PWM_OUT = (ServoPosition x 58.3μs) + 903
1024: PWM_OUT = (ServoPosition x 116.6μs) + 903 */
/* scaled integer for decent accuracy while staying efficient */
value = ((int32_t)value * 1194) / 2048 + 903;
/*
* Store the decoded channel into the R/C input buffer, taking into
* account the different ideas about channel assignement that we have.
*
* Specifically, the first four channels in rc_channel_data are roll, pitch, thrust, yaw,
* but the first four channels from the DSM receiver are thrust, roll, pitch, yaw.
*/
switch (channel) {
case 0:
channel = 2;
break;
case 1:
channel = 0;
break;
case 2:
channel = 1;
break;
default:
break;
}
values[channel] = value;
}
/*
* XXX Note that we may be in failsafe here; we need to work out how to detect that.
*/
return true;
}
2018-04-10 03:14:54 -03:00
/*
start bind on DSM satellites
*/
void AP_RCProtocol_DSM::start_bind(void)
{
bind_state = BIND_STATE1;
}
/*
update function used for bind state machine
*/
void AP_RCProtocol_DSM::update(void)
{
#if defined(HAL_GPIO_SPEKTRUM_PWR) && defined(HAL_GPIO_SPEKTRUM_RC)
switch (bind_state) {
case BIND_STATE_NONE:
break;
case BIND_STATE1:
hal.gpio->write(HAL_GPIO_SPEKTRUM_PWR, !HAL_SPEKTRUM_PWR_ENABLED);
hal.gpio->pinMode(HAL_GPIO_SPEKTRUM_RC, 1);
hal.gpio->write(HAL_GPIO_SPEKTRUM_RC, 1);
bind_last_ms = AP_HAL::millis();
bind_state = BIND_STATE2;
break;
case BIND_STATE2: {
uint32_t now = AP_HAL::millis();
if (now - bind_last_ms > 500) {
hal.gpio->write(HAL_GPIO_SPEKTRUM_PWR, HAL_SPEKTRUM_PWR_ENABLED);
bind_last_ms = now;
bind_state = BIND_STATE3;
}
break;
}
case BIND_STATE3: {
uint32_t now = AP_HAL::millis();
if (now - bind_last_ms > 72) {
// 9 pulses works with all satellite receivers, and supports the highest
// available protocol
const uint8_t num_pulses = 9;
for (uint8_t i=0; i<num_pulses; i++) {
hal.scheduler->delay_microseconds(120);
hal.gpio->write(HAL_GPIO_SPEKTRUM_RC, 0);
hal.scheduler->delay_microseconds(120);
hal.gpio->write(HAL_GPIO_SPEKTRUM_RC, 1);
2018-04-10 03:14:54 -03:00
}
bind_last_ms = now;
bind_state = BIND_STATE4;
}
break;
}
2018-04-10 03:14:54 -03:00
case BIND_STATE4: {
uint32_t now = AP_HAL::millis();
if (now - bind_last_ms > 50) {
hal.gpio->pinMode(HAL_GPIO_SPEKTRUM_RC, 0);
bind_state = BIND_STATE_NONE;
}
break;
}
}
#endif
}
/*
parse one DSM byte, maintaining decoder state
*/
bool AP_RCProtocol_DSM::dsm_parse_byte(uint32_t frame_time_us, uint8_t b, uint16_t *values,
uint16_t *num_values, uint16_t max_channels)
{
/* this is set by the decoding state machine and will default to false
* once everything that was decodable has been decoded.
*/
bool decode_ret = false;
// we took too long decoding, start again
if (byte_input.ofs > 0 && (frame_time_us - start_frame_time_us) > 6000U) {
start_frame_time_us = frame_time_us;
byte_input.ofs = 0;
}
// there will be at least a 5ms gap between successive DSM frames. if we see it
// assume we are starting a new frame
if ((frame_time_us - last_rx_time_us) > 5000U) {
start_frame_time_us = frame_time_us;
byte_input.ofs = 0;
}
/* overflow check */
if (byte_input.ofs >= AP_DSM_FRAME_SIZE) {
start_frame_time_us = frame_time_us;
byte_input.ofs = 0;
}
if (byte_input.ofs == 1) {
// saw a beginning of frame marker
if (b == DSM2_1024_22MS || b == DSM2_2048_11MS || b == DSMX_2048_22MS || b == DSMX_2048_11MS) {
if (b == DSM2_1024_22MS) {
// 10 bit frames
channel_shift = 2;
channel_mask = 0x03;
} else {
// 11 bit frames
channel_shift = 3;
channel_mask = 0x07;
}
// bad frame marker so reset
} else {
start_frame_time_us = frame_time_us;
byte_input.ofs = 0;
}
}
byte_input.buf[byte_input.ofs++] = b;
/* decode whatever we got and expect */
if (byte_input.ofs == AP_DSM_FRAME_SIZE) {
log_data(AP_RCProtocol::DSM, frame_time_us, byte_input.buf, byte_input.ofs);
#ifdef DSM_DEBUG
for (uint16_t i = 0; i < 16; i++) {
printf("%02x", byte_input.buf[i]);
}
printf("\n%02x%02x", byte_input.buf[0], byte_input.buf[1]);
for (uint16_t i = 2; i < 16; i+=2) {
printf(" %01x/%03x", (byte_input.buf[i] & 0x78) >> 4, (byte_input.buf[i] & 0x7) << 8 | byte_input.buf[i+1]);
}
printf("\n");
#endif
decode_ret = dsm_decode(frame_time_us, byte_input.buf, values, &chan_count, max_channels);
/* we consumed the partial frame, reset */
byte_input.ofs = 0;
}
if (decode_ret) {
*num_values = chan_count;
}
last_rx_time_us = frame_time_us;
/* return false as default */
return decode_ret;
}
// support byte input
void AP_RCProtocol_DSM::_process_byte(uint32_t timestamp_us, uint8_t b)
{
uint16_t v[AP_DSM_MAX_CHANNELS];
uint16_t nchan;
memcpy(v, last_values, sizeof(v));
if (dsm_parse_byte(timestamp_us, b, v, &nchan, AP_DSM_MAX_CHANNELS)) {
memcpy(last_values, v, sizeof(v));
if (nchan >= MIN_RCIN_CHANNELS) {
add_input(nchan, last_values, false);
}
}
}
2018-11-04 23:56:00 -04:00
// support byte input
void AP_RCProtocol_DSM::process_byte(uint8_t b, uint32_t baudrate)
{
if (baudrate != 115200) {
return;
}
_process_byte(AP_HAL::micros(), b);
2018-11-04 23:56:00 -04:00
}