ardupilot/libraries/AP_HAL_FLYMAPLE/RCInput.cpp

219 lines
7.5 KiB
C++
Raw Normal View History

/*
This program 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 program 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/>.
*/
/*
Flymaple port by Mike McCauley
Monitor a PPM-SUM input pin, and decode the channels based on pulse widths
Uses a timer to capture the time between negative transitions of the PPM-SUM pin
*/
#include <AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
// Flymaple RCInput
// PPM input from a single pin
#include "RCInput.h"
#include "FlymapleWirish.h"
using namespace AP_HAL_FLYMAPLE_NS;
extern const AP_HAL::HAL& hal;
/* private variables to communicate with input capture isr */
volatile uint16_t FLYMAPLERCInput::_pulse_capt[FLYMAPLE_RC_INPUT_NUM_CHANNELS] = {0};
volatile uint8_t FLYMAPLERCInput::_valid_channels = 0;
volatile uint32_t FLYMAPLERCInput::_last_input_interrupt_time = 0; // Last time the input interrupt ran
// Pin 6 is connected to timer 1 channel 1
#define FLYMAPLE_RC_INPUT_PIN 6
// This is the rollover count of the timer
// each count is 0.5us, so 600000 = 30ms
// We cant reliably measure intervals that exceed this time.
#define FLYMAPLE_TIMER_RELOAD 60000
FLYMAPLERCInput::FLYMAPLERCInput()
{}
// This interrupt triggers on a negative transiution of the PPM-SIM pin
void FLYMAPLERCInput::_timer_capt_cb(void)
{
_last_input_interrupt_time = hal.scheduler->millis();
static uint16 previous_count;
static uint8 channel_ctr;
// Read the CCR register, where the time count since the last input pin transition will be
timer_dev *tdev = PIN_MAP[FLYMAPLE_RC_INPUT_PIN].timer_device;
uint8 timer_channel = PIN_MAP[FLYMAPLE_RC_INPUT_PIN].timer_channel;
uint16 current_count = timer_get_compare(tdev, timer_channel);
uint32 sr = (tdev->regs).gen->SR;
uint32 overcapture_mask = (1 << (TIMER_SR_CC1OF_BIT + timer_channel - 1));
if (sr & overcapture_mask)
{
// Hmmm, lost an interrupt somewhere? Ignore this sample
(tdev->regs).gen->SR &= ~overcapture_mask; // Clear overcapture flag
return;
}
uint16_t pulse_width;
if (current_count < previous_count) {
pulse_width = current_count + FLYMAPLE_TIMER_RELOAD - previous_count;
} else {
pulse_width = current_count - previous_count;
}
// Pulse sequence repetition rate is about 22ms.
// Longest servo pulse is about 1.8ms
// Shortest servo pulse is about 0.5ms
// Shortest possible PPM sync pulse with 10 channels is about 4ms = 22 - (10 channels * 1.8)
if (pulse_width > 8000) { // 4ms
// sync pulse detected. Pass through values if at least a minimum number of channels received
if( channel_ctr >= FLYMAPLE_RC_INPUT_MIN_CHANNELS ) {
_valid_channels = channel_ctr;
// Clear any remaining channels, in case they were corrupted during a connect or something
while (channel_ctr < FLYMAPLE_RC_INPUT_NUM_CHANNELS)
_pulse_capt[channel_ctr++] = 0;
}
channel_ctr = 0;
} else {
// if (channel_ctr == 0)
// hal.uartA->printf("ch 0 %d\n", pulse_width);
if (channel_ctr < FLYMAPLE_RC_INPUT_NUM_CHANNELS) {
_pulse_capt[channel_ctr] = pulse_width;
channel_ctr++;
if (channel_ctr == FLYMAPLE_RC_INPUT_NUM_CHANNELS) {
_valid_channels = FLYMAPLE_RC_INPUT_NUM_CHANNELS;
}
}
}
previous_count = current_count;
}
void FLYMAPLERCInput::init(void* machtnichts)
{
/* initialize overrides */
clear_overrides();
// Configure pin 6 input to timer 1 CH1 bRin Input Capture mode
pinMode(FLYMAPLE_RC_INPUT_PIN, INPUT_PULLDOWN);
timer_dev *tdev = PIN_MAP[FLYMAPLE_RC_INPUT_PIN].timer_device;
uint8 timer_channel = PIN_MAP[FLYMAPLE_RC_INPUT_PIN].timer_channel;
timer_pause(tdev); // disabled
timer_set_prescaler(tdev, (CYCLES_PER_MICROSECOND/2) - 1); // 2MHz = 0.5us timer ticks
timer_set_reload(tdev, FLYMAPLE_TIMER_RELOAD-1);
// Without a filter, can get triggering on the wrong edge and other problems.
(tdev->regs).gen->CCMR1 = TIMER_CCMR1_CC1S_INPUT_TI1 | (3 << 4); // no prescaler, input from T1, filter internal clock, N=8
(tdev->regs).gen->CCER = TIMER_CCER_CC1P | TIMER_CCER_CC1E; // falling edge, enable capture
timer_attach_interrupt(tdev, timer_channel, _timer_capt_cb);
timer_generate_update(tdev);
timer_resume(tdev); // reenabled
}
bool FLYMAPLERCInput::new_input() {
if ((hal.scheduler->millis() - _last_input_interrupt_time) > 50)
_valid_channels = 0; // Lost RC Input?
return _valid_channels != 0;
}
uint8_t FLYMAPLERCInput::num_channels() {
return _valid_channels;
}
/* constrain captured pulse to be between min and max pulsewidth. */
static inline uint16_t constrain_pulse(uint16_t p) {
if (p > RC_INPUT_MAX_PULSEWIDTH) return RC_INPUT_MAX_PULSEWIDTH;
if (p < RC_INPUT_MIN_PULSEWIDTH) return RC_INPUT_MIN_PULSEWIDTH;
return p;
}
uint16_t FLYMAPLERCInput::read(uint8_t ch) {
timer_dev *tdev = PIN_MAP[FLYMAPLE_RC_INPUT_PIN].timer_device;
uint8 timer_channel = PIN_MAP[FLYMAPLE_RC_INPUT_PIN].timer_channel;
/* constrain ch */
if (ch >= FLYMAPLE_RC_INPUT_NUM_CHANNELS)
return 0;
/* grab channel from isr's memory in critical section*/
timer_disable_irq(tdev, timer_channel);
uint16_t capt = _pulse_capt[ch];
timer_enable_irq(tdev, timer_channel);
/* scale _pulse_capt from 0.5us units to 1us units. */
uint16_t pulse = constrain_pulse(capt >> 1);
/* Check for override */
uint16_t over = _override[ch];
return (over == 0) ? pulse : over;
}
uint8_t FLYMAPLERCInput::read(uint16_t* periods, uint8_t len) {
timer_dev *tdev = PIN_MAP[FLYMAPLE_RC_INPUT_PIN].timer_device;
uint8 timer_channel = PIN_MAP[FLYMAPLE_RC_INPUT_PIN].timer_channel;
/* constrain len */
if (len > FLYMAPLE_RC_INPUT_NUM_CHANNELS)
len = FLYMAPLE_RC_INPUT_NUM_CHANNELS;
/* grab channels from isr's memory in critical section */
timer_disable_irq(tdev, timer_channel);
for (uint8_t i = 0; i < len; i++) {
periods[i] = _pulse_capt[i];
}
timer_enable_irq(tdev, timer_channel);
/* Outside of critical section, do the math (in place) to scale and
* constrain the pulse. */
for (uint8_t i = 0; i < len; i++) {
/* scale _pulse_capt from 0.5us units to 1us units. */
periods[i] = constrain_pulse(periods[i] >> 1);
/* check for override */
if (_override[i] != 0) {
periods[i] = _override[i];
}
}
return _valid_channels;
}
bool FLYMAPLERCInput::set_overrides(int16_t *overrides, uint8_t len) {
bool res = false;
for (uint8_t i = 0; i < len; i++) {
res |= set_override(i, overrides[i]);
}
return res;
}
bool FLYMAPLERCInput::set_override(uint8_t channel, int16_t override) {
if (override < 0) return false; /* -1: no change. */
if (channel < FLYMAPLE_RC_INPUT_NUM_CHANNELS) {
_override[channel] = override;
if (override != 0) {
_valid_channels = 1;
return true;
}
}
return false;
}
void FLYMAPLERCInput::clear_overrides()
{
for (uint8_t i = 0; i < FLYMAPLE_RC_INPUT_NUM_CHANNELS; i++) {
_override[i] = 0;
}
}
#endif