mirror of https://github.com/ArduPilot/ardupilot
New version of APM_FastSerial library
git-svn-id: https://arducopter.googlecode.com/svn/trunk@89 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jjulio1234
parent
67a19893e7
commit
4d1cbc1cca
|
|
@ -1,5 +1,6 @@
|
|||
/*
|
||||
APM_FastSerial.cpp - Fast Serial Output for Ardupilot Mega Hardware (Atmega1280)
|
||||
APM_FastSerial.cpp - Fast Serial Output for Ardupilot Mega Hardware (atmega1280)
|
||||
It´s also compatible with standard Arduino boards (atmega 168 and 328)
|
||||
Interrupt driven Serial output with intermediate buffer
|
||||
Code Jose Julio and Jordi Muñoz. DIYDrones.com
|
||||
|
||||
|
|
@ -10,8 +11,8 @@
|
|||
|
||||
This library works as a complement of the standard Arduino Serial
|
||||
library. So user must initialize Standard Serial Arduino library first.
|
||||
This library works in Serial port 0 and Serial port3 (telemetry port)
|
||||
Methods: (the same as standard arduino library)
|
||||
This library works in Serial port 0 and Serial port3(telemetry port)[APM]
|
||||
Methods: (the same as standard arduino library, inherits from Print)
|
||||
write() for bytes or array of bytes (binary output)
|
||||
print() for chars, strings, numbers and floats
|
||||
println()
|
||||
|
|
@ -26,7 +27,6 @@ extern "C" {
|
|||
#include <avr/io.h>
|
||||
#include "WConstants.h"
|
||||
}
|
||||
|
||||
#define TX_BUFFER_SIZE 80 // Serial output buffer size
|
||||
|
||||
// Serial0 buffer
|
||||
|
|
@ -34,12 +34,14 @@ uint8_t tx_buffer0[TX_BUFFER_SIZE];
|
|||
volatile int tx_buffer0_head=0;
|
||||
volatile int tx_buffer0_tail=0;
|
||||
|
||||
#if defined(__AVR_ATmega1280__)
|
||||
// Serial3 buffer
|
||||
uint8_t tx_buffer3[TX_BUFFER_SIZE];
|
||||
volatile int tx_buffer3_head=0;
|
||||
volatile int tx_buffer3_tail=0;
|
||||
#endif
|
||||
|
||||
|
||||
#if defined(__AVR_ATmega1280__) // For atmega1280 we use Serial port 0 and 3
|
||||
// Serial0 interrupt
|
||||
ISR(SIG_USART0_DATA)
|
||||
{
|
||||
|
|
@ -67,6 +69,22 @@ ISR(SIG_USART3_DATA)
|
|||
UDR3 = data;
|
||||
}
|
||||
}
|
||||
#else
|
||||
|
||||
// Serial interrupt
|
||||
ISR(USART_UDRE_vect)
|
||||
{
|
||||
uint8_t data;
|
||||
|
||||
if (tx_buffer0_tail == tx_buffer0_head)
|
||||
UCSR0B &= ~(_BV(UDRIE0)); // Disable interrupt
|
||||
else {
|
||||
data = tx_buffer0[tx_buffer0_tail];
|
||||
tx_buffer0_tail = (tx_buffer0_tail + 1) % TX_BUFFER_SIZE;
|
||||
UDR0 = data;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
// Constructors ////////////////////////////////////////////////////////////////
|
||||
APM_FastSerial_Class::APM_FastSerial_Class(uint8_t SerialPort)
|
||||
|
|
@ -98,6 +116,7 @@ void APM_FastSerial_Class::write(uint8_t b)
|
|||
if (Enable_tx_int)
|
||||
UCSR0B |= _BV(UDRIE0); // Enable Serial TX interrupt
|
||||
}
|
||||
#if defined(__AVR_ATmega1280__)
|
||||
else // Serial Port 3
|
||||
{
|
||||
// if buffer was empty then we enable Serial TX interrupt
|
||||
|
|
@ -114,180 +133,18 @@ void APM_FastSerial_Class::write(uint8_t b)
|
|||
if (Enable_tx_int)
|
||||
UCSR3B |= _BV(UDRIE3); // Enable Serial TX interrupt
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
// Send a buffer of bytes (this is util for binary protocols)
|
||||
void APM_FastSerial_Class::write(const uint8_t *buffer, int size)
|
||||
{
|
||||
while (size--)
|
||||
while (size--)
|
||||
write(*buffer++);
|
||||
}
|
||||
|
||||
void APM_FastSerial_Class::print(uint8_t b)
|
||||
{
|
||||
write(b);
|
||||
}
|
||||
|
||||
void APM_FastSerial_Class::print(const char *s)
|
||||
{
|
||||
while (*s)
|
||||
print(*s++);
|
||||
}
|
||||
|
||||
void APM_FastSerial_Class::print(char c)
|
||||
{
|
||||
write((uint8_t) c);
|
||||
}
|
||||
|
||||
void APM_FastSerial_Class::print(int n)
|
||||
{
|
||||
print((long) n);
|
||||
}
|
||||
|
||||
void APM_FastSerial_Class::print(unsigned int n)
|
||||
{
|
||||
print((unsigned long) n);
|
||||
}
|
||||
|
||||
void APM_FastSerial_Class::print(long n)
|
||||
{
|
||||
if (n < 0) {
|
||||
print('-');
|
||||
n = -n;
|
||||
}
|
||||
printNumber(n, 10);
|
||||
}
|
||||
|
||||
void APM_FastSerial_Class::print(unsigned long n)
|
||||
{
|
||||
printNumber(n, 10);
|
||||
}
|
||||
|
||||
void APM_FastSerial_Class::print(long n, int base)
|
||||
{
|
||||
if (base == 0)
|
||||
print((char) n);
|
||||
else if (base == 10)
|
||||
print(n);
|
||||
else
|
||||
printNumber(n, base);
|
||||
}
|
||||
|
||||
void APM_FastSerial_Class::println(void)
|
||||
{
|
||||
print('\r');
|
||||
print('\n');
|
||||
}
|
||||
|
||||
void APM_FastSerial_Class::println(char c)
|
||||
{
|
||||
print(c);
|
||||
println();
|
||||
}
|
||||
|
||||
void APM_FastSerial_Class::println(const char c[])
|
||||
{
|
||||
print(c);
|
||||
println();
|
||||
}
|
||||
|
||||
void APM_FastSerial_Class::println(uint8_t b)
|
||||
{
|
||||
print(b);
|
||||
println();
|
||||
}
|
||||
|
||||
void APM_FastSerial_Class::println(int n)
|
||||
{
|
||||
print(n);
|
||||
println();
|
||||
}
|
||||
|
||||
void APM_FastSerial_Class::println(long n)
|
||||
{
|
||||
print(n);
|
||||
println();
|
||||
}
|
||||
|
||||
void APM_FastSerial_Class::println(unsigned long n)
|
||||
{
|
||||
print(n);
|
||||
println();
|
||||
}
|
||||
|
||||
void APM_FastSerial_Class::println(long n, int base)
|
||||
{
|
||||
print(n, base);
|
||||
println();
|
||||
}
|
||||
|
||||
// To print floats
|
||||
void APM_FastSerial_Class::print(double n, int digits)
|
||||
{
|
||||
printFloat(n, digits);
|
||||
}
|
||||
|
||||
void APM_FastSerial_Class::println(double n, int digits)
|
||||
{
|
||||
print(n, digits);
|
||||
println();
|
||||
}
|
||||
// Private Methods /////////////////////////////////////////////////////////////
|
||||
|
||||
void APM_FastSerial_Class::printNumber(unsigned long n, uint8_t base)
|
||||
{
|
||||
unsigned char buf[8 * sizeof(long)]; // Assumes 8-bit chars.
|
||||
unsigned long i = 0;
|
||||
|
||||
if (n == 0) {
|
||||
print('0');
|
||||
return;
|
||||
}
|
||||
|
||||
while (n > 0) {
|
||||
buf[i++] = n % base;
|
||||
n /= base;
|
||||
}
|
||||
|
||||
for (; i > 0; i--)
|
||||
print((char) (buf[i - 1] < 10 ? '0' + buf[i - 1] : 'A' + buf[i - 1] - 10));
|
||||
}
|
||||
|
||||
void APM_FastSerial_Class::printFloat(double number, uint8_t digits)
|
||||
{
|
||||
// Handle negative numbers
|
||||
if (number < 0.0)
|
||||
{
|
||||
print('-');
|
||||
number = -number;
|
||||
}
|
||||
|
||||
// Round correctly so that print(1.999, 2) prints as "2.00"
|
||||
double rounding = 0.5;
|
||||
for (uint8_t i=0; i<digits; ++i)
|
||||
rounding /= 10.0;
|
||||
|
||||
number += rounding;
|
||||
|
||||
// Extract the integer part of the number and print it
|
||||
unsigned long int_part = (unsigned long)number;
|
||||
double remainder = number - (double)int_part;
|
||||
print(int_part);
|
||||
|
||||
// Print the decimal point, but only if there are digits beyond
|
||||
if (digits > 0)
|
||||
print(".");
|
||||
|
||||
// Extract digits from the remainder one at a time
|
||||
while (digits-- > 0)
|
||||
{
|
||||
remainder *= 10.0;
|
||||
int toPrint = int(remainder);
|
||||
print(toPrint);
|
||||
remainder -= toPrint;
|
||||
}
|
||||
}
|
||||
|
||||
// We create this two instances
|
||||
APM_FastSerial_Class APM_FastSerial(0); // For Serial port 0
|
||||
APM_FastSerial_Class APM_FastSerial3(3); // For Serial port 3
|
||||
APM_FastSerial_Class APM_FastSerial(0); // For Serial port 0
|
||||
#if defined(__AVR_ATmega1280__)
|
||||
APM_FastSerial_Class APM_FastSerial3(3); // For Serial port 3 (only Atmega1280)
|
||||
#endif
|
||||
|
|
@ -3,37 +3,23 @@
|
|||
|
||||
#include <inttypes.h>
|
||||
|
||||
class APM_FastSerial_Class
|
||||
#include "Print.h"
|
||||
|
||||
class APM_FastSerial_Class : public Print // Inherit from Print
|
||||
{
|
||||
private:
|
||||
void printNumber(unsigned long, uint8_t);
|
||||
void printFloat(double number, uint8_t digits);
|
||||
uint8_t SerialPortNumber;
|
||||
|
||||
public:
|
||||
APM_FastSerial_Class(uint8_t SerialPort); // Constructor
|
||||
void write(uint8_t b); // basic funtion : send a byte
|
||||
// we overwrite the write methods
|
||||
void write(uint8_t b); // basic funtion : send a byte
|
||||
void write(const uint8_t *buffer, int size);
|
||||
void print(char);
|
||||
void print(const char[]);
|
||||
void print(uint8_t);
|
||||
void print(int);
|
||||
void print(unsigned int);
|
||||
void print(long);
|
||||
void print(unsigned long);
|
||||
void print(long, int);
|
||||
void print(double, int=2);
|
||||
void println(void);
|
||||
void println(char);
|
||||
void println(const char[]);
|
||||
void println(uint8_t);
|
||||
void println(int);
|
||||
void println(long);
|
||||
void println(unsigned long);
|
||||
void println(long, int);
|
||||
void println(double, int=2);
|
||||
};
|
||||
|
||||
extern APM_FastSerial_Class APM_FastSerial;
|
||||
extern APM_FastSerial_Class APM_FastSerial3;
|
||||
#if defined(__AVR_ATmega1280__)
|
||||
extern APM_FastSerial_Class APM_FastSerial3;
|
||||
#endif
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -6,7 +6,11 @@
|
|||
// FastSerial is implemented for Serial port 0 (connection to PC) and 3 (telemetry)
|
||||
#include <APM_FastSerial.h> // ArduPilot Mega Fast Serial Output Library
|
||||
|
||||
#define LED_RED 35
|
||||
#if defined(__AVR_ATmega1280__)
|
||||
#define LED 35
|
||||
#else
|
||||
#define LED 13
|
||||
#endif
|
||||
|
||||
unsigned long timer;
|
||||
unsigned long counter;
|
||||
|
|
@ -22,17 +26,17 @@ void setup()
|
|||
for (int i=0;i<10;i++)
|
||||
bc_bufIn[i]=i*10+30; // we fill the byte array
|
||||
|
||||
pinMode(LED_RED,OUTPUT);
|
||||
pinMode(LED,OUTPUT);
|
||||
|
||||
// We use the standard serial port initialization
|
||||
Serial.begin(57600);
|
||||
Serial3.begin(57600); // if we want to use port3 also...
|
||||
//Serial3.begin(57600); // if we want to use port3 also (Mega boards)...
|
||||
delay(100);
|
||||
// We can use both methods to write to serial port:
|
||||
Serial.println("ArduPilot Mega FastSerial library test"); // Standard serial output
|
||||
APM_FastSerial.println("FAST_SERIAL : ArduPilot Mega FastSerial library test"); // Fast serial output
|
||||
// We can use the same on serial port3 (telemetry)
|
||||
APM_FastSerial3.println("Serial Port3 : ArduPilot Mega FastSerial library test");
|
||||
// APM_FastSerial3.println("Serial Port3 : ArduPilot Mega FastSerial library test");
|
||||
delay(1000);
|
||||
// Examples of data types (same result as standard arduino library)
|
||||
APM_FastSerial.println("Numbers:");
|
||||
|
|
@ -56,28 +60,28 @@ void loop()
|
|||
if (counter < 250) // we use the Normal Serial output for 5 seconds
|
||||
{
|
||||
// Example of typical telemetry output
|
||||
digitalWrite(LED_RED,HIGH);
|
||||
digitalWrite(LED,HIGH);
|
||||
Serial.println("!ATT:14.2;-5.5;-26.8"); // 20 bytes
|
||||
digitalWrite(LED_RED,LOW);
|
||||
digitalWrite(LED,LOW);
|
||||
if ((counter%5)==0) // GPS INFO at 5Hz
|
||||
{
|
||||
digitalWrite(LED_RED,HIGH);
|
||||
digitalWrite(LED,HIGH);
|
||||
Serial.println("!LAT:26.345324;LON:-57.372638;ALT:46.7;GC:121.3;GS:23.1"); // 54 bytes
|
||||
digitalWrite(LED_RED,LOW);
|
||||
digitalWrite(LED,LOW);
|
||||
}
|
||||
}
|
||||
else // and Fast Serial Output for other 5 seconds
|
||||
{
|
||||
// The same info...
|
||||
digitalWrite(LED_RED,HIGH);
|
||||
digitalWrite(LED,HIGH);
|
||||
APM_FastSerial.println("#ATT:14.2;-5.5;-26.8"); // 20 bytes
|
||||
digitalWrite(LED_RED,LOW);
|
||||
digitalWrite(LED,LOW);
|
||||
if ((counter%5)==0) // GPS INFO at 5Hz
|
||||
{
|
||||
digitalWrite(LED_RED,HIGH);
|
||||
digitalWrite(LED,HIGH);
|
||||
|
||||
APM_FastSerial.println("#LAT:26.345324;LON:-57.372638;ALT:46.7;GC:121.3;GS:23.1"); // 54 bytes
|
||||
digitalWrite(LED_RED,LOW);
|
||||
digitalWrite(LED,LOW);
|
||||
}
|
||||
if (counter>500) // Counter reset
|
||||
counter=0;
|
||||
|
|
|
|||
Loading…
Reference in New Issue