mirror of https://github.com/ArduPilot/ardupilot
215 lines
7.5 KiB
C++
215 lines
7.5 KiB
C++
/*
|
|
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
|
|
}
|
|
|
|
uint8_t FLYMAPLERCInput::valid_channels() {
|
|
if ((hal.scheduler->millis() - _last_input_interrupt_time) > 50)
|
|
_valid_channels = 0; // Lost RC Input?
|
|
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
|