// -*- 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>8) ^ (tmp<<8) ^ (tmp<<3) ^ (tmp>>4); i++; } return sum; } /* we use a hand-crafted MAVLink packet based on the following message definition Status generated by radio local signal strength remote signal strength percentage free space in transmit buffer background noise level remote background noise level receive errors count of error corrected packets */ 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; }