/* 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 . */ /* 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 #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