mirror of https://github.com/ArduPilot/ardupilot
150 lines
4.6 KiB
C++
150 lines
4.6 KiB
C++
/*
|
||
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
|
||
|
||
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)[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()
|
||
*/
|
||
|
||
//#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;
|
||
|
||
#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)
|
||
{
|
||
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;
|
||
}
|
||
}
|
||
#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)
|
||
{
|
||
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
|
||
}
|
||
#if defined(__AVR_ATmega1280__)
|
||
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
|
||
}
|
||
#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--)
|
||
write(*buffer++);
|
||
}
|
||
|
||
// We create this two instances
|
||
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 |