diff --git a/sw/airborne/main_ap.c b/sw/airborne/main_ap.c index a5f9a2202d..30ceb15878 100644 --- a/sw/airborne/main_ap.c +++ b/sw/airborne/main_ap.c @@ -536,6 +536,7 @@ void init_ap( void ) { } #ifdef WAVECARD wc_end_reset(); + wc_configure(); #endif } diff --git a/sw/airborne/wavecard.c b/sw/airborne/wavecard.c index 57e97a84ad..a5e487a76c 100644 --- a/sw/airborne/wavecard.c +++ b/sw/airborne/wavecard.c @@ -37,16 +37,6 @@ #define WC_RESET_PIN 1 #define WC_RESET_PORT PORTD -void wc_reset( void ) { - WC_RESET_DDR |= _BV(WC_RESET_PIN); - sbi(WC_RESET_PORT, WC_RESET_PIN); -} - -void wc_end_reset( void ) { - cbi(WC_RESET_PORT, WC_RESET_PIN); -} - - #define WC_MAX_PAYLOAD 256 uint8_t wc_payload[WC_MAX_PAYLOAD]; @@ -70,6 +60,22 @@ bool_t waiting_ack, wc_msg_received; uint8_t wc_protocol_error, wc_ovrn, wc_error; +void wc_reset( void ) { + WC_RESET_DDR |= _BV(WC_RESET_PIN); + sbi(WC_RESET_PORT, WC_RESET_PIN); +} + +void wc_end_reset( void ) { + cbi(WC_RESET_PORT, WC_RESET_PIN); +} + +void wc_configure( void ) { + /** Set a minimal awakening period (20ms, c.f. manual p. 17) */ + WcSendWriteRadioParamReq(0x00, 0); + waiting_ack = TRUE; +} + + /** Delayed ACK */ SIGNAL(SIG_OUTPUT_COMPARE1B) { WcSendAck(); @@ -111,7 +117,6 @@ void wc_parse_payload() { } static inline void parse_wc( uint8_t c ) { - // printf("s=%d\n", wc_status); switch (wc_status) { case UNINIT: if (c == WC_SYNC) diff --git a/sw/airborne/wavecard.h b/sw/airborne/wavecard.h index 28612a199c..2a15100f55 100644 --- a/sw/airborne/wavecard.h +++ b/sw/airborne/wavecard.h @@ -35,7 +35,7 @@ #define WcPut1CtlByte(x) uart0_transmit(x) #endif -#define g_message(_) +#define g_message(_,...) #define WC_CTL_BYTE_LEN 4 #define WC_ADDR_LEN 6 @@ -48,6 +48,7 @@ void wc_parse_payload(void); void wc_reset( void ); void wc_end_reset( void ); +void wc_configure( void ); #define WC_SYNC 0xff diff --git a/sw/ground_segment/tmtc/link.ml b/sw/ground_segment/tmtc/link.ml index 828931cff3..42e7fb899a 100644 --- a/sw/ground_segment/tmtc/link.ml +++ b/sw/ground_segment/tmtc/link.ml @@ -102,7 +102,92 @@ let use_tele_message = fun payload -> let msg = Tm_Pprz.message_of_id msg_id in Tm_Pprz.message_send (string_of_int ac_id) msg.Pprz.name values -let send = fun device ac_device payload -> + +type priority = Null | Low | Normal | High + +(******** Wavecard ******************************************************) +module Wc = struct + type status = Ready | Busy + + let buffer_size = 5 + let null_buffer_entry = (Null, Unix.stdout, (W.ACK, "")) + let priority_of = fun (p, _, _) -> p + let buffer = (ref Ready, Array.create buffer_size null_buffer_entry) + let flush = fun () -> + let status, b = buffer in + if !status = Ready then + let (priority, fd, cmd) = b.(0) in + if priority <> Null then begin + status := Busy; + W.send fd cmd; + for i = 0 to buffer_size - 2 do (** A circular buf would be better *) + b.(i) <- b.(i+1) + done; + b.(buffer_size-1) <- null_buffer_entry + end + + let buffer_ready = fun () -> + let (status, _) = buffer in + status := Ready; + flush () + + + + let send_buffered = fun fd cmd priority -> + let status, b = buffer in + if !status = Ready then begin + status := Busy; + W.send fd cmd + end else + (** Set the message in the right place in the buffer *) + let rec loop = fun i -> + if i < buffer_size then + if priority_of b.(i) > priority + then loop (i+1) + else begin + for j = i + 1 to buffer_size - 1 do (** Shift *) + b.(j) <- b.(j-1) + done; + b.(i) <- (priority, fd, cmd) + end in + loop 0; + flush () + + let send = fun fd addr payload priority -> + let data = W.addressed addr (Serial.string_of_payload payload) in + send_buffered fd (W.REQ_SEND_MESSAGE, data) priority + + let ack_delay = 10 (* ms *) + let send_ack = fun fd () -> + ignore (GMain.Timeout.add ack_delay (fun _ -> W.send fd (W.ACK, ""); false)) + let use_message = fun (com, data) -> + match com with + W.RECEIVED_FRAME -> + use_tele_message (Serial.payload_of_string data) + | W.RES_READ_REMOTE_RSSI -> + Tm_Pprz.message_send "link" "WC_RSSI" ["raw_level", Pprz.Int (Char.code data.[0])]; + Debug.call 'w' (fun f -> fprintf f "wv remote RSSI %d\n" (Char.code data.[0])); + | W.RES_READ_RADIO_PARAM -> + Ivy.send (sprintf "WC_ADDR %s" data); + Debug.call 'w' (fun f -> fprintf f "wv local addr : %s\n" (Debug.xprint data)); + | W.ACK -> + Debug.trace 'w' "wv ACK"; + buffer_ready () + | _ -> + Debug.call 'w' (fun f -> fprintf f "wv receiving: %02x %s\n" (W.code_of_cmd com) (Debug.xprint data)); + () + + let rssi_period = 5000 (** ms *) + let req_rssi = fun device addr -> + let data = W.addressed addr "" in + send_buffered device.fd (W.REQ_READ_REMOTE_RSSI, data) Low +end (** Wc module *) + + + + + +let send = fun device ac_device payload priority -> match ac_device with Uart -> let o = Unix.out_channel_of_descr device.fd in @@ -110,7 +195,7 @@ let send = fun device ac_device payload -> Printf.fprintf o "%s" buf; flush o; Debug.call 'l' (fun f -> fprintf f "mm sending: %s\n" (Debug.xprint buf)); | WavecardDevice addr -> - W.send_addressed device.fd (W.REQ_SEND_MESSAGE, addr, Serial.string_of_payload payload) + Wc.send device.fd addr payload priority | _ -> failwith "send: not yet" @@ -142,7 +227,7 @@ let get_fp = fun device _sender vs -> "speed", cm_of_m gspeed] in let msg_id, _ = Dl_Pprz.message_of_name "ACINFO" in let s = Dl_Pprz.payload_of_values msg_id my_id vs in - send device ac_device s + send device ac_device s Low with NotSendingToThis -> ()) airframes @@ -166,7 +251,7 @@ let move_wp = fun device _sender vs -> "alt", cm_of_m alt] in let msg_id, _ = Dl_Pprz.message_of_name "MOVE_WP" in let s = Dl_Pprz.payload_of_values msg_id my_id vs in - send device ac_device s + send device ac_device s High with NotSendingToThis -> () @@ -179,7 +264,7 @@ let send_event = fun device _sender vs -> let vs = ["event", Pprz.Int ev_id] in let msg_id, _ = Dl_Pprz.message_of_name "EVENT" in let s = Dl_Pprz.payload_of_values msg_id my_id vs in - send device ac_device s + send device ac_device s High with NotSendingToThis -> () @@ -193,7 +278,7 @@ let setting = fun device _sender vs -> let vs = ["index", Pprz.Int idx; "value", List.assoc "value" vs] in let msg_id, _ = Dl_Pprz.message_of_name "SETTING" in let s = Dl_Pprz.payload_of_values msg_id my_id vs in - send device ac_device s + send device ac_device s High with NotSendingToThis -> () @@ -207,7 +292,7 @@ let jump_block = fun device _sender vs -> let vs = ["block_id", Pprz.Int block_id] in let msg_id, _ = Dl_Pprz.message_of_name "BLOCK" in let s = Dl_Pprz.payload_of_values msg_id my_id vs in - send device ac_device s + send device ac_device s High with NotSendingToThis -> () @@ -222,7 +307,7 @@ let raw_datalink = fun device _sender vs -> done; let msg_id, vs = Dl_Pprz.values_of_string m in let s = Dl_Pprz.payload_of_values msg_id my_id vs in - send device ac_device s + send device ac_device s Normal with NotSendingToThis -> () @@ -293,24 +378,6 @@ module PprzModem = struct end (* PprzModem module *) -let wc_ack_delay = 10 (* ms *) -let wc_send_ack = fun fd () -> - ignore (GMain.Timeout.add wc_ack_delay (fun _ -> W.send fd (W.ACK, ""); false)) -let wc_use_message = fun (com, data) -> - match com with - W.RECEIVED_FRAME -> - use_tele_message (Serial.payload_of_string data) - | W.RES_READ_REMOTE_RSSI -> - Tm_Pprz.message_send "link" "WC_RSSI" ["raw_level", Pprz.Int (Char.code data.[0])]; - Debug.call 'w' (fun f -> fprintf f "wv remote RSSI %d\n" (Char.code data.[0])); - | W.RES_READ_RADIO_PARAM -> - Ivy.send (sprintf "WC_ADDR %s" data); - Debug.call 'w' (fun f -> fprintf f "wv local addr : %s\n" (Debug.xprint data)); - | _ -> - Debug.call 'w' (fun f -> fprintf f "wv receiving: %02x %s\n" (W.code_of_cmd com) (Debug.xprint data));; -let rssi_period = 10000 (** ms *) -let req_rssi = fun device addr -> - Wavecard.send_addressed device.fd (W.REQ_READ_REMOTE_RSSI, addr,"") @@ -322,7 +389,7 @@ let parse_of_transport device = function let module ModemTransport = Serial.Transport(Modem.Protocol) in ModemTransport.parse PprzModem.use_message | Wavecard -> - fun buf -> Wavecard.parse buf ~ack:(wc_send_ack device.fd) (wc_use_message) + fun buf -> Wavecard.parse buf ~ack:(Wc.send_ack device.fd) (Wc.use_message) | _ -> failwith "parse_of_transport: not yet" @@ -332,12 +399,12 @@ let _ = let baurate = ref "9600" in let transport = ref "pprz" in let uplink = ref false in - let rssi_addr = ref "" in + let rssi_id = ref (-1) in let options = [ "-b", Arg.Set_string ivy_bus, (sprintf "Ivy bus (%s)" !ivy_bus); "-d", Arg.Set_string port, (sprintf "Port (%s)" !port); - "-rssi", Arg.Set_string rssi_addr, (sprintf "Rssi addr (%s)" !rssi_addr); + "-rssi", Arg.Set_int rssi_id, (sprintf "Rssi A/C id"); "-transport", Arg.Set_string transport, (sprintf "Transport (%s): modem,pprz,wavecard" !transport); "-uplink", Arg.Set uplink, (sprintf "Uplink (%b)" !uplink); "-s", Arg.Set_string baurate, (sprintf "Baudrate (%s)" !baurate)] in @@ -388,22 +455,31 @@ let _ = ignore (Ground_Pprz.message_bind "RAW_DATALINK" (raw_datalink device)) end; - if transport = Wavecard then begin - (* request own address *) - let s = String.make 1 (char_of_int 5) in - Wavecard.send device.fd (W.REQ_READ_RADIO_PARAM, s) - end; - (** Periodic tasks *) + (** Init and Periodic tasks *) begin match transport with Modem -> (** Sending periodically modem and downlink status messages *) ignore (Glib.Timeout.add PprzModem.msg_period (fun () -> PprzModem.send_msg (); true)) | Wavecard -> - if !rssi_addr <> "" then - let addr = W.addr_of_string !rssi_addr in - ignore (GMain.Timeout.add rssi_period (fun _ -> req_rssi device addr; true)); + (** Set the wavecard in short wakeup mode *) + let data = String.create 2 in + data.[0] <- Char.chr (W.code_of_config_param W.WAKEUP_TYPE); + data.[1] <- Char.chr (W.code_of_wakeup_type W.SHORT_WAKEUP); + W.send fd (W.REQ_WRITE_RADIO_PARAM,data); + + (* request own address *) + let s = String.make 1 (char_of_int 5) in + ignore (GMain.Timeout.add 500 (fun _ -> W.send device.fd (W.REQ_READ_RADIO_PARAM, s); false)); + + (** Ask for rssi if required *) + if !rssi_id > 0 then begin + match airborne_device !rssi_id airframes transport with + WavecardDevice addr -> + ignore (GMain.Timeout.add Wc.rssi_period (fun _ -> Wc.req_rssi device addr; true)) + | _ -> failwith (sprintf "Rssi not supported by A/C '%d'" !rssi_id) + end | _ -> () end; diff --git a/sw/lib/ocaml/debug.ml b/sw/lib/ocaml/debug.ml index c37cdbcd54..2190cd9b39 100644 --- a/sw/lib/ocaml/debug.ml +++ b/sw/lib/ocaml/debug.ml @@ -34,6 +34,8 @@ let call lev f = flush !log end; true) + +let trace lev s = call lev (fun f -> Printf.fprintf f "%s\n" s) let xprint = fun s -> let n = String.length s in diff --git a/sw/lib/ocaml/debug.mli b/sw/lib/ocaml/debug.mli index 32e8182f04..183169b845 100644 --- a/sw/lib/ocaml/debug.mli +++ b/sw/lib/ocaml/debug.mli @@ -46,5 +46,7 @@ val call : char -> (out_channel -> unit) -> unit (** No debug information is output if the program was compiled with the -noassert flag *) +val trace : char -> string -> unit + val xprint : string -> string (** Returns the hexadecimal representation of a string *) diff --git a/sw/lib/ocaml/wavecard.ml b/sw/lib/ocaml/wavecard.ml index a7112127d2..cc5ae22230 100644 --- a/sw/lib/ocaml/wavecard.ml +++ b/sw/lib/ocaml/wavecard.ml @@ -113,6 +113,20 @@ let rec cossa = fun x l -> let cmd_of_code = fun x -> try List.assoc x cmd_names with Not_found -> failwith (sprintf "Unknown command: %2x" x) let code_of_cmd = fun x -> cossa x cmd_names + +type config_param = + AWAKENING_PERIOD + | WAKEUP_TYPE + | WAKEUP_LENGTH + +let code_of_config_param = fun x -> Obj.magic x + +type wakeup_type = + LONG_WAKEUP + | SHORT_WAKEUP + +let code_of_wakeup_type = fun x -> Obj.magic x + let sync = Char.chr 0xff let stx = Char.chr 0x02 let etx = Char.chr 0x03 @@ -203,9 +217,12 @@ let addr_length = 6 let addr_of_string = Int64.of_string let string_of_addr = Int64.to_string -let send_addressed = fun fd (cmd, addr, data) -> +let addressed = fun addr data -> let s = String.create addr_length in for i = 0 to addr_length - 1 do s.[addr_length-i-1] <- Char.chr (Int64.to_int (Int64.shift_right addr (8*i)) land 0xff) done; - send fd (cmd, s^data) + s^data + +let send_addressed = fun fd (cmd, addr, data) -> + send fd (cmd, addressed addr data) diff --git a/sw/lib/ocaml/wavecard.mli b/sw/lib/ocaml/wavecard.mli index 58336b763a..a2ca427d27 100644 --- a/sw/lib/ocaml/wavecard.mli +++ b/sw/lib/ocaml/wavecard.mli @@ -63,10 +63,22 @@ type cmd_name = | SERVICE_RESPONSE (** available commands *) - val code_of_cmd : cmd_name -> int (** Code of command *) +type config_param = + AWAKENING_PERIOD + | WAKEUP_TYPE + | WAKEUP_LENGTH + +val code_of_config_param : config_param -> int + +type wakeup_type = + LONG_WAKEUP + | SHORT_WAKEUP + +val code_of_wakeup_type : wakeup_type -> int + type data = string type cmd = cmd_name * data (** A command is composed of a command name and some untyped data *) @@ -80,6 +92,7 @@ val addr_of_string : string -> addr val send : Unix.file_descr -> cmd -> unit (** Send a command on the channel connected to the serial port of the wavecard *) +val addressed : addr -> data -> data val send_addressed : Unix.file_descr -> (cmd_name*addr*data) -> unit (** [send_addressed fd (cmd, a, data)] Sends [cmd] with data obtained by concatenation of codinf of [a] and [data] *)