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

Added APM_FastSerial library for fast serial output 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@74 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jjulio1234 2010-08-11 21:59:16 +00:00
parent 83c5e06025
commit 84fb1f015d
4 changed files with 421 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

293
libraries/APM_FastSerial/APM_FastSerial.cpp Normal file
View File

@ -0,0 +1,293 @@
/*
APM_FastSerial.cpp - Fast Serial Output for Ardupilot Mega Hardware (Atmega1280)
Interrupt driven Serial output with intermediate buffer
Code Jose Julio and Jordi Muñoz. 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.
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)
write() for bytes or array of bytes (binary output)
print() for chars, strings, numbers and floats
println()
*/
//#include "WProgram.h"
#include "APM_FastSerial.h"
extern "C" {
// AVR LibC Includes
#include <inttypes.h>
#include <avr/interrupt.h>
#include <avr/io.h>
#include "WConstants.h"
}
#define TX_BUFFER_SIZE 80 // Serial output buffer size
// Serial0 buffer
uint8_t tx_buffer0[TX_BUFFER_SIZE];
volatile int tx_buffer0_head=0;
volatile int tx_buffer0_tail=0;
// Serial3 buffer
uint8_t tx_buffer3[TX_BUFFER_SIZE];
volatile int tx_buffer3_head=0;
volatile int tx_buffer3_tail=0;
// Serial0 interrupt
ISR(SIG_USART0_DATA)
{
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;
}
}
// Serial3 interrupt
ISR(SIG_USART3_DATA)
{
uint8_t data;
if (tx_buffer3_tail == tx_buffer3_head)
UCSR3B &= ~(_BV(UDRIE3)); // Disable interrupt
else {
data = tx_buffer3[tx_buffer3_tail];
tx_buffer3_tail = (tx_buffer3_tail + 1) % TX_BUFFER_SIZE;
UDR3 = data;
}
}
// Constructors ////////////////////////////////////////////////////////////////
APM_FastSerial_Class::APM_FastSerial_Class(uint8_t SerialPort)
{
SerialPortNumber=SerialPort; // This could be serial port 0 or 3
}
// Public Methods //////////////////////////////////////////////////////////////
// This is the important function (basic funtion: send a byte)
void APM_FastSerial_Class::write(uint8_t b)
{
uint8_t Enable_tx_int=0;
uint8_t new_head;
if (SerialPortNumber==0) // Serial Port 0
{
// if buffer was empty then we enable Serial TX interrupt
if (tx_buffer0_tail == tx_buffer0_head)
Enable_tx_int=1;
new_head = (tx_buffer0_head + 1) % TX_BUFFER_SIZE; // Move to next position in the ring buffer
if (new_head==tx_buffer0_tail)
return; // This is an Error : BUFFER OVERFLOW. We lost this character!!
tx_buffer0[tx_buffer0_head] = b; // Add data to the tx buffer
tx_buffer0_head = new_head; // Update head pointer
if (Enable_tx_int)
UCSR0B |= _BV(UDRIE0); // Enable Serial TX interrupt
}
else // Serial Port 3
{
// if buffer was empty then we enable Serial TX interrupt
if (tx_buffer3_tail == tx_buffer3_head)
Enable_tx_int=1;
new_head = (tx_buffer3_head + 1) % TX_BUFFER_SIZE; // Move to next position in the ring buffer
if (new_head==tx_buffer3_tail)
return; // This is an Error : BUFFER OVERFLOW. We lost this character!!
tx_buffer3[tx_buffer3_head] = b; // Add data to the tx buffer
tx_buffer3_head = new_head; // Update head pointer
if (Enable_tx_int)
UCSR3B |= _BV(UDRIE3); // Enable Serial TX interrupt
}
}
// Send a buffer of bytes (this is util for binary protocols)
void APM_FastSerial_Class::write(const uint8_t *buffer, int 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

39
libraries/APM_FastSerial/APM_FastSerial.h Normal file
View File

@ -0,0 +1,39 @@
#ifndef APM_FastSerial_h
#define APM_FastSerial_h
#include <inttypes.h>
class APM_FastSerial_Class
{
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
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;
#endif

87
libraries/APM_FastSerial/examples/APM_FastSerial/APM_FastSerial.pde Normal file
View File

@ -0,0 +1,87 @@
/*
Example of APM_FastSerial library.
Code by Jose Julio and Jordi Muñoz. DIYDrones.com
*/
// 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
unsigned long timer;
unsigned long counter;
void setup()
{
int myint=14235; // Examples of data tytpes
long mylong=-23456432;
float myfloat=-26.669;
byte mybyte=0xD1;
byte bc_bufIn[50];
for (int i=0;i<10;i++)
bc_bufIn[i]=i*10+30; // we fill the byte array
pinMode(LED_RED,OUTPUT);
// We use the standard serial port initialization
Serial.begin(57600);
Serial3.begin(57600); // if we want to use port3 also...
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");
delay(1000);
// Examples of data types (same result as standard arduino library)
APM_FastSerial.println("Numbers:");
APM_FastSerial.println(myint);
APM_FastSerial.println(mylong);
APM_FastSerial.println(myfloat);
APM_FastSerial.println("Byte:");
APM_FastSerial.write(mybyte);
APM_FastSerial.println();
APM_FastSerial.println("Bytes:");
APM_FastSerial.write(bc_bufIn,10);
APM_FastSerial.println();
delay(4000);
}
void loop()
{
if((millis()- timer) > 20) // 50Hz loop
{
timer = millis();
if (counter < 250) // we use the Normal Serial output for 5 seconds
{
// Example of typical telemetry output
digitalWrite(LED_RED,HIGH);
Serial.println("!ATT:14.2;-5.5;-26.8"); // 20 bytes
digitalWrite(LED_RED,LOW);
if ((counter%5)==0) // GPS INFO at 5Hz
{
digitalWrite(LED_RED,HIGH);
Serial.println("!LAT:26.345324;LON:-57.372638;ALT:46.7;GC:121.3;GS:23.1"); // 54 bytes
digitalWrite(LED_RED,LOW);
}
}
else // and Fast Serial Output for other 5 seconds
{
// The same info...
digitalWrite(LED_RED,HIGH);
APM_FastSerial.println("#ATT:14.2;-5.5;-26.8"); // 20 bytes
digitalWrite(LED_RED,LOW);
if ((counter%5)==0) // GPS INFO at 5Hz
{
digitalWrite(LED_RED,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);
}
if (counter>500) // Counter reset
counter=0;
}
counter++;
}
}

2
libraries/APM_FastSerial/keywords.txt Normal file
View File

@ -0,0 +1,2 @@
APM_FastSerial KEYWORD1
APM_FastSerial3 KEYWORD1