diff --git a/conf/flight_plans/muret_mini.xml b/conf/flight_plans/muret_mini.xml index 045a82d01f..021c7b3652 100644 --- a/conf/flight_plans/muret_mini.xml +++ b/conf/flight_plans/muret_mini.xml @@ -1,4 +1,4 @@ - + @@ -17,27 +17,8 @@ - - - - - - - - - - - - - - - - - - - diff --git a/sw/airborne/autopilot/Makefile b/sw/airborne/autopilot/Makefile index 6de5117d1d..9c3eaeb211 100644 --- a/sw/airborne/autopilot/Makefile +++ b/sw/airborne/autopilot/Makefile @@ -24,7 +24,7 @@ FBW=../fly_by_wire -LOCAL_CFLAGS= $(CTL_BRD_FLAGS) $(GPS_FLAGS) $(SIMUL_FLAGS) +LOCAL_CFLAGS= $(CTL_BRD_FLAGS) $(GPS_FLAGS) $(SIMUL_FLAGS) -DWAVECARD_ON_GPS VARINCLUDE=$(PAPARAZZI_HOME)/var/include ACINCLUDE = $(PAPARAZZI_HOME)/var/$(AIRCRAFT) @@ -63,7 +63,8 @@ $(TARGET).srcs = \ mainloop.c \ cam.c \ traffic_info.c \ - datalink.c + datalink.c \ + wavecard.c ifeq ($(AIRCRAFT), Gorazoptere_brushless_ANALOG) $(TARGET).srcs += ahrs.S diff --git a/sw/airborne/autopilot/datalink.c b/sw/airborne/autopilot/datalink.c index 2a94b80811..5faa594ffb 100644 --- a/sw/airborne/autopilot/datalink.c +++ b/sw/airborne/autopilot/datalink.c @@ -32,28 +32,34 @@ #include "traffic_info.h" #include "nav.h" #include "datalink.h" +#include "flight_plan.h" #define MOfCm(_x) (((float)_x)/100.) -#define MSG_SIZE 128 -char dl_buffer[MSG_SIZE]; +/* void dl_parse_msg(void) { */ +/* uint8_t msg_id = dl_buffer[0]; */ +/* if (msg_id == DL_ACINFO_ID) { */ +/* uint8_t id = DL_ACINFO_ac_id(dl_buffer); */ +/* float ux = MOfCm(DL_ACINFO_utm_east(dl_buffer)); */ +/* float uy = MOfCm(DL_ACINFO_utm_north(dl_buffer)); */ +/* float a = MOfCm(DL_ACINFO_alt(dl_buffer)); */ +/* float c = RadOfDeg(((float)DL_ACINFO_course(dl_buffer))/ 10.); */ +/* float s = MOfCm(DL_ACINFO_speed(dl_buffer)); */ +/* SetAcInfo(id, ux, uy, c, a, s); */ +/* } else if (msg_id == DL_MOVE_WP_ID) { */ +/* uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer); */ +/* float ux = MOfCm(DL_MOVE_WP_utm_east(dl_buffer)); */ +/* float uy = MOfCm(DL_MOVE_WP_utm_north(dl_buffer)); */ +/* float a = MOfCm(DL_MOVE_WP_alt(dl_buffer)); */ +/* MoveWaypoint(wp_id, ux, uy, a); */ +/* } */ +/* } */ -static uint8_t msg_id; - +#include "uart.h" void dl_parse_msg(void) { - if (msg_id == DL_ACINFO_ID) { - uint8_t id = DL_ACINFO_ac_id(dl_buffer); - float ux = MOfCm(DL_ACINFO_utm_east(dl_buffer)); - float uy = MOfCm(DL_ACINFO_utm_north(dl_buffer)); - float a = MOfCm(DL_ACINFO_alt(dl_buffer)); - float c = RadOfDeg(((float)DL_ACINFO_course(dl_buffer))/ 10.); - SetAcInfo(id, ux, uy, c, a); - } else if (msg_id == DL_MOVE_WP_ID) { - uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer); - float ux = MOfCm(DL_MOVE_WP_utm_east(dl_buffer)); - float uy = MOfCm(DL_MOVE_WP_utm_north(dl_buffer)); - float a = MOfCm(DL_MOVE_WP_alt(dl_buffer)); - MoveWaypoint(wp_id, ux, uy, a); - } + dl_buffer[6] = '\0'; + uart0_print_string(dl_buffer); + uart0_transmit('\n'); } + diff --git a/sw/airborne/autopilot/datalink.h b/sw/airborne/autopilot/datalink.h index 2ed300a819..e90f9d72df 100644 --- a/sw/airborne/autopilot/datalink.h +++ b/sw/airborne/autopilot/datalink.h @@ -36,6 +36,10 @@ EXTERN bool_t dl_msg_available; -EXTERN void dl_parse_msg(void); +#define MSG_SIZE 128 +EXTERN char dl_buffer[MSG_SIZE]; + +void dl_parse_msg(void); /** Should be called when dl_msg_available is set */ + #endif diff --git a/sw/airborne/autopilot/gps_ubx.c b/sw/airborne/autopilot/gps_ubx.c index e2b592488b..bb6b8ce0eb 100644 --- a/sw/airborne/autopilot/gps_ubx.c +++ b/sw/airborne/autopilot/gps_ubx.c @@ -208,7 +208,9 @@ static inline void parse_ubx( uint8_t c ) { #ifdef SIMUL ReceiveUart0(parse_ubx); -#else +#endif + +#ifndef WAVECARD_ON_GPS ReceiveUart1(parse_ubx); #endif diff --git a/sw/airborne/autopilot/mainloop.c b/sw/airborne/autopilot/mainloop.c index 2959fadf63..bbda2d2504 100644 --- a/sw/airborne/autopilot/mainloop.c +++ b/sw/airborne/autopilot/mainloop.c @@ -41,6 +41,7 @@ #include "downlink.h" #include "uart.h" #include "datalink.h" +#include "wavecard.h" #ifdef SECTION_IMU_ANALOG #include "ahrs.h" @@ -113,6 +114,10 @@ int main( void ) { gps_pos_available = FALSE; } } + if (wc_msg_received) { + wc_parse_payload(); + wc_msg_received = FALSE; + } if (dl_msg_available) { dl_parse_msg(); dl_msg_available = FALSE; diff --git a/sw/airborne/autopilot/uart.c b/sw/airborne/autopilot/uart.c index d562a3aa94..17b54f2e2d 100644 --- a/sw/airborne/autopilot/uart.c +++ b/sw/airborne/autopilot/uart.c @@ -120,9 +120,10 @@ SIGNAL(SIG_UART1_TRANS) { void uart0_init( void ) { /* Baudrate is 38.4k */ - UBRR0H = 0; - UBRR0L = 25; // 38.4 + UBRR0H = 0; // UBRR0L = 103; //9600 + UBRR0L = 25; // 38.4 + /* single speed */ UCSR0A = 0; /* Enable receiver and transmitter */ @@ -136,8 +137,11 @@ void uart0_init( void ) { void uart1_init( void ) { /* Baudrate is 38.4k */ UBRR1H = 0; +#ifdef WAVECARD_ON_GPS + UBRR1L = 103; //9600 +#else UBRR1L = 25; // 38.4 - // UBRR1L = 103; //9600 +#endif /* single speed */ diff --git a/sw/airborne/autopilot/wavecard.c b/sw/airborne/autopilot/wavecard.c new file mode 100644 index 0000000000..5403aed9d4 --- /dev/null +++ b/sw/airborne/autopilot/wavecard.c @@ -0,0 +1,160 @@ +/* + * Paparazzi mcu0 $Id$ + * + * Copyright (C) 2005 Pascal Brisset, Antoine Drouin + * + * This file is part of paparazzi. + * + * paparazzi 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, or (at your option) + * any later version. + * + * paparazzi 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 paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +/* Coronis wavecard serial input and output */ + +#include +#include +#include +#include + +#include "std.h" +#include "link_autopilot.h" +#include "uart.h" +#include "wavecard.h" +#include "datalink.h" + +#define WC_MAX_PAYLOAD 256 +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; + + +/** Delayed ACK */ +SIGNAL(SIG_OUTPUT_COMPARE1B) { + WcSendAck(); + cbi(TIMSK, OCIE1B); +} + +inline void delayed_send_ack( void ) { + OCR1B = TCNT1 + 1000*CLOCK; /* 1ms delay */ + /* clear interrupt flag */ + sbi(TIFR, OCF1B); + /* enable OC1B interrupt */ + sbi(TIMSK, OCIE1B); +} + +void wc_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; + case WC_RECEIVED_FRAME : { + uint8_t i; + for(i = 0; i < wc_length-4-WC_ADDR_LEN; i++) + dl_buffer[i] = wc_payload[i+WC_ADDR_LEN+1]; + dl_msg_available = TRUE; + delayed_send_ack(); + break; + } + + default: + delayed_send_ack(); + } +} + +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; +} + +#ifdef WAVECARD_ON_GPS +ReceiveUart1(parse_wc); +#else +ReceiveUart0(parse_wc); +#endif diff --git a/sw/airborne/autopilot/wavecard.c.old b/sw/airborne/autopilot/wavecard.c.old deleted file mode 100644 index 4e366e0f4e..0000000000 --- a/sw/airborne/autopilot/wavecard.c.old +++ /dev/null @@ -1,112 +0,0 @@ -#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.old b/sw/airborne/autopilot/wavecard.h similarity index 88% rename from sw/airborne/autopilot/wavecard.h.old rename to sw/airborne/autopilot/wavecard.h index 1233d2b671..c3bc5d44dc 100644 --- a/sw/airborne/autopilot/wavecard.h.old +++ b/sw/airborne/autopilot/wavecard.h @@ -1,20 +1,48 @@ +/* + * Paparazzi mcu0 $Id$ + * + * Copyright (C) 2005 Pascal Brisset, Antoine Drouin + * + * This file is part of paparazzi. + * + * paparazzi 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, or (at your option) + * any later version. + * + * paparazzi 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 paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +/* Coronis wavecard serial input and output */ + #ifndef WAVECARD_H #define WAVECARD_H +#include "uart.h" + +#ifdef WAVECARD_ON_GPS +#define WcPut1CtlByte(x) uart1_transmit(x) +#else +#define WcPut1CtlByte(x) uart0_transmit(x) +#endif + +#define g_message(_) + #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); @@ -25,7 +53,6 @@ void parse_payload(void); #define WC_ETX 0x03 - #define WC_ACK 0x06 #define WC_NAK 0x15 #define WC_ERROR 0x00 @@ -77,9 +104,6 @@ void parse_payload(void); #define WC_RADIO_PARAM_SWITCH_MODE_STATUS 0x10 -void parse_wc( uint8_t); - - #define update_crc(_byte) { \ uint8_t i; \ crc ^= _byte; \ diff --git a/sw/ground_segment/tmtc/Makefile b/sw/ground_segment/tmtc/Makefile index 354e49b5bf..f3dc7e6cdd 100644 --- a/sw/ground_segment/tmtc/Makefile +++ b/sw/ground_segment/tmtc/Makefile @@ -70,6 +70,12 @@ receive : wind.cmo receive.ml ../../lib/ocaml/lib-pprz.cma @echo 'exec lablgtk2 -I $$PAPARAZZI_SRC/sw/lib/ocaml glibivy-ocaml.cma lib-pprz.cma -I $$PAPARAZZI_SRC/sw/ground_segment/tmtc $$PAPARAZZI_SRC/sw/ground_segment/tmtc/wind.cmo $$PAPARAZZI_SRC/sw/ground_segment/tmtc/receive.ml $$*' >> $@ @chmod a+x $@ +wavecard : wavecard_connect.ml ../../lib/ocaml/lib-pprz.cma + $(OCAMLC) $(INCLUDES) -o $@.out lablgtk.cma glibivy-ocaml.cma lib-pprz.cma $^ + @cat ../../../pprz_src_test.sh > $@ + @echo 'exec lablgtk2 -I $$PAPARAZZI_SRC/sw/lib/ocaml glibivy-ocaml.cma lib-pprz.cma -I $$PAPARAZZI_SRC/sw/ground_segment/tmtc $$PAPARAZZI_SRC/sw/ground_segment/tmtc/wavecard_connect.ml $$*' >> $@ + @chmod a+x $@ + modem.cmo : modem.cmi diff --git a/sw/ground_segment/tmtc/wavecard_connect.ml b/sw/ground_segment/tmtc/wavecard_connect.ml index 38c82faef1..d4ccd41e78 100644 --- a/sw/ground_segment/tmtc/wavecard_connect.ml +++ b/sw/ground_segment/tmtc/wavecard_connect.ml @@ -26,28 +26,32 @@ open Printf module W = Wavecard -module Tc_Class = struct let name = "non" end -module Tc_Pprz = Pprz.Protocol(Tc_Class) - let send_ack = fun delay fd () -> ignore (GMain.Timeout.add delay (fun _ -> W.send fd (W.ACK, ""); false)) -let send = fun fd a -> +(*** +let send = fun fd addr a -> let (id, values) = Tc_Pprz.values_of_string a in let s = Tc_Pprz.payload_of_values id values in - Wavecard.send fd (W.REQ_SEND_FRAME,s) + Wavecard.send fd (W.REQ_SEND_MESSAGE,s) +***) + +let send = fun fd addr s -> + Wavecard.send_addressed fd (W.REQ_SEND_MESSAGE,addr,s) let broadcast_msg = fun (com, data) -> + prerr_endline "bc"; match com with W.RECEIVED_FRAME -> - let id, vs = Tc_Pprz.values_of_payload data in - let m = Tc_Pprz.message_of_id id in - let s = Tc_Pprz.string_of_message m vs in - Ivy.send (sprintf "FROM_WAVECARD %s" s) + Ivy.send (sprintf "FROM_WAVECARD %s" data) | _ -> + Debug.call 'w' (fun f -> fprintf f "wv receiving: %x " (W.code_of_cmd com); for i = 0 to String.length data - 1 do fprintf f "%x " (Char.code data.[i]) done; fprintf f "\n"); Ivy.send (sprintf "RAW_FROM_WAVECARD %2x %s" (W.code_of_cmd com) data) +let addr = Wavecard.addr_of_ints [|0x01; 0x18; 0x04; 0xc0; 0x01; 0x34|] +let addr = Wavecard.addr_of_ints [|0x01; 0x18; 0x04; 0xc0; 0x01; 0x2d|] + let _ = let ivy_bus = ref "127.255.255.255:2010" in let port = ref "/dev/ttyS0" in @@ -65,14 +69,15 @@ let _ = try let fd = Serial.opendev !port Serial.B9600 in (* Listening *) + let listener = Wavecard.receive ~ack:(send_ack 10 fd) broadcast_msg in let cb = fun _ -> - Wavecard.receive ~ack:(send_ack 100 fd) broadcast_msg fd; + listener fd; true in ignore (Glib.Io.add_watch [`IN] cb (GMain.Io.channel_of_descr fd)); (* Sending request from Ivy *) - ignore (Ivy.bind (fun _ a -> send fd a.(0)) "TO_WAVECARD +(.*)"); + ignore (Ivy.bind (fun _ a -> send fd addr a.(0)) "TO_WAVECARD +(.*)"); (* Main Loop *) let loop = Glib.Main.create true in diff --git a/sw/lib/ocaml/wavecard.ml b/sw/lib/ocaml/wavecard.ml index 9c73e1f152..79da6e9b91 100644 --- a/sw/lib/ocaml/wavecard.ml +++ b/sw/lib/ocaml/wavecard.ml @@ -146,25 +146,26 @@ let compute_checksum = let checksum = fun buf -> let crc = compute_checksum buf and l = length buf in - crc land 0xff = Char.code buf.[l] && crc lsr 8 = Char.code buf.[l+1] + true || (crc land 0xff = Char.code buf.[l] && crc lsr 8 = Char.code buf.[l+1]) let parse = fun buf ?ack f -> let n = String.length buf in - if n < 3 || n < total_length buf then + Debug.call 'w' (fun f -> fprintf f "wv input: "; for i = 0 to String.length buf - 1 do fprintf f "%x " (Char.code buf.[i]) done; fprintf f "\n"); + if n < 3 || n < total_length buf then begin 0 (* Not enough chars to read *) - else if buf.[0] <> sync then + end else if buf.[0] <> sync then 1 - else if buf.[1] <> stx || not (checksum buf) then + else if buf.[1] <> stx || not (checksum buf) then begin 2 - else begin + end else begin f (payload buf); begin match ack with - None -> () - | Some ack -> ack () + Some ack when cmd_of_code (Char.code buf.[3]) <> ACK -> ack () + | _ -> () end; total_length buf end @@ -191,4 +192,21 @@ let send = fun fd (cmd, data) -> buf.[l+2] <- etx; let o = Unix.out_channel_of_descr fd in Printf.fprintf o "%s" buf; + Debug.call 'w' (fun f -> fprintf f "wv sending: "; for i = 0 to String.length buf - 1 do fprintf f "%x " (Char.code buf.[i]) done; fprintf f "\n"); flush o + +type addr = string + +let addr_length = 6 + +let addr_of_ints = fun a -> + assert(Array.length a = addr_length); + let s = String.create addr_length in + for i = 0 to addr_length - 1 do + s.[i] <- Char.chr a.(i) + done; + s + +let send_addressed = fun fd (cmd, addr, data) -> + assert(String.length addr = addr_length); + send fd (cmd, addr^data) diff --git a/sw/lib/ocaml/wavecard.mli b/sw/lib/ocaml/wavecard.mli index bea618d7b1..11f8034b34 100644 --- a/sw/lib/ocaml/wavecard.mli +++ b/sw/lib/ocaml/wavecard.mli @@ -69,3 +69,7 @@ val send : Unix.file_descr -> cmd -> unit val receive : ?ack:(unit -> unit) -> (cmd -> 'a) -> (Unix.file_descr -> unit) val code_of_cmd : cmd_name -> int + +type addr +val addr_of_ints : int array -> addr +val send_addressed : Unix.file_descr -> (cmd_name*addr*data) -> unit diff --git a/sw/tools/gen_dl.ml b/sw/tools/gen_dl.ml index b21b39dd63..b7d2980227 100644 --- a/sw/tools/gen_dl.ml +++ b/sw/tools/gen_dl.ml @@ -60,7 +60,7 @@ let parse_message = fun m -> fprintf out "\n"; define (sprintf "DL_%s_ID" msg_name) (Xml.attrib m "ID"); - let offset = ref 0 in + let offset = ref 1 in (** 1 for the tag of the message *) let rec parse_field = fun f -> match Xml.tag f with "field" ->