diff --git a/conf/autopilot/lisa_test_progs.makefile b/conf/autopilot/lisa_test_progs.makefile index b351961907..4d3b766d0d 100644 --- a/conf/autopilot/lisa_test_progs.makefile +++ b/conf/autopilot/lisa_test_progs.makefile @@ -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 @@ ptw.CFLAGS += -DUSE_I2C1 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. diff --git a/sw/airborne/can.c b/sw/airborne/can.c index 901ecef7dc..a40c2e54f9 100644 --- a/sw/airborne/can.c +++ b/sw/airborne/can.c @@ -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); -} - -int can_transmit(uint8_t client_id, uint8_t msg_id, const uint8_t *buf, uint8_t len) -{ - uint32_t id = can_id(client_id, msg_id); return can_hw_transmit(id, buf, len); } + +void _can_run_rx_callback(uint32_t id, uint8_t *buf, uint8_t len) +{ + if(can_rx_callback) + can_rx_callback(id, buf, len); +} diff --git a/sw/airborne/can.h b/sw/airborne/can.h index 53d48a3831..4a474d7298 100644 --- a/sw/airborne/can.h +++ b/sw/airborne/can.h @@ -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 */ diff --git a/sw/airborne/csc/csc_msg_def.h b/sw/airborne/csc/csc_msg_def.h index c26172c4c3..d2eede654f 100644 --- a/sw/airborne/csc/csc_msg_def.h +++ b/sw/airborne/csc/csc_msg_def.h @@ -21,7 +21,7 @@ #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 { diff --git a/sw/airborne/csc/csc_protocol.c b/sw/airborne/csc/csc_protocol.c new file mode 100644 index 0000000000..2de1813663 --- /dev/null +++ b/sw/airborne/csc/csc_protocol.c @@ -0,0 +1,177 @@ +/* + * $Id:$ + * + * Copyright (C) 2010 Piotr Esden-Tempski + * + * 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 +#include + +#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; +} diff --git a/sw/airborne/csc/csc_protocol.h b/sw/airborne/csc/csc_protocol.h new file mode 100644 index 0000000000..9c3b0544ca --- /dev/null +++ b/sw/airborne/csc/csc_protocol.h @@ -0,0 +1,35 @@ +/* + * $Id:$ + * + * Copyright (C) 2010 Piotr Esden-Tempski + * + * 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 diff --git a/sw/airborne/lisa/test_csc_servo.c b/sw/airborne/lisa/test_csc_servo.c index e85df26f4f..ef8d1516c9 100644 --- a/sw/airborne/lisa/test_csc_servo.c +++ b/sw/airborne/lisa/test_csc_servo.c @@ -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_warning = RESET; 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 @@ int main(void) { 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 @@ static inline void main_periodic_task( void ) { 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_periodic_task( void ) { 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);}); } diff --git a/sw/airborne/stm32/can_hw.c b/sw/airborne/stm32/can_hw.c index 05fafa9916..e5d82a1471 100644 --- a/sw/airborne/stm32/can_hw.c +++ b/sw/airborne/stm32/can_hw.c @@ -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 @@ int can_hw_transmit(uint32_t id, const uint8_t *buf, uint8_t len) 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 } +