Erik van de Coevering / Mbed 2 deprecated Multicopter_2018

Latest version of my quadcopter controller with an LPC1768 and MPU9250.

Dependencies:   mbed

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?

UserRevisionLine numberNew 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