Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-19 14:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_AVR: RCOutput tested implementation for APM2

Browse Source
This commit is contained in:
Pat Hickey 2012-08-27 21:05:19 -07:00 committed by Andrew Tridgell
parent 0f96c93362
commit 5445ad02f8
5 changed files with 208 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_HAL/RCOutput.h
View File

@ -4,6 +4,22 @@
#include "AP_HAL_Namespace.h"
/* Define the CH_n names, indexed from 1, if we don't have them already */
#ifndef CH_1
#define CH_1 0
#define CH_2 1
#define CH_3 2
#define CH_4 3
#define CH_5 4
#define CH_6 5
#define CH_7 6
#define CH_8 7
#define CH_9 8
#define CH_10 9
#define CH_11 10
#endif
class AP_HAL::RCOutput {
public:
virtual void init(void* implspecific) = 0;
@ -21,13 +37,13 @@ public:
virtual void disable_mask(uint32_t chmask) = 0;
/* Output, either single channel or bulk array of channels */
virtual void write(uint8_t ch, uint16_t period_ms) = 0;
virtual void write(uint8_t ch, uint16_t* period_ms, uint8_t len) = 0;
virtual void write(uint8_t ch, uint16_t period_us) = 0;
virtual void write(uint8_t ch, uint16_t* period_us, uint8_t len) = 0;
/* Read back current output state, as either single channel or
* array of channels. */
virtual uint16_t read(uint8_t ch) = 0;
virtual void read(uint16_t* period_ms, uint8_t len) = 0;
virtual void read(uint16_t* period_us, uint8_t len) = 0;
};
#endif // __AP_HAL_RC_OUTPUT_H__

12
libraries/AP_HAL_AVR/RCOutput.h
View File

@ -30,6 +30,9 @@ public:
* array of channels. */
uint16_t read(uint8_t ch);
void read(uint16_t* period_ms, uint8_t len);
private:
uint16_t _timer_period(uint16_t speed_hz);
};
class AP_HAL_AVR::APM2RCOutput : public AP_HAL::RCOutput {
@ -50,13 +53,16 @@ public:
void disable_mask(uint32_t chmask);
/* Output, either single channel or bulk array of channels */
void write(uint8_t ch, uint16_t period_ms);
void write(uint8_t ch, uint16_t* period_ms, uint8_t len);
void write(uint8_t ch, uint16_t period_us);
void write(uint8_t ch, uint16_t* period_us, uint8_t len);
/* Read back current output state, as either single channel or
* array of channels starting at 0. */
uint16_t read(uint8_t ch);
void read(uint16_t* period_ms, uint8_t len);
void read(uint16_t* period_us, uint8_t len);
private:
uint16_t _timer_period(uint16_t speed_hz);
};
#endif // __AP_HAL_AVR_RC_OUTPUT_H__

8
libraries/AP_HAL_AVR/RCOutput_APM1.cpp
View File

@ -1,13 +1,16 @@
#include <avr/interrupt.h>
#include <AP_HAL.h>
#include <AP_HAL_AVR.h>
#include "RCOutput.h"
using namespace AP_HAL;
using namespace AP_HAL_AVR;
extern const HAL& hal;
/* No init argument required */
void APM1RCOutput::init(void* machtnicht) {
}
/* Output freq (1/period) control */
@ -57,3 +60,6 @@ void APM1RCOutput::read(uint16_t* period_ms, uint8_t len) {
}
uint16_t APM1RCOutput::_timer_period(uint16_t speed_hz) {
return 2000000UL / speed_hz;
}

173
libraries/AP_HAL_AVR/RCOutput_APM2.cpp
View File

@ -1,59 +1,212 @@
#include <avr/interrupt.h>
#include <AP_HAL.h>
#include <AP_HAL_AVR.h>
#include "RCOutput.h"
using namespace AP_HAL_AVR;
extern const AP_HAL::HAL& hal;
/* No init argument required */
void APM2RCOutput::init(void* machtnicht) {
// --------------------- TIMER1: OUT1 and OUT2 -----------------------
hal.gpio->pinMode(12,GPIO_OUTPUT); // OUT1 (PB6/OC1B)
hal.gpio->pinMode(11,GPIO_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;
// --------------- TIMER4: OUT3, OUT4, and OUT5 ---------------------
hal.gpio->pinMode(8,GPIO_OUTPUT); // OUT3 (PH5/OC4C)
hal.gpio->pinMode(7,GPIO_OUTPUT); // OUT4 (PH4/OC4B)
hal.gpio->pinMode(6,GPIO_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
//--------------- TIMER3: OUT6, OUT7, and OUT8 ----------------------
hal.gpio->pinMode(3,GPIO_OUTPUT); // OUT6 (PE5/OC3C)
hal.gpio->pinMode(2,GPIO_OUTPUT); // OUT7 (PE4/OC3B)
hal.gpio->pinMode(5,GPIO_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
//--------------- TIMER5: OUT10, and OUT11 ---------------
// NB TIMER5 is shared with PPM input from RCInput_APM1.cpp
// The TIMER5 registers are assumed to be setup already.
hal.gpio->pinMode(45, GPIO_OUTPUT); // OUT10 (PL4/OC5B)
hal.gpio->pinMode(44, GPIO_OUTPUT); // OUT11 (PL5/OC5C)
}
/* Output freq (1/period) control */
void APM2RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) {
uint16_t icr = _timer_period(freq_hz);
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_6) | _BV(CH_7) | _BV(CH_8))) != 0) {
ICR3 = icr;
}
}
uint16_t APM2RCOutput::get_freq(uint8_t ch) {
uint16_t icr;
switch (ch) {
case CH_1:
case CH_2:
icr = ICR1;
break;
case CH_3:
case CH_4:
case CH_5:
icr = ICR4;
break;
case CH_6:
case CH_7:
case CH_8:
icr = ICR3;
break;
default:
return 0;
}
return (2000000UL / icr); /* inverse of _time_period(icr) */
}
/* Output active/highZ control, either by single channel at a time
* or a mask of channels */
void APM2RCOutput::enable_ch(uint8_t ch) {
enable_mask(1 << 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
case 3: TCCR4A |= (1<<COM4B1); break; // CH_4 : OC4B
case 4: TCCR4A |= (1<<COM4A1); break; // CH_5 : OC4A
case 5: TCCR3A |= (1<<COM3C1); break; // CH_6 : OC3C
case 6: TCCR3A |= (1<<COM3B1); break; // CH_7 : OC3B
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 APM2RCOutput::enable_mask(uint32_t chmask) {
for (int i = 0; i < 32; i++) {
uint32_t c = chmask >> i;
if (c & 1) {
enable_ch(i);
}
}
}
void APM2RCOutput::disable_ch(uint8_t ch) {
disable_mask(1 << 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
case 3: TCCR4A &= ~(1<<COM4B1); break; // CH_4 : OC4B
case 4: TCCR4A &= ~(1<<COM4A1); break; // CH_5 : OC4A
case 5: TCCR3A &= ~(1<<COM3C1); break; // CH_6 : OC3C
case 6: TCCR3A &= ~(1<<COM3B1); break; // CH_7 : OC3B
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 APM2RCOutput::disable_mask(uint32_t chmask) {
for (int i = 0; i < 32; i++) {
if ((chmask >> i) & 1) {
disable_ch(i);
}
}
}
/* 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;
}
/* Output, either single channel or bulk array of channels */
void APM2RCOutput::write(uint8_t ch, uint16_t period_ms) {
void APM2RCOutput::write(uint8_t ch, uint16_t period_us) {
/* constrain, then scale from 1us resolution (input units)
* to 0.5us (timer units) */
uint16_t pwm = constrain_period(period_us) << 1;
switch(ch)
{
case 0: OCR1B=pwm; break; // out1
case 1: OCR1A=pwm; break; // out2
case 2: OCR4C=pwm; break; // out3
case 3: OCR4B=pwm; break; // out4
case 4: OCR4A=pwm; break; // out5
case 5: OCR3C=pwm; break; // out6
case 6: OCR3B=pwm; break; // out7
case 7: OCR3A=pwm; break; // out8
case 9: OCR5B=pwm; break; // out10
case 10: OCR5C=pwm; break; // out11
}
}
void APM2RCOutput::write(uint8_t ch, uint16_t* period_ms, uint8_t len) {
void APM2RCOutput::write(uint8_t ch, uint16_t* period_us, uint8_t len) {
for (int i = 0; i < ch; i++) {
write(i + ch, period_us[i]);
}
}
/* Read back current output state, as either single channel or
* array of channels. */
uint16_t APM2RCOutput::read(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
}
/* scale from 0.5us resolution (timer units) to 1us units */
return pwm>>1;
}
void APM2RCOutput::read(uint16_t* period_ms, uint8_t len) {
void APM2RCOutput::read(uint16_t* period_us, uint8_t len) {
for (int i = 0; i < len; i++) {
period_us[i] = read(i);
}
}
uint16_t APM2RCOutput::_timer_period(uint16_t speed_hz) {
return 2000000UL / speed_hz;
}

13
libraries/AP_HAL_AVR/examples/RCPassthroughTest/RCPassthroughTest.pde
View File

@ -37,7 +37,6 @@ void individualwrite(AP_HAL::RCOutput* out, uint16_t* channels) {
for (int ch = 0; ch < 8; ch++) {
out->write(ch, channels[ch]);
}
}
void loop (void) {
@ -46,7 +45,6 @@ void loop (void) {
hal.gpio->write(27, 1);
/* Cycle between using the individual read method
* and the multi read method*/
if (ctr < 500) {
@ -70,9 +68,18 @@ void loop (void) {
void setup (void) {
hal.uart0->begin(115200);
hal.uart0->printf_P(PSTR("reading rc in:"));
hal.gpio->pinMode(27, GPIO_OUTPUT);
hal.gpio->write(27, 0);
hal.rcout->enable_mask(0x000000FF);
/* Bottom 4 channels at 400hz (like on a quad) */
hal.rcout->set_freq(0x0000000F, 400);
for(int i = 0; i < 12; i++) {
hal.uart0->printf_P(PSTR("rcout ch %d has frequency %d\r\n"),
i, hal.rcout->get_freq(i));
}
/* Delay to let the user see the above printouts on the terminal */
hal.scheduler->delay(1000);
}