Latest version of my quadcopter controller with an LPC1768 and MPU9250.
Currently running on a custom PCB with 30.5 x 30.5mm mounts. There are also 2 PC apps that go with the software; one to set up the PID controller and one to balance the motors and props. If anyone is interested, send me a message and I'll upload them.
Serialbuffered/SerialBuffered.cpp@8:981f7e2365b6, 2018-07-31 (annotated)
- Committer:
- Anaesthetix
- Date:
- Tue Jul 31 20:36:57 2018 +0000
- Revision:
- 8:981f7e2365b6
- Parent:
- 0:0929d3d566cf
Switched from Madgwick to Mahony as I'm having trouble with slow oscillations caused by the madgwick filter. Fixed an error on the PID algorithm also.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Anaesthetix | 0:0929d3d566cf | 1 | |
Anaesthetix | 0:0929d3d566cf | 2 | #include "mbed.h" |
Anaesthetix | 0:0929d3d566cf | 3 | #include "SerialBuffered.h" |
Anaesthetix | 0:0929d3d566cf | 4 | |
Anaesthetix | 0:0929d3d566cf | 5 | //extern Serial loggerSerial; |
Anaesthetix | 0:0929d3d566cf | 6 | |
Anaesthetix | 0:0929d3d566cf | 7 | SerialBuffered::SerialBuffered( size_t bufferSize, PinName tx, PinName rx ) : Serial( tx, rx ) |
Anaesthetix | 0:0929d3d566cf | 8 | { |
Anaesthetix | 0:0929d3d566cf | 9 | m_buffSize = 0; |
Anaesthetix | 0:0929d3d566cf | 10 | m_contentStart = 0; |
Anaesthetix | 0:0929d3d566cf | 11 | m_contentEnd = 0; |
Anaesthetix | 0:0929d3d566cf | 12 | m_timeout = 1.0; |
Anaesthetix | 0:0929d3d566cf | 13 | |
Anaesthetix | 0:0929d3d566cf | 14 | |
Anaesthetix | 0:0929d3d566cf | 15 | attach( this, &SerialBuffered::handleInterrupt ); |
Anaesthetix | 0:0929d3d566cf | 16 | |
Anaesthetix | 0:0929d3d566cf | 17 | m_buff = (uint8_t *) malloc( bufferSize ); |
Anaesthetix | 0:0929d3d566cf | 18 | if( m_buff == NULL ) |
Anaesthetix | 0:0929d3d566cf | 19 | { |
Anaesthetix | 0:0929d3d566cf | 20 | //loggerSerial.printf("SerialBuffered - failed to alloc buffer size %d\r\n", (int) bufferSize ); |
Anaesthetix | 0:0929d3d566cf | 21 | } |
Anaesthetix | 0:0929d3d566cf | 22 | else |
Anaesthetix | 0:0929d3d566cf | 23 | { |
Anaesthetix | 0:0929d3d566cf | 24 | m_buffSize = bufferSize; |
Anaesthetix | 0:0929d3d566cf | 25 | } |
Anaesthetix | 0:0929d3d566cf | 26 | } |
Anaesthetix | 0:0929d3d566cf | 27 | |
Anaesthetix | 0:0929d3d566cf | 28 | |
Anaesthetix | 0:0929d3d566cf | 29 | SerialBuffered::~SerialBuffered() |
Anaesthetix | 0:0929d3d566cf | 30 | { |
Anaesthetix | 0:0929d3d566cf | 31 | if( m_buff ) |
Anaesthetix | 0:0929d3d566cf | 32 | free( m_buff ); |
Anaesthetix | 0:0929d3d566cf | 33 | } |
Anaesthetix | 0:0929d3d566cf | 34 | |
Anaesthetix | 0:0929d3d566cf | 35 | void SerialBuffered::setTimeout( float seconds ) |
Anaesthetix | 0:0929d3d566cf | 36 | { |
Anaesthetix | 0:0929d3d566cf | 37 | m_timeout = seconds; |
Anaesthetix | 0:0929d3d566cf | 38 | } |
Anaesthetix | 0:0929d3d566cf | 39 | |
Anaesthetix | 0:0929d3d566cf | 40 | size_t SerialBuffered::readBytes( uint8_t *bytes, size_t requested ) |
Anaesthetix | 0:0929d3d566cf | 41 | { |
Anaesthetix | 0:0929d3d566cf | 42 | int i = 0; |
Anaesthetix | 0:0929d3d566cf | 43 | |
Anaesthetix | 0:0929d3d566cf | 44 | for( ; i < requested; ) |
Anaesthetix | 0:0929d3d566cf | 45 | { |
Anaesthetix | 0:0929d3d566cf | 46 | int c = getc(); |
Anaesthetix | 0:0929d3d566cf | 47 | if( c < 0 ) |
Anaesthetix | 0:0929d3d566cf | 48 | break; |
Anaesthetix | 0:0929d3d566cf | 49 | bytes[i] = c; |
Anaesthetix | 0:0929d3d566cf | 50 | i++; |
Anaesthetix | 0:0929d3d566cf | 51 | } |
Anaesthetix | 0:0929d3d566cf | 52 | |
Anaesthetix | 0:0929d3d566cf | 53 | return i; |
Anaesthetix | 0:0929d3d566cf | 54 | |
Anaesthetix | 0:0929d3d566cf | 55 | } |
Anaesthetix | 0:0929d3d566cf | 56 | |
Anaesthetix | 0:0929d3d566cf | 57 | |
Anaesthetix | 0:0929d3d566cf | 58 | int SerialBuffered::getc() |
Anaesthetix | 0:0929d3d566cf | 59 | { |
Anaesthetix | 0:0929d3d566cf | 60 | m_timer.reset(); |
Anaesthetix | 0:0929d3d566cf | 61 | m_timer.start(); |
Anaesthetix | 0:0929d3d566cf | 62 | while( m_contentStart == m_contentEnd ) |
Anaesthetix | 0:0929d3d566cf | 63 | { |
Anaesthetix | 0:0929d3d566cf | 64 | |
Anaesthetix | 0:0929d3d566cf | 65 | |
Anaesthetix | 0:0929d3d566cf | 66 | wait_ms( 1 ); |
Anaesthetix | 0:0929d3d566cf | 67 | if( m_timeout > 0 && m_timer.read() > m_timeout ) |
Anaesthetix | 0:0929d3d566cf | 68 | return EOF; |
Anaesthetix | 0:0929d3d566cf | 69 | } |
Anaesthetix | 0:0929d3d566cf | 70 | |
Anaesthetix | 0:0929d3d566cf | 71 | m_timer.stop(); |
Anaesthetix | 0:0929d3d566cf | 72 | |
Anaesthetix | 0:0929d3d566cf | 73 | uint8_t result = m_buff[m_contentStart++]; |
Anaesthetix | 0:0929d3d566cf | 74 | m_contentStart = m_contentStart % m_buffSize; |
Anaesthetix | 0:0929d3d566cf | 75 | |
Anaesthetix | 0:0929d3d566cf | 76 | |
Anaesthetix | 0:0929d3d566cf | 77 | return result; |
Anaesthetix | 0:0929d3d566cf | 78 | } |
Anaesthetix | 0:0929d3d566cf | 79 | |
Anaesthetix | 0:0929d3d566cf | 80 | |
Anaesthetix | 0:0929d3d566cf | 81 | int SerialBuffered::readable() |
Anaesthetix | 0:0929d3d566cf | 82 | { |
Anaesthetix | 0:0929d3d566cf | 83 | return m_contentStart != m_contentEnd ; |
Anaesthetix | 0:0929d3d566cf | 84 | } |
Anaesthetix | 0:0929d3d566cf | 85 | |
Anaesthetix | 0:0929d3d566cf | 86 | void SerialBuffered::handleInterrupt() |
Anaesthetix | 0:0929d3d566cf | 87 | { |
Anaesthetix | 0:0929d3d566cf | 88 | |
Anaesthetix | 0:0929d3d566cf | 89 | while( Serial::readable()) |
Anaesthetix | 0:0929d3d566cf | 90 | { |
Anaesthetix | 0:0929d3d566cf | 91 | if( m_contentStart == (m_contentEnd +1) % m_buffSize) |
Anaesthetix | 0:0929d3d566cf | 92 | { |
Anaesthetix | 0:0929d3d566cf | 93 | //loggerSerial.printf("SerialBuffered - buffer overrun, data lost!\r\n" ); |
Anaesthetix | 0:0929d3d566cf | 94 | Serial::getc(); |
Anaesthetix | 0:0929d3d566cf | 95 | |
Anaesthetix | 0:0929d3d566cf | 96 | } |
Anaesthetix | 0:0929d3d566cf | 97 | else |
Anaesthetix | 0:0929d3d566cf | 98 | { |
Anaesthetix | 0:0929d3d566cf | 99 | |
Anaesthetix | 0:0929d3d566cf | 100 | m_buff[ m_contentEnd ++ ] = Serial::getc(); |
Anaesthetix | 0:0929d3d566cf | 101 | m_contentEnd = m_contentEnd % m_buffSize; |
Anaesthetix | 0:0929d3d566cf | 102 | |
Anaesthetix | 0:0929d3d566cf | 103 | |
Anaesthetix | 0:0929d3d566cf | 104 | |
Anaesthetix | 0:0929d3d566cf | 105 | } |
Anaesthetix | 0:0929d3d566cf | 106 | } |
Anaesthetix | 0:0929d3d566cf | 107 | } |
Anaesthetix | 0:0929d3d566cf | 108 | |
Anaesthetix | 0:0929d3d566cf | 109 |