[paparazzi-commits] [5187] More work on the can protocol implementation.

[Piotr Esden-Tempski]

Revision: 5187
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5187
Author:   esden
Date:     2010-07-29 04:34:42 +0000 (Thu, 29 Jul 2010)
Log Message:
-----------
More work on the can protocol implementation. Cleaned up a lot and added
a abstracted version of csc protocol implementation.

Modified Paths:
--------------
    paparazzi3/trunk/conf/autopilot/lisa_test_progs.makefile
    paparazzi3/trunk/sw/airborne/can.c
    paparazzi3/trunk/sw/airborne/can.h
    paparazzi3/trunk/sw/airborne/csc/csc_msg_def.h
    paparazzi3/trunk/sw/airborne/lisa/test_csc_servo.c
    paparazzi3/trunk/sw/airborne/stm32/can_hw.c

Added Paths:
-----------
    paparazzi3/trunk/sw/airborne/csc/csc_protocol.c
    paparazzi3/trunk/sw/airborne/csc/csc_protocol.h

Modified: paparazzi3/trunk/conf/autopilot/lisa_test_progs.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/lisa_test_progs.makefile    2010-07-29 
04:34:39 UTC (rev 5186)
+++ paparazzi3/trunk/conf/autopilot/lisa_test_progs.makefile    2010-07-29 
04:34:42 UTC (rev 5187)
@@ -31,6 +31,8 @@
 #
 
################################################################################
 
+SRC_CSC=csc
+
 overo_test_spi.ARCHDIR = omap
 overo_test_spi.srcs=$(SRC_FMS)/overo_test_spi.c
 
@@ -1123,16 +1125,25 @@
 test_csc_servo.ARCHDIR = $(ARCHI)
 test_csc_servo.TARGET = test_csc_servo
 test_csc_servo.TARGETDIR = test_csc_servo
-test_csc_servo.CFLAGS = -I$(SRC_LISA) -I$(ARCHI) -DPERIPHERALS_AUTO_INIT
+test_csc_servo.CFLAGS = -I $(SRC_CSC) -I$(SRC_LISA) -I$(ARCHI) 
-DPERIPHERALS_AUTO_INIT
 test_csc_servo.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG)
-test_csc_servo.srcs = $(SRC_LISA)/test_csc_servo.c      \
-               $(SRC_ARCH)/stm32_exceptions.c   \
-               $(SRC_ARCH)/stm32_vector_table.c
+test_csc_servo.srcs = $(SRC_CSC)/csc_protocol.c \
+               $(SRC_LISA)/test_csc_servo.c      \
+               $(SRC_ARCH)/stm32_exceptions.c   \
+               $(SRC_ARCH)/stm32_vector_table.c
 test_csc_servo.CFLAGS += -DUSE_LED
 test_csc_servo.srcs += $(SRC_ARCH)/led_hw.c
 test_csc_servo.CFLAGS += -DUSE_SYS_TIME -DSYS_TIME_LED=1
 test_csc_servo.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(1./512.)'
 test_csc_servo.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
+
+test_csc_servo.CFLAGS += -DUSE_UART2 -DUART2_BAUD=B57600
+test_csc_servo.srcs += $(SRC_ARCH)/uart_hw.c
+test_csc_servo.CFLAGS += -DDATALINK=PPRZ -DPPRZ_UART=Uart2
+
+test_csc_servo.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport 
-DDOWNLINK_DEVICE=Uart2 
+test_csc_servo.srcs += downlink.c pprz_transport.c
+
 # setting CAN prescaler to generate 3MHz time quanta, drift compensiation to 1
 # time quanta, bit section 1 to 3 time quanta and bit section 2 to 4 time 
quanta
 # resulting in a 375kHz CAN bitrate expected by the CSC.

Modified: paparazzi3/trunk/sw/airborne/can.c
===================================================================
--- paparazzi3/trunk/sw/airborne/can.c  2010-07-29 04:34:39 UTC (rev 5186)
+++ paparazzi3/trunk/sw/airborne/can.c  2010-07-29 04:34:42 UTC (rev 5187)
@@ -27,18 +27,23 @@
 #include "can.h"
 #include "can_hw.h"
 
-void can_init(void)
+can_rx_callback_t can_rx_callback;
+
+void _can_run_rx_callback(uint32_t id, uint8_t *buf, uint8_t len);
+
+void can_init(can_rx_callback_t callback)
 {
+       can_rx_callback = callback;
        can_hw_init();
 }
 
-static inline uint32_t can_id(uint8_t client_id, uint8_t msg_id)
+int can_transmit(uint32_t id, const uint8_t *buf, uint8_t len)
 {
-       return ((client_id & 0xF) << 7) | (msg_id & 0x7F);
+       return can_hw_transmit(id, buf, len);
 }
 
-int can_transmit(uint8_t client_id, uint8_t msg_id, const uint8_t *buf, 
uint8_t len)
+void _can_run_rx_callback(uint32_t id, uint8_t *buf, uint8_t len)
 {
-       uint32_t id = can_id(client_id, msg_id);
-       return can_hw_transmit(id, buf, len);
+       if(can_rx_callback)
+               can_rx_callback(id, buf, len);
 }

Modified: paparazzi3/trunk/sw/airborne/can.h
===================================================================
--- paparazzi3/trunk/sw/airborne/can.h  2010-07-29 04:34:39 UTC (rev 5186)
+++ paparazzi3/trunk/sw/airborne/can.h  2010-07-29 04:34:42 UTC (rev 5187)
@@ -25,7 +25,9 @@
 #ifndef CAN_H
 #define CAN_H
 
-void can_init(void);
-int can_transmit(uint8_t client_id, uint8_t msg_id, const uint8_t *buf, 
uint8_t len);
+typedef void(* can_rx_callback_t)(uint32_t id, uint8_t *buf, int len);
 
+void can_init(can_rx_callback_t callback);
+int can_transmit(uint32_t id, const uint8_t *buf, uint8_t len);
+
 #endif /* CAN_H */

Modified: paparazzi3/trunk/sw/airborne/csc/csc_msg_def.h
===================================================================
--- paparazzi3/trunk/sw/airborne/csc/csc_msg_def.h      2010-07-29 04:34:39 UTC 
(rev 5186)
+++ paparazzi3/trunk/sw/airborne/csc/csc_msg_def.h      2010-07-29 04:34:42 UTC 
(rev 5187)
@@ -21,8 +21,8 @@
 #define CSC_VANE_MSG_ID       11
 #define CSC_PRESSURE_MSG_ID   12
 #define CSC_BARO_MSG_ID       13
+#define CSC_ID_COUNT          14
 
-
 /* Received from the autopilot */
 struct CscServoCmd {
   uint16_t servos[4];

Added: paparazzi3/trunk/sw/airborne/csc/csc_protocol.c
===================================================================
--- paparazzi3/trunk/sw/airborne/csc/csc_protocol.c                             
(rev 0)
+++ paparazzi3/trunk/sw/airborne/csc/csc_protocol.c     2010-07-29 04:34:42 UTC 
(rev 5187)
@@ -0,0 +1,177 @@
+/*
+ * $Id:$
+ *
+ * Copyright (C) 2010 Piotr Esden-Tempski <address@hidden>
+ *
+ * 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.
+ *
+ */
+
+#include <stdint.h>
+#include <string.h>
+
+#include "csc_protocol.h"
+#include "can.h"
+#include "csc_msg_def.h"
+
+
+#define CSCP_QUEUE_LEN 8
+
+struct cscp_msg {
+       uint8_t id;
+       uint8_t len;
+       uint8_t data[8];
+};
+
+struct cscp_msg_queue {
+       int head;
+       int tail;
+       int full;
+       struct cscp_msg msgs[CSCP_QUEUE_LEN];
+} cscp_msg_queue;
+
+/*
+ * This handle is being used internally in the protocol driver
+ * data points to user supplied memory (user allocated struct)
+ * this way we make sure that the user allocates only the amount
+ * of memory necessary and we don't have to mess around with malloc
+ */
+struct cscp_callback_handle {
+       cscp_callback_t callback;
+       void *data;
+};
+
+struct cscp_callback_handle cscp_callback_handles[CSC_ID_COUNT];
+
+void cscp_can_rx_callback(uint32_t id, uint8_t *buf, int len);
+void cscp_queue_init(void);
+int cscp_enqueue(uint32_t msg_id, uint8_t *buf, int len);
+int cscp_peek_msg_id(void);
+int cscp_dequeue(uint8_t *buf);
+void cscp_callback_init(void);
+
+void cscp_init(void)
+{
+       can_init(cscp_can_rx_callback);
+       cscp_queue_init();
+       cscp_callback_init();
+}
+
+static inline uint32_t cscp_can_id(uint8_t client_id, uint8_t msg_id)
+{
+       return ((client_id & 0xF) << 7) | (msg_id & 0x7F);
+}
+
+static inline uint32_t cscp_msg_id(uint32_t id)
+{
+       return (id & 0x7F);
+}
+
+int cscp_transmit(uint32_t client_id, uint8_t msg_id, const uint8_t *buf, 
uint8_t len)
+{
+       uint32_t id = cscp_can_id(client_id, msg_id);
+       return can_transmit(id, buf, len);
+}
+
+void cscp_can_rx_callback(uint32_t id, uint8_t *buf, int len)
+{
+       /* just store the incoming data in a buffer with very little processing 
*/
+
+       /* TODO we should handle the return value of enqueue somehow,
+        * the returnvalue tells us if the queue had enough space to
+        * store our message or not
+        */
+       uint32_t msg_id = cscp_msg_id(id);
+       cscp_enqueue(msg_id, buf, len);
+}
+
+void cscp_event(void)
+{
+       /* this periodig is the main csc protocol event dispatcher */
+       int msg_id = cscp_peek_msg_id();
+
+       if(msg_id == -1){
+               return;
+       }
+
+       if(cscp_callback_handles[msg_id].callback){
+               cscp_dequeue(cscp_callback_handles[msg_id].data);
+               
cscp_callback_handles[msg_id].callback(cscp_callback_handles[msg_id].data);
+       }
+
+}
+
+void cscp_queue_init(void)
+{
+       cscp_msg_queue.head = 0;
+       cscp_msg_queue.tail = 0;
+       cscp_msg_queue.full = 0;
+}
+
+int cscp_enqueue(uint32_t msg_id, uint8_t *buf, int len)
+{
+       if (cscp_msg_queue.full) {
+               return 1;
+       }
+
+       cscp_msg_queue.msgs[cscp_msg_queue.tail].id = msg_id;
+       cscp_msg_queue.msgs[cscp_msg_queue.tail].len = len;
+       memcpy(cscp_msg_queue.msgs[cscp_msg_queue.tail].data, buf, len);
+       cscp_msg_queue.tail = (cscp_msg_queue.tail + 1) % CSCP_QUEUE_LEN;
+       if (cscp_msg_queue.head == cscp_msg_queue.tail)
+               cscp_msg_queue.full = 1;
+
+       return 0;
+}
+
+int cscp_peek_msg_id(void)
+{
+       if(!cscp_msg_queue.full &&
+               (cscp_msg_queue.head == cscp_msg_queue.tail))
+       {
+               return -1;
+       }
+       return cscp_msg_queue.msgs[cscp_msg_queue.head].id;
+}
+
+int cscp_dequeue(uint8_t *buf)
+{
+       if (!cscp_msg_queue.full &&
+               (cscp_msg_queue.head == cscp_msg_queue.tail)) {
+               return 1;
+       }
+
+       memcpy(buf,
+               cscp_msg_queue.msgs[cscp_msg_queue.head].data,
+               cscp_msg_queue.msgs[cscp_msg_queue.head].len);
+       cscp_msg_queue.head = (cscp_msg_queue.head + 1) % CSCP_QUEUE_LEN;
+       cscp_msg_queue.full = 0;
+
+       return 0;
+}
+
+void cscp_callback_init(void)
+{
+       memset(cscp_callback_handles, 0, sizeof(cscp_callback_handles));
+}
+
+void cscp_register_callback(uint32_t msg_id, cscp_callback_t callback, uint8_t 
*data)
+{
+       cscp_callback_handles[msg_id].callback = callback;
+       cscp_callback_handles[msg_id].data = data;
+}

Copied: paparazzi3/trunk/sw/airborne/csc/csc_protocol.h (from rev 5186, 
paparazzi3/trunk/sw/airborne/can.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/csc/csc_protocol.h                             
(rev 0)
+++ paparazzi3/trunk/sw/airborne/csc/csc_protocol.h     2010-07-29 04:34:42 UTC 
(rev 5187)
@@ -0,0 +1,35 @@
+/*
+ * $Id:$
+ *
+ * Copyright (C) 2010 Piotr Esden-Tempski <address@hidden>
+ *
+ * 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.
+ *
+ */
+
+#ifndef CSC_PROTOCOL_H
+#define CSC_PROTOCOL_H
+
+typedef void(* cscp_callback_t)(void *data);
+
+void cscp_init(void);
+int cscp_transmit(uint32_t client_id, uint8_t msg_id, const uint8_t *buf, 
uint8_t len);
+void cscp_event(void);
+void cscp_register_callback(uint32_t msg_id, cscp_callback_t callback, uint8_t 
*data);
+
+#endif

Modified: paparazzi3/trunk/sw/airborne/lisa/test_csc_servo.c
===================================================================
--- paparazzi3/trunk/sw/airborne/lisa/test_csc_servo.c  2010-07-29 04:34:39 UTC 
(rev 5186)
+++ paparazzi3/trunk/sw/airborne/lisa/test_csc_servo.c  2010-07-29 04:34:42 UTC 
(rev 5187)
@@ -24,8 +24,10 @@
 
 #include "init_hw.h"
 #include "sys_time.h"
-#include "can.h"
+#include "csc_msg_def.h"
+#include "csc_protocol.h"
 #include "stm32/can.h"
+#include "downlink.h"
 
 static inline void main_init( void );
 static inline void main_periodic_task( void );
@@ -37,6 +39,10 @@
 FlagStatus can_error_passive = RESET;
 FlagStatus can_bus_off = RESET;
 
+struct CscVaneMsg csc_vane_msg;
+
+void main_on_vane_msg(void *data);
+
 int main(void) {
   main_init();
 
@@ -57,7 +63,8 @@
 static inline void main_init( void ) {
        hw_init();
        sys_time_init();
-       can_init();
+       cscp_init();
+       cscp_register_callback(CSC_VANE_MSG_ID, main_on_vane_msg, (void 
*)&csc_vane_msg);
 }
 
 static inline void main_periodic_task( void ) {
@@ -82,7 +89,7 @@
                LED_OFF(0);
        }
 
-       can_transmit(0, 0, (uint8_t *)servos, 8);
+       cscp_transmit(0, 0, (uint8_t *)servos, 8);
 
        LED_PERIODIC();
 }
@@ -90,5 +97,11 @@
 
 
 static inline void main_event_task( void ) {
+       cscp_event();
+}
 
+void main_on_vane_msg(void *data)
+{
+       int zero = 0;
+       RunOnceEvery(10, {DOWNLINK_SEND_VANE_SENSOR(DefaultChannel, 
&(csc_vane_msg.vane_angle1), &zero, &csc_vane_msg.vane_angle2, &zero, &zero, 
&zero, &zero);});
 }

Modified: paparazzi3/trunk/sw/airborne/stm32/can_hw.c
===================================================================
--- paparazzi3/trunk/sw/airborne/stm32/can_hw.c 2010-07-29 04:34:39 UTC (rev 
5186)
+++ paparazzi3/trunk/sw/airborne/stm32/can_hw.c 2010-07-29 04:34:42 UTC (rev 
5187)
@@ -43,6 +43,10 @@
 CanTxMsg can_tx_msg;
 CanRxMsg can_rx_msg;
 
+RCC_ClocksTypeDef rcc_clocks;
+
+void _can_run_rx_callback(uint32_t id, uint8_t *buf, uint8_t len);
+
 void can_hw_init(void)
 {
         RCC_ClocksTypeDef rcc_clocks;
@@ -143,8 +147,13 @@
        return 0;
 }
 
-
 void usb_lp_can1_rx0_irq_handler(void)
 {
        CAN_Receive(CAN1, CAN_FIFO0, &can_rx_msg);
+#ifdef USE_CAN_EXT_ID
+       _can_run_rx_callback(can_rx_msg.ExtId, can_rx_msg.Data, can_rx_msg.DLC);
+#else
+       _can_run_rx_callback(can_rx_msg.StdId, can_rx_msg.Data, can_rx_msg.DLC);
+#endif
 }
+