diff --git a/sw/airborne/autopilot/Makefile b/sw/airborne/autopilot/Makefile index 93f54f21fa..ba3dc3956b 100644 --- a/sw/airborne/autopilot/Makefile +++ b/sw/airborne/autopilot/Makefile @@ -89,5 +89,9 @@ $(OBJDIR)/nav.o : $(ACINCLUDE)/flight_plan.h $(OBJDIR)/gps_ubx.o : $(VARINCLUDE)/ubx_protocol.h $(OBJDIR)/if_calib.o : $(ACINCLUDE)/inflight_calib.h +wavecard_glib : wavecard_glib.c wavecard.c + gcc -Wall -o wavecard_glib wavecard.c wavecard_glib.c `pkg-config --cflags glib-2.0` `pkg-config --libs glib-2.0` + + clean : avr_clean rm -f *.out *.cm* messages.h flight_plan.h ubx_protocol.h inflight_calib.h diff --git a/sw/airborne/autopilot/wavecard.c b/sw/airborne/autopilot/wavecard.c new file mode 100644 index 0000000000..4e366e0f4e --- /dev/null +++ b/sw/airborne/autopilot/wavecard.c @@ -0,0 +1,112 @@ +#include +#include + +#include "wavecard.h" + +typedef uint8_t bool_t; + +uint8_t wc_payload[WC_MAX_PAYLOAD]; + +static uint8_t wc_status; + +const uint16_t poly = 0x8408; +uint16_t crc; +uint8_t wc_length, payload_idx; + +#define UNINIT 0 +#define GOT_SYNC1 1 +#define GOT_STX 2 +#define GOT_LENGTH 3 +#define GOT_PAYLOAD 4 +#define GOT_CRC1 5 +#define GOT_CRC2 6 + + +bool_t waiting_ack, wc_msg_received; + +uint8_t wc_protocol_error, wc_ovrn, wc_error; + +void parse_payload() { + switch (wc_payload[0]) { + case WC_ACK: + if (waiting_ack) + waiting_ack = FALSE; + else + wc_protocol_error++; + break; + case WC_NAK: + case WC_ERROR: + wc_protocol_error++; + break; + default: + WcSendAck(); + } + wc_msg_received = FALSE; +} + +inline void parse_wc( uint8_t c ) { + // printf("s=%d\n", wc_status); + switch (wc_status) { + case UNINIT: + if (c == WC_SYNC) + wc_status++; + break; + case GOT_SYNC1: + if (c != WC_STX) + goto error; + crc = 0; + wc_status++; + break; + case GOT_STX: + if (wc_msg_received) { + wc_ovrn++; + goto error; + } + wc_length = c; + update_crc(c); + wc_status++; + payload_idx = 0; + break; + case GOT_LENGTH: + wc_payload[payload_idx] = c; + update_crc(c); + payload_idx++; + if (payload_idx == wc_length-3) + wc_status++; + break; + case GOT_PAYLOAD: + if (c != (crc & 0xff)) + goto error; + wc_status++; + break; + case GOT_CRC1: + if (c != (crc >> 8)) + goto error; + wc_status++; + break; + case GOT_CRC2: + if (c != WC_ETX) + goto error; + wc_msg_received = TRUE; + goto restart; + break; + } + return; + error: + wc_error++; + restart: + wc_status = UNINIT; + return; +} + + +/*** +#define WcPut1CtlByte(_byte) { \ + tx_buf[tx_head] = _byte; \ + tx_head++; \ + if (tx_head >= TX_BUF_SIZE) tx_head = 0; \ +} +***/ + + + diff --git a/sw/airborne/autopilot/wavecard.h b/sw/airborne/autopilot/wavecard.h new file mode 100644 index 0000000000..1233d2b671 --- /dev/null +++ b/sw/airborne/autopilot/wavecard.h @@ -0,0 +1,267 @@ +#ifndef WAVECARD_H +#define WAVECARD_H + +#define WC_CTL_BYTE_LEN 4 +#define WC_ADDR_LEN 6 + + +#include "wavecard_glib.h" + + +extern uint16_t crc; +extern const uint16_t poly; + +extern uint8_t wc_msg_received; + +#define WC_MAX_PAYLOAD 256 +extern uint8_t wc_payload[WC_MAX_PAYLOAD]; +extern uint8_t wc_length; +void parse_payload(void); + + + +#define WC_SYNC 0xff +#define WC_STX 0x02 +#define WC_ETX 0x03 + + + +#define WC_ACK 0x06 +#define WC_NAK 0x15 +#define WC_ERROR 0x00 +#define WC_REQ_WRITE_RADIO_PARAM 0x40 +#define WC_RES_WRITE_RADIO_PARAM 0x41 +#define WC_REQ_READ_RADIO_PARAM 0x50 +#define WC_RES_READ_RADIO_PARAM 0x51 +#define WC_REQ_SELECT_CHANNEL 0x60 +#define WC_RES_SELECT_CHANNEL 0x61 +#define WC_REQ_READ_CHANNEL 0x62 +#define WC_RES_READ_CHANNEL 0x63 +#define WC_REQ_SELECT_PHYCONFIG 0x64 +#define WC_RES_SELECT_PHYCONFIG 0x65 +#define WC_REQ_READ_PHYCONFIG 0x66 +#define WC_RES_READ_PHYCONFIG 0x67 +#define WC_REQ_READ_REMOTE_RSSI 0x68 +#define WC_RES_READ_REMOTE_RSSI 0x69 +#define WC_REQ_READ_LOCAL_RSSI 0x6A +#define WC_RES_READ_LOCAL_RSSI 0x6B +#define WC_REQ_FIRMWARE_VERSION 0xA0 +#define WC_RES_FIRMWARE_VERSION 0xA1 +#define WC_MODE_TEST 0xB0 +#define WC_REQ_SEND_FRAME 0x20 +#define WC_RES_SEND_FRAME 0x21 +#define WC_REQ_SEND_MESSAGE 0x22 +#define WC_REQ_SEND_POLLING 0x26 +#define WC_REQ_SEND_BROADCAST 0x28 +#define WC_RECEIVED_FRAME 0x30 +#define WC_RECEPTION_ERROR 0x31 +#define WC_RECEIVED_FRAME_POLLING 0x32 +#define WC_RECEIVED_FRAME_BROADCAST 0x34 +#define WC_RECEIVED_MULTIFRAME 0x36 +#define WC_REQ_SEND_SERVICE 0x80 +#define WC_RES_SEND_SERVICE 0x81 +#define WC_SERVICE_RESPONSE 0x82 + + +#define WC_RADIO_PARAM_AWAKENING_PERIOD 0x00 +#define WC_RADIO_PARAM_WAKE_UP_TYPE 0x01 +#define WC_RADIO_PARAM_WAKE_UP_LENGTH 0x02 +#define WC_RADIO_PARAM_RADIO_ACKNOLEDGE 0x04 +#define WC_RADIO_PARAM_RADIO_ADDRESS 0x05 +#define WC_RADIO_PARAM_RELAY_ROUTE 0x07 +#define WC_RADIO_PARAM_POLLING_ROUTE 0x08 +#define WC_RADIO_PARAM_GROUP_NUMBER 0x09 +#define WC_RADIO_PARAM_POLLING_TIME 0x0A +#define WC_RADIO_PARAM_RADIO_USER_TIMEOUT 0x0C +#define WC_RADIO_PARAM_RECEPT_ERROR_STATUS 0x0E +#define WC_RADIO_PARAM_SWITCH_MODE_STATUS 0x10 + + +void parse_wc( uint8_t); + + +#define update_crc(_byte) { \ + uint8_t i; \ + crc ^= _byte; \ + for(i = 0; i < 8; i++) { \ + uint8_t carry = crc & 0x1; \ + crc /= 2; \ + if (carry) \ + crc ^= poly; \ + } \ +} + + +#define WcPut1PayloadByte(_byte) { \ + WcPut1CtlByte(_byte); \ + update_crc(_byte);\ +} + +#define WcStart() \ + WcPut1CtlByte(WC_SYNC); \ + WcPut1CtlByte(WC_STX); \ + crc = 0; + +#define WcEnd() \ + WcPut1CtlByte(crc&0xff); \ + WcPut1CtlByte(crc>>8); \ + WcPut1CtlByte(WC_ETX); + +#define WcSendAck() \ + g_message("sending ACK"); \ + WcStart(); \ + WcPut1PayloadByte(WC_CTL_BYTE_LEN); \ + WcPut1PayloadByte(WC_ACK); \ + WcEnd() + +#define WcSendFirmwareReq() \ + g_message("sending REQ_FIRMWARE_VERSION"); \ + WcStart(); \ + WcPut1PayloadByte(WC_CTL_BYTE_LEN); \ + WcPut1PayloadByte(WC_REQ_FIRMWARE_VERSION); \ + WcEnd() + +#define WcSendReadRadioParamReq(no_param) \ + g_message("sending REQ_READ_RADIO_PARAM %d", no_param); \ + WcStart(); \ + WcPut1PayloadByte( WC_CTL_BYTE_LEN + 1); \ + WcPut1PayloadByte(WC_REQ_READ_RADIO_PARAM); \ + WcPut1PayloadByte(no_param); \ + WcEnd() + +#define WcSendWriteRadioParamReq(no_param, value) \ + g_message("sending REQ_WRITE_RADIO_PARAM %d %d", no_param, value); \ + WcStart(); \ + WcPut1PayloadByte( WC_CTL_BYTE_LEN + 2); \ + WcPut1PayloadByte(WC_REQ_WRITE_RADIO_PARAM); \ + WcPut1PayloadByte(no_param); \ + WcPut1PayloadByte(value); \ + WcEnd() + +#define WcSendReqSendService(addr, type) \ + { \ + uint8_t i; \ + g_message("sending REQ_SEND_SERVICE %02x %02x %02x %02x %02x %02x", addr[0], addr[1], addr[2] , addr[3], addr[4], addr[5]); \ + WcStart(); \ + WcPut1PayloadByte(WC_CTL_BYTE_LEN + WC_ADDR_LEN + 1); \ + WcPut1PayloadByte(WC_REQ_SEND_SERVICE); \ + WcPut1PayloadByte(addr[0]);\ + WcPut1PayloadByte(addr[1]);\ + WcPut1PayloadByte(addr[2]);\ + WcPut1PayloadByte(addr[3]);\ + WcPut1PayloadByte(addr[4]);\ + WcPut1PayloadByte(addr[5]);\ + WcPut1PayloadByte(type); \ + WcEnd() \ + } + +#define WcSendMsg(addr, len, msg) \ + { \ + uint8_t i; \ + GString* str = g_string_new( "sending REQ_SEND_MESSAGE " ); \ + wc_glib_append_addr(str, addr); \ + g_string_append_printf(str, "%*s ", len, msg); \ + g_message(str->str); \ + g_string_free(str, TRUE); \ + WcStart(); \ + WcPut1PayloadByte(WC_CTL_BYTE_LEN + WC_ADDR_LEN + len ); \ + WcPut1PayloadByte(WC_REQ_SEND_MESSAGE); \ + WcPut1PayloadByte(addr[0]); \ + WcPut1PayloadByte(addr[1]); \ + WcPut1PayloadByte(addr[2]); \ + WcPut1PayloadByte(addr[3]); \ + WcPut1PayloadByte(addr[4]); \ + WcPut1PayloadByte(addr[5]); \ + WcPut1PayloadByte(msg[0]); \ + WcPut1PayloadByte(msg[1]); \ + WcPut1PayloadByte(msg[2]); \ + WcPut1PayloadByte(msg[3]); \ + WcPut1PayloadByte(msg[4]); \ + WcPut1PayloadByte(msg[5]); \ + WcPut1PayloadByte(msg[6]); \ + WcPut1PayloadByte(msg[7]); \ + WcPut1PayloadByte(msg[8]); \ + WcPut1PayloadByte(msg[9]); \ + WcPut1PayloadByte(msg[10]); \ + WcPut1PayloadByte(msg[11]); \ + WcPut1PayloadByte(msg[12]); \ + WcEnd() \ + } \ + +#define WcSendReqReadRemoteRssi(addr) \ + { \ + GString* str = g_string_new( "sending REQ_READ_REMOTE_RSSI " ); \ + wc_glib_append_addr(str, addr); \ + g_message(str->str); \ + g_string_free(str, TRUE); \ + WcStart(); \ + WcPut1PayloadByte(WC_CTL_BYTE_LEN + WC_ADDR_LEN ); \ + WcPut1PayloadByte(WC_REQ_READ_REMOTE_RSSI); \ + WcPut1PayloadByte(addr[0]); \ + WcPut1PayloadByte(addr[1]); \ + WcPut1PayloadByte(addr[2]); \ + WcPut1PayloadByte(addr[3]); \ + WcPut1PayloadByte(addr[4]); \ + WcPut1PayloadByte(addr[5]); \ + WcEnd() \ + } \ + +#define WcSendReqReadLocalRssi(addr) \ + { \ + GString* str = g_string_new( "sending REQ_READ_LOCAL_RSSI " ); \ + wc_glib_append_addr(str, addr); \ + g_message(str->str); \ + g_string_free(str, TRUE); \ + WcStart(); \ + WcPut1PayloadByte(WC_CTL_BYTE_LEN + WC_ADDR_LEN ); \ + WcPut1PayloadByte(WC_REQ_READ_LOCAL_RSSI); \ + WcPut1PayloadByte(addr[0]); \ + WcPut1PayloadByte(addr[1]); \ + WcPut1PayloadByte(addr[2]); \ + WcPut1PayloadByte(addr[3]); \ + WcPut1PayloadByte(addr[4]); \ + WcPut1PayloadByte(addr[5]); \ + WcEnd() \ + } \ + +#define WcSendReqReadPhyconfig() \ + { \ + g_message("sending REQ_READ_PHYCONFIG "); \ + WcStart(); \ + WcPut1PayloadByte(WC_CTL_BYTE_LEN ); \ + WcPut1PayloadByte(WC_REQ_READ_PHYCONFIG); \ + WcEnd() \ + } \ + +#define WcSendReqSelectPhyconfig(cfg) \ + { \ + g_message("sending REQ_SELECT_PHYCONFIG "); \ + WcStart(); \ + WcPut1PayloadByte(WC_CTL_BYTE_LEN + 2); \ + WcPut1PayloadByte(WC_REQ_SELECT_PHYCONFIG); \ + WcPut1PayloadByte(cfg>>8); \ + WcPut1PayloadByte(cfg&0xff); \ + WcEnd() \ + } \ + +#define WcSendReqReadChannel() \ + { \ + g_message("sending REQ_READ_CHANNEL "); \ + WcStart(); \ + WcPut1PayloadByte(WC_CTL_BYTE_LEN ); \ + WcPut1PayloadByte(WC_REQ_READ_CHANNEL); \ + WcEnd() \ + } \ + +#define WcSendReqSelectChannel(channel) \ + { \ + g_message("sending REQ_SELECT_CHANNEL "); \ + WcStart(); \ + WcPut1PayloadByte(WC_CTL_BYTE_LEN + 1); \ + WcPut1PayloadByte(WC_REQ_SELECT_CHANNEL); \ + WcPut1PayloadByte(channel); \ + WcEnd() \ + } \ + + +#endif /* WAVECARD_H */ diff --git a/sw/airborne/autopilot/wavecard_glib.c b/sw/airborne/autopilot/wavecard_glib.c new file mode 100644 index 0000000000..0eb2891e52 --- /dev/null +++ b/sw/airborne/autopilot/wavecard_glib.c @@ -0,0 +1,355 @@ +// gcc -Wall -o wavecard.c wavecard_glib.c `pkg-config --cflags glib-2.0` `pkg-config --libs glib-2.0` +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include "wavecard.h" +#include "wavecard_glib.h" +uint8_t t1_addr[WC_ADDR_LEN] = { 0x01, 0x18, 0x04, 0xc0, 0x01, 0x34 }; +//uint8_t gs_addr[WC_ADDR_LEN] = { 0x01, 0x18, 0x04, 0xc0, 0x00, 0x4f }; +uint8_t gs_addr[WC_ADDR_LEN] = { 0x01, 0x18, 0x04, 0xc0, 0x01, 0x2d }; +uint8_t* hosts_adress[] = { t1_addr, gs_addr}; +uint8_t nb_hosts = 2; + +struct WcGlib wc_glib; +static void wc_glib_setup_sig_handler( void ); +static void wc_glib_quit( int i ); +static guint usage (int argc, char** argv); +static guint parse_options (int argc, char** argv); + + + +static GIOChannel* open_serial_port( const gchar* serial_dev) { + GIOChannel* input_channel; + struct termios orig_termios; + struct termios cur_termios; + int fd = open(serial_dev, O_RDWR | O_NONBLOCK); + + if (fd == -1) { + g_message("opening serial device %s : %s", serial_dev, strerror(errno)); + return NULL; + } + + if (tcgetattr(fd, &orig_termios)) { + g_message("getting serial device attr (%s) : %s", serial_dev, strerror(errno)); + return NULL; + } + cur_termios = orig_termios; + + /* input modes */ + cur_termios.c_iflag &= ~(IGNBRK|BRKINT|IGNPAR|PARMRK|INPCK|ISTRIP|INLCR|IGNCR + |ICRNL |IUCLC|IXON|IXANY|IXOFF|IMAXBEL); + /* pas IGNCR sinon il vire les 0x0D */ + cur_termios.c_iflag |= BRKINT; + + /* output_flags */ + cur_termios.c_oflag &=~(OPOST|OLCUC|ONLCR|OCRNL|ONOCR|ONLRET|OFILL|OFDEL); + + /* control modes */ + cur_termios.c_cflag &= ~(CSIZE|CSTOPB|CREAD|PARENB|PARODD|HUPCL|CLOCAL|CRTSCTS); + cur_termios.c_cflag |= CREAD|CS8|CLOCAL; + + /* local modes */ + cur_termios.c_lflag &= ~(ISIG|ICANON|IEXTEN|ECHO|FLUSHO|PENDIN); + cur_termios.c_lflag |= NOFLSH; + + if (cfsetispeed(&cur_termios, B9600)) { + g_message("setting serial device speed (%s) : %s", serial_dev, strerror(errno)); + return NULL; + } + + if (tcsetattr(fd, TCSADRAIN, &cur_termios)) { + g_message("setting serial device attr (%s) : %s", serial_dev, strerror(errno)); + return NULL; + } + + input_channel = g_io_channel_unix_new(fd); + g_io_channel_set_encoding(input_channel, NULL, NULL); + return input_channel; +} + +static void print_hex(gsize len, guchar* buf) { + const char d2h[] = "0123456789ABCDEF"; + int i=0; + for (i=0; istr); + g_string_free(str, TRUE); + + if (!memcmp(&wc_payload[2], gs_addr, WC_ADDR_LEN)) + wc_glib.distant_addr = t1_addr; + else + wc_glib.distant_addr = gs_addr; + + break; + case WC_RADIO_PARAM_SWITCH_MODE_STATUS: + g_message("switch_mode_status %s", wc_payload[2]?"ON":"OFF"); + break; + default: + g_message("radio param %02x %d", wc_glib.radio_param, wc_payload[2]); + break; + } + } + else + g_message("WC_RES_READ_RADIO_PARAM failed"); + break; + case WC_RES_WRITE_RADIO_PARAM : + g_message("WC_RES_WRITE_RADIO_PARAM %s", wc_payload[1]?"FAILED":"OK"); + break; + case WC_RES_SEND_SERVICE : + g_message("got WC_RES_SEND_SERVICE"); + break; + case WC_SERVICE_RESPONSE : + g_message("got WC_SERVICE_RESPONSE"); + printf("addr : "); wc_glib_print_addr(&wc_payload[1]); + printf("\n"); + g_message("acquitement %d", wc_payload[7]); + g_message("type %d", wc_payload[8]); + g_message("rssi %d", wc_payload[9]); + g_message("periode reveil %d", wc_payload[10]); + g_message("type_2 %d", wc_payload[11]); + break; + + case WC_RES_SEND_FRAME : + g_message("got WC_RES_SEND_FRAME"); + break; + + case WC_RES_READ_REMOTE_RSSI : + g_message("got WC_RES_READ_REMOTE_RSSI %d", wc_payload[1] ); + break; + + case WC_RES_READ_LOCAL_RSSI : + g_message("got WC_RES_READ_LOCAL_RSSI %d", wc_payload[1] ); + break; + + case WC_RECEIVED_FRAME : + g_message("got WC_RECEIVED_FRAME"); + GString* str = g_string_new("sender_address "); + wc_glib_append_addr(str, &wc_payload[1]); + g_string_append_printf(str, "data : %*s ", wc_length - WC_ADDR_LEN, &wc_payload[1+WC_ADDR_LEN]); + g_message(str->str); + g_string_free(str, TRUE); + break; + + case WC_ACK : + g_message("got ACK"); + break; + + case WC_NAK : + g_message("got NAK"); + break; + + default: + g_message("received unhandled %02x", wc_payload[0]); + } +} + +static gboolean on_data_received(GIOChannel *source, GIOCondition condition, gpointer data) { + guchar buf[128]; + gsize len; + GError *err = NULL; + g_io_channel_read_chars(source, buf, 3, &len, &err); + int i; + // g_message("received"); + // print_both(len, buf); + for (i=0; i 7) { + static uint8_t foo = 1; + hello[12] = foo++; + if (!wc_glib.options.silent) WcSendMsg(wc_glib.distant_addr, strlen(hello), hello); + } + foo1++; + + return TRUE; +} + +int main (int argc, char** argv) { + + if (parse_options(argc, argv)) + return usage(argc, argv); + + wc_glib_setup_sig_handler(); + wc_glib.in_ch = open_serial_port(wc_glib.options.serial_device); + in_ch = wc_glib.in_ch; + g_io_add_watch (wc_glib.in_ch, G_IO_IN , on_data_received, NULL); + g_timeout_add(2000, timeout_callback, in_ch); + wc_glib.ml = g_main_loop_new(NULL, FALSE); + wc_glib.distant_addr = t1_addr; + g_main_loop_run(wc_glib.ml); + + return 0; +} + +static void wc_glib_setup_sig_handler( void ) { + struct sigaction act; + act.sa_handler = wc_glib_quit; + act.sa_restorer = NULL; + if (sigaction(SIGINT, &act, NULL) < 0) + g_message("Could not install signal handler for quitting\n i will be unable to close log\n"); +} + +static void wc_glib_quit( int i ) { + printf("\nquitting\n"); + g_main_loop_quit(wc_glib.ml); +} + +static guint usage (int argc, char** argv) { + g_message("%s [-h] [-d serial_device]", argv[0]); + return -1; +} + + +static guint parse_options (int argc, char** argv) { + const gchar *optstr = "h:d:s"; + gchar ch; + wc_glib.options.serial_device = "/dev/ttyS0"; + wc_glib.options.silent = FALSE; + while ((ch=getopt(argc,argv,optstr)) != -1) { + switch(ch) { + case 'h': + return -1; + case 'd': + wc_glib.options.serial_device = strdup(optarg); + break; + case 's': + wc_glib.options.silent = TRUE; + break; + case '?': + printf("unrecognized option: %c\n",optopt); + return -1; + default: + printf("error? condition unaccounted for?\n"); + break; + } + } + g_message("%s starting", argv[0]); + g_message(" serial device : %s", wc_glib.options.serial_device); + return 0; +} diff --git a/sw/airborne/autopilot/wavecard_glib.h b/sw/airborne/autopilot/wavecard_glib.h new file mode 100644 index 0000000000..cfd74c6a5c --- /dev/null +++ b/sw/airborne/autopilot/wavecard_glib.h @@ -0,0 +1,34 @@ +#ifndef WAVECARD_GLIB_H +#define WAVECARD_GLIB_H + +#include +#include + +#include "wavecard.h" + +struct WcGlibOptions { + gchar* serial_device; + gboolean silent; +}; + +struct WcGlib { + GMainLoop* ml; + GIOChannel* in_ch; + uint8_t radio_param; + uint8_t self_addr[WC_ADDR_LEN]; + uint8_t* distant_addr; + struct WcGlibOptions options; +}; + +GIOChannel* in_ch; + +extern struct WcGlib wc_glib; + +#define WcPut1CtlByte(_byte) { \ + int bytes_written; \ + uint8_t b = _byte; \ + g_io_channel_write_chars (wc_glib.in_ch, &b, 1, &bytes_written, NULL); \ + g_io_channel_flush(wc_glib.in_ch, NULL); \ +} + +#endif /* WAVECARD_GLIB_H */