diff --git a/libraries/APM_FastSerial/APM_FastSerial.cpp b/libraries/APM_FastSerial/APM_FastSerial.cpp new file mode 100644 index 0000000000..6cc39e888b --- /dev/null +++ b/libraries/APM_FastSerial/APM_FastSerial.cpp @@ -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 + #include + #include + #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 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 \ No newline at end of file diff --git a/libraries/APM_FastSerial/APM_FastSerial.h b/libraries/APM_FastSerial/APM_FastSerial.h new file mode 100644 index 0000000000..51842a26b1 --- /dev/null +++ b/libraries/APM_FastSerial/APM_FastSerial.h @@ -0,0 +1,39 @@ +#ifndef APM_FastSerial_h +#define APM_FastSerial_h + +#include + +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 + diff --git a/libraries/APM_FastSerial/examples/APM_FastSerial/APM_FastSerial.pde b/libraries/APM_FastSerial/examples/APM_FastSerial/APM_FastSerial.pde new file mode 100644 index 0000000000..fe5c2d900a --- /dev/null +++ b/libraries/APM_FastSerial/examples/APM_FastSerial/APM_FastSerial.pde @@ -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 // 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++; + } +} \ No newline at end of file diff --git a/libraries/APM_FastSerial/keywords.txt b/libraries/APM_FastSerial/keywords.txt new file mode 100644 index 0000000000..c95eae578b --- /dev/null +++ b/libraries/APM_FastSerial/keywords.txt @@ -0,0 +1,2 @@ +APM_FastSerial KEYWORD1 +APM_FastSerial3 KEYWORD1