Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_HAL_F4Light/RCOutput.cpp

723 lines
20 KiB
C++
Raw Blame History

/*
(c) 2017 night_ghost@ykoctpa.ru
*/
#pragma GCC optimize ("O2")
#include "AP_HAL_F4Light.h"
#include "RCOutput.h"
#include <AP_Param_Helper/AP_Param_Helper.h>
#include <4way/serial_4way.h>
#include "GPIO.h"
using namespace F4Light;
// only one!
//#define DEBUG_PWM 5 // motor 6
//#define DEBUG_INT 5
#define F4Light_OUT_CHANNELS 6 // motor's channels enabled by default
// #if FRAME_CONFIG == QUAD_FRAME // this is only QUAD layouts
// ArduCopter
static const uint8_t output_channels_arducopter[]= { // pin assignment
SERVO_PIN_1, //Timer3/3 - 1
SERVO_PIN_2, //Timer3/4 - 2
SERVO_PIN_3, //Timer2/3 - 3
SERVO_PIN_4, //Timer2/2 - 4
#ifdef SERVO_PIN_5
SERVO_PIN_5, //Timer2/1
#endif
#ifdef SERVO_PIN_6
SERVO_PIN_6, //Timer2/0
#endif
// servo channels on input port
4, // PB14 CH1_IN - PPM1 Use this channels asd servo only if you use DSM via UART as RC
5, // PB15 CH2_IN - PPM2
12, // PC6 CH3_IN UART6
13, // PC7 CH4_IN UART6
14, // PC8 CH5_IN i2c
15, // PC9 CH6_IN i2c
};
// mimics foreign layouts
// OpenPilot
static const uint8_t output_channels_openpilot[]= { // pin assignment
SERVO_PIN_2, //Timer3/4 - 2
SERVO_PIN_4, //Timer2/2 - 4
SERVO_PIN_1, //Timer3/3 - 1
SERVO_PIN_3, //Timer2/3 - 3
#ifdef SERVO_PIN_5
SERVO_PIN_5, //Timer2/1
#endif
#ifdef SERVO_PIN_6
SERVO_PIN_6, //Timer2/0
#endif
// servo channels on input port
// 4, // PB14 CH1_IN - PPM1 Use this channels asd servo only if you use DSM via UART as RC
5, // PB15 CH2_IN - PPM2
12, // PC6 CH3_IN UART6
13, // PC7 CH4_IN UART6
14, // PC8 CH5_IN i2c
15, // PC9 CH6_IN i2c
};
// Cleanflight
static const uint8_t output_channels_cleanflight[]= { // pin assignment
SERVO_PIN_2, //Timer3/4 - 2
SERVO_PIN_3, //Timer2/3 - 3
SERVO_PIN_4, //Timer2/2 - 4
SERVO_PIN_1, //Timer3/3 - 1
#ifdef SERVO_PIN_5
SERVO_PIN_5, //Timer2/1
#endif
#ifdef SERVO_PIN_6
SERVO_PIN_6, //Timer2/0
#endif
// servo channels on input port
// 4, // PB14 CH1_IN - PPM1 Use this channels asd servo only if you use DSM via UART as RC
5, // PB15 CH2_IN - PPM2
12, // PC6 CH3_IN UART6
13, // PC7 CH4_IN UART6
14, // PC8 CH5_IN i2c
15, // PC9 CH6_IN i2c
};
// Arducopter,shifted 2 pins right to use up to 2 servos
static const uint8_t output_channels_servo[]= { // pin assignment
SERVO_PIN_3, //Timer2/3 - 1
SERVO_PIN_4, //Timer2/2 - 2
#ifdef SERVO_PIN_5
SERVO_PIN_5, //Timer2/1 - 3
#endif
#ifdef SERVO_PIN_6
SERVO_PIN_6, //Timer2/0 - 4
#endif
SERVO_PIN_1, //Timer3/3 servo1
SERVO_PIN_2, //Timer3/4 servo2
// servo channels on input port
// 4, // PB14 CH1_IN - PPM1 Use this channels as servo only if you use DSM via UART as RC
5, // PB15 CH2_IN - PPM2
12, // PC6 CH3_IN UART6
13, // PC7 CH4_IN UART6
14, // PC8 CH5_IN i2c
15, // PC9 CH6_IN i2c
};
static const uint8_t * const revo_motor_map[]={
output_channels_arducopter,
output_channels_servo,
output_channels_openpilot,
output_channels_cleanflight,
};
// #endif
static const uint8_t *output_channels = output_channels_openpilot; // current pin assignment
enum BOARD_PWM_MODES RCOutput::_mode = BOARD_PWM_NORMAL;
bool RCOutput::_once_mode = false;
uint16_t RCOutput::_period[F4Light_MAX_OUTPUT_CHANNELS] IN_CCM;
uint16_t RCOutput::_freq[F4Light_MAX_OUTPUT_CHANNELS] IN_CCM;
uint8_t RCOutput::_initialized[F4Light_MAX_OUTPUT_CHANNELS] IN_CCM;
uint16_t RCOutput::_enabled_channels=0;
bool RCOutput::_sbus_enabled=0;
bool RCOutput::_corked=0;
uint8_t RCOutput::_used_channels=0;
uint8_t RCOutput::_servo_mask=0;
uint32_t RCOutput::_timer2_preload;
uint16_t RCOutput::_timer3_preload;
uint8_t RCOutput::_pwm_type=0;
const timer_dev* RCOutput::out_timers[16] IN_CCM;
uint8_t RCOutput::num_out_timers IN_CCM;
#define PWM_TIMER_KHZ 2000 // 1000 in cleanflight
#define ONESHOT125_TIMER_KHZ 8000 // 8000 in cleanflight
#define ONESHOT42_TIMER_KHZ 28000 // 24000 in cleanflight
#define PWM_BRUSHED_TIMER_KHZ 16000 // 8000 in cleanflight
#define _BV(bit) (1U << (bit))
void RCOutput::init()
{
memset(&_period[0], 0, sizeof(_period));
memset(&_initialized[0], 0, sizeof(_initialized));
_used_channels=0;
}
void RCOutput::do_4way_if(AP_HAL::UARTDriver* uart) {
esc4wayInit(output_channels, F4Light_OUT_CHANNELS);
esc4wayProcess(uart);
}
#ifdef DEBUG_INT
extern "C" int printf(const char *msg, ...);
static void isr_handler(){
uint32_t *sp;
#define FRAME_SHIFT 10 // uses IRQ handler itself
asm volatile ("mov %0, sp\n\t" : "=rm" (sp) );
// Stack frame contains:
// r0, r1, r2, r3, r12, r14, the return address and xPSR
// - Stacked R0 = sp[0]
// - Stacked R1 = sp[1]
// - Stacked R2 = sp[2]
// - Stacked R3 = sp[3]
// - Stacked R12 = sp[4]
// - Stacked LR = sp[5]
// - Stacked PC = sp[6]
// - Stacked xPSR= sp[7]
printf("pc=%lx lr=%lx\n", sp[FRAME_SHIFT+6], sp[FRAME_SHIFT+5]);
}
#endif
void RCOutput::lateInit(){ // 2nd stage with loaded parameters
uint8_t map = hal_param_helper->_motor_layout;
_servo_mask = hal_param_helper->_servo_mask;
_pwm_type = hal_param_helper->_pwm_type;
if(map >= ARRAY_SIZE(revo_motor_map)) return; // don't initialize if parameter is wrong
output_channels = revo_motor_map[map];
InitPWM();
#ifdef DEBUG_INT
uint8_t spin = output_channels[DEBUG_INT]; // motor 6 as button
GPIO::_pinMode(spin, INPUT_PULLUP);
Revo_handler h = { .vp = isr_handler };
GPIO::_attach_interrupt(spin, h.h, FALLING, 0);
#endif
}
void RCOutput::InitPWM()
{
for(uint8_t i = 0; i < F4Light_MAX_OUTPUT_CHANNELS; i++) {
_freq[i] = 50;
}
fill_timers();
_set_output_mode(MODE_PWM_NORMAL); // init timers
}
// not from _freq to take channel dependency
uint16_t RCOutput::get_freq(uint8_t ch) {
if(ch >= F4Light_MAX_OUTPUT_CHANNELS) return 0;
const timer_dev *dev = PIN_MAP[output_channels[ch]].timer_device;
/* transform to period by inverse of _time_period(icr) */
return (uint16_t)(dev->state->freq / timer_get_reload(dev));
}
// fill array of used timers
void RCOutput::fill_timers(){
memset(out_timers, 0, sizeof(out_timers)); // clear it first
num_out_timers=0;
for (uint16_t ch = 0; ch < F4Light_OUT_CHANNELS; ch++) {
if (!(_enabled_channels & _BV(ch))) continue; // not enabled
const timer_dev *tim = PIN_MAP[output_channels[ch]].timer_device;
bool add=true;
for(uint8_t i =0; i<num_out_timers;i++){
if(out_timers[i]==tim){
add=false;
break;
}
}
if(add) out_timers[num_out_timers++] = tim;
}
}
void RCOutput::_set_output_mode(enum RCOutput::output_mode mode) {
uint32_t period=0;
uint32_t freq;
_once_mode=false;
switch(_pwm_type) {
case 0:
default:
switch(mode){
case MODE_PWM_NORMAL:
_mode=BOARD_PWM_NORMAL;
break;
case MODE_PWM_BRUSHED:
_mode=BOARD_PWM_BRUSHED;
break;
default:
case MODE_PWM_ONESHOT:
_mode=BOARD_PWM_ONESHOT;
break;
}
break;
case 1:
_mode=BOARD_PWM_ONESHOT;
break;
case 2:
_mode=BOARD_PWM_ONESHOT125;
break;
case 3:
_mode=BOARD_PWM_ONESHOT42;
break;
case 4:
_mode=BOARD_PWM_PWM125;
break;
}
// TODO: remove hardwiring timers
// TODO: we should change mode only for channels with freq > 50Hz
switch(_mode){
case BOARD_PWM_NORMAL:
default:
// output uses timers 2 & 3 so let init them for PWM mode
period = ((PWM_TIMER_KHZ*1000UL) / 50); // 50Hz by default - will be corrected late per-channel in init_channel()
freq = PWM_TIMER_KHZ; // 2MHz 0.5us ticks - for 50..490Hz PWM
break;
case BOARD_PWM_ONESHOT: // same as PWM but with manual restarting
// output uses timers 2 & 3 so let init them for PWM mode
period = (uint32_t)-1; // max possible
freq = PWM_TIMER_KHZ; // 2MHz 0.5us ticks - for 50..490Hz PWM
_once_mode=true;
break;
case BOARD_PWM_ONESHOT125:
period = (uint32_t)-1; // max possible
freq = ONESHOT125_TIMER_KHZ; // 16MHz 62.5ns ticks - for 125uS..490Hz OneShot125
// at 16Mhz, period with 65536 will be 244Hz so even 16-bit timer will never overflows at 500Hz loop
_once_mode=true;
break;
case BOARD_PWM_PWM125: // like Oneshot125 but PWM so not once
period = (uint32_t)-1; // max possible
freq = ONESHOT125_TIMER_KHZ; // 16MHz 62.5ns ticks - for 125uS..490Hz OneShot125
// at 16Mhz, period with 65536 will be 244Hz so even 16-bit timer will never overflows at 500Hz loop
break;
case BOARD_PWM_ONESHOT42:
period = (uint32_t)-1; // max possible
freq = ONESHOT42_TIMER_KHZ; // 16MHz 62.5ns ticks - for 125uS..490Hz OneShot125
// at 28Mhz, period with 65536 will be 427Hz so even 16-bit timer should not overflows at 500Hz loop, but very close to
_once_mode=true;
break;
case BOARD_PWM_BRUSHED:
// dev period freq, kHz
period = 1000;
freq = PWM_BRUSHED_TIMER_KHZ; // 16MHz - 0..1 in 1000 steps
break;
}
#if 1
// correct code should init all timers used for outputs
for (uint16_t ch = 0; ch < F4Light_OUT_CHANNELS; ch++) {
const timer_dev *tim = PIN_MAP[output_channels[ch]].timer_device;
tim->state->update=false; // reset flag first
}
for (uint16_t ch = 0; ch < F4Light_OUT_CHANNELS; ch++) {
if (!(_enabled_channels & _BV(ch))) continue; // not enabled
if(_freq[ch]>50){
const timer_dev *tim = PIN_MAP[output_channels[ch]].timer_device;
tim->state->update=true; // set flag for update for needed timer
}
}
for (uint16_t ch = 0; ch < F4Light_OUT_CHANNELS; ch++) {
uint8_t pin = output_channels[ch];
const timer_dev *tim = PIN_MAP[pin].timer_device;
if(tim->state->update) {
configTimeBase(tim, period, freq);
tim->state->update = false; // only once
if(_mode == BOARD_PWM_BRUSHED) tim->state->freq_scale=1;
}
}
#else // raw and dirty way
// dev period freq, kHz
configTimeBase(TIMER2, period, freq); // 16MHz 62.5ns ticks - for 125uS..490Hz OneShot125
configTimeBase(TIMER3, period, freq); // 16MHz 62.5ns ticks
if(_mode == BOARD_PWM_BRUSHED) {
TIMER2->state->freq_scale=1;
TIMER3->state->freq_scale=1;
}
#endif
init_channels();
#if 1
for(uint8_t i =0; i<num_out_timers;i++){
timer_resume(out_timers[i]);
}
#else
timer_resume(TIMER2);
timer_resume(TIMER3);
#endif
}
void RCOutput::_set_pin_output_mode(uint8_t ch) {
uint32_t period=0;
uint32_t freq;
switch(_mode){
case BOARD_PWM_NORMAL:
default:
// output uses timers 2 & 3 so let init them for PWM mode
period = ((PWM_TIMER_KHZ*1000UL) / 50); // 50Hz by default - will be corrected late per-channel in init_channel()
freq = PWM_TIMER_KHZ; // 2MHz 0.5us ticks - for 50..490Hz PWM
break;
case BOARD_PWM_ONESHOT: // same as PWM but with manual restarting
// output uses timers 2 & 3 so let init them for PWM mode
period = (uint32_t)-1; // max possible
freq = PWM_TIMER_KHZ; // 2MHz 0.5us ticks - for 50..490Hz PWM
break;
case BOARD_PWM_ONESHOT125:
period = (uint32_t)-1; // max possible
freq = ONESHOT125_TIMER_KHZ; // 16MHz 62.5ns ticks - for 125uS..490Hz OneShot125
// at 16Mhz, period with 65536 will be 244Hz so even 16-bit timer will never overflows at 500Hz loop
break;
case BOARD_PWM_PWM125: // like Oneshot125 but PWM so not once
period = (uint32_t)-1; // max possible
freq = ONESHOT125_TIMER_KHZ; // 16MHz 62.5ns ticks - for 125uS..490Hz OneShot125
// at 16Mhz, period with 65536 will be 244Hz so even 16-bit timer will never overflows at 500Hz loop
break;
case BOARD_PWM_ONESHOT42:
period = (uint32_t)-1; // max possible
freq = ONESHOT42_TIMER_KHZ; // 16MHz 62.5ns ticks - for 125uS..490Hz OneShot125
// at 28Mhz, period with 65536 will be 427Hz so even 16-bit timer should not overflows at 500Hz loop, but very close to
break;
case BOARD_PWM_BRUSHED:
// dev period freq, kHz
period = 1000;
freq = PWM_BRUSHED_TIMER_KHZ; // 16MHz - 0..1 in 1000 steps
break;
}
uint8_t pin = output_channels[ch];
const timer_dev *tim = PIN_MAP[pin].timer_device;
configTimeBase(tim, period, freq);
if(_mode == BOARD_PWM_BRUSHED) tim->state->freq_scale=1;
}
bool RCOutput::is_servo_enabled(uint8_t ch){
if(ch>=F4Light_OUT_CHANNELS){ // servos
uint8_t sn = ch - F4Light_OUT_CHANNELS;
if(!(_servo_mask & (1<<sn)) ) return false;
}
return true;
}
// for Oneshot125
// [1000;2000] => [125;250]
// so frequency of timers should be 8 times more - 16MHz, but timers on 84MHz can give only 16.8MHz
// channels 1&2, 3&4&5&6 can has a different rates
void RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) {
uint32_t mask=1;
uint16_t freq = freq_hz;
for(uint8_t i=0; i< F4Light_MAX_OUTPUT_CHANNELS; i++) { // кто последний тот и папа
if(chmask & mask) {
if(!(_enabled_channels & mask) ) return; // not enabled
// for true one-shot if(_once_mode && freq_hz>50) continue; // no frequency in OneShoot modes
_freq[i] = freq_hz;
if(_once_mode && freq_hz>50) freq = freq_hz / 2; // divide frequency by 2 in OneShoot modes
const uint8_t pin = output_channels[i];
const timer_dev *dev = PIN_MAP[pin].timer_device;
timer_set_reload(dev, _timer_period(freq, dev));
}
mask <<= 1;
}
}
void RCOutput::init_channel(uint8_t ch){
if(ch>=F4Light_MAX_OUTPUT_CHANNELS) return;
uint8_t pin = output_channels[ch];
if (pin >= BOARD_NR_GPIO_PINS) return;
const stm32_pin_info &p = PIN_MAP[pin];
const timer_dev *dev = p.timer_device;
timer_set_mode( dev, p.timer_channel, TIMER_PWM);
uint16_t freq = _freq[ch];
if(_once_mode && freq>50) freq/=2;
timer_set_reload(dev, _timer_period(freq, dev));
if(_once_mode) {
timer_set_compare(dev, p.timer_channel, 0); // to prevent outputs in case of timer overflow
}
}
void RCOutput::init_channels(){
for (uint16_t ch = 0; ch < F4Light_OUT_CHANNELS; ch++) {
init_channel(ch);
}
}
/* constrain pwm to be between min and max pulsewidth */
static inline uint16_t constrain_period(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;
}
void RCOutput::set_pwm(uint8_t ch, uint16_t pwm){
if(ch>=F4Light_MAX_OUTPUT_CHANNELS) return;
if (!(_enabled_channels & _BV(ch))) return; // not enabled
if(!is_servo_enabled(ch)) return; // disabled servo
uint8_t pin = output_channels[ch];
if (pin >= BOARD_NR_GPIO_PINS) return;
switch(_mode){
case BOARD_PWM_BRUSHED:
pwm -= 1000; // move from 1000..2000 to 0..1000
break;
case BOARD_PWM_ONESHOT42: // works at single freq
case BOARD_PWM_ONESHOT125:
break;
default:
pwm <<= 1; // frequency of timers 2MHz
break;
}
const stm32_pin_info &p = PIN_MAP[pin];
const timer_dev *dev = p.timer_device;
pwm *= dev->state->freq_scale; // take into account the inaccuracy of setting the timer frequency for small prescalers
timer_set_compare(dev, p.timer_channel, pwm);
}
void RCOutput::write(uint8_t ch, uint16_t period_us)
{
if(ch>=F4Light_MAX_OUTPUT_CHANNELS) return;
if(_used_channels<ch) _used_channels=ch+1;
uint16_t pwm = constrain_period(period_us);
if(_period[ch]==pwm) return; // already so
_period[ch]=pwm;
if(_corked) return;
set_pwm(ch, pwm);
}
void RCOutput::write(uint8_t ch, uint16_t* period_us, uint8_t len)
{
if(ch>=F4Light_MAX_OUTPUT_CHANNELS) return;
for (int i = 0; i < len; i++) {
write(i + ch, period_us[i]);
}
}
uint16_t RCOutput::read(uint8_t ch)
{
if(ch>=F4Light_MAX_OUTPUT_CHANNELS) return RC_INPUT_MIN_PULSEWIDTH;
return _period[ch];
}
void RCOutput::read(uint16_t* period_us, uint8_t len)
{// here we don't need to limit channel count - all unsupported will be read as RC_INPUT_MIN_PULSEWIDT
for (int i = 0; i < len; i++) {
period_us[i] = read(i);
}
}
void RCOutput::enable_ch(uint8_t ch)
{
if (ch >= F4Light_MAX_OUTPUT_CHANNELS)
return;
if(_enabled_channels & (1U<<ch) ) return; // already OK
if(!is_servo_enabled(ch)) return; // disabled servo
_enabled_channels |= (1U<<ch);
if (_period[ch] == PWM_IGNORE_THIS_CHANNEL) {
_period[ch] = 0;
}
if(!_initialized[ch]) {
uint8_t pin = output_channels[ch];
_set_pin_output_mode(ch);
GPIO::_pinMode(pin, PWM);
init_channel(ch);
const timer_dev *dev = PIN_MAP[pin].timer_device;
timer_resume(dev);
_initialized[ch]=true;
}
fill_timers(); // re-calculate list of used timers
#ifdef DEBUG_INT
uint8_t spin = output_channels[DEBUG_INT]; // motor 6
GPIO::_pinMode(spin, INPUT_PULLUP);
#endif
}
void RCOutput::disable_ch(uint8_t ch)
{
if (ch >= F4Light_MAX_OUTPUT_CHANNELS) {
return;
}
if(!is_servo_enabled(ch)) return; // disabled servo
_enabled_channels &= ~(1U<<ch);
_period[ch] = PWM_IGNORE_THIS_CHANNEL;
uint8_t pin = output_channels[ch];
if (pin >= BOARD_NR_GPIO_PINS) return;
GPIO::_pinMode(pin, OUTPUT);
GPIO::_write(pin, 0);
fill_timers(); // re-calculate list of used timers
}
void RCOutput::push()
{
#ifdef DEBUG_PWM
uint8_t spin = output_channels[DEBUG_PWM]; // motor 6 as strobe
GPIO::_pinMode(spin, OUTPUT);
GPIO::_write(spin, 1);
#endif
for (uint16_t ch = 0; ch < _used_channels; ch++) {
set_pwm(ch, _period[ch]);
}
if(_once_mode){ // generate timer's update on ALL used pins, but only once per timer
#if 1
for(uint8_t i =0; i<num_out_timers;i++){
timer_generate_update(out_timers[i]);
}
for (uint16_t ch = 0; ch < F4Light_OUT_CHANNELS; ch++) {
const stm32_pin_info &p = PIN_MAP[output_channels[ch]];
timer_set_compare(p.timer_device, p.timer_channel, 0); // to prevent outputs in case of timer overflows
}
#else
for (uint16_t ch = 0; ch < F4Light_OUT_CHANNELS; ch++) {
const timer_dev *tim = PIN_MAP[output_channels[ch]].timer_device;
tim->state->update=false; // reset flag first
}
for (uint16_t ch = 0; ch < F4Light_OUT_CHANNELS; ch++) {
if (!(_enabled_channels & _BV(ch))) continue; // not enabled
const timer_dev *tim = PIN_MAP[output_channels[ch]].timer_device;
tim->state->update=true; // set flag for update for needed timer
}
for (uint16_t ch = 0; ch < F4Light_OUT_CHANNELS; ch++) {
const timer_dev *tim = PIN_MAP[output_channels[ch]].timer_device;
if(tim->state->update) {
timer_generate_update(tim);
tim->state->update = false; // only once
}
}
#endif
}
_corked = false;
#ifdef DEBUG_PWM
GPIO::_write(spin, 0);
#endif
}
Reference in New Issue View Git Blame Copy Permalink