mavlink.h

// 
// Mavlink frame detection: 
// Used to monitor the mavlink stream such that radio status packets can safely be
// injected into the stream without interfering with existing packets.
// 

//Field name			Index (Bytes)	Purpose
//----------------------------------------------------------------------------------------------------------------------------------------------------------------------------
// Start-of-frame		0				Denotes the start of frame transmission (v1.0: 0xFE)
// Payload length		1				Length of the following payload
// Packet sequence		2				Each component counts up his send sequence. Allows to detect packet loss
// System ID			3				Identification of the SENDING system. Allows to differentiate different systems on the same network.
// Component ID			4				Identification of the SENDING component. Allows to differentiate different components of the same system, e.g. the IMU and the autopilot.
// Message ID			5				Identification of the message - the id defines what the payload �means� and how it should be correctly decoded.
// Payload				6 to (n+6)		The data into the message, depends on the message id.
// CRC					(n+7) to (n+8)	Check-sum of the entire packet, excluding the packet start sign (LSB to MSB)
//----------------------------------------------------------------------------------------------------------------------------------------------------------------------------

#define MAVLINK_PACKET_START 0xFE


class MavlinkFrameDetector
{
public:

  MavlinkFrameDetector() {
    Reset();
  }

  // Returns true if a mavlink frame has been detected.
  bool Parse(uint8_t ch) {
    switch (m_state) {
    case MavParse_Idle:
      if (ch == MAVLINK_PACKET_START) {
        Reset();
        m_state = MavParse_PayloadLength;
      }
      break;
    case MavParse_PayloadLength:
      m_payloadLength = ch;
      m_state = MavParse_PacketSequence;
      break;
    case MavParse_Payload:
      if (++m_payloadByteParsedCount >= m_payloadLength) {
        ++m_state;
      }
      break;
    case MavParse_PacketSequence:
    case MavParse_SystemID:
    case MavParse_ComponentID:
    case MavParse_MessageID:
    case MavParse_CRC1:
      ++m_state;
      break;
    case MavParse_CRC2:
      m_state = MavParse_Idle;
      return true;
    }
    return false;
  }

  void Reset() {
    m_payloadLength = 0;
    m_payloadByteParsedCount = 0; // clear helper
    m_state = MavParse_Idle;
  }

  bool IsIdle() {
    return m_state == MavParse_Idle;
  }

private:
  enum MavlinkParseState {
    MavParse_Idle,
    MavParse_PayloadLength,
    MavParse_PacketSequence,
    MavParse_SystemID,
    MavParse_ComponentID,
    MavParse_MessageID,
    MavParse_Payload,
    MavParse_CRC1,
    MavParse_CRC2
  };

  uint8_t m_payloadLength;
  uint8_t m_payloadByteParsedCount;
  uint8_t m_state;
};


// 
// Mavlink injection code:
// Injects radio status mavlink packets into the stream sent to the APM / ground station.
//

struct mavlink_RADIO_v10 {
  uint16_t rxerrors;
  uint16_t fixed;
  uint8_t rssi;
  uint8_t remrssi;
  uint8_t txbuf;
  uint8_t noise;
  uint8_t remnoise;
};

// use '3D' for 3DRadio
#define RADIO_SOURCE_SYSTEM '3'
#define RADIO_SOURCE_COMPONENT 'D'

#define MAVLINK_MSG_ID_RADIO 166
#define MAVLINK_RADIO_CRC_EXTRA 21
#define MAV_HEADER_SIZE 6
#define MAV_MAX_PACKET_LENGTH  (MAV_HEADER_SIZE + sizeof(struct mavlink_RADIO_v10) + 2) // 17


static uint8_t g_mavlinkBuffer[MAV_MAX_PACKET_LENGTH];
static uint8_t g_sequenceNumber = 0;

/*
          we use a hand-crafted MAVLink packet based on the following
	  message definition

	  <message name="RADIO" id="166">
	    <description>Status generated by radio</description>
            <field type="uint8_t" name="rssi">local signal strength</field>
            <field type="uint8_t" name="remrssi">remote signal strength</field>
	    <field type="uint8_t" name="txbuf">percentage free space in transmit buffer</field>
	    <field type="uint8_t" name="noise">background noise level</field>
	    <field type="uint8_t" name="remnoise">remote background noise level</field>
	    <field type="uint16_t" name="rxerrors">receive errors</field>
	    <field type="uint16_t" name="fixed">count of error corrected packets</field>
	  </message>
*/


/*
 * Calculates the MAVLink checksum on a packet in parameter buffer
 * and append it after the data
 */
static void mavlink_crc(uint8_t* buf)
{
  register uint8_t length = buf[1];
  uint16_t sum = 0xFFFF;
  uint8_t i, stoplen;

  stoplen = length + MAV_HEADER_SIZE + 1;

  // MAVLink 1.0 has an extra CRC seed
  buf[length + MAV_HEADER_SIZE] = MAVLINK_RADIO_CRC_EXTRA;

  i = 1;
  while (i<stoplen) {
    register uint8_t tmp;
    tmp = buf[i] ^ (uint8_t)(sum&0xff);
    tmp ^= (tmp<<4);
    sum = (sum>>8) ^ (tmp<<8) ^ (tmp<<3) ^ (tmp>>4);
    i++;
  }

  buf[length+MAV_HEADER_SIZE] = sum&0xFF;
  buf[length+MAV_HEADER_SIZE+1] = sum>>8;
}


// return available space in rx buffer as a percentage
inline uint8_t	serial_space(uint16_t available, uint16_t max)
{
  uint16_t space = max - available;
  space = (100 * (space / 8)) / (max / 8);
  return space;
}


/// send a MAVLink status report packet
void MAVLink_report(uint8_t space, uint8_t RSSI_remote, uint16_t RSSI_local, uint16_t rxerrors)
{
  g_mavlinkBuffer[0] = 254;
  g_mavlinkBuffer[1] = sizeof(struct mavlink_RADIO_v10);
  g_mavlinkBuffer[2] = g_sequenceNumber++;
  g_mavlinkBuffer[3] = RADIO_SOURCE_SYSTEM;
  g_mavlinkBuffer[4] = RADIO_SOURCE_COMPONENT;
  g_mavlinkBuffer[5] = MAVLINK_MSG_ID_RADIO;


  // NOTE:
  // In mission planner, the Link quality is a percentage of the number
  // of good packets received
  // to the number of packets missed (detected by mavlink seq no.)
  // mission planner does disregard packets with '3D' in header for this calculation

  struct mavlink_RADIO_v10 *m = (struct mavlink_RADIO_v10 *)&g_mavlinkBuffer[MAV_HEADER_SIZE];
  m->rxerrors = rxerrors; // errors.rx_errors;
  m->fixed    = 0; //errors.corrected_packets;
  m->txbuf    = space; //serial_read_space();
  m->rssi     = RSSI_local; //statistics.average_rssi;
  m->remrssi  = RSSI_remote; //remote_statistics.average_rssi;
  m->noise    = 0; //statistics.average_noise;
  m->remnoise = 0; //remote_statistics.average_noise;

  mavlink_crc(g_mavlinkBuffer);

  uint8_t size = sizeof(g_mavlinkBuffer);
  uint8_t *buffer = (uint8_t*)g_mavlinkBuffer;
  if (Serial.txspace() >= size) {	// don't cause an overflow
    //MavlinkSerialPort.write(g_mavlinkBuffer, sizeof(g_mavlinkBuffer)); // TODO: Fix error: no matching function for call to 'SerialPort::write (it should be in Print class which SerialPort is derived from through Stream
    while (size--) {
      Serial.write(*buffer++);
    }
  }

}