mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 02:18:29 -04:00
uncrustify libraries/APM_RC/APM_RC_APM2.cpp
This commit is contained in:
uncrustify
Pat Hickey
parent
49686f18f9
commit
d74e505c79
@ -1,34 +1,34 @@
|
||||
/*
|
||||
APM_RC_APM2.cpp - Radio Control Library for Ardupilot Mega 2.0. Arduino
|
||||
Code by Jordi Muñoz and Jose Julio. DIYDrones.com
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
RC Input : PPM signal on IC4 pin
|
||||
RC Output : 11 Servo outputs (standard 20ms frame)
|
||||
|
||||
Methods:
|
||||
Init() : Initialization of interrupts an Timers
|
||||
OutpuCh(ch,pwm) : Output value to servos (range : 900-2100us) ch=0..10
|
||||
InputCh(ch) : Read a channel input value. ch=0..7
|
||||
GetState() : Returns the state of the input. 1 => New radio frame to process
|
||||
Automatically resets when we call InputCh to read channels
|
||||
|
||||
*/
|
||||
* APM_RC_APM2.cpp - Radio Control Library for Ardupilot Mega 2.0. Arduino
|
||||
* Code by Jordi Muñoz and Jose Julio. DIYDrones.com
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* RC Input : PPM signal on IC4 pin
|
||||
* RC Output : 11 Servo outputs (standard 20ms frame)
|
||||
*
|
||||
* Methods:
|
||||
* Init() : Initialization of interrupts an Timers
|
||||
* OutpuCh(ch,pwm) : Output value to servos (range : 900-2100us) ch=0..10
|
||||
* InputCh(ch) : Read a channel input value. ch=0..7
|
||||
* GetState() : Returns the state of the input. 1 => New radio frame to process
|
||||
* Automatically resets when we call InputCh to read channels
|
||||
*
|
||||
*/
|
||||
#include "APM_RC_APM2.h"
|
||||
|
||||
#include <avr/interrupt.h>
|
||||
#if defined(ARDUINO) && ARDUINO >= 100
|
||||
#include "Arduino.h"
|
||||
#include "Arduino.h"
|
||||
#else
|
||||
#include "WProgram.h"
|
||||
#include "WProgram.h"
|
||||
#endif
|
||||
|
||||
#if !defined(__AVR_ATmega1280__) && !defined(__AVR_ATmega2560__)
|
||||
# error Please check the Tools/Board menu to ensure you have selected Arduino Mega as your target.
|
||||
# error Please check the Tools/Board menu to ensure you have selected Arduino Mega as your target.
|
||||
#else
|
||||
|
||||
// Variable definition for Input Capture interrupt
|
||||
@ -36,38 +36,38 @@ volatile uint16_t APM_RC_APM2::_PWM_RAW[NUM_CHANNELS] = {2400,2400,2400,2400,240
|
||||
volatile uint8_t APM_RC_APM2::_radio_status=0;
|
||||
|
||||
/****************************************************
|
||||
Input Capture Interrupt ICP5 => PPM signal read
|
||||
****************************************************/
|
||||
* Input Capture Interrupt ICP5 => PPM signal read
|
||||
****************************************************/
|
||||
void APM_RC_APM2::_timer5_capt_cb(void)
|
||||
{
|
||||
static uint16_t prev_icr;
|
||||
static uint8_t frame_idx;
|
||||
uint16_t icr;
|
||||
uint16_t pwidth;
|
||||
static uint16_t prev_icr;
|
||||
static uint8_t frame_idx;
|
||||
uint16_t icr;
|
||||
uint16_t pwidth;
|
||||
|
||||
icr = ICR5;
|
||||
// Calculate pulse width assuming timer overflow TOP = 40000
|
||||
if ( icr < prev_icr ) {
|
||||
pwidth = ( icr + 40000 ) - prev_icr;
|
||||
} else {
|
||||
pwidth = icr - prev_icr;
|
||||
}
|
||||
|
||||
// Was it a sync pulse? If so, reset frame.
|
||||
if ( pwidth > 8000 ) {
|
||||
frame_idx=0;
|
||||
} else {
|
||||
// Save pulse into _PWM_RAW array.
|
||||
if ( frame_idx < NUM_CHANNELS ) {
|
||||
_PWM_RAW[ frame_idx++ ] = pwidth;
|
||||
// If this is the last pulse in a frame, set _radio_status.
|
||||
if (frame_idx >= NUM_CHANNELS) {
|
||||
_radio_status = 1;
|
||||
}
|
||||
icr = ICR5;
|
||||
// Calculate pulse width assuming timer overflow TOP = 40000
|
||||
if ( icr < prev_icr ) {
|
||||
pwidth = ( icr + 40000 ) - prev_icr;
|
||||
} else {
|
||||
pwidth = icr - prev_icr;
|
||||
}
|
||||
}
|
||||
// Save icr for next call.
|
||||
prev_icr = icr;
|
||||
|
||||
// Was it a sync pulse? If so, reset frame.
|
||||
if ( pwidth > 8000 ) {
|
||||
frame_idx=0;
|
||||
} else {
|
||||
// Save pulse into _PWM_RAW array.
|
||||
if ( frame_idx < NUM_CHANNELS ) {
|
||||
_PWM_RAW[ frame_idx++ ] = pwidth;
|
||||
// If this is the last pulse in a frame, set _radio_status.
|
||||
if (frame_idx >= NUM_CHANNELS) {
|
||||
_radio_status = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
// Save icr for next call.
|
||||
prev_icr = icr;
|
||||
}
|
||||
|
||||
|
||||
@ -80,74 +80,74 @@ APM_RC_APM2::APM_RC_APM2()
|
||||
// Public Methods //////////////////////////////////////////////////////////////
|
||||
void APM_RC_APM2::Init( Arduino_Mega_ISR_Registry * isr_reg )
|
||||
{
|
||||
// --------------------- TIMER1: OUT1 and OUT2 -----------------------
|
||||
pinMode(12,OUTPUT); // OUT1 (PB6/OC1B)
|
||||
pinMode(11,OUTPUT); // OUT2 (PB5/OC1A)
|
||||
// --------------------- TIMER1: OUT1 and OUT2 -----------------------
|
||||
pinMode(12,OUTPUT); // OUT1 (PB6/OC1B)
|
||||
pinMode(11,OUTPUT); // OUT2 (PB5/OC1A)
|
||||
|
||||
// WGM: 1 1 1 0. Clear Timer on Compare, TOP is ICR1.
|
||||
// CS11: prescale by 8 => 0.5us tick
|
||||
TCCR1A =((1<<WGM11));
|
||||
TCCR1B = (1<<WGM13)|(1<<WGM12)|(1<<CS11);
|
||||
ICR1 = 40000; // 0.5us tick => 50hz freq
|
||||
OCR1A = 0xFFFF; // Init OCR registers to nil output signal
|
||||
OCR1B = 0xFFFF;
|
||||
// WGM: 1 1 1 0. Clear Timer on Compare, TOP is ICR1.
|
||||
// CS11: prescale by 8 => 0.5us tick
|
||||
TCCR1A =((1<<WGM11));
|
||||
TCCR1B = (1<<WGM13)|(1<<WGM12)|(1<<CS11);
|
||||
ICR1 = 40000; // 0.5us tick => 50hz freq
|
||||
OCR1A = 0xFFFF; // Init OCR registers to nil output signal
|
||||
OCR1B = 0xFFFF;
|
||||
|
||||
// --------------- TIMER4: OUT3, OUT4, and OUT5 ---------------------
|
||||
pinMode(8,OUTPUT); // OUT3 (PH5/OC4C)
|
||||
pinMode(7,OUTPUT); // OUT4 (PH4/OC4B)
|
||||
pinMode(6,OUTPUT); // OUT5 (PH3/OC4A)
|
||||
// --------------- TIMER4: OUT3, OUT4, and OUT5 ---------------------
|
||||
pinMode(8,OUTPUT); // OUT3 (PH5/OC4C)
|
||||
pinMode(7,OUTPUT); // OUT4 (PH4/OC4B)
|
||||
pinMode(6,OUTPUT); // OUT5 (PH3/OC4A)
|
||||
|
||||
// WGM: 1 1 1 0. Clear Timer on Compare, TOP is ICR4.
|
||||
// CS41: prescale by 8 => 0.5us tick
|
||||
TCCR4A =((1<<WGM41));
|
||||
TCCR4B = (1<<WGM43)|(1<<WGM42)|(1<<CS41);
|
||||
OCR4A = 0xFFFF; // Init OCR registers to nil output signal
|
||||
OCR4B = 0xFFFF;
|
||||
OCR4C = 0xFFFF;
|
||||
ICR4 = 40000; // 0.5us tick => 50hz freq
|
||||
// WGM: 1 1 1 0. Clear Timer on Compare, TOP is ICR4.
|
||||
// CS41: prescale by 8 => 0.5us tick
|
||||
TCCR4A =((1<<WGM41));
|
||||
TCCR4B = (1<<WGM43)|(1<<WGM42)|(1<<CS41);
|
||||
OCR4A = 0xFFFF; // Init OCR registers to nil output signal
|
||||
OCR4B = 0xFFFF;
|
||||
OCR4C = 0xFFFF;
|
||||
ICR4 = 40000; // 0.5us tick => 50hz freq
|
||||
|
||||
//--------------- TIMER3: OUT6, OUT7, and OUT8 ----------------------
|
||||
pinMode(3,OUTPUT); // OUT6 (PE5/OC3C)
|
||||
pinMode(2,OUTPUT); // OUT7 (PE4/OC3B)
|
||||
pinMode(5,OUTPUT); // OUT8 (PE3/OC3A)
|
||||
//--------------- TIMER3: OUT6, OUT7, and OUT8 ----------------------
|
||||
pinMode(3,OUTPUT); // OUT6 (PE5/OC3C)
|
||||
pinMode(2,OUTPUT); // OUT7 (PE4/OC3B)
|
||||
pinMode(5,OUTPUT); // OUT8 (PE3/OC3A)
|
||||
|
||||
// WGM: 1 1 1 0. Clear timer on Compare, TOP is ICR3
|
||||
// CS31: prescale by 8 => 0.5us tick
|
||||
TCCR3A =((1<<WGM31));
|
||||
TCCR3B = (1<<WGM33)|(1<<WGM32)|(1<<CS31);
|
||||
OCR3A = 0xFFFF; // Init OCR registers to nil output signal
|
||||
OCR3B = 0xFFFF;
|
||||
OCR3C = 0xFFFF;
|
||||
ICR3 = 40000; // 0.5us tick => 50hz freq
|
||||
// WGM: 1 1 1 0. Clear timer on Compare, TOP is ICR3
|
||||
// CS31: prescale by 8 => 0.5us tick
|
||||
TCCR3A =((1<<WGM31));
|
||||
TCCR3B = (1<<WGM33)|(1<<WGM32)|(1<<CS31);
|
||||
OCR3A = 0xFFFF; // Init OCR registers to nil output signal
|
||||
OCR3B = 0xFFFF;
|
||||
OCR3C = 0xFFFF;
|
||||
ICR3 = 40000; // 0.5us tick => 50hz freq
|
||||
|
||||
//--------------- TIMER5: PPM INPUT, OUT10, and OUT11 ---------------
|
||||
// Init PPM input on Timer 5
|
||||
pinMode(48, INPUT); // PPM Input (PL1/ICP5)
|
||||
pinMode(45, OUTPUT); // OUT10 (PL4/OC5B)
|
||||
pinMode(44, OUTPUT); // OUT11 (PL5/OC5C)
|
||||
//--------------- TIMER5: PPM INPUT, OUT10, and OUT11 ---------------
|
||||
// Init PPM input on Timer 5
|
||||
pinMode(48, INPUT); // PPM Input (PL1/ICP5)
|
||||
pinMode(45, OUTPUT); // OUT10 (PL4/OC5B)
|
||||
pinMode(44, OUTPUT); // OUT11 (PL5/OC5C)
|
||||
|
||||
// WGM: 1 1 1 1. Fast PWM, TOP is OCR5A
|
||||
// COM all disabled.
|
||||
// CS51: prescale by 8 => 0.5us tick
|
||||
// ICES5: Input Capture on rising edge
|
||||
TCCR5A =((1<<WGM50)|(1<<WGM51));
|
||||
// Input Capture rising edge
|
||||
TCCR5B = ((1<<WGM53)|(1<<WGM52)|(1<<CS51)|(1<<ICES5));
|
||||
OCR5A = 40000; // 0.5us tick => 50hz freq. The input capture routine
|
||||
// assumes this 40000 for TOP.
|
||||
// WGM: 1 1 1 1. Fast PWM, TOP is OCR5A
|
||||
// COM all disabled.
|
||||
// CS51: prescale by 8 => 0.5us tick
|
||||
// ICES5: Input Capture on rising edge
|
||||
TCCR5A =((1<<WGM50)|(1<<WGM51));
|
||||
// Input Capture rising edge
|
||||
TCCR5B = ((1<<WGM53)|(1<<WGM52)|(1<<CS51)|(1<<ICES5));
|
||||
OCR5A = 40000; // 0.5us tick => 50hz freq. The input capture routine
|
||||
// assumes this 40000 for TOP.
|
||||
|
||||
isr_reg->register_signal( ISR_REGISTRY_TIMER5_CAPT, _timer5_capt_cb );
|
||||
// Enable Input Capture interrupt
|
||||
TIMSK5 |= (1<<ICIE5);
|
||||
isr_reg->register_signal( ISR_REGISTRY_TIMER5_CAPT, _timer5_capt_cb );
|
||||
// Enable Input Capture interrupt
|
||||
TIMSK5 |= (1<<ICIE5);
|
||||
}
|
||||
|
||||
void APM_RC_APM2::OutputCh(unsigned char ch, uint16_t pwm)
|
||||
{
|
||||
pwm=constrain(pwm,MIN_PULSEWIDTH,MAX_PULSEWIDTH);
|
||||
pwm<<=1; // pwm*2;
|
||||
pwm=constrain(pwm,MIN_PULSEWIDTH,MAX_PULSEWIDTH);
|
||||
pwm<<=1; // pwm*2;
|
||||
|
||||
switch(ch)
|
||||
{
|
||||
switch(ch)
|
||||
{
|
||||
case 0: OCR1B=pwm; break; // out1
|
||||
case 1: OCR1A=pwm; break; // out2
|
||||
case 2: OCR4C=pwm; break; // out3
|
||||
@ -158,30 +158,30 @@ void APM_RC_APM2::OutputCh(unsigned char ch, uint16_t pwm)
|
||||
case 7: OCR3A=pwm; break; // out8
|
||||
case 9: OCR5B=pwm; break; // out10
|
||||
case 10: OCR5C=pwm; break; // out11
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t APM_RC_APM2::OutputCh_current(uint8_t ch)
|
||||
{
|
||||
uint16_t pwm=0;
|
||||
switch(ch){
|
||||
case 0: pwm=OCR1B; break; // out1
|
||||
case 1: pwm=OCR1A; break; // out2
|
||||
case 2: pwm=OCR4C; break; // out3
|
||||
case 3: pwm=OCR4B; break; // out4
|
||||
case 4: pwm=OCR4A; break; // out5
|
||||
case 5: pwm=OCR3C; break; // out6
|
||||
case 6: pwm=OCR3B; break; // out7
|
||||
case 7: pwm=OCR3A; break; // out8
|
||||
case 9: pwm=OCR5B; break; // out10
|
||||
case 10: pwm=OCR5C; break; // out11
|
||||
}
|
||||
return pwm>>1;
|
||||
uint16_t pwm=0;
|
||||
switch(ch) {
|
||||
case 0: pwm=OCR1B; break; // out1
|
||||
case 1: pwm=OCR1A; break; // out2
|
||||
case 2: pwm=OCR4C; break; // out3
|
||||
case 3: pwm=OCR4B; break; // out4
|
||||
case 4: pwm=OCR4A; break; // out5
|
||||
case 5: pwm=OCR3C; break; // out6
|
||||
case 6: pwm=OCR3B; break; // out7
|
||||
case 7: pwm=OCR3A; break; // out8
|
||||
case 9: pwm=OCR5B; break; // out10
|
||||
case 10: pwm=OCR5C; break; // out11
|
||||
}
|
||||
return pwm>>1;
|
||||
}
|
||||
|
||||
void APM_RC_APM2::enable_out(uint8_t ch)
|
||||
{
|
||||
switch(ch) {
|
||||
switch(ch) {
|
||||
case 0: TCCR1A |= (1<<COM1B1); break; // CH_1 : OC1B
|
||||
case 1: TCCR1A |= (1<<COM1A1); break; // CH_2 : OC1A
|
||||
case 2: TCCR4A |= (1<<COM4C1); break; // CH_3 : OC4C
|
||||
@ -192,12 +192,12 @@ void APM_RC_APM2::enable_out(uint8_t ch)
|
||||
case 7: TCCR3A |= (1<<COM3A1); break; // CH_8 : OC3A
|
||||
case 9: TCCR5A |= (1<<COM5B1); break; // CH_10 : OC5B
|
||||
case 10: TCCR5A |= (1<<COM5C1); break; // CH_11 : OC5C
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void APM_RC_APM2::disable_out(uint8_t ch)
|
||||
{
|
||||
switch(ch) {
|
||||
switch(ch) {
|
||||
case 0: TCCR1A &= ~(1<<COM1B1); break; // CH_1 : OC1B
|
||||
case 1: TCCR1A &= ~(1<<COM1A1); break; // CH_2 : OC1A
|
||||
case 2: TCCR4A &= ~(1<<COM4C1); break; // CH_3 : OC4C
|
||||
@ -208,60 +208,64 @@ void APM_RC_APM2::disable_out(uint8_t ch)
|
||||
case 7: TCCR3A &= ~(1<<COM3A1); break; // CH_8 : OC3A
|
||||
case 9: TCCR5A &= ~(1<<COM5B1); break; // CH_10 : OC5B
|
||||
case 10: TCCR5A &= ~(1<<COM5C1); break; // CH_11 : OC5C
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
uint16_t APM_RC_APM2::InputCh(unsigned char ch)
|
||||
{
|
||||
uint16_t result;
|
||||
uint16_t result;
|
||||
|
||||
if (_HIL_override[ch] != 0) {
|
||||
return _HIL_override[ch];
|
||||
}
|
||||
if (_HIL_override[ch] != 0) {
|
||||
return _HIL_override[ch];
|
||||
}
|
||||
|
||||
// we need to block interrupts during the read of a 16 bit
|
||||
// value
|
||||
cli();
|
||||
result = _PWM_RAW[ch];
|
||||
sei();
|
||||
// Because timer runs at 0.5us we need to do value/2
|
||||
result >>= 1;
|
||||
// we need to block interrupts during the read of a 16 bit
|
||||
// value
|
||||
cli();
|
||||
result = _PWM_RAW[ch];
|
||||
sei();
|
||||
// Because timer runs at 0.5us we need to do value/2
|
||||
result >>= 1;
|
||||
|
||||
// Limit values to a valid range
|
||||
result = constrain(result,MIN_PULSEWIDTH,MAX_PULSEWIDTH);
|
||||
_radio_status = 0; // Radio channel read
|
||||
return result;
|
||||
// Limit values to a valid range
|
||||
result = constrain(result,MIN_PULSEWIDTH,MAX_PULSEWIDTH);
|
||||
_radio_status = 0; // Radio channel read
|
||||
return result;
|
||||
}
|
||||
|
||||
unsigned char APM_RC_APM2::GetState(void)
|
||||
{
|
||||
return(_radio_status);
|
||||
return(_radio_status);
|
||||
}
|
||||
|
||||
// InstantPWM is not implemented!
|
||||
|
||||
void APM_RC_APM2::Force_Out(void) { }
|
||||
void APM_RC_APM2::Force_Out0_Out1(void) { }
|
||||
void APM_RC_APM2::Force_Out2_Out3(void) { }
|
||||
void APM_RC_APM2::Force_Out6_Out7(void) { }
|
||||
void APM_RC_APM2::Force_Out(void) {
|
||||
}
|
||||
void APM_RC_APM2::Force_Out0_Out1(void) {
|
||||
}
|
||||
void APM_RC_APM2::Force_Out2_Out3(void) {
|
||||
}
|
||||
void APM_RC_APM2::Force_Out6_Out7(void) {
|
||||
}
|
||||
|
||||
/* ---------------- OUTPUT SPEED CONTROL ------------------ */
|
||||
|
||||
void APM_RC_APM2::SetFastOutputChannels(uint32_t chmask, uint16_t speed_hz)
|
||||
{
|
||||
uint16_t icr = _map_speed(speed_hz);
|
||||
uint16_t icr = _map_speed(speed_hz);
|
||||
|
||||
if ((chmask & ( _BV(CH_1) | _BV(CH_2))) != 0) {
|
||||
ICR1 = icr;
|
||||
}
|
||||
if ((chmask & ( _BV(CH_1) | _BV(CH_2))) != 0) {
|
||||
ICR1 = icr;
|
||||
}
|
||||
|
||||
if ((chmask & ( _BV(CH_3) | _BV(CH_4) | _BV(CH_5))) != 0) {
|
||||
ICR4 = icr;
|
||||
}
|
||||
if ((chmask & ( _BV(CH_3) | _BV(CH_4) | _BV(CH_5))) != 0) {
|
||||
ICR4 = icr;
|
||||
}
|
||||
|
||||
if ((chmask & ( _BV(CH_6) | _BV(CH_7) | _BV(CH_8))) != 0) {
|
||||
ICR3 = icr;
|
||||
}
|
||||
if ((chmask & ( _BV(CH_6) | _BV(CH_7) | _BV(CH_8))) != 0) {
|
||||
ICR3 = icr;
|
||||
}
|
||||
}
|
||||
|
||||
// allow HIL override of RC values
|
||||
@ -269,28 +273,28 @@ void APM_RC_APM2::SetFastOutputChannels(uint32_t chmask, uint16_t speed_hz)
|
||||
// A value of 0 means no override, use the real RC values
|
||||
bool APM_RC_APM2::setHIL(int16_t v[NUM_CHANNELS])
|
||||
{
|
||||
uint8_t sum = 0;
|
||||
for (unsigned char i=0; i<NUM_CHANNELS; i++) {
|
||||
if (v[i] != -1) {
|
||||
_HIL_override[i] = v[i];
|
||||
}
|
||||
if (_HIL_override[i] != 0) {
|
||||
sum++;
|
||||
}
|
||||
}
|
||||
if (sum == 0) {
|
||||
return 0;
|
||||
} else {
|
||||
return 1;
|
||||
}
|
||||
_radio_status = 1;
|
||||
uint8_t sum = 0;
|
||||
for (unsigned char i=0; i<NUM_CHANNELS; i++) {
|
||||
if (v[i] != -1) {
|
||||
_HIL_override[i] = v[i];
|
||||
}
|
||||
if (_HIL_override[i] != 0) {
|
||||
sum++;
|
||||
}
|
||||
}
|
||||
if (sum == 0) {
|
||||
return 0;
|
||||
} else {
|
||||
return 1;
|
||||
}
|
||||
_radio_status = 1;
|
||||
}
|
||||
|
||||
void APM_RC_APM2::clearOverride(void)
|
||||
{
|
||||
for (unsigned char i=0; i<NUM_CHANNELS; i++) {
|
||||
_HIL_override[i] = 0;
|
||||
}
|
||||
for (unsigned char i=0; i<NUM_CHANNELS; i++) {
|
||||
_HIL_override[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
Loading…
Reference in New Issue
Block a user