serval-dna/mavlink.c

// -*- Mode: C; c-basic-offset: 2; -*-
//
// Copyright (c) 2012 Andrew Tridgell, All Rights Reserved
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
//  o Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//  o Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in
//    the documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
// OF THE POSSIBILITY OF SUCH DAMAGE.
//

/*
Portions Copyright (C) 2013 Paul Gardner-Stephen
 
This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.
 
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.
 
You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
*/

#include "serval.h"
#include "conf.h"

#define MAVLINK_MSG_ID_RADIO 166
#define MAVLINK_MSG_ID_DATASTREAM 67
int MAVLINK_MESSAGE_CRCS[]={72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 188, 118, 242, 19, 97, 233, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7};

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

uint16_t mavlink_crc(unsigned char *buf,int length)
{
  uint16_t sum = 0xFFFF;
  uint8_t i, stoplen;
  
  stoplen = length + 6;
  
  // MAVLink 1.0 has an extra CRC seed
  buf[length+6] = MAVLINK_MESSAGE_CRCS[buf[5]];
  stoplen++;
  
  i = 1;
  while (i<stoplen) {
    uint8_t tmp;
    tmp = buf[i] ^ (uint8_t)(sum&0xff);
    tmp ^= (tmp<<4);
    sum = (sum>>8) ^ (tmp<<8) ^ (tmp<<3) ^ (tmp>>4);
    i++;
  }
  
  return sum;
}


/*
  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>
*/
struct mavlink_RADIO_v09 {
  uint8_t rssi;
  uint8_t remrssi;
  uint8_t txbuf;
  uint8_t noise;
  uint8_t remnoise;
  uint16_t rxerrors;
  uint16_t fixed;
};
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;
};

int stream_as_mavlink(int sequence_number,int startP,int endP,
		      unsigned char *data,int count,
		      unsigned char *frame,int *outlen)
{
  if (count>252-6-4) return -1;
  
  frame[0]=0xfe; // mavlink v1.0 frame
  /* payload len, excluding 6 byte header and 2 byte CRC.
     But we use a 4-byte CRC, so need to add two to count to make packet lengths
     be as expected.
     Note that this construction will result in CRC errors by non-servald
     programmes, which is probably more helpful than otherwise.
  */
  frame[1]=count+2; 
  frame[2]=sequence_number; // packet sequence
  frame[3]=0x00; // system ID of sender (MAV_TYPE_GENERIC)
  frame[4]=0x40; // component ID of sender: we are reusing this to mark start,end of MDP frames
  if (startP) frame[4]|=0x01;
  if (endP) frame[4]|=0x02;
  frame[5]=MAVLINK_MSG_ID_DATASTREAM; // message ID type of this frame: DATA_STREAM
  // payload follows (we reuse the DATA_STREAM message type parameters)
  bcopy(data,&frame[6],count);
  // Add our own 4-byte CRC of the data only
  uint32_t crc=Crc32_ComputeBuf(0,&frame[1],-1+count+6);
  frame[6+count+0]=(crc>>0)&0xff;
  frame[6+count+1]=(crc>>8)&0xff;
  frame[6+count+2]=(crc>>16)&0xff;
  frame[6+count+3]=(crc>>24)&0xff;

  *outlen=count+6+4;
  return 0;
}

int mavlink_heartbeat(unsigned char *frame,int *outlen)
{
  int count=9;
  frame[0]=0xfe; // mavlink v1.0 frame
  frame[1]=count; // payload len, excluding 6 byte header and 2 byte CRC
  frame[2]=0; // packet sequence
  frame[3]=0x00; // system ID of sender (MAV_TYPE_GENERIC)
  frame[4]=0xf1; // component ID of sender (MAV_COMP_ID_UART_BRIDGE)
  frame[5]=0; // message ID type of this frame: DATA_STREAM

  // payload follows
  bzero(&frame[6],count);

  // two-byte CRC follows
  uint16_t crc=mavlink_crc(frame,count); // automatically adds 6 for header length
  frame[count+6]=crc&0xff;
  frame[count+7]=crc>>8;
  
  *outlen=count+6+2;
  
  return 0;
}

#define MAVLINK_STATE_UNKNOWN 0
#define MAVLINK_STATE_LENGTH 1
#define MAVLINK_STATE_SEQ 2
#define MAVLINK_STATE_SYSID 3
#define MAVLINK_STATE_COMPONENTID 4
#define MAVLINK_STATE_MSGID 5
#define MAVLINK_STATE_PAYLOAD 6
#define MAVLINK_STATE_CRC1 7
#define MAVLINK_STATE_CRC2 8

char *mavlink_describe_state(int state)
{
  switch(state) {
  case MAVLINK_STATE_UNKNOWN: return "MAVLINK_STATE_UNKNOWN";
  case MAVLINK_STATE_LENGTH: return "MAVLINK_STATE_LENGTH";
  case MAVLINK_STATE_SEQ: return "MAVLINK_STATE_SEQ";
  case MAVLINK_STATE_SYSID: return "MAVLINK_STATE_SYSID";
  case MAVLINK_STATE_COMPONENTID: return "MAVLINK_STATE_COMPONENTID";
  case MAVLINK_STATE_MSGID: return "MAVLINK_STATE_MSGID";
  case MAVLINK_STATE_PAYLOAD: return "MAVLINK_STATE_PAYLOAD";
  case MAVLINK_STATE_CRC1: return "MAVLINK_STATE_CRC1";
  case MAVLINK_STATE_CRC2: return "MAVLINK_STATE_CRC2";
  default: return "INVALID_STATE";
  }
}

extern unsigned long long last_rssi_time;
extern int last_radio_rssi;
extern int last_radio_temperature;
extern int last_radio_rxpackets;


int mavlink_parse(struct slip_decode_state *state)
{
  switch(state->mavlink_msgid) {
  case MAVLINK_MSG_ID_RADIO:
    if (config.debug.mavlink) DEBUG("Received MAVLink radio report");
    last_radio_rssi=(1.0*state->mavlink_payload[5]-state->mavlink_payload[8])/1.9;
    last_radio_temperature=-999; // doesn't get reported
    last_radio_rxpackets=-999; // doesn't get reported
    if (config.debug.mavlink||gettime_ms()-last_rssi_time>30000) {
      INFOF("Link budget = %+ddB, remote link budget = %+ddB",
	    last_radio_rssi,
	    (int)((1.0*state->mavlink_payload[6]-state->mavlink_payload[9])/1.9));
      last_rssi_time=gettime_ms();
    }
    return 0;
  case MAVLINK_MSG_ID_DATASTREAM:
    // Extract and return packet
    // XXX - Ignores CRC at present 
    if (config.debug.mavlink) DEBUG("Received MAVLink DATASTREAM message for us");
    if (state->mavlink_componentid&0x01) {
      if (config.debug.mavlink) {
	DEBUG("Found start of PDU");
	if (state->packet_length) DEBUGF("... previous packet had not ended, discarding");
      }
      state->packet_length=0;
    }
    if (state->packet_length+state->mavlink_payload_length>sizeof(state->dst))
      {
	if (config.debug.mavlink) DEBUG("Too many extension frames for packet - discarding");
	return 0;
      }
    bcopy(state->mavlink_payload,&state->dst[state->packet_length],
	  state->mavlink_payload_length);
    state->packet_length+=state->mavlink_payload_length;
    if (state->mavlink_componentid&0x02) {
      if (config.debug.mavlink) DEBUG("Found end of PDU");      
      return 1;
    } else return 0;
    break;
  default:
    if (config.debug.mavlink) 
      DEBUGF("Received unknown MAVLink message type 0x%02x",
	    state->mavlink_msgid);
    return -1;
  }
}

int mavlink_decode(struct slip_decode_state *state,uint8_t c)
{
  if (config.debug.mavlinkfsm) DEBUGF("RX character %02x in state %s",
				      c,mavlink_describe_state(state->state));
  switch(state->state) {
  case MAVLINK_STATE_LENGTH:
    state->mavlink_crc=0;
    state->mavlink_crc=Crc32_ComputeBuf(state->mavlink_crc,&c,1);
    state->mavlink_payload_length=c;
    state->mavlink_payload_offset=0;
    state->state++;
    break;
  case MAVLINK_STATE_SEQ:
    state->mavlink_crc=Crc32_ComputeBuf(state->mavlink_crc,&c,1);
    state->mavlink_sequence=c;
    state->state++;
    break;
  case MAVLINK_STATE_SYSID:
    state->mavlink_crc=Crc32_ComputeBuf(state->mavlink_crc,&c,1);
    state->mavlink_sysid=c;
    state->state++;
    break;
  case MAVLINK_STATE_COMPONENTID:
    state->mavlink_crc=Crc32_ComputeBuf(state->mavlink_crc,&c,1);
    state->mavlink_componentid=c;
    state->state++;
    break;
  case MAVLINK_STATE_MSGID:
    state->mavlink_crc=Crc32_ComputeBuf(state->mavlink_crc,&c,1);
    state->mavlink_msgid=c;
    state->state++;
    break;
  case MAVLINK_STATE_PAYLOAD:
    if (state->mavlink_payload_length-state->mavlink_payload_offset>2)
      state->mavlink_crc=Crc32_ComputeBuf(state->mavlink_crc,&c,1);
    if (state->mavlink_payload_length-state->mavlink_payload_offset==2)
      state->mavlink_rxcrc=c;
    if (state->mavlink_payload_length-state->mavlink_payload_offset==1)
      state->mavlink_rxcrc|=c<<8;
    state->mavlink_payload[state->mavlink_payload_offset++]=c;
    if (state->mavlink_payload_offset==state->mavlink_payload_length)
      state->state++;
    break;
  case MAVLINK_STATE_CRC1:
    state->mavlink_rxcrc|=c<<16;
    state->state++;
    break;
  case MAVLINK_STATE_CRC2:
    state->mavlink_rxcrc|=c<<24;
    state->state=MAVLINK_STATE_UNKNOWN;
    state->mavlink_payload_length-=2; // don't count the upper 16 bits of our 32bit CRC
    if (state->mavlink_rxcrc==state->mavlink_crc)
      return mavlink_parse(state);
    if (config.debug.mavlink)
      DEBUGF("CRC mismatch on mavlink encapsulated data: rxed=%08x, calced=%08x",
	     state->mavlink_rxcrc,state->mavlink_crc);
    state->mavlink_payload_length=0;
    break;
  case MAVLINK_STATE_UNKNOWN:
  default:
    if (c==0xfe) state->state=MAVLINK_STATE_LENGTH; 
    else { 
      state->state=MAVLINK_STATE_UNKNOWN;
    }
  }

  return 0;
}