mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-31 20:38:27 +08:00
*** empty log message ***
This commit is contained in:
Pascal Brisset
@@ -352,7 +352,7 @@ let _ =
|
||||
load_map geomap xml_map_file
|
||||
end;
|
||||
|
||||
ignore (Glib.Timeout.add 1000 (fun () -> Ground_Pprz.message_req "map2d" "AIRCRAFTS" [] (fun _sender vs -> live_aircrafts_msg geomap vs); false));
|
||||
ignore (Glib.Timeout.add 5000 (fun () -> Ground_Pprz.message_req "map2d" "AIRCRAFTS" [] (fun _sender vs -> live_aircrafts_msg geomap vs); false));
|
||||
|
||||
ignore (Ground_Pprz.message_bind "NEW_AIRCRAFT" (fun _sender vs -> one_new_ac geomap (Pprz.string_assoc "ac_id" vs)));
|
||||
|
||||
|
||||
@@ -81,7 +81,8 @@ static float corr_space_q[CORRLEN];
|
||||
|
||||
/* ---------------------------------------------------------------------- */
|
||||
|
||||
#define INPUT_BUF_LEN 1000
|
||||
#define TEMP_BUF_LEN 10
|
||||
#define OUTPUT_BUF_LEN 512
|
||||
|
||||
|
||||
static int verbose_level = 0;
|
||||
@@ -103,12 +104,9 @@ static void
|
||||
pprz_tmtc_send(unsigned char *data, unsigned int len, char* data_received)
|
||||
{
|
||||
unsigned char i;
|
||||
/***/printf("%d:", len);
|
||||
for (i=0; i < len && glob_data_received_len < INPUT_BUF_LEN; i++) {
|
||||
for (i=0; i < len && glob_data_received_len < OUTPUT_BUF_LEN; i++) {
|
||||
data_received[glob_data_received_len++] = data[i];
|
||||
/***/printf("%02x", data[i]);
|
||||
}
|
||||
/***/printf("\ngdrl=%d\n", glob_data_received_len);
|
||||
}
|
||||
|
||||
|
||||
@@ -131,7 +129,7 @@ my_pprz_baudot_rxbit(struct demod_state *s, int bit, char* data)
|
||||
}
|
||||
}
|
||||
|
||||
if ((s->l2.hdlc.rxptr - s->l2.hdlc.rxbuf) >= INPUT_BUF_LEN) {
|
||||
if ((s->l2.hdlc.rxptr - s->l2.hdlc.rxbuf) >= TEMP_BUF_LEN) {
|
||||
pprz_tmtc_send(s->l2.hdlc.rxbuf, s->l2.hdlc.rxptr - s->l2.hdlc.rxbuf, data);
|
||||
/* reset data buffer */
|
||||
s->l2.hdlc.rxptr = s->l2.hdlc.rxbuf;
|
||||
@@ -236,74 +234,70 @@ union {
|
||||
int stereo = 1;
|
||||
unsigned int sample_rate = FREQ_SAMP;
|
||||
|
||||
int fdebug;
|
||||
|
||||
static int
|
||||
input_init(const char *ifname) {
|
||||
int sndparam;
|
||||
|
||||
fdebug=open("dump.dsp", O_WRONLY|O_CREAT);
|
||||
|
||||
if ((fd = open(ifname, O_RDONLY)) < 0) {
|
||||
perror("open");
|
||||
exit (10);
|
||||
}
|
||||
#if 0
|
||||
sndparam = AFMT_S16_LE; /* we want 16 bits/sample signed */
|
||||
/* little endian; works only on little endian systems! */
|
||||
if (ioctl(fd, SNDCTL_DSP_SETFMT, &sndparam) == -1) {
|
||||
perror("ioctl: SNDCTL_DSP_SETFMT");
|
||||
exit (10);
|
||||
}
|
||||
if (sndparam != AFMT_S16_LE) {
|
||||
fmt = 1;
|
||||
sndparam = AFMT_U8;
|
||||
if (!strncmp("/dev", ifname, 4)) {
|
||||
sndparam = AFMT_S16_LE; /* we want 16 bits/sample signed */
|
||||
/* little endian; works only on little endian systems! */
|
||||
if (ioctl(fd, SNDCTL_DSP_SETFMT, &sndparam) == -1) {
|
||||
perror("ioctl: SNDCTL_DSP_SETFMT");
|
||||
exit (10);
|
||||
}
|
||||
if (sndparam != AFMT_U8) {
|
||||
perror("ioctl: SNDCTL_DSP_SETFMT");
|
||||
if (sndparam != AFMT_S16_LE) {
|
||||
fmt = 1;
|
||||
sndparam = AFMT_U8;
|
||||
if (ioctl(fd, SNDCTL_DSP_SETFMT, &sndparam) == -1) {
|
||||
perror("ioctl: SNDCTL_DSP_SETFMT");
|
||||
exit (10);
|
||||
}
|
||||
if (sndparam != AFMT_U8) {
|
||||
perror("ioctl: SNDCTL_DSP_SETFMT");
|
||||
exit (10);
|
||||
}
|
||||
}
|
||||
stereo = TRUE;
|
||||
sndparam = 1; /* we want 2 channels */
|
||||
if (ioctl(fd, SNDCTL_DSP_STEREO, &sndparam) == -1) {
|
||||
perror("ioctl: SNDCTL_DSP_STEREO");
|
||||
exit (10);
|
||||
}
|
||||
if (sndparam == 0) {
|
||||
fprintf(stderr, "soundif: Error, cannot set the channel "
|
||||
"number to 2: using mono\n");
|
||||
stereo=FALSE;
|
||||
} else if (sndparam != 1) {
|
||||
fprintf(stderr, "soundif: Error, cannot set the channel "
|
||||
"number to 2\n");
|
||||
exit (10);
|
||||
}
|
||||
sndparam = sample_rate;
|
||||
if (ioctl(fd, SNDCTL_DSP_SPEED, &sndparam) == -1) {
|
||||
perror("ioctl: SNDCTL_DSP_SPEED");
|
||||
exit (10);
|
||||
}
|
||||
if ((10*abs(sndparam-sample_rate)) > sample_rate) {
|
||||
perror("ioctl: SNDCTL_DSP_SPEED");
|
||||
exit (10);
|
||||
}
|
||||
if (sndparam != sample_rate) {
|
||||
fprintf(stderr, "Warning: Sampling rate is %u, "
|
||||
"requested %u\n", sndparam, sample_rate);
|
||||
}
|
||||
}
|
||||
stereo = TRUE;
|
||||
sndparam = 1; /* we want 2 channels */
|
||||
if (ioctl(fd, SNDCTL_DSP_STEREO, &sndparam) == -1) {
|
||||
perror("ioctl: SNDCTL_DSP_STEREO");
|
||||
exit (10);
|
||||
}
|
||||
if (sndparam == 0) {
|
||||
fprintf(stderr, "soundif: Error, cannot set the channel "
|
||||
"number to 2: using mono\n");
|
||||
stereo=FALSE;
|
||||
} else if (sndparam != 1) {
|
||||
fprintf(stderr, "soundif: Error, cannot set the channel "
|
||||
"number to 2\n");
|
||||
exit (10);
|
||||
}
|
||||
sndparam = sample_rate;
|
||||
if (ioctl(fd, SNDCTL_DSP_SPEED, &sndparam) == -1) {
|
||||
perror("ioctl: SNDCTL_DSP_SPEED");
|
||||
exit (10);
|
||||
}
|
||||
if ((10*abs(sndparam-sample_rate)) > sample_rate) {
|
||||
perror("ioctl: SNDCTL_DSP_SPEED");
|
||||
exit (10);
|
||||
}
|
||||
if (sndparam != sample_rate) {
|
||||
fprintf(stderr, "Warning: Sampling rate is %u, "
|
||||
"requested %u\n", sndparam, sample_rate);
|
||||
}
|
||||
#endif
|
||||
return fd;
|
||||
}
|
||||
|
||||
|
||||
static struct demod_state dem_st[2];
|
||||
|
||||
char data_left[INPUT_BUF_LEN+1];
|
||||
char data_right[INPUT_BUF_LEN+1];
|
||||
char data_left[OUTPUT_BUF_LEN+1];
|
||||
char data_right[OUTPUT_BUF_LEN+1];
|
||||
struct data data_received = {data_left, 0, data_right, 0};
|
||||
unsigned int fbuf_cnt = 0;
|
||||
|
||||
@@ -340,8 +334,6 @@ pprz_demod_read_data(void) {
|
||||
}
|
||||
} else {
|
||||
i = read(fd, sp = b.s, sizeof(b.s));
|
||||
// *** int j; for(j=0; j < 10; j++) printf("%04x", *(sp+j)); printf("\n");
|
||||
write(fdebug, sp, i);
|
||||
if (i < 0 && errno != EAGAIN) {
|
||||
perror("read");
|
||||
exit(4);
|
||||
@@ -361,17 +353,13 @@ pprz_demod_read_data(void) {
|
||||
if (fbuf_cnt > overlap) {
|
||||
|
||||
glob_data_received_len = 0;
|
||||
/***/printf("XXX\n");
|
||||
afsk48p_demod(&dem_st[LEFT], fbuf[LEFT], fbuf_cnt-overlap, data_received.data_left);
|
||||
|
||||
/***/printf("gl=%d\n", glob_data_received_len);
|
||||
data_received.len_left = glob_data_received_len;
|
||||
memmove(fbuf[LEFT], fbuf[LEFT]+fbuf_cnt-overlap, overlap*sizeof(fbuf[LEFT][0]));
|
||||
if (stereo) {
|
||||
glob_data_received_len = 0;
|
||||
/***/printf("YYY\n");
|
||||
afsk48p_demod(&dem_st[RIGHT], fbuf[RIGHT], fbuf_cnt-overlap, data_received.data_right);
|
||||
/***/printf("gl=%d\n", glob_data_received_len);
|
||||
data_received.len_right = glob_data_received_len;
|
||||
memmove(fbuf[RIGHT], fbuf[RIGHT]+fbuf_cnt-overlap, overlap*sizeof(fbuf[RIGHT][0]));
|
||||
}
|
||||
|
||||
@@ -24,7 +24,7 @@ include ../../../conf/Makefile.local
|
||||
CONF = ../../../conf
|
||||
VAR = ../../../var
|
||||
|
||||
all: hw_modem_listen receive messages $(VAR)/boa.conf
|
||||
all: hw_modem_listen receive messages $(VAR)/boa.conf stereo_demod
|
||||
|
||||
clean:
|
||||
rm -f hw_modem_listen wavecard_connect receive messages *.bak *~ core *.o .depend *.opt *.out *.cm*
|
||||
@@ -57,10 +57,10 @@ hw_modem_listen : modem.cmo hw_modem_listen.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/modem.cmo $$PAPARAZZI_SRC/sw/ground_segment/tmtc/hw_modem_listen.ml $$*' >> $@
|
||||
@chmod a+x $@
|
||||
|
||||
demod_listen : ../multimon/multimon.cma demod_listen.ml ../../lib/ocaml/lib-pprz.cma
|
||||
stereo_demod : ../multimon/multimon.cma stereo_demod.ml ../../lib/ocaml/lib-pprz.cma
|
||||
$(OCAMLC) $(INCLUDES) -I ../multimon -o $@ lablgtk.cma glibivy-ocaml.cma lib-pprz.cma multimon.cma $^
|
||||
@cat ../../../pprz_src_test.sh > $@
|
||||
@echo 'exec lablgtk2 -I $$PAPARAZZI_SRC/sw/lib/ocaml -I $$PAPARAZZI_SRC/sw/ground_segment/multimon glibivy-ocaml.cma lib-pprz.cma -I $$PAPARAZZI_SRC/sw/ground_segment/tmtc $$PAPARAZZI_SRC/sw/ground_segment/multimon/multimon.cma $$PAPARAZZI_SRC/sw/ground_segment/tmtc/demod_listen.ml $$*' >> $@
|
||||
@echo 'exec lablgtk2 -I $$PAPARAZZI_SRC/sw/lib/ocaml -I $$PAPARAZZI_SRC/sw/ground_segment/multimon glibivy-ocaml.cma lib-pprz.cma -I $$PAPARAZZI_SRC/sw/ground_segment/tmtc $$PAPARAZZI_SRC/sw/ground_segment/multimon/multimon.cma $$PAPARAZZI_SRC/sw/ground_segment/tmtc/stereo_demod.ml $$*' >> $@
|
||||
@chmod a+x $@
|
||||
|
||||
receive : receive.ml ../../lib/ocaml/lib-pprz.cma
|
||||
|
||||
@@ -1,133 +0,0 @@
|
||||
(*
|
||||
* $Id$
|
||||
*
|
||||
* Hardware modem receiver
|
||||
*
|
||||
* Copyright (C) 2004 CENA/ENAC, 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.
|
||||
*
|
||||
*)
|
||||
|
||||
open Printf
|
||||
|
||||
(** Ivy messages are initially tagged "modem" and with the A/C
|
||||
id as soon as it is identified (IDENT message) *)
|
||||
let ac_id = ref "modem"
|
||||
|
||||
let modem_msg_period = 1000 (** ms *)
|
||||
|
||||
module Tele_Class = struct let name = "telemetry_ap" end
|
||||
module Tele_Pprz = Pprz.Protocol(Tele_Class)
|
||||
module PprzTransport = Serial.Transport(Tele_Pprz)
|
||||
|
||||
(** Monitoring of the message reception *)
|
||||
type status = {
|
||||
mutable rx_byte : int;
|
||||
mutable rx_msg : int;
|
||||
mutable rx_err : int
|
||||
}
|
||||
let status = { rx_byte = 0; rx_msg = 0; rx_err = 0 }
|
||||
|
||||
(** Callback for each decoded message *)
|
||||
let use_pprz_message = fun (msg_id, values) ->
|
||||
status.rx_msg <- status.rx_msg + 1; (** Monitoring update *)
|
||||
let msg = Tele_Pprz.message_of_id msg_id in
|
||||
if msg.Pprz.name = "IDENT" then
|
||||
ac_id := Pprz.string_assoc "id" values;
|
||||
(***)prerr_endline msg.Pprz.name;
|
||||
Tele_Pprz.message_send !ac_id msg.Pprz.name values
|
||||
|
||||
(** Listen on a dsp device *)
|
||||
let listen_pprz_modem = fun pprz_message_cb devdsp ->
|
||||
let fd = Demod.init devdsp in
|
||||
|
||||
(** Callback for a checksumed pprz message *)
|
||||
let use_pprz_buf = fun buf ->
|
||||
status.rx_byte <- status.rx_byte + String.length buf;
|
||||
Debug.call 'P' (fun f -> fprintf f "use_pprz: %s\n" (Debug.xprint buf));
|
||||
pprz_message_cb (Tele_Pprz.values_of_bin buf) in
|
||||
|
||||
(** Callback for available chars *)
|
||||
let cb =
|
||||
let buffer = ref "" in
|
||||
fun _ ->
|
||||
printf("get_date\n%!");
|
||||
let (data_left, data_right) = Demod.get_data () in
|
||||
(***)printf "left=%s\n%!" (Debug.xprint data_left);
|
||||
(***)printf "right=%s\n%!" (Debug.xprint data_right);
|
||||
(** Accumulate in a buffer *)
|
||||
let b = !buffer ^ data_left in
|
||||
Debug.call 'M' (fun f -> fprintf f "Pprz buffer: %s\n" (Debug.xprint b));
|
||||
(** Parse as pprz message and ... *)
|
||||
let x = PprzTransport.parse use_pprz_buf b in
|
||||
status.rx_err <- !PprzTransport.nb_err;
|
||||
(** ... remove from the buffer the chars which have been used *)
|
||||
buffer := String.sub b x (String.length b - x);
|
||||
true
|
||||
in
|
||||
|
||||
(** Attach the callback to the channel *)
|
||||
ignore (Glib.Io.add_watch [`IN] cb (Glib.Io.channel_of_descr fd))
|
||||
|
||||
(** Modem monitoring messages *)
|
||||
let send_modem_msg =
|
||||
let rx_msg = ref 0
|
||||
and rx_byte = ref 0
|
||||
and start = Unix.gettimeofday () in
|
||||
fun () ->
|
||||
let dt = float modem_msg_period /. 1000. in
|
||||
let t = int_of_float (Unix.gettimeofday () -. start) in
|
||||
let byte_rate = float (status.rx_byte - !rx_byte) /. dt
|
||||
and msg_rate = float (status.rx_msg - !rx_msg) /. dt in
|
||||
rx_msg := status.rx_msg;
|
||||
rx_byte := status.rx_byte;
|
||||
let vs = ["run_time", Pprz.Int t;
|
||||
"rx_bytes_rate", Pprz.Float byte_rate;
|
||||
"rx_msgs_rate", Pprz.Float msg_rate;
|
||||
"rx_err", Pprz.Int status.rx_err;
|
||||
"rx_bytes", Pprz.Int status.rx_byte;
|
||||
"rx_msgs", Pprz.Int status.rx_msg
|
||||
] in
|
||||
Tele_Pprz.message_send !ac_id "DOWNLINK_STATUS" vs
|
||||
|
||||
(* main loop *)
|
||||
let _ =
|
||||
let ivy_bus = ref "127.255.255.255:2010" in
|
||||
let port = ref "/dev/dsp" in
|
||||
let options =
|
||||
[ "-b", Arg.Set_string ivy_bus, (sprintf "Ivy bus (%s)" !ivy_bus);
|
||||
"-d", Arg.Set_string port, (sprintf "Port (%s)" !port)] in
|
||||
Arg.parse
|
||||
options
|
||||
(fun x -> fprintf stderr "Warning:ignoring %s\n" x)
|
||||
"Usage: ";
|
||||
|
||||
Ivy.init "Paparazzi hw_modem_listen" "READY" (fun _ _ -> ());
|
||||
Ivy.start !ivy_bus;
|
||||
|
||||
(** Listening on the given port (serial device or multimon fifo)*)
|
||||
listen_pprz_modem use_pprz_message !port;
|
||||
|
||||
(** Sending periodically modem and downlink status messages *)
|
||||
ignore (Glib.Timeout.add modem_msg_period (fun () -> send_modem_msg (); true));
|
||||
|
||||
let loop = Glib.Main.create true in
|
||||
while Glib.Main.is_running loop do
|
||||
ignore (Glib.Main.iteration true)
|
||||
done
|
||||
@@ -42,7 +42,7 @@ value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, valu
|
||||
for(i = 0; i < gps_nb_channels; i++) {
|
||||
gps_svinfos[i].svid = 7 + i;
|
||||
gps_svinfos[i].elev = (cos(((100*i)+time)/100.) + 1) * 45;
|
||||
gps_svinfos[i].azim = (time/gps_nb_channels + 20 * i) % 360;
|
||||
gps_svinfos[i].azim = (time/gps_nb_channels + 50 * i) % 360;
|
||||
gps_svinfos[i].cno = 40 + sin(time/100.) * 10.;
|
||||
gps_svinfos[i].flags = 0x01;
|
||||
gps_svinfos[i].qi = (int)((time / 1000.) + i) % 8;
|
||||
|
||||
Reference in New Issue
Block a user