Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 20:38:27 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

First message received by airborne wavecard

Browse Source
This commit is contained in:
Pascal Brisset
2005-11-07 16:56:46 +00:00
parent 8c3dca77ed
commit a33c4883d5
15 changed files with 296 additions and 188 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/flight_plans/muret_mini.xml
+1 -20
View File
@@ -1,4 +1,4 @@
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
<!-- DOCTYPE flight_plan SYSTEM "flight_plan.dtd" -->
<flight_plan NAME="Muret mini" LON0="1.27289" MAX_DIST_FROM_HOME="1000" GROUND_ALT="185" SECURITY_HEIGHT="25" QFU="270" ALT="250" LAT0="43.46223">
<rc_control>
@@ -17,27 +17,8 @@
<blocks>
<block NAME="home">
<exception COND="(RcEvent1())" DEROUTE="c50"/>
<circle wp="HOME" alt="GROUND_ALT+50" radius="-75"/>
</block>
<block NAME="c50">
<exception COND="(RcEvent1())" DEROUTE="c40"/>
<exception COND="(RcEvent2())" DEROUTE="home"/>
<circle wp="HOME" alt="GROUND_ALT+50" radius="-50"/>
</block>
<block NAME="c40">
<exception COND="(RcEvent1())" DEROUTE="c30"/>
<exception COND="(RcEvent2())" DEROUTE="home"/>
<circle wp="HOME" alt="GROUND_ALT+50" radius="-40"/>
</block>
<block NAME="c30">
<exception COND="(RcEvent1())" DEROUTE="c20"/>
<circle wp="HOME" alt="GROUND_ALT+30" radius="-30"/>
</block>
<block NAME="c20">
<exception COND="(RcEvent1())" DEROUTE="home"/>
<circle wp="HOME" alt="GROUND_ALT+25" radius="-20"/>
</block>
</blocks>
</flight_plan>
sw/airborne/autopilot/Makefile
+3 -2
View File
@@ -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
sw/airborne/autopilot/datalink.c
+24 -18
View File
@@ -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');
}
sw/airborne/autopilot/datalink.h
+5 -1
View File
@@ -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
sw/airborne/autopilot/gps_ubx.c
+3 -1
View File
@@ -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
sw/airborne/autopilot/mainloop.c
+5
View File
@@ -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;
sw/airborne/autopilot/uart.c
+7 -3
View File
@@ -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 */
sw/airborne/autopilot/wavecard.c
+160
View File
@@ -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 <inttypes.h>
#include <stdlib.h>
#include <avr/signal.h>
#include <avr/interrupt.h>
#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
sw/airborne/autopilot/wavecard.c.old
-112
View File
@@ -1,112 +0,0 @@
#include <inttypes.h>
#include <stdlib.h>
#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; \
}
***/
sw/airborne/autopilot/wavecard.h.old → sw/airborne/autopilot/wavecard.h
+36 -12
View File
@@ -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; \
sw/ground_segment/tmtc/Makefile
+6
View File
@@ -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
sw/ground_segment/tmtc/wavecard_connect.ml
+16 -11
View File
@@ -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
sw/lib/ocaml/wavecard.ml
+25 -7
View File
@@ -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)
sw/lib/ocaml/wavecard.mli
+4
View File
@@ -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
sw/tools/gen_dl.ml
+1 -1
View File
@@ -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" ->