diff --git a/conf_schema.h b/conf_schema.h index a3bab9c5..3257b89d 100644 --- a/conf_schema.h +++ b/conf_schema.h @@ -239,6 +239,7 @@ ATOM(bool_t, gateway, 0, boolean,, "") ATOM(bool_t, keyring, 0, boolean,, "") ATOM(bool_t, security, 0, boolean,, "") ATOM(bool_t, mdprequests, 0, boolean,, "") +ATOM(bool_t, mavlink, 0, boolean,, "") ATOM(bool_t, peers, 0, boolean,, "") ATOM(bool_t, overlayframes, 0, boolean,, "") ATOM(bool_t, overlayabbreviations, 0, boolean,, "") diff --git a/lsif.c b/lsif.c index 8f70527e..52e80c91 100644 --- a/lsif.c +++ b/lsif.c @@ -54,10 +54,13 @@ #ifdef HAVE_LINUX_IF_H #include #else -#ifdef HAVE_NET_IF_H +#if HAVE_NET_IF_H || __MACH__ || __NetBSD__ || __OpenBSD__ || __FreeBSD__ #include #endif #endif +#ifdef HAVE_IFADDRS_H +#include +#endif /* On platforms that have variable length ifreq use the old fixed length interface instead */ diff --git a/mavlink.c b/mavlink.c index 9dfcf54e..4e297187 100644 --- a/mavlink.c +++ b/mavlink.c @@ -1,4 +1,4 @@ -// -*- Mode: C; c-basic-offset: 8; -*- +// -*- Mode: C; c-basic-offset: 2; -*- // // Copyright (c) 2012 Andrew Tridgell, All Rights Reserved // @@ -26,140 +26,248 @@ // OF THE POSSIBILITY OF SUCH DAMAGE. // -/// -/// @file mavlink.c -/// -/// mavlink reporting code -/// +/* +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 -#include "radio.h" -#include "packet.h" -#include "timer.h" - -extern __xdata uint8_t pbuf[MAX_PACKET_LENGTH]; -static __pdata uint8_t seqnum; -extern bool using_mavlink_10; +#include "serval.h" +#include "conf.h" #define MAVLINK_MSG_ID_RADIO 166 -#define MAVLINK_RADIO_CRC_EXTRA 21 +#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' -/* - * Calculates the MAVLink checksum on a packet in pbuf[] - * and append it after the data - */ -static void mavlink_crc(void) +uint16_t mavlink_crc(unsigned char *buf,int length) { - register uint8_t length = pbuf[1]; - __pdata uint16_t sum = 0xFFFF; - __pdata uint8_t i, stoplen; - - stoplen = length + 6; - - if (using_mavlink_10) { - // MAVLink 1.0 has an extra CRC seed - pbuf[length+6] = MAVLINK_RADIO_CRC_EXTRA; - stoplen++; - } - - i = 1; - while (i>8) ^ (tmp<<8) ^ (tmp<<3) ^ (tmp>>4); - i++; - } - - pbuf[length+6] = sum&0xFF; - pbuf[length+7] = sum>>8; + 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 - + 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; + 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; + uint16_t rxerrors; + uint16_t fixed; + uint8_t rssi; + uint8_t remrssi; + uint8_t txbuf; + uint8_t noise; + uint8_t remnoise; }; -static void swap_bytes(__pdata uint8_t ofs, __pdata uint8_t len) +int stream_as_mavlink(int f,unsigned char *data,int count, + unsigned char *frame,int *outlen) { - register uint8_t i; - for (i=ofs; i252-6-2) return -1; + + frame[0]=0xfe; // mavlink v1.0 frame + frame[1]=count; // payload len, excluding 6 byte header and 2 byte CRC + frame[2]=f; // 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]=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); + // 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; } -/// send a MAVLink status report packet -void MAVLink_report(void) +int mavlink_heartbeat(unsigned char *frame,int *outlen) { - pbuf[0] = using_mavlink_10?254:'U'; - pbuf[1] = sizeof(struct mavlink_RADIO_v09); - pbuf[2] = seqnum++; - pbuf[3] = RADIO_SOURCE_SYSTEM; - pbuf[4] = RADIO_SOURCE_COMPONENT; - pbuf[5] = MAVLINK_MSG_ID_RADIO; + 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 - if (using_mavlink_10) { - struct mavlink_RADIO_v10 *m = (struct mavlink_RADIO_v10 *)&pbuf[6]; - m->rxerrors = errors.rx_errors; - m->fixed = errors.corrected_packets; - m->txbuf = serial_read_space(); - m->rssi = statistics.average_rssi; - m->remrssi = remote_statistics.average_rssi; - m->noise = statistics.average_noise; - m->remnoise = remote_statistics.average_noise; - } else { - struct mavlink_RADIO_v09 *m = (struct mavlink_RADIO_v09 *)&pbuf[6]; - m->rxerrors = errors.rx_errors; - m->fixed = errors.corrected_packets; - m->txbuf = serial_read_space(); - m->rssi = statistics.average_rssi; - m->remrssi = remote_statistics.average_rssi; - m->noise = statistics.average_noise; - m->remnoise = remote_statistics.average_noise; - swap_bytes(6+5, 4); - } - mavlink_crc(); + // payload follows + bzero(&frame[6],count); - if (serial_write_space() < sizeof(struct mavlink_RADIO_v09)+8) { - // don't cause an overflow - return; - } - - serial_write_buf(pbuf, sizeof(struct mavlink_RADIO_v09)+8); + // 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"); + bcopy(state->mavlink_payload,state->dst,state->mavlink_payload_length); + state->packet_length=state->mavlink_payload_length; + return 1; + 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.mavlink) DEBUGF("RX character %02x in state %s", + c,mavlink_describe_state(state->state)); + switch(state->state) { + case MAVLINK_STATE_LENGTH: + state->mavlink_payload_length=c; + state->mavlink_payload_offset=0; + state->state++; + break; + case MAVLINK_STATE_SEQ: + state->mavlink_sequence=c; + state->state++; + break; + case MAVLINK_STATE_SYSID: + state->mavlink_sysid=c; + state->state++; + break; + case MAVLINK_STATE_COMPONENTID: + state->mavlink_componentid=c; + state->state++; + break; + case MAVLINK_STATE_MSGID: + state->mavlink_msgid=c; + state->state++; + break; + case MAVLINK_STATE_PAYLOAD: + 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; + state->state++; + break; + case MAVLINK_STATE_CRC2: + state->mavlink_rxcrc|=c<<8; + state->state=MAVLINK_STATE_UNKNOWN; + return mavlink_parse(state); + break; + case MAVLINK_STATE_UNKNOWN: + default: + if (c==0xfe) state->state=MAVLINK_STATE_LENGTH; + else { + state->state=MAVLINK_STATE_UNKNOWN; + } + } + + return 0; } diff --git a/overlay_interface.c b/overlay_interface.c index 609cad02..b850ad20 100644 --- a/overlay_interface.c +++ b/overlay_interface.c @@ -57,7 +57,7 @@ static void overlay_interface_close(overlay_interface *interface){ link_interface_down(interface); INFOF("Interface %s addr %s is down", - interface->name, inet_ntoa(interface->address.sin_addr)); + interface->name, inet_ntoa(interface->address.sin_addr)); unschedule(&interface->alarm); unwatch(&interface->alarm); close(interface->alarm.poll.fd); @@ -495,7 +495,7 @@ overlay_interface_init(const char *name, struct in_addr src_addr, struct in_addr interface->slip_decode_state.dst_offset=0; /* The encapsulation type should be configurable, but for now default to the one that should be safe on the RFD900 radios, and that also allows us to receive RSSI reports inline */ - interface->slip_decode_state.encapsulator=SLIP_FORMAT_UPPER7; + interface->slip_decode_state.encapsulator=SLIP_FORMAT_MAVLINK; interface->alarm.poll.events=POLLIN; watch(&interface->alarm); break; @@ -882,7 +882,7 @@ overlay_broadcast_ensemble(struct network_destination *destination, unsigned char *buffer = interface->txbuffer; int out_len=0; - int encoded = slip_encode(SLIP_FORMAT_UPPER7, + int encoded = slip_encode(SLIP_FORMAT_MAVLINK, bytes, len, buffer+out_len, sizeof(interface->txbuffer) - out_len); if (encoded < 0) return WHY("Buffer overflow"); @@ -891,7 +891,7 @@ overlay_broadcast_ensemble(struct network_destination *destination, { // Test decoding of the packet we send struct slip_decode_state state; - state.encapsulator=SLIP_FORMAT_UPPER7; + state.encapsulator=SLIP_FORMAT_MAVLINK; state.src_size=encoded; state.src_offset=0; state.src=buffer+out_len; @@ -1063,7 +1063,7 @@ void overlay_interface_discover(struct sched_ent *alarm) int i; for (i = 0; i < overlay_interface_count; i++) if (overlay_interfaces[i].state==INTERFACE_STATE_UP) - overlay_interfaces[i].state=INTERFACE_STATE_DETECTING; + overlay_interfaces[i].state=INTERFACE_STATE_DETECTING; /* Register new dummy interfaces */ int detect_real_interfaces = 0; @@ -1115,8 +1115,10 @@ void overlay_interface_discover(struct sched_ent *alarm) // Close any interfaces that have gone away. for(i = 0; i < overlay_interface_count; i++) - if (overlay_interfaces[i].state==INTERFACE_STATE_DETECTING) + if (overlay_interfaces[i].state==INTERFACE_STATE_DETECTING) { + DEBUGF("Closing interface stuck in DETECTING state."); overlay_interface_close(&overlay_interfaces[i]); + } alarm->alarm = gettime_ms()+5000; alarm->deadline = alarm->alarm + 10000; diff --git a/overlay_packetradio.c b/overlay_packetradio.c index 0b3eb330..4151a218 100644 --- a/overlay_packetradio.c +++ b/overlay_packetradio.c @@ -68,21 +68,20 @@ int overlay_packetradio_setup_port(overlay_interface *interface) if (tcsetattr(interface->alarm.poll.fd, TCSANOW, &t)) WHY_perror("Failed to set terminal parameters"); - // Ask radio to report RSSI - (void)write_all(interface->alarm.poll.fd,"\r",1); - sleep_ms(600); - (void)write_all(interface->alarm.poll.fd,"\r",1); - sleep_ms(600); - (void)write_all(interface->alarm.poll.fd,"+++",3); - sleep_ms(1200); - (void)write_all(interface->alarm.poll.fd,"\rAT&T\rAT&T=RSSI\rATO\r",20); + // Enable MAVLink mode to get regular RSSI reports + uint8_t buf[256]; + int buflen=0; + buflen=0; + mavlink_heartbeat(buf,&buflen); + (void)write_all(interface->alarm.poll.fd,buf,buflen); + + if (config.debug.packetradio) { tcgetattr(interface->alarm.poll.fd, &t); int in_speed=cfgetispeed(&t); int out_speed=cfgetospeed(&t); - DEBUGF("Enabled RSSI reporting for RFD900 radios"); - DEBUGF("Sent ATO to make sure we are in on-line mode"); + DEBUGF("Enabled MAVLink based RSSI reporting for RFD900 radios"); DEBUGF("uart speed reported as %d/%d",in_speed,out_speed); } diff --git a/serval.h b/serval.h index f21c7c7f..12474880 100644 --- a/serval.h +++ b/serval.h @@ -368,6 +368,7 @@ extern int overlayMode; struct slip_decode_state{ #define SLIP_FORMAT_SLIP 0 #define SLIP_FORMAT_UPPER7 1 +#define SLIP_FORMAT_MAVLINK 2 int encapsulator; int state; unsigned char *src; @@ -379,6 +380,15 @@ struct slip_decode_state{ uint32_t crc; int src_offset; int dst_offset; + + uint8_t mavlink_payload_length; + uint8_t mavlink_payload_offset; + uint8_t mavlink_payload[256]; + uint8_t mavlink_sequence; + uint8_t mavlink_sysid; + uint8_t mavlink_componentid; + uint8_t mavlink_msgid; + uint16_t mavlink_rxcrc; }; struct overlay_interface; @@ -866,4 +876,9 @@ int link_add_destinations(struct overlay_frame *frame); int generate_nonce(unsigned char *nonce,int bytes); +int mavlink_decode(struct slip_decode_state *state,uint8_t c); +int mavlink_heartbeat(unsigned char *frame,int *outlen); +int stream_as_mavlink(int f,unsigned char *data,int count, + unsigned char *frame,int *outlen); + #endif // __SERVALD_SERVALD_H diff --git a/slip.c b/slip.c index e1f4248b..c53ee141 100644 --- a/slip.c +++ b/slip.c @@ -1,3 +1,22 @@ +/* +Serval Distributed Numbering Architecture (DNA) +Copyright (C) 2012 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" #include "log.h" @@ -25,6 +44,32 @@ int slip_encode(int format, unsigned char *src, int src_bytes, unsigned char *dst, int dst_len) { switch(format) { + case SLIP_FORMAT_MAVLINK: + { + int i; + int dst_offset=0; + for(i=0;isrc_bytes) slice_bytes=src_bytes-i; + if (dst_offset+slice_bytes+6+2>=dst_len) { + if (config.debug.mavlink) + DEBUGF("Would overflow output buffer."); + return -1; + } else { + stream_as_mavlink(0,&src[i],slice_bytes,&dst[dst_offset],&slice_len); + if (config.debug.mavlink) { + DEBUGF("Wrote %d bytes as %d byte MAVLink frame", + slice_bytes,slice_len); + dump("original data",&src[i],slice_bytes); + dump("mavlink frame",&dst[dst_offset],slice_len); + } + dst_offset+=slice_len; + } + } + return dst_offset; + } + break; case SLIP_FORMAT_SLIP: { int offset=0; @@ -330,6 +375,23 @@ int upper7_decode(struct slip_decode_state *state,unsigned char byte) int slip_decode(struct slip_decode_state *state) { switch(state->encapsulator) { + case SLIP_FORMAT_MAVLINK: + { + dump("rx data",state->src,state->src_size); + for(;state->src_offsetsrc_size;state->src_offset++) { + // flag complete reception of a packet + DEBUGF("src_offset=%d, src_size=%d, c=%02x", + state->src_offset,state->src_size,state->src[state->src_offset]); + if (mavlink_decode(state,state->src[state->src_offset])==1) { + // We have to increment src_offset manually here, because returning + // prevents the post-increment in the for loop from triggering + state->src_offset++; + return 1; + } + } + } + return 0; + break; case SLIP_FORMAT_SLIP: { /* diff --git a/sourcefiles.mk b/sourcefiles.mk index eb63a20e..4d2fe33d 100644 --- a/sourcefiles.mk +++ b/sourcefiles.mk @@ -17,6 +17,7 @@ SERVAL_SOURCES = \ $(SERVAL_BASE)log.c \ $(SERVAL_BASE)lsif.c \ $(SERVAL_BASE)main.c \ + $(SERVAL_BASE)mavlink.c \ $(SERVAL_BASE)meshms.c \ $(SERVAL_BASE)mdp_client.c \ $(SERVAL_BASE)os.c \