Alex Millane / Mbed 2 deprecated IFARanging

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Node/Observer/MavlinkPassthrough/MavlinkPassthrough.cpp@0:99928431bb44, 2015-07-07 (annotated)

Committer:
millanea
Date:
Tue Jul 07 09:36:12 2015 +0000
Revision:
0:99928431bb44
First commit. Committing the entire project such that it can be published.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
millanea 0:99928431bb44 1 #include "MavlinkPassthrough.h"
millanea 0:99928431bb44 2
millanea 0:99928431bb44 3 MavlinkPassthrough::MavlinkPassthrough( Serial& serial, BufferedOutput& bufferedOutput ) : mavlinkIn(serial), bufferedOutput(bufferedOutput)
millanea 0:99928431bb44 4 {
millanea 0:99928431bb44 5 // Debug startup message
millanea 0:99928431bb44 6 debugprintf("Initializing Mavlink Passthrough object\r\n") ;
millanea 0:99928431bb44 7
millanea 0:99928431bb44 8 // Attaching the serial interrupt callbacks
millanea 0:99928431bb44 9 mavlinkIn.attach(this, &MavlinkPassthrough::mavlinkInCallback, Serial::RxIrq ) ;
millanea 0:99928431bb44 10
millanea 0:99928431bb44 11 // The state of the current mavlink message
millanea 0:99928431bb44 12 mavlinkTransferState = START ;
millanea 0:99928431bb44 13 currentMavlinkFrameLength = 0 ;
millanea 0:99928431bb44 14
millanea 0:99928431bb44 15 // Initializing the buffer pointers
millanea 0:99928431bb44 16 mavlinkBufferIn = 0 ;
millanea 0:99928431bb44 17 mavlinkBufferOut = 0 ;
millanea 0:99928431bb44 18 mavlinkBufferRead = 0 ;
millanea 0:99928431bb44 19 }
millanea 0:99928431bb44 20
millanea 0:99928431bb44 21 void MavlinkPassthrough::executeBackgroundTask()
millanea 0:99928431bb44 22 {
millanea 0:99928431bb44 23 // Blinking LED for debug
millanea 0:99928431bb44 24 //led = !led ;
millanea 0:99928431bb44 25 // Maintaing the buffer
millanea 0:99928431bb44 26 maintainMavlinkBuffer() ;
millanea 0:99928431bb44 27 }
millanea 0:99928431bb44 28
millanea 0:99928431bb44 29 void MavlinkPassthrough::mavlinkInCallback()
millanea 0:99928431bb44 30 {
millanea 0:99928431bb44 31 // Reading out all chars in the USART buffer in the mavlink buffer
millanea 0:99928431bb44 32 // while the mavlink buffer is not full
millanea 0:99928431bb44 33 while( mavlinkIn.readable() && !((( mavlinkBufferIn + 1 ) % mavlinkBufferSize) == mavlinkBufferOut ) )
millanea 0:99928431bb44 34 {
millanea 0:99928431bb44 35 // Taking character from device and putting the buffer
millanea 0:99928431bb44 36 mavlinkBuffer[ mavlinkBufferIn ] = mavlinkIn.getc() ;
millanea 0:99928431bb44 37 // Incrementing the buffer pointer
millanea 0:99928431bb44 38 mavlinkBufferIn = ( mavlinkBufferIn + 1 ) % mavlinkBufferSize ;
millanea 0:99928431bb44 39 }
millanea 0:99928431bb44 40 }
millanea 0:99928431bb44 41
millanea 0:99928431bb44 42 void MavlinkPassthrough::maintainMavlinkBuffer()
millanea 0:99928431bb44 43 {
millanea 0:99928431bb44 44 // Switching actions depending on the mavlink transfer case
millanea 0:99928431bb44 45 switch( mavlinkTransferState )
millanea 0:99928431bb44 46 {
millanea 0:99928431bb44 47 case START :
millanea 0:99928431bb44 48 // Searching for start character until end of mavlink buffer
millanea 0:99928431bb44 49 while( !( mavlinkBufferRead == mavlinkBufferIn ) )
millanea 0:99928431bb44 50 {
millanea 0:99928431bb44 51 //debugprintf("Buffer not empty searching for start char\r\n") ;
millanea 0:99928431bb44 52 // Detect the start character
millanea 0:99928431bb44 53 if( mavlinkBuffer[mavlinkBufferRead] == MAVLINKSTARTCHAR )
millanea 0:99928431bb44 54 {
millanea 0:99928431bb44 55 //debugprintf("Start character found. Switching state\r\n") ;
millanea 0:99928431bb44 56 // Changing state, advancing read pointer and stopping search
millanea 0:99928431bb44 57 mavlinkTransferState = LENGTH ;
millanea 0:99928431bb44 58 debugprintf("Mavlink state LENGTH\r\n") ;
millanea 0:99928431bb44 59 mavlinkBufferRead = ( mavlinkBufferRead + 1 ) % mavlinkBufferSize ;
millanea 0:99928431bb44 60 break ;
millanea 0:99928431bb44 61 }
millanea 0:99928431bb44 62 // Else advance the read and output pointer and continue the search
millanea 0:99928431bb44 63 else
millanea 0:99928431bb44 64 {
millanea 0:99928431bb44 65 mavlinkBufferRead = ( mavlinkBufferRead + 1 ) % mavlinkBufferSize ;
millanea 0:99928431bb44 66 mavlinkBufferOut = ( mavlinkBufferOut + 1 ) % mavlinkBufferSize ;
millanea 0:99928431bb44 67 }
millanea 0:99928431bb44 68 }
millanea 0:99928431bb44 69 break ;
millanea 0:99928431bb44 70 case LENGTH :
millanea 0:99928431bb44 71 // Checking if there's another byte in the mavlink buffer
millanea 0:99928431bb44 72 if( !( mavlinkBufferRead == mavlinkBufferIn ) )
millanea 0:99928431bb44 73 {
millanea 0:99928431bb44 74 // Reading the character as the datalength
millanea 0:99928431bb44 75 currentMavlinkFrameLength = mavlinkBuffer[mavlinkBufferRead] + MAVLINKADDITIONALDATA ;
millanea 0:99928431bb44 76 //debugprintf("Length character found: %u\r\n", currentMavlinkFrameLength ) ;
millanea 0:99928431bb44 77 // Advancing the read pointer and changing the state
millanea 0:99928431bb44 78 mavlinkTransferState = DATA ;
millanea 0:99928431bb44 79 debugprintf("Mavlink state DATA\r\n") ;
millanea 0:99928431bb44 80 mavlinkBufferRead = ( mavlinkBufferRead + 1 ) % mavlinkBufferSize ;
millanea 0:99928431bb44 81 }
millanea 0:99928431bb44 82 break ;
millanea 0:99928431bb44 83 case DATA :
millanea 0:99928431bb44 84 // Looping while there are still data bits to be read
millanea 0:99928431bb44 85 while( !( mavlinkBufferRead == mavlinkBufferIn ) )
millanea 0:99928431bb44 86 {
millanea 0:99928431bb44 87 // Incrementing the read pointer and decrementing the data bits left counter
millanea 0:99928431bb44 88 mavlinkBufferRead = ( mavlinkBufferRead + 1 ) % mavlinkBufferSize ;
millanea 0:99928431bb44 89 currentMavlinkFrameLength -= 1 ;
millanea 0:99928431bb44 90 //debugprintf("Databits left: %u\r\n", currentMavlinkFrameLength) ;
millanea 0:99928431bb44 91 // If no databits left move to the next state
millanea 0:99928431bb44 92 if( currentMavlinkFrameLength == 0 )
millanea 0:99928431bb44 93 {
millanea 0:99928431bb44 94 // Advancing the read pointer and changing the state
millanea 0:99928431bb44 95 mavlinkTransferState = TRANSFER ;
millanea 0:99928431bb44 96 debugprintf("Mavlink state TRANSFER\r\n") ;
millanea 0:99928431bb44 97 break ;
millanea 0:99928431bb44 98 }
millanea 0:99928431bb44 99 }
millanea 0:99928431bb44 100 break ;
millanea 0:99928431bb44 101 case TRANSFER :
millanea 0:99928431bb44 102
millanea 0:99928431bb44 103 // Disabling while accessing shared output buffer
millanea 0:99928431bb44 104 __disable_irq() ;
millanea 0:99928431bb44 105 // Looping until all characters moved to the transferBuffer frame
millanea 0:99928431bb44 106 while( !( mavlinkBufferOut == mavlinkBufferRead ) )
millanea 0:99928431bb44 107 {
millanea 0:99928431bb44 108 // Transfering a character to the frames buffer
millanea 0:99928431bb44 109 bufferedOutput.addChar( mavlinkBuffer[mavlinkBufferOut] ) ;
millanea 0:99928431bb44 110 // Incrementing the mavlinkOut pointer
millanea 0:99928431bb44 111 mavlinkBufferOut = ( mavlinkBufferOut + 1 ) % mavlinkBufferSize ;
millanea 0:99928431bb44 112 }
millanea 0:99928431bb44 113 // Re-enabling interrupts
millanea 0:99928431bb44 114 __enable_irq() ;
millanea 0:99928431bb44 115
millanea 0:99928431bb44 116 // Changing back to start state
millanea 0:99928431bb44 117 mavlinkTransferState = START ;
millanea 0:99928431bb44 118 debugprintf("Mavlink state START\r\n") ;
millanea 0:99928431bb44 119 break ;
millanea 0:99928431bb44 120 }
millanea 0:99928431bb44 121 }