Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-21 03:43:26 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

More work on the can protocol implementation. Cleaned up a lot and added

Browse Source 
a abstracted version of csc protocol implementation.
This commit is contained in:
Piotr Esden-Tempski
2010-07-29 04:34:42 +00:00
parent 6117f4e8c9
commit 52d3036a2d
8 changed files with 272 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/autopilot/lisa_test_progs.makefile
+15 -4
View File
@@ -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.
sw/airborne/can.c
+13 -8
View File
@@ -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);
}
sw/airborne/can.h
+4 -2
View File
@@ -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 */
sw/airborne/csc/csc_msg_def.h
+1 -1
View File
@@ -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 {
sw/airborne/csc/csc_protocol.c
+177
View File
@@ -0,0 +1,177 @@
/*
* $Id:$
*
* Copyright (C) 2010 Piotr Esden-Tempski <piotr@esden.net>
*
* 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;
}
sw/airborne/csc/csc_protocol.h
+35
View File
@@ -0,0 +1,35 @@
/*
* $Id:$
*
* Copyright (C) 2010 Piotr Esden-Tempski <piotr@esden.net>
*
* 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
sw/airborne/lisa/test_csc_servo.c
+17 -4
View File
@@ -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);});
}
sw/airborne/stm32/can_hw.c
+10 -1
View File
@@ -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
}