Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 05:42:49 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

[airborne] only cosmetic cleanup: convert leading tabs to spaces

Browse Source
This commit is contained in:
Felix Ruess
2014-03-04 01:08:12 +01:00
parent 8f575cd053
commit 8cad56d140
103 changed files with 2589 additions and 2589 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/arch/lpc21/mcu_periph/adc_arch.c
+16 -16
View File
@@ -199,8 +199,8 @@ void adc_init( void ) {
#ifdef USE_AD0 #ifdef USE_AD0
/* FIXME: this needs to be investigated, we should run just below 4.5MHz, /* FIXME: this needs to be investigated, we should run just below 4.5MHz,
but we are a lot slower (e.g. 58.6kHz with PCLK = 15MHz), see but we are a lot slower (e.g. 58.6kHz with PCLK = 15MHz), see
lpc_vor_convertions.c for right timing code */ lpc_vor_convertions.c for right timing code */
/* setup hw scan - PCLK/256 ( 58.6kHz/117.2kHz/234.4kHz ) - BURST ON */ /* setup hw scan - PCLK/256 ( 58.6kHz/117.2kHz/234.4kHz ) - BURST ON */
AD0CR = ADC_AD0CR_SEL_HW_SCAN | 0xFF << 8 | 1 << 16 | 0x01 << 21 ; AD0CR = ADC_AD0CR_SEL_HW_SCAN | 0xFF << 8 | 1 << 16 | 0x01 << 21 ;
/* AD0 selected as IRQ */ /* AD0 selected as IRQ */
@@ -214,8 +214,8 @@ void adc_init( void ) {
#ifdef USE_AD1 #ifdef USE_AD1
/* FIXME: this needs to be investigated, we should run just below 4.5MHz, /* FIXME: this needs to be investigated, we should run just below 4.5MHz,
but we are a lot slower (e.g. 58.6kHz with PCLK = 15MHz), see but we are a lot slower (e.g. 58.6kHz with PCLK = 15MHz), see
lpc_vor_convertions.c for right timing code */ lpc_vor_convertions.c for right timing code */
/* setup hw scan - PCLK/256 ( 58.6kHz/117.2kHz/234.4kHz ) - BURST ON */ /* setup hw scan - PCLK/256 ( 58.6kHz/117.2kHz/234.4kHz ) - BURST ON */
AD1CR = ADC_AD1CR_SEL_HW_SCAN | 0xFF << 8 | 1 << 16 | 0x01 << 21 ; AD1CR = ADC_AD1CR_SEL_HW_SCAN | 0xFF << 8 | 1 << 16 | 0x01 << 21 ;
/* AD1 selected as IRQ */ /* AD1 selected as IRQ */
@@ -239,12 +239,12 @@ void adcISR0 ( void ) {
struct adc_buf* buf = buffers[channel]; struct adc_buf* buf = buffers[channel];
if (buf) { if (buf) {
uint8_t new_head = buf->head + 1; uint8_t new_head = buf->head + 1;
if (new_head >= buf->av_nb_sample) new_head = 0; if (new_head >= buf->av_nb_sample) new_head = 0;
buf->sum -= buf->values[new_head]; buf->sum -= buf->values[new_head];
buf->values[new_head] = value; buf->values[new_head] = value;
buf->sum += value; buf->sum += value;
buf->head = new_head; buf->head = new_head;
} }
VICVectAddr = 0x00000000; // clear this interrupt from the VIC VICVectAddr = 0x00000000; // clear this interrupt from the VIC
@@ -259,12 +259,12 @@ void adcISR1 ( void ) {
adc1_val[channel] = value; adc1_val[channel] = value;
struct adc_buf* buf = buffers[channel+NB_ADC]; struct adc_buf* buf = buffers[channel+NB_ADC];
if (buf) { if (buf) {
uint8_t new_head = buf->head + 1; uint8_t new_head = buf->head + 1;
if (new_head >= buf->av_nb_sample) new_head = 0; if (new_head >= buf->av_nb_sample) new_head = 0;
buf->sum -= buf->values[new_head]; buf->sum -= buf->values[new_head];
buf->values[new_head] = value; buf->values[new_head] = value;
buf->sum += value; buf->sum += value;
buf->head = new_head; buf->head = new_head;
} }
VICVectAddr = 0x00000000; // clear this interrupt from the VIC VICVectAddr = 0x00000000; // clear this interrupt from the VIC
sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.c
+1 -1
View File
@@ -186,7 +186,7 @@ static void SSP_ISR(void) {
if (uart0_tx_insert_idx != uart0_tx_extract_idx) if (uart0_tx_insert_idx != uart0_tx_extract_idx)
{ {
U0THR = uart0_tx_buffer[uart0_tx_extract_idx]; U0THR = uart0_tx_buffer[uart0_tx_extract_idx];
uart0_tx_extract_idx++; uart0_tx_extract_idx++;
uart0_tx_extract_idx %= UART0_TX_BUFFER_SIZE; uart0_tx_extract_idx %= UART0_TX_BUFFER_SIZE;
} }
else else
sw/airborne/arch/lpc21/modules/adcs/max11040_hw.c
+2 -2
View File
@@ -221,9 +221,9 @@ static void SSP_ISR(void) {
if (i >= MAX11040_BUF_SIZE) i=0; if (i >= MAX11040_BUF_SIZE) i=0;
if (i != max11040_buf_out) { if (i != max11040_buf_out) {
max11040_buf_in = i; max11040_buf_in = i;
} else { } else {
//throw error; //throw error;
} }
} }
} }
break; break;
sw/airborne/arch/lpc21/modules/sensors/mag_micromag_fw_hw.h
+51 -51
View File
@@ -28,64 +28,64 @@ extern volatile uint8_t micromag_cur_axe;
#define MmReset() SetBit(MM_RESET_IOCLR,MM_RESET_PIN) #define MmReset() SetBit(MM_RESET_IOCLR,MM_RESET_PIN)
#define MmSet() SetBit(MM_RESET_IOSET,MM_RESET_PIN) #define MmSet() SetBit(MM_RESET_IOSET,MM_RESET_PIN)
#define MmOnSpiIt() { \ #define MmOnSpiIt() { \
switch (micromag_status) { \ switch (micromag_status) { \
case MM_SENDING_REQ: \ case MM_SENDING_REQ: \
{ \ { \
/* read dummy control byte reply */ \ /* read dummy control byte reply */ \
uint8_t foo __attribute__ ((unused)) = SSPDR; \ uint8_t foo __attribute__ ((unused)) = SSPDR; \
micromag_status = MM_WAITING_EOC; \ micromag_status = MM_WAITING_EOC; \
MmUnselect(); \ MmUnselect(); \
SpiClearRti(); \ SpiClearRti(); \
SpiDisableRti(); \ SpiDisableRti(); \
SpiDisable(); \ SpiDisable(); \
} \ } \
break; \ break; \
case MM_READING_RES: \ case MM_READING_RES: \
{ \ { \
int16_t new_val; \ int16_t new_val; \
new_val = SSPDR << 8; \ new_val = SSPDR << 8; \
new_val += SSPDR; \ new_val += SSPDR; \
if (abs(new_val) < 2000) \ if (abs(new_val) < 2000) \
micromag_values[micromag_cur_axe] = new_val; \ micromag_values[micromag_cur_axe] = new_val; \
MmUnselect(); \ MmUnselect(); \
SpiClearRti(); \ SpiClearRti(); \
SpiDisableRti(); \ SpiDisableRti(); \
SpiDisable(); \ SpiDisable(); \
micromag_cur_axe++; \ micromag_cur_axe++; \
if (micromag_cur_axe > 2) { \ if (micromag_cur_axe > 2) { \
micromag_cur_axe = 0; \ micromag_cur_axe = 0; \
micromag_status = MM_DATA_AVAILABLE; \ micromag_status = MM_DATA_AVAILABLE; \
} \ } \
else \ else \
micromag_status = MM_IDLE; \ micromag_status = MM_IDLE; \
} \ } \
break; \ break; \
} \ } \
} }
#define MmSendReq() { \ #define MmSendReq() { \
MmSelect(); \ MmSelect(); \
micromag_status = MM_SENDING_REQ; \ micromag_status = MM_SENDING_REQ; \
MmSet(); \ MmSet(); \
SpiClearRti(); \ SpiClearRti(); \
SpiEnableRti(); \ SpiEnableRti(); \
MmReset(); \ MmReset(); \
uint8_t control_byte = (micromag_cur_axe+1) << 0 | MM_DIVISOR_1024 << 4; \ uint8_t control_byte = (micromag_cur_axe+1) << 0 | MM_DIVISOR_1024 << 4; \
SSPDR = control_byte; \ SSPDR = control_byte; \
SpiEnable(); \ SpiEnable(); \
} }
#define MmReadRes() { \ #define MmReadRes() { \
micromag_status = MM_READING_RES; \ micromag_status = MM_READING_RES; \
MmSelect(); \ MmSelect(); \
/* trigger 2 bytes read */ \ /* trigger 2 bytes read */ \
SSPDR = 0; \ SSPDR = 0; \
SSPDR = 0; \ SSPDR = 0; \
SpiEnable(); \ SpiEnable(); \
SpiClearRti(); \ SpiClearRti(); \
SpiEnableRti(); \ SpiEnableRti(); \
} }
extern void micromag_hw_init( void ); extern void micromag_hw_init( void );
sw/airborne/arch/lpc21/subsystems/actuators/servos_4017_hw.h
+1 -1
View File
@@ -58,7 +58,7 @@ extern uint8_t servos_4017_idx;
servos_4017_idx = 0; \ servos_4017_idx = 0; \
SetBit(IO1CLR, SERVO_RESET_PIN); \ SetBit(IO1CLR, SERVO_RESET_PIN); \
} \ } \
\ \
/* request clock high on next match */ \ /* request clock high on next match */ \
T0MR1 += servos_values[servos_4017_idx]; \ T0MR1 += servos_values[servos_4017_idx]; \
/* lower clock pin */ \ /* lower clock pin */ \
sw/airborne/arch/lpc21/subsystems/datalink/audio_telemetry_hw.c
+1 -1
View File
@@ -53,7 +53,7 @@ static inline uint8_t get_next_bit( void ) {
tx_bit_idx = 0; tx_bit_idx = 0;
tx_tail++; tx_tail++;
if( tx_tail >= TX_BUF_SIZE ) if( tx_tail >= TX_BUF_SIZE )
tx_tail = 0; tx_tail = 0;
} }
} }
return ret; return ret;
sw/airborne/arch/lpc21/test/ledswitch.c
+14 -14
View File
@@ -47,21 +47,21 @@ int main(void)
} }
while (1) while (1)
{ {
if (BUTTTON1_OFF()) { if (BUTTTON1_OFF()) {
YELLOW_LED_ON(); YELLOW_LED_ON();
}
else {
YELLOW_LED_OFF();
}
if (BUTTTON2_OFF()) {
GREEN_LED_ON();
}
else {
GREEN_LED_OFF();
}
} }
else {
YELLOW_LED_OFF();
}
if (BUTTTON2_OFF()) {
GREEN_LED_ON();
}
else {
GREEN_LED_OFF();
}
}
return 0; // never reached return 0; // never reached
} }
sw/airborne/arch/omap/serial_port.c
+1 -1
View File
@@ -79,7 +79,7 @@ int serial_port_open_raw(struct SerialPort* me, const char* device, speed_t spe
} }
int serial_port_open(struct SerialPort* me, const char* device, int serial_port_open(struct SerialPort* me, const char* device,
void(*term_conf_callback)(struct termios*, speed_t*)) { void(*term_conf_callback)(struct termios*, speed_t*)) {
speed_t speed; speed_t speed;
if ((me->fd = open(device, O_RDWR | O_NONBLOCK)) < 0) { if ((me->fd = open(device, O_RDWR | O_NONBLOCK)) < 0) {
sw/airborne/arch/omap/serial_port.h
+1 -1
View File
@@ -37,7 +37,7 @@ extern void serial_port_flush(struct SerialPort* me);
extern void serial_port_flush_output(struct SerialPort* me); extern void serial_port_flush_output(struct SerialPort* me);
extern int serial_port_open_raw(struct SerialPort* me, const char* device, speed_t speed); extern int serial_port_open_raw(struct SerialPort* me, const char* device, speed_t speed);
extern int serial_port_open(struct SerialPort* me, const char* device, extern int serial_port_open(struct SerialPort* me, const char* device,
void(*term_conf_callback)(struct termios*, speed_t*)); void(*term_conf_callback)(struct termios*, speed_t*));
extern void serial_port_close(struct SerialPort* me); extern void serial_port_close(struct SerialPort* me);
#endif /* SERIAL_PORT_H */ #endif /* SERIAL_PORT_H */
sw/airborne/arch/sim/sim_ap.c
+2 -2
View File
@@ -139,6 +139,6 @@ value set_datalink_message(value s) {
/** Required by electrical */ /** Required by electrical */
void adc_buf_channel(void* a __attribute__ ((unused)), void adc_buf_channel(void* a __attribute__ ((unused)),
void* b __attribute__ ((unused)), void* b __attribute__ ((unused)),
void* c __attribute__ ((unused))) { void* c __attribute__ ((unused))) {
} }
sw/airborne/arch/stm32/peripherals/hmc5843_arch.c
+1 -1
View File
@@ -37,7 +37,7 @@ void hmc5843_arch_init( void ) {
rcc_periph_clock_enable(RCC_GPIOB); rcc_periph_clock_enable(RCC_GPIOB);
rcc_periph_clock_enable(RCC_AFIO); rcc_periph_clock_enable(RCC_AFIO);
gpio_set_mode(GPIOB, GPIO_MODE_INPUT, gpio_set_mode(GPIOB, GPIO_MODE_INPUT,
GPIO_CNF_INPUT_FLOAT, GPIO5); GPIO_CNF_INPUT_FLOAT, GPIO5);
#ifdef HMC5843_USE_INT #ifdef HMC5843_USE_INT
exti_select_source(EXTI5, GPIOB); exti_select_source(EXTI5, GPIOB);
sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.c
+3 -3
View File
@@ -55,7 +55,7 @@ void imu_aspirin_arch_init(void) {
/* "mag reset" (PC13) is shorted to I2C2 SCL */ /* "mag reset" (PC13) is shorted to I2C2 SCL */
rcc_periph_clock_enable(RCC_GPIOC); rcc_periph_clock_enable(RCC_GPIOC);
gpio_set_mode(GPIOC, GPIO_MODE_INPUT, gpio_set_mode(GPIOC, GPIO_MODE_INPUT,
GPIO_CNF_INPUT_FLOAT, GPIO12 | GPIO13); GPIO_CNF_INPUT_FLOAT, GPIO12 | GPIO13);
#endif #endif
/* Gyro --------------------------------------------------------------------*/ /* Gyro --------------------------------------------------------------------*/
@@ -63,7 +63,7 @@ void imu_aspirin_arch_init(void) {
rcc_periph_clock_enable(RCC_GPIOC); rcc_periph_clock_enable(RCC_GPIOC);
rcc_periph_clock_enable(RCC_AFIO); rcc_periph_clock_enable(RCC_AFIO);
gpio_set_mode(GPIOC, GPIO_MODE_INPUT, gpio_set_mode(GPIOC, GPIO_MODE_INPUT,
GPIO_CNF_INPUT_FLOAT, GPIO14); GPIO_CNF_INPUT_FLOAT, GPIO14);
#ifdef ASPIRIN_USE_GYRO_INT #ifdef ASPIRIN_USE_GYRO_INT
exti_select_source(EXTI14, GPIOC); exti_select_source(EXTI14, GPIOC);
@@ -74,7 +74,7 @@ void imu_aspirin_arch_init(void) {
/* configure external interrupt exti2 on PB2( accel int ) */ /* configure external interrupt exti2 on PB2( accel int ) */
rcc_periph_clock_enable(RCC_GPIOB); rcc_periph_clock_enable(RCC_GPIOB);
gpio_set_mode(GPIOB, GPIO_MODE_INPUT, gpio_set_mode(GPIOB, GPIO_MODE_INPUT,
GPIO_CNF_INPUT_FLOAT, GPIO2); GPIO_CNF_INPUT_FLOAT, GPIO2);
exti_select_source(EXTI2, GPIOB); exti_select_source(EXTI2, GPIOB);
exti_set_trigger(EXTI2, EXTI_TRIGGER_FALLING); exti_set_trigger(EXTI2, EXTI_TRIGGER_FALLING);
exti_enable_request(EXTI2); exti_enable_request(EXTI2);
sw/airborne/boards/ardrone/actuators_ardrone2_raw.c
+6 -6
View File
@@ -220,12 +220,12 @@ void actuators_ardrone_motor_status(void)
void actuators_ardrone_led_run(void); void actuators_ardrone_led_run(void);
void actuators_ardrone_led_run(void) void actuators_ardrone_led_run(void)
{ {
static uint32_t previous_led_hw_values = 0x00; static uint32_t previous_led_hw_values = 0x00;
if (previous_led_hw_values != led_hw_values) if (previous_led_hw_values != led_hw_values)
{ {
previous_led_hw_values = led_hw_values; previous_led_hw_values = led_hw_values;
actuators_ardrone_set_leds(BIT_NUMBER(led_hw_values,0), BIT_NUMBER(led_hw_values,2), BIT_NUMBER(led_hw_values,4), BIT_NUMBER(led_hw_values,6) ); actuators_ardrone_set_leds(BIT_NUMBER(led_hw_values,0), BIT_NUMBER(led_hw_values,2), BIT_NUMBER(led_hw_values,4), BIT_NUMBER(led_hw_values,6) );
} }
} }
void actuators_ardrone_commit(void) void actuators_ardrone_commit(void)
sw/airborne/boards/ardrone/at_com.c
+1 -1
View File
@@ -138,7 +138,7 @@ int at_com_recieve_navdata(unsigned char* buffer) {
int n; int n;
// FIXME(ben): not clear why recvfrom() and not recv() is used. // FIXME(ben): not clear why recvfrom() and not recv() is used.
n = recvfrom(navdata_socket, buffer, ARDRONE_NAVDATA_BUFFER_SIZE, 0x0, n = recvfrom(navdata_socket, buffer, ARDRONE_NAVDATA_BUFFER_SIZE, 0x0,
(struct sockaddr *) &from, (socklen_t *) &l); (struct sockaddr *) &from, (socklen_t *) &l);
return n; return n;
} }
sw/airborne/boards/ardrone/gpio_ardrone.c
+16 -16
View File
@@ -42,14 +42,14 @@ struct gpio_data {
}; };
enum gpio_mode { enum gpio_mode {
GPIO_INPUT = 0, //!< Pin configured for input GPIO_INPUT = 0, //!< Pin configured for input
GPIO_OUTPUT_LOW, //!< Pin configured for output with low level GPIO_OUTPUT_LOW, //!< Pin configured for output with low level
GPIO_OUTPUT_HIGH, //!< Pin configured for output with high level GPIO_OUTPUT_HIGH, //!< Pin configured for output with high level
}; };
struct gpio_direction { struct gpio_direction {
int pin; int pin;
enum gpio_mode mode; enum gpio_mode mode;
}; };
@@ -59,7 +59,7 @@ void gpio_set(uint32_t port, uint16_t pin)
struct gpio_data data; struct gpio_data data;
// Open the device if not open // Open the device if not open
if (gpiofp == 0) if (gpiofp == 0)
gpiofp = open("/dev/gpio",O_RDWR); gpiofp = open("/dev/gpio",O_RDWR);
// Read the GPIO value // Read the GPIO value
data.pin = pin; data.pin = pin;
@@ -74,7 +74,7 @@ void gpio_clear(uint32_t port, uint16_t pin)
struct gpio_data data; struct gpio_data data;
// Open the device if not open // Open the device if not open
if (gpiofp == 0) if (gpiofp == 0)
gpiofp = open("/dev/gpio",O_RDWR); gpiofp = open("/dev/gpio",O_RDWR);
// Read the GPIO value // Read the GPIO value
data.pin = pin; data.pin = pin;
@@ -100,17 +100,17 @@ void gpio_setup_input(uint32_t port, uint16_t pin)
void gpio_setup_output(uint32_t port, uint16_t pin) void gpio_setup_output(uint32_t port, uint16_t pin)
{ {
/* /*
if (port != 0x32524) return; // protect ardrone board from unauthorized use if (port != 0x32524) return; // protect ardrone board from unauthorized use
struct gpio_direction dir; struct gpio_direction dir;
// Open the device if not open // Open the device if not open
if (gpiofp == 0) if (gpiofp == 0)
gpiofp = open("/dev/gpio",O_RDWR); gpiofp = open("/dev/gpio",O_RDWR);
// Read the GPIO value // Read the GPIO value
dir.pin = pin; dir.pin = pin;
dir.mode = GPIO_OUTPUT_LOW; dir.mode = GPIO_OUTPUT_LOW;
ioctl(gpiofp, GPIO_DIRECTION, &dir); ioctl(gpiofp, GPIO_DIRECTION, &dir);
*/ */
} }
sw/airborne/boards/ardrone/navdata.c
+4 -4
View File
@@ -66,7 +66,7 @@ ssize_t full_write(int fd, const uint8_t *buf, size_t count)
if (n < 0) if (n < 0)
{ {
if (errno == EAGAIN || errno == EWOULDBLOCK) if (errno == EAGAIN || errno == EWOULDBLOCK)
continue; continue;
return n; return n;
} }
written += n; written += n;
@@ -85,7 +85,7 @@ ssize_t full_read(int fd, uint8_t *buf, size_t count)
if (n < 0) if (n < 0)
{ {
if (errno == EAGAIN || errno == EWOULDBLOCK) if (errno == EAGAIN || errno == EWOULDBLOCK)
continue; continue;
return n; return n;
} }
readed += n; readed += n;
@@ -469,8 +469,8 @@ void navdata_update()
// Check if there is a new sonar measurement and update the sonar // Check if there is a new sonar measurement and update the sonar
if (navdata.ultrasound >> 15) if (navdata.ultrasound >> 15)
{ {
sonar_meas = (navdata.ultrasound & 0x7FFF); sonar_meas = (navdata.ultrasound & 0x7FFF);
ins_update_sonar(); ins_update_sonar();
} }
#endif #endif
sw/airborne/boards/lisa_m/baro_board.c
+1 -1
View File
@@ -49,7 +49,7 @@ void baro_init(void) {
gpio_clear(GPIOB, GPIO0); gpio_clear(GPIOB, GPIO0);
gpio_set_mode(GPIOB, GPIO_MODE_INPUT, gpio_set_mode(GPIOB, GPIO_MODE_INPUT,
GPIO_CNF_INPUT_PULL_UPDOWN, GPIO0); GPIO_CNF_INPUT_PULL_UPDOWN, GPIO0);
#ifdef BARO_LED #ifdef BARO_LED
LED_OFF(BARO_LED); LED_OFF(BARO_LED);
sw/airborne/firmwares/beth/main_beth.c
+1 -1
View File
@@ -48,7 +48,7 @@ static inline void main_event_task( void ) {
static inline void main_on_bench_sensors( void ) { static inline void main_on_bench_sensors( void ) {
DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, &bench_sensors_angle_1, DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, &bench_sensors_angle_1,
&bench_sensors_angle_2); &bench_sensors_angle_2);
} }
sw/airborne/firmwares/beth/main_coders.c
+1 -1
View File
@@ -219,7 +219,7 @@ static inline void main_init_adc(void) {
/* Enable ADC1 and GPIOC clock */ /* Enable ADC1 and GPIOC clock */
RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2 |
RCC_APB2Periph_GPIOC, ENABLE); RCC_APB2Periph_GPIOC, ENABLE);
/* Configure PC.01 (ADC Channel11) and PC.04 (ADC Channel14) as analog input-*/ /* Configure PC.01 (ADC Channel11) and PC.04 (ADC Channel14) as analog input-*/
GPIO_InitTypeDef GPIO_InitStructure; GPIO_InitTypeDef GPIO_InitStructure;
sw/airborne/firmwares/beth/main_overo.c
+17 -17
View File
@@ -121,25 +121,25 @@ static void main_periodic(int my_sig_num) {
ImuScaleGyro(imu); ImuScaleGyro(imu);
RunOnceEvery(50, {DOWNLINK_SEND_BETH(gcs_com.udp_transport, RunOnceEvery(50, {DOWNLINK_SEND_BETH(gcs_com.udp_transport,
&msg_in.payload.msg_up.bench_sensor.x,&msg_in.payload.msg_up.bench_sensor.y, &msg_in.payload.msg_up.bench_sensor.x,&msg_in.payload.msg_up.bench_sensor.y,
&msg_in.payload.msg_up.bench_sensor.z,&msg_in.payload.msg_up.cnt, &msg_in.payload.msg_up.bench_sensor.z,&msg_in.payload.msg_up.cnt,
&msg_in.payload.msg_up.can_errs,&msg_in.payload.msg_up.spi_errs, &msg_in.payload.msg_up.can_errs,&msg_in.payload.msg_up.spi_errs,
&msg_in.payload.msg_up.thrust_out,&msg_in.payload.msg_up.pitch_out);}); &msg_in.payload.msg_up.thrust_out,&msg_in.payload.msg_up.pitch_out);});
estimator_run(msg_in.payload.msg_up.bench_sensor.z,msg_in.payload.msg_up.bench_sensor.y, estimator_run(msg_in.payload.msg_up.bench_sensor.z,msg_in.payload.msg_up.bench_sensor.y,
msg_in.payload.msg_up.bench_sensor.x); msg_in.payload.msg_up.bench_sensor.x);
if ( msg_in.payload.msg_up.cnt == 0) printf("STM indicates overo link is lost! %d %d\n", if ( msg_in.payload.msg_up.cnt == 0) printf("STM indicates overo link is lost! %d %d\n",
msg_in.payload.msg_up.cnt,msg_in.payload.msg_up.can_errs); msg_in.payload.msg_up.cnt,msg_in.payload.msg_up.can_errs);
if ( msg_in.payload.msg_up.cnt == 1) printf("STM indicates overo link is regained. %d %d\n", if ( msg_in.payload.msg_up.cnt == 1) printf("STM indicates overo link is regained. %d %d\n",
msg_in.payload.msg_up.cnt,msg_in.payload.msg_up.can_errs); msg_in.payload.msg_up.cnt,msg_in.payload.msg_up.can_errs);
//If the stm32 cut the motors due to an error, we force the state machine into spinup mode. //If the stm32 cut the motors due to an error, we force the state machine into spinup mode.
//when the stm32 resumes after the error, the system will need to be rearmed by the user. //when the stm32 resumes after the error, the system will need to be rearmed by the user.
if ( (controller.armed != 0) && (msg_in.payload.msg_up.pitch_out == PITCH_MAGIC_NUMBER) ) { if ( (controller.armed != 0) && (msg_in.payload.msg_up.pitch_out == PITCH_MAGIC_NUMBER) ) {
controller.armed = 0; last_state=1; controller.armed = 0; last_state=1;
printf("STM cut motor power. %d %d\n", printf("STM cut motor power. %d %d\n",
msg_in.payload.msg_up.cnt,msg_in.payload.msg_up.can_errs); msg_in.payload.msg_up.cnt,msg_in.payload.msg_up.can_errs);
} }
@@ -152,20 +152,20 @@ static void main_periodic(int my_sig_num) {
//file_logger_periodic(); //file_logger_periodic();
/* RunOnceEvery(10, {DOWNLINK_SEND_IMU_GYRO_RAW(gcs_com.udp_transport, /* RunOnceEvery(10, {DOWNLINK_SEND_IMU_GYRO_RAW(gcs_com.udp_transport,
//&msg_in.payload.msg_up.gyro.p,&msg_in.payload.msg_up.gyro.q,&msg_in.payload.msg_up.gyro.r) //&msg_in.payload.msg_up.gyro.p,&msg_in.payload.msg_up.gyro.q,&msg_in.payload.msg_up.gyro.r)
&imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r);}); &imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r);});
RunOnceEvery(10, {DOWNLINK_SEND_IMU_ACCEL_RAW(gcs_com.udp_transport, RunOnceEvery(10, {DOWNLINK_SEND_IMU_ACCEL_RAW(gcs_com.udp_transport,
//&msg_in.payload.msg_up.accel.x,&msg_in.payload.msg_up.accel.y,&msg_in.payload.msg_up.accel.z //&msg_in.payload.msg_up.accel.x,&msg_in.payload.msg_up.accel.y,&msg_in.payload.msg_up.accel.z
&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z);}) &imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z);})
RunOnceEvery(50, {DOWNLINK_SEND_IMU_GYRO_SCALED(gcs_com.udp_transport, RunOnceEvery(50, {DOWNLINK_SEND_IMU_GYRO_SCALED(gcs_com.udp_transport,
//&msg_in.payload.msg_up.gyro.p,&msg_in.payload.msg_up.gyro.q,&msg_in.payload.msg_up.gyro.r) //&msg_in.payload.msg_up.gyro.p,&msg_in.payload.msg_up.gyro.q,&msg_in.payload.msg_up.gyro.r)
&imu.gyro.p,&imu.gyro.q,&imu.gyro.r);}); &imu.gyro.p,&imu.gyro.q,&imu.gyro.r);});
RunOnceEvery(50, {DOWNLINK_SEND_AHRS_EULER(gcs_com.udp_transport, RunOnceEvery(50, {DOWNLINK_SEND_AHRS_EULER(gcs_com.udp_transport,
&estimator.tilt, &estimator.elevation, &estimator.azimuth );}); &estimator.tilt, &estimator.elevation, &estimator.azimuth );});
RunOnceEvery(50, {DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, RunOnceEvery(50, {DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel,
//&msg_in.payload.msg_up.accel.x,&msg_in.payload.msg_up.accel.y,&msg_in.payload.msg_up.accel.z //&msg_in.payload.msg_up.accel.x,&msg_in.payload.msg_up.accel.y,&msg_in.payload.msg_up.accel.z
&imu.accel.x,&imu.accel.y,&imu.accel.z);});*/ &imu.accel.x,&imu.accel.y,&imu.accel.z);});*/
RunOnceEvery(33, gcs_com_periodic()); RunOnceEvery(33, gcs_com_periodic());
sw/airborne/firmwares/beth/overo_controller.c
+8 -8
View File
@@ -57,12 +57,12 @@ void control_init(void) {
void control_send_messages(void) { void control_send_messages(void) {
RunOnceEvery(15, {DOWNLINK_SEND_BETH_CONTROLLER(gcs_com.udp_transport, RunOnceEvery(15, {DOWNLINK_SEND_BETH_CONTROLLER(gcs_com.udp_transport,
&controller.cmd_pitch,&controller.cmd_thrust, &controller.cmd_pitch,&controller.cmd_thrust,
&controller.cmd_pitch_ff,&controller.cmd_pitch_fb, &controller.cmd_pitch_ff,&controller.cmd_pitch_fb,
&controller.cmd_thrust_ff,&controller.cmd_thrust_fb, &controller.cmd_thrust_ff,&controller.cmd_thrust_fb,
&controller.tilt_sp,&controller.tilt_ref,&controller.tilt_dot_ref, &controller.tilt_sp,&controller.tilt_ref,&controller.tilt_dot_ref,
&controller.elevation_sp,&controller.elevation_ref,&controller.elevation_dot_ref, &controller.elevation_sp,&controller.elevation_ref,&controller.elevation_dot_ref,
&controller.azimuth_sp);}); &controller.azimuth_sp);});
} }
void control_run(void) { void control_run(void) {
@@ -109,11 +109,11 @@ void control_run(void) {
controller.cmd_pitch_ff = controller.one_over_J * controller.tilt_ddot_ref; controller.cmd_pitch_ff = controller.one_over_J * controller.tilt_ddot_ref;
controller.cmd_pitch_fb = controller.one_over_J * (2 * controller.xi_cl * controller.omega_cl * err_tilt_dot) + controller.cmd_pitch_fb = controller.one_over_J * (2 * controller.xi_cl * controller.omega_cl * err_tilt_dot) +
controller.one_over_J * (controller.omega_cl * controller.omega_cl * err_tilt); controller.one_over_J * (controller.omega_cl * controller.omega_cl * err_tilt);
controller.cmd_thrust_ff = controller.mass * controller.elevation_ddot_ref; controller.cmd_thrust_ff = controller.mass * controller.elevation_ddot_ref;
controller.cmd_thrust_fb = -controller.mass * (2 * controller.xi_cl * controller.omega_cl * err_elevation_dot) - controller.cmd_thrust_fb = -controller.mass * (2 * controller.xi_cl * controller.omega_cl * err_elevation_dot) -
controller.mass * (controller.omega_cl * controller.omega_cl * err_elevation); controller.mass * (controller.omega_cl * controller.omega_cl * err_elevation);
controller.cmd_azimuth_ff = controller.one_over_J * controller.azimuth_ddot_ref; controller.cmd_azimuth_ff = controller.one_over_J * controller.azimuth_ddot_ref;
controller.cmd_azimuth_fb = controller.one_over_J * (2 * controller.xi_cl * controller.omega_cl * err_azimuth_dot) + controller.cmd_azimuth_fb = controller.one_over_J * (2 * controller.xi_cl * controller.omega_cl * err_azimuth_dot) +
sw/airborne/firmwares/beth/overo_estimator.c
+3 -3
View File
@@ -17,9 +17,9 @@ void estimator_init(void) {
void estimator_send_messages(void) { void estimator_send_messages(void) {
RunOnceEvery(25, {DOWNLINK_SEND_BETH_ESTIMATOR(gcs_com.udp_transport, RunOnceEvery(25, {DOWNLINK_SEND_BETH_ESTIMATOR(gcs_com.udp_transport,
&estimator.tilt,&estimator.tilt_dot, &estimator.tilt,&estimator.tilt_dot,
&estimator.elevation,&estimator.elevation_dot, &estimator.elevation,&estimator.elevation_dot,
&estimator.azimuth,&estimator.azimuth_dot);}); &estimator.azimuth,&estimator.azimuth_dot);});
} }
//bench sensors z,y,x values passed in //bench sensors z,y,x values passed in
sw/airborne/firmwares/beth/overo_test_uart.c
+11 -11
View File
@@ -175,16 +175,16 @@ uint16_t downlink_nb_msgs;
#define __DOWNLINK_SEND_HITL_UBX(_chan, class, id, ac_id, nb_ubx_payload, ubx_payload){ \ #define __DOWNLINK_SEND_HITL_UBX(_chan, class, id, ac_id, nb_ubx_payload, ubx_payload){ \
if (DownlinkCheckFreeSpace(_chan, DownlinkSizeOf(_chan, 0+1+1+1+1+nb_ubx_payload*1))) {\ if (DownlinkCheckFreeSpace(_chan, DownlinkSizeOf(_chan, 0+1+1+1+1+nb_ubx_payload*1))) {\
DownlinkCountBytes(_chan, DownlinkSizeOf(_chan, 0+1+1+1+1+nb_ubx_payload*1)); \ DownlinkCountBytes(_chan, DownlinkSizeOf(_chan, 0+1+1+1+1+nb_ubx_payload*1)); \
DownlinkStartMessage(_chan, "HITL_UBX", DL_HITL_UBX, 0+1+1+1+1+nb_ubx_payload*1) \ DownlinkStartMessage(_chan, "HITL_UBX", DL_HITL_UBX, 0+1+1+1+1+nb_ubx_payload*1) \
DownlinkPutUint8ByAddr(_chan, (class)); \ DownlinkPutUint8ByAddr(_chan, (class)); \
DownlinkPutUint8ByAddr(_chan, (id)); \ DownlinkPutUint8ByAddr(_chan, (id)); \
DownlinkPutUint8ByAddr(_chan, (ac_id)); \ DownlinkPutUint8ByAddr(_chan, (ac_id)); \
DownlinkPutUint8Array(_chan, nb_ubx_payload, ubx_payload); \ DownlinkPutUint8Array(_chan, nb_ubx_payload, ubx_payload); \
DownlinkEndMessage(_chan ) \ DownlinkEndMessage(_chan ) \
} else \ } else \
DownlinkOverrun(_chan ); \ DownlinkOverrun(_chan ); \
} }
#endif #endif
@@ -204,7 +204,7 @@ void check_gps(void){
else { else {
if (!donegpsconf) { if (!donegpsconf) {
printf("Finished GPS configuration.\n"); printf("Finished GPS configuration.\n");
donegpsconf=1; donegpsconf=1;
} }
parse_gps_msg(); parse_gps_msg();
} }
sw/airborne/firmwares/beth/overo_twist_controller.c
+146 -146
View File
@@ -13,15 +13,15 @@ struct OveroTwistController controller;
void control_send_messages(void) { void control_send_messages(void) {
RunOnceEvery(15, {DOWNLINK_SEND_BETH_CONTROLLER(gcs_com.udp_transport, RunOnceEvery(15, {DOWNLINK_SEND_BETH_CONTROLLER(gcs_com.udp_transport,
&controller.cmd_pitch,&controller.cmd_thrust, &controller.cmd_pitch,&controller.cmd_thrust,
&controller.cmd_pitch_ff,&controller.cmd_pitch_fb, &controller.cmd_pitch_ff,&controller.cmd_pitch_fb,
&controller.cmd_thrust_ff,&controller.cmd_thrust_fb, &controller.cmd_thrust_ff,&controller.cmd_thrust_fb,
&controller.tilt_sp,&controller.tilt_ref,&controller.tilt_dot_ref, &controller.tilt_sp,&controller.tilt_ref,&controller.tilt_dot_ref,
&controller.elevation_sp,&controller.elevation_ref,&controller.elevation_dot_ref, &controller.elevation_sp,&controller.elevation_ref,&controller.elevation_dot_ref,
&controller.azimuth_sp);}); &controller.azimuth_sp);});
RunOnceEvery(15, {DOWNLINK_SEND_BETH_CONTROLLER_TWIST(gcs_com.udp_transport, RunOnceEvery(15, {DOWNLINK_SEND_BETH_CONTROLLER_TWIST(gcs_com.udp_transport,
&controller.S[1],&controller.S_dot,&controller.U_twt[1],&controller.error);}); &controller.S[1],&controller.S_dot,&controller.U_twt[1],&controller.error);});
} }
@@ -143,13 +143,13 @@ void control_run(void) {
controller.cmd_pitch_ff = controller.one_over_J * controller.tilt_ddot_ref; controller.cmd_pitch_ff = controller.one_over_J * controller.tilt_ddot_ref;
/* controller.cmd_pitch_fb = controller.one_over_J * (2 * controller.xi_cl * controller.omega_cl * err_tilt_dot) + /* controller.cmd_pitch_fb = controller.one_over_J * (2 * controller.xi_cl * controller.omega_cl * err_tilt_dot) +
controller.one_over_J * (controller.omega_cl * controller.omega_cl * err_tilt);*/ controller.one_over_J * (controller.omega_cl * controller.omega_cl * err_tilt);*/
controller.cmd_pitch_fb = get_U_twt(); controller.cmd_pitch_fb = get_U_twt();
controller.cmd_thrust_ff = controller.mass * controller.elevation_ddot_ref; controller.cmd_thrust_ff = controller.mass * controller.elevation_ddot_ref;
controller.cmd_thrust_fb = -controller.mass * (2 * controller.xi_cl * controller.omega_cl * err_elevation_dot) - controller.cmd_thrust_fb = -controller.mass * (2 * controller.xi_cl * controller.omega_cl * err_elevation_dot) -
controller.mass * (controller.omega_cl * controller.omega_cl * err_elevation); controller.mass * (controller.omega_cl * controller.omega_cl * err_elevation);
#if USE_AZIMUTH #if USE_AZIMUTH
controller.cmd_azimuth_ff = controller.one_over_J * controller.azimuth_ddot_ref; controller.cmd_azimuth_ff = controller.one_over_J * controller.azimuth_ddot_ref;
@@ -188,184 +188,184 @@ void control_run(void) {
float get_U_twt() float get_U_twt()
{ {
/**Definition des constantes du modèle**/ /**Definition des constantes du modèle**/
const float Gain = -65; const float Gain = -65;
const float Te = 1/512.; const float Te = 1/512.;
/**Variables utilisés par la loi de commande**/ /**Variables utilisés par la loi de commande**/
static volatile float yd[2] = {0.0,0.0}; static volatile float yd[2] = {0.0,0.0};
static volatile float y[2] = {0.,0.}; static volatile float y[2] = {0.,0.};
//static float emax = 0.035; // en rad, initialement 2 //static float emax = 0.035; // en rad, initialement 2
/**Variables pour l'algorithme**/ /**Variables pour l'algorithme**/
float udot; float udot;
float sens; float sens;
//Acquisition consigne //Acquisition consigne
yd[1] = controller.tilt_ref; yd[1] = controller.tilt_ref;
//Acquisition mesure //Acquisition mesure
y[1] = estimator.tilt; y[1] = estimator.tilt;
/***************************/ /***************************/
/**Calcul Surface et derive Surface**/ /**Calcul Surface et derive Surface**/
// S[1],y[1],yd[1] new value // S[1],y[1],yd[1] new value
// S[0],y[0],yd[0] last value // S[0],y[0],yd[0] last value
//gain K=Te //gain K=Te
//controller.S[1] = (double)( ( (1+controller.c) * (y[1]-yd[1]) - (y[0]-yd[0]) ) ) ; //controller.S[1] = (double)( ( (1+controller.c) * (y[1]-yd[1]) - (y[0]-yd[0]) ) ) ;
//controller.S[1] = (double)( ( (1+controller.c) * (y[1]-yd[1]) - estimator.tilt_dot ) * 0.8 ) ; //controller.S[1] = (double)( ( (1+controller.c) * (y[1]-yd[1]) - estimator.tilt_dot ) * 0.8 ) ;
controller.S[1] = (float)( controller.c * (y[1]-yd[1]) + estimator.tilt_dot - controller.tilt_dot_ref ); controller.S[1] = (float)( controller.c * (y[1]-yd[1]) + estimator.tilt_dot - controller.tilt_dot_ref );
controller.S_dot = (controller.S[1] - controller.S[0]); controller.S_dot = (controller.S[1] - controller.S[0]);
/*************************************/ /*************************************/
//On va dire que si l'erreur est d'un valeur inferieur a emax, on applique la commande anterieure //On va dire que si l'erreur est d'un valeur inferieur a emax, on applique la commande anterieure
/* if ( abs(y[1] - yd[1]) < emax ) { /* if ( abs(y[1] - yd[1]) < emax ) {
U_twt[1] = U_twt[0]; U_twt[1] = U_twt[0];
} else {*/ } else {*/
/**Algorithme twisting**/ /**Algorithme twisting**/
if ( controller.S[1] < 0.0 ) if ( controller.S[1] < 0.0 )
sens = -1.0; sens = -1.0;
else else
sens = 1.0; sens = 1.0;
if ( abs(controller.U_twt[1]) < controller.ulim ) { if ( abs(controller.U_twt[1]) < controller.ulim ) {
if ( (controller.S[1] * controller.S_dot) > 0) { if ( (controller.S[1] * controller.S_dot) > 0) {
udot = -controller.VM * sens; udot = -controller.VM * sens;
} }
else { else {
udot = -controller.Vm * sens; udot = -controller.Vm * sens;
} }
} }
else { else {
udot = -controller.U_twt[1]; udot = -controller.U_twt[1];
} }
// Integration de u, qu'avec 2 valeurs, penser à faire plus // Integration de u, qu'avec 2 valeurs, penser à faire plus
// u[1] new , u[0] old // u[1] new , u[0] old
controller.U_twt[1] = controller.U_twt[0] + (Te * udot); controller.U_twt[1] = controller.U_twt[0] + (Te * udot);
//} //}
/**********************/ /**********************/
/**Saturation de l'integrateur**/ /**Saturation de l'integrateur**/
if ( (controller.S[1] > -controller.satval1) && (controller.S[1] < controller.satval1) ){ if ( (controller.S[1] > -controller.satval1) && (controller.S[1] < controller.satval1) ){
Bound(controller.U_twt[1],-controller.satval1,controller.satval1); Bound(controller.U_twt[1],-controller.satval1,controller.satval1);
} }
else { else {
Bound(controller.U_twt[1],-controller.satval2,controller.satval2); Bound(controller.U_twt[1],-controller.satval2,controller.satval2);
} }
/********************************/ /********************************/
/**Mises à jour**/ /**Mises à jour**/
controller.U_twt[0] = controller.U_twt[1]; controller.U_twt[0] = controller.U_twt[1];
yd[0] = yd[1]; yd[0] = yd[1];
y[0] = y[1]; y[0] = y[1];
controller.S[0] = controller.S[1]; controller.S[0] = controller.S[1];
return Gain * controller.U_twt[1]; return Gain * controller.U_twt[1];
} }
float get_U_twt2() float get_U_twt2()
{ {
/**Definition des constantes du modèle**/ /**Definition des constantes du modèle**/
const float Gain = 800.; const float Gain = 800.;
const float Te = 1/512.; const float Te = 1/512.;
/**Variables utilisés par la loi de commande**/ /**Variables utilisés par la loi de commande**/
static volatile float yd2[2] = {0.0,0.0}; static volatile float yd2[2] = {0.0,0.0};
static volatile float y2[2] = {0.,0.}; static volatile float y2[2] = {0.,0.};
//static float emax = 0.035; // en rad, initialement 2 //static float emax = 0.035; // en rad, initialement 2
/**Variables pour l'algorithme**/ /**Variables pour l'algorithme**/
float udot2; float udot2;
float sens2; float sens2;
static float S2[2]= {0.0,0.0}; static float S2[2]= {0.0,0.0};
static float S2_dot=0; static float S2_dot=0;
static float U2_twt[2]= {0.0,0.0}; static float U2_twt[2]= {0.0,0.0};
//Acquisition consigne //Acquisition consigne
yd2[1] = controller.elevation_ref; yd2[1] = controller.elevation_ref;
//Acquisition mesure //Acquisition mesure
y2[1] = estimator.elevation; y2[1] = estimator.elevation;
/***************************/ /***************************/
/**Calcul Surface et derive Surface**/ /**Calcul Surface et derive Surface**/
// S[1],y[1],yd[1] new value // S[1],y[1],yd[1] new value
// S[0],y[0],yd[0] last value // S[0],y[0],yd[0] last value
//gain K=Te //gain K=Te
//controller.S[1] = (double)( ( (1+controller.c) * (y[1]-yd[1]) - (y[0]-yd[0]) ) ) ; //controller.S[1] = (double)( ( (1+controller.c) * (y[1]-yd[1]) - (y[0]-yd[0]) ) ) ;
//controller.S[1] = (double)( ( (1+controller.c) * (y[1]-yd[1]) - estimator.tilt_dot ) * 0.8 ) ; //controller.S[1] = (double)( ( (1+controller.c) * (y[1]-yd[1]) - estimator.tilt_dot ) * 0.8 ) ;
S2[1] = (float)( controller.c * (y2[1]-yd2[1]) + estimator.elevation_dot - controller.elevation_dot_ref ); S2[1] = (float)( controller.c * (y2[1]-yd2[1]) + estimator.elevation_dot - controller.elevation_dot_ref );
S2_dot = (S2[1] - S2[0]); S2_dot = (S2[1] - S2[0]);
/*************************************/ /*************************************/
//On va dire que si l'erreur est d'un valeur inferieur a emax, on applique la commande anterieure //On va dire que si l'erreur est d'un valeur inferieur a emax, on applique la commande anterieure
/* if ( abs(y[1] - yd[1]) < emax ) { /* if ( abs(y[1] - yd[1]) < emax ) {
U_twt[1] = U_twt[0]; U_twt[1] = U_twt[0];
} else {*/ } else {*/
/**Algorithme twisting**/ /**Algorithme twisting**/
if ( S2[1] < 0.0 ) if ( S2[1] < 0.0 )
sens2 = -1.0; sens2 = -1.0;
else else
sens2 = 1.0; sens2 = 1.0;
if ( abs(U2_twt[1]) < controller.ulim ) { if ( abs(U2_twt[1]) < controller.ulim ) {
if ( (S2[1] * S2_dot) > 0) { if ( (S2[1] * S2_dot) > 0) {
udot2 = -controller.VM * sens2; udot2 = -controller.VM * sens2;
} }
else { else {
udot2 = -controller.Vm * sens2; udot2 = -controller.Vm * sens2;
} }
} }
else { else {
udot2 = -U2_twt[1]; udot2 = -U2_twt[1];
} }
// Integration de u, qu'avec 2 valeurs, penser à faire plus // Integration de u, qu'avec 2 valeurs, penser à faire plus
// u[1] new , u[0] old // u[1] new , u[0] old
U2_twt[1] = U2_twt[0] + (Te * udot2); U2_twt[1] = U2_twt[0] + (Te * udot2);
//} //}
/**********************/ /**********************/
/**Saturation de l'integrateur**/ /**Saturation de l'integrateur**/
if ( (S2[1] > -controller.satval1) && (S2[1] < controller.satval1) ){ if ( (S2[1] > -controller.satval1) && (S2[1] < controller.satval1) ){
Bound(U2_twt[1],-controller.satval1,controller.satval1); Bound(U2_twt[1],-controller.satval1,controller.satval1);
} }
else { else {
Bound(U2_twt[1],-controller.satval2,controller.satval2); Bound(U2_twt[1],-controller.satval2,controller.satval2);
} }
/********************************/ /********************************/
/**Mises à jour**/ /**Mises à jour**/
U2_twt[0] = U2_twt[1]; U2_twt[0] = U2_twt[1];
yd2[0] = yd2[1]; yd2[0] = yd2[1];
y2[0] = y2[1]; y2[0] = y2[1];
S2[0] = S2[1]; S2[0] = S2[1];
#define NUMSAMPS (1000) #define NUMSAMPS (1000)
float retval = Gain * U2_twt[1]; float retval = Gain * U2_twt[1];
static int sum = 0; static int sum = 0;
static int i = 0; static int i = 0;
sum = sum + retval; sum = sum + retval;
if (i == (NUMSAMPS-1)) { if (i == (NUMSAMPS-1)) {
i = 0; i = 0;
printf("avg: %f\n",sum/(float)NUMSAMPS); printf("avg: %f\n",sum/(float)NUMSAMPS);
sum = 0; sum = 0;
} else { } else {
i++; i++;
} }
return retval; return retval;
} }
sw/airborne/firmwares/logger/main_logger.c
+158 -158
View File
@@ -207,38 +207,38 @@ unsigned int clock_lsb_last = 0;
void set_filename(unsigned int local, char* name) void set_filename(unsigned int local, char* name)
{ {
/* do not use sprintf or similar */ /* do not use sprintf or similar */
int i; int i;
for (i=7; i>=0; i--) { for (i=7; i>=0; i--) {
name[i] = (local % 10) + '0'; name[i] = (local % 10) + '0';
local /= 10; local /= 10;
} }
name[8]='.';name[9]='t';name[10]='l';name[11]='m';name[12]=0; name[8]='.';name[9]='t';name[10]='l';name[11]='m';name[12]=0;
} }
unsigned char checksum(unsigned char start, unsigned char* data, int length) unsigned char checksum(unsigned char start, unsigned char* data, int length)
{ {
int i; int i;
unsigned char retval = start; unsigned char retval = start;
for (i=0;i<length;i++) retval += data[i]; for (i=0;i<length;i++) retval += data[i];
return retval; return retval;
} }
unsigned int getclock(void) unsigned int getclock(void)
{ {
uint64_t clock; uint64_t clock;
uint32_t clock_lsb; uint32_t clock_lsb;
clock_lsb = T0TC; clock_lsb = T0TC;
if (clock_lsb < clock_lsb_last) clock_msb++; if (clock_lsb < clock_lsb_last) clock_msb++;
clock_lsb_last = clock_lsb; clock_lsb_last = clock_lsb;
clock = (((uint64_t)clock_msb << 32) | (uint64_t)clock_lsb) / LOG_DIV; clock = (((uint64_t)clock_msb << 32) | (uint64_t)clock_lsb) / LOG_DIV;
return(clock & 0xFFFFFFFF); return(clock & 0xFFFFFFFF);
} }
/** Parsing a frame data and copy the payload to the log buffer */ /** Parsing a frame data and copy the payload to the log buffer */
@@ -286,49 +286,49 @@ void log_xbee(unsigned char c, unsigned char source)
static unsigned char cs, payload_idx, i; static unsigned char cs, payload_idx, i;
switch (xbeel_status) { switch (xbeel_status) {
case XBEE_UNINIT: case XBEE_UNINIT:
if (c == XBEE_START) if (c == XBEE_START)
{ {
// serial receive broken with MAX // serial receive broken with MAX
#ifndef USE_MAX11040 #ifndef USE_MAX11040
xbeel_timestamp = getclock(); xbeel_timestamp = getclock();
#endif #endif
xbeel_status++;
}
break;
case XBEE_GOT_START:
xbeel_payload_len = c<<8;
xbeel_status++; xbeel_status++;
} break;
break; case XBEE_GOT_LENGTH_MSB:
case XBEE_GOT_START: xbeel_payload_len |= c;
xbeel_payload_len = c<<8;
xbeel_status++;
break;
case XBEE_GOT_LENGTH_MSB:
xbeel_payload_len |= c;
xbeel_status++;
payload_idx = 0;
cs=0;
break;
case XBEE_GOT_LENGTH_LSB:
xbeel_payload[payload_idx] = c;
cs += c;
payload_idx++;
if (payload_idx == xbeel_payload_len)
xbeel_status++; xbeel_status++;
break; payload_idx = 0;
case XBEE_GOT_PAYLOAD: cs=0;
if (c + cs != 0xff) break;
goto error; case XBEE_GOT_LENGTH_LSB:
if ((xbeel_payload[0] != XBEE_RX16_ID) && xbeel_payload[payload_idx] = c;
(xbeel_payload[0] != XBEE_TX16_ID)) cs += c;
goto error; payload_idx++;
/* copy the XBee message to the logger buffer */ if (payload_idx == xbeel_payload_len)
for (i = 0; i < xbeel_payload_len-XBEE_RFDATA_OFFSET; i++) { xbeel_status++;
log_buffer[i+LOG_DATA_OFFSET] = xbeel_payload[i+XBEE_RFDATA_OFFSET]; break;
} case XBEE_GOT_PAYLOAD:
// serial receive broken with MAX if (c + cs != 0xff)
goto error;
if ((xbeel_payload[0] != XBEE_RX16_ID) &&
(xbeel_payload[0] != XBEE_TX16_ID))
goto error;
/* copy the XBee message to the logger buffer */
for (i = 0; i < xbeel_payload_len-XBEE_RFDATA_OFFSET; i++) {
log_buffer[i+LOG_DATA_OFFSET] = xbeel_payload[i+XBEE_RFDATA_OFFSET];
}
// serial receive broken with MAX
#ifndef USE_MAX11040 #ifndef USE_MAX11040
log_payload(xbeel_payload_len-XBEE_RFDATA_OFFSET, source, xbeel_timestamp); log_payload(xbeel_payload_len-XBEE_RFDATA_OFFSET, source, xbeel_timestamp);
#endif #endif
LED_TOGGLE(LED_GREEN); LED_TOGGLE(LED_GREEN);
goto restart; goto restart;
} }
return; return;
error: error:
@@ -344,45 +344,45 @@ void log_pprz(unsigned char c, unsigned char source)
static unsigned char _ck_a, _ck_b, payload_idx, i; static unsigned char _ck_a, _ck_b, payload_idx, i;
switch (pprzl_status) { switch (pprzl_status) {
case UNINIT: case UNINIT:
if (c == STX) if (c == STX)
// serial receive broken with MAX // serial receive broken with MAX
#ifndef USE_MAX11040 #ifndef USE_MAX11040
pprzl_timestamp = getclock(); pprzl_timestamp = getclock();
#endif #endif
pprzl_status++; pprzl_status++;
break; break;
case GOT_STX: case GOT_STX:
pprzl_payload_len = c-4; /* Counting STX, LENGTH and CRC1 and CRC2 */ pprzl_payload_len = c-4; /* Counting STX, LENGTH and CRC1 and CRC2 */
_ck_a = _ck_b = c; _ck_a = _ck_b = c;
pprzl_status++;
payload_idx = 0;
break;
case GOT_LENGTH:
pprzl_payload[payload_idx] = c;
_ck_a += c; _ck_b += _ck_a;
payload_idx++;
if (payload_idx == pprzl_payload_len)
pprzl_status++; pprzl_status++;
break; payload_idx = 0;
case GOT_PAYLOAD: break;
if (c != _ck_a) case GOT_LENGTH:
goto error; pprzl_payload[payload_idx] = c;
pprzl_status++; _ck_a += c; _ck_b += _ck_a;
break; payload_idx++;
case GOT_CRC1: if (payload_idx == pprzl_payload_len)
if (c != _ck_b) pprzl_status++;
goto error; break;
/* copy the pprz message to the logger buffer */ case GOT_PAYLOAD:
for (i = 0; i < pprzl_payload_len; i++) { if (c != _ck_a)
log_buffer[i+LOG_DATA_OFFSET] = pprzl_payload[i]; goto error;
} pprzl_status++;
// serial receive broken with MAX break;
case GOT_CRC1:
if (c != _ck_b)
goto error;
/* copy the pprz message to the logger buffer */
for (i = 0; i < pprzl_payload_len; i++) {
log_buffer[i+LOG_DATA_OFFSET] = pprzl_payload[i];
}
// serial receive broken with MAX
#ifndef USE_MAX11040 #ifndef USE_MAX11040
log_payload(pprzl_payload_len, source, pprzl_timestamp); log_payload(pprzl_payload_len, source, pprzl_timestamp);
#endif #endif
LED_TOGGLE(LED_GREEN); LED_TOGGLE(LED_GREEN);
goto restart; goto restart;
} }
return; return;
error: error:
@@ -394,100 +394,100 @@ void log_pprz(unsigned char c, unsigned char source)
int do_log(void) int do_log(void)
{ {
unsigned int count; unsigned int count;
unsigned char name[13]; unsigned char name[13];
unsigned char inc; unsigned char inc;
int temp; int temp;
if(efs_init(&efs, 0) != 0) { if(efs_init(&efs, 0) != 0) {
return(-1); return(-1);
} }
/* find an unused file number the dumb way */ /* find an unused file number the dumb way */
for (count = 1; count < 0xFFFFFFF; count++) for (count = 1; count < 0xFFFFFFF; count++)
{ {
set_filename(count, name); set_filename(count, name);
if(file_fopen(&filer, &efs.myFs, name,'r')!=0) break; if(file_fopen(&filer, &efs.myFs, name,'r')!=0) break;
file_fclose(&filer); file_fclose(&filer);
} }
if (file_fopen(&filew, &efs.myFs, name, 'w' ) != 0) if (file_fopen(&filew, &efs.myFs, name, 'w' ) != 0)
{ {
return(-1); return(-1);
} }
/* write to SD until key is pressed */ /* write to SD until key is pressed */
while ((IO0PIN & (1<<LOG_STOP_KEY))>>LOG_STOP_KEY) while ((IO0PIN & (1<<LOG_STOP_KEY))>>LOG_STOP_KEY)
{ {
#ifdef USE_MAX11040 #ifdef USE_MAX11040
if ((max11040_data == MAX11040_DATA_AVAILABLE) && if ((max11040_data == MAX11040_DATA_AVAILABLE) &&
(max11040_buf_in != max11040_buf_out)) { (max11040_buf_in != max11040_buf_out)) {
// LED_TOGGLE(LED_GREEN); // LED_TOGGLE(LED_GREEN);
int i; int i;
max11040_data = MAX11040_IDLE; max11040_data = MAX11040_IDLE;
log_buffer[LOG_DATA_OFFSET+0] = AC_ID; // sender_id log_buffer[LOG_DATA_OFFSET+0] = AC_ID; // sender_id
log_buffer[LOG_DATA_OFFSET+1] = 61; // message_id (DL_TURB_PRESSURE_RAW) log_buffer[LOG_DATA_OFFSET+1] = 61; // message_id (DL_TURB_PRESSURE_RAW)
while(max11040_buf_in != max11040_buf_out) { while(max11040_buf_in != max11040_buf_out) {
for (i=0; i<16; i++) { for (i=0; i<16; i++) {
log_buffer[LOG_DATA_OFFSET+2 + i*4 + 0] = (max11040_values[max11040_buf_out][i] ) & 0xFF; log_buffer[LOG_DATA_OFFSET+2 + i*4 + 0] = (max11040_values[max11040_buf_out][i] ) & 0xFF;
log_buffer[LOG_DATA_OFFSET+2 + i*4 + 1] = (max11040_values[max11040_buf_out][i] >> 8 ) & 0xFF; log_buffer[LOG_DATA_OFFSET+2 + i*4 + 1] = (max11040_values[max11040_buf_out][i] >> 8 ) & 0xFF;
log_buffer[LOG_DATA_OFFSET+2 + i*4 + 2] = (max11040_values[max11040_buf_out][i] >> 16) & 0xFF; log_buffer[LOG_DATA_OFFSET+2 + i*4 + 2] = (max11040_values[max11040_buf_out][i] >> 16) & 0xFF;
log_buffer[LOG_DATA_OFFSET+2 + i*4 + 3] = (max11040_values[max11040_buf_out][i] >> 24) & 0xFF; log_buffer[LOG_DATA_OFFSET+2 + i*4 + 3] = (max11040_values[max11040_buf_out][i] >> 24) & 0xFF;
} }
log_payload(2 + 16 * 4, LOG_SOURCE_UART0, max11040_timestamp[max11040_buf_out]); log_payload(2 + 16 * 4, LOG_SOURCE_UART0, max11040_timestamp[max11040_buf_out]);
i = max11040_buf_out+1; i = max11040_buf_out+1;
if (i >= MAX11040_BUF_SIZE) i=0; if (i >= MAX11040_BUF_SIZE) i=0;
max11040_buf_out = i; max11040_buf_out = i;
}
} }
}
#endif #endif
#if USE_UART0 #if USE_UART0
temp = 0; temp = 0;
while (uart_char_available(&uart0) && (temp++ < 128)) while (uart_char_available(&uart0) && (temp++ < 128))
{ {
// LED_TOGGLE(LED_GREEN); // LED_TOGGLE(LED_GREEN);
inc = uart_getch(&uart1); inc = uart_getch(&uart1);
#ifdef LOG_XBEE #ifdef LOG_XBEE
log_xbee(inc, LOG_SOURCE_UART0); log_xbee(inc, LOG_SOURCE_UART0);
#else #else
#ifdef LOG_PPRZ #ifdef LOG_PPRZ
log_pprz(inc, LOG_SOURCE_UART0); log_pprz(inc, LOG_SOURCE_UART0);
#else #else
#error no log transport protocol selected #error no log transport protocol selected
#endif #endif
#endif
}
#endif
#if USE_UART1
temp = 0;
while (uart_char_available(&uart1) && (temp++ < 128))
{
// LED_TOGGLE(LED_GREEN);
inc = uart_getch(&uart1);
#ifdef LOG_XBEE
log_xbee(inc, LOG_SOURCE_UART1);
#else
#ifdef LOG_PPRZ
log_pprz(inc, LOG_SOURCE_UART1);
#else
#error no log transport protocol selected
#endif
#endif
}
#endif #endif
} }
LED_OFF(LED_GREEN); #endif
#if USE_UART1
temp = 0;
while (uart_char_available(&uart1) && (temp++ < 128))
{
// LED_TOGGLE(LED_GREEN);
inc = uart_getch(&uart1);
#ifdef LOG_XBEE
log_xbee(inc, LOG_SOURCE_UART1);
#else
#ifdef LOG_PPRZ
log_pprz(inc, LOG_SOURCE_UART1);
#else
#error no log transport protocol selected
#endif
#endif
}
#endif
}
LED_OFF(LED_GREEN);
file_fclose( &filew ); file_fclose( &filew );
fs_umount( &efs.myFs ) ; fs_umount( &efs.myFs ) ;
return 0; return 0;
} }
int main(void) int main(void)
sw/airborne/firmwares/motor_bench/mb_scale.h
+20 -20
View File
@@ -23,29 +23,29 @@ extern volatile uint8_t mb_scale_calib;
void mb_scale_init ( void ); void mb_scale_init ( void );
#define MB_SCALE_IT TIR_CR3I #define MB_SCALE_IT TIR_CR3I
#define MB_SCALE_ICP_ISR() { \ #define MB_SCALE_ICP_ISR() { \
static uint32_t last; \ static uint32_t last; \
uint32_t now = T0CR3; \ uint32_t now = T0CR3; \
mb_scale_pulse_len = now - last; \ mb_scale_pulse_len = now - last; \
last = now; \ last = now; \
if (mb_scale_calib > 0) { \ if (mb_scale_calib > 0) { \
mb_scale_thrust += mb_scale_pulse_len; \ mb_scale_thrust += mb_scale_pulse_len; \
if (mb_scale_calib >= MB_SCALE_NB_CALIB) { \ if (mb_scale_calib >= MB_SCALE_NB_CALIB) { \
mb_scale_neutral = mb_scale_thrust / MB_SCALE_NB_CALIB; \ mb_scale_neutral = mb_scale_thrust / MB_SCALE_NB_CALIB; \
mb_scale_calib = 0; \ mb_scale_calib = 0; \
} \ } \
else \ else \
mb_scale_calib++; \ mb_scale_calib++; \
} \ } \
else { \ else { \
int32_t diff = (int32_t)mb_scale_pulse_len - (int32_t)mb_scale_neutral; \ int32_t diff = (int32_t)mb_scale_pulse_len - (int32_t)mb_scale_neutral; \
mb_scale_thrust = mb_scale_gain * diff; \ mb_scale_thrust = mb_scale_gain * diff; \
} \ } \
} }
#define mb_scale_Calib(_val) { \ #define mb_scale_Calib(_val) { \
mb_scale_calib = 1; \ mb_scale_calib = 1; \
mb_scale_thrust = 0.; \ mb_scale_thrust = 0.; \
} }
#endif /* MB_SCALE_H */ #endif /* MB_SCALE_H */
sw/airborne/firmwares/motor_bench/mb_twi_controller_asctech.c
+26 -26
View File
@@ -37,34 +37,34 @@ void mb_twi_controller_set( float throttle ) {
mb_twi_controller_asctech_command = FALSE; mb_twi_controller_asctech_command = FALSE;
switch (mb_twi_controller_asctech_command_type) { switch (mb_twi_controller_asctech_command_type) {
case MB_TWI_CONTROLLER_COMMAND_TEST : case MB_TWI_CONTROLLER_COMMAND_TEST :
i2c0_buf[0] = 251; i2c0_buf[0] = 251;
i2c0_buf[1] = mb_twi_controller_asctech_addr; i2c0_buf[1] = mb_twi_controller_asctech_addr;
i2c0_buf[2] = 0; i2c0_buf[2] = 0;
i2c0_buf[3] = 231 + mb_twi_controller_asctech_addr; i2c0_buf[3] = 231 + mb_twi_controller_asctech_addr;
// mb_twi_i2c_done = FALSE; // mb_twi_i2c_done = FALSE;
i2c0_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done); i2c0_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
break; break;
case MB_TWI_CONTROLLER_COMMAND_REVERSE : case MB_TWI_CONTROLLER_COMMAND_REVERSE :
i2c0_buf[0] = 254; i2c0_buf[0] = 254;
i2c0_buf[1] = mb_twi_controller_asctech_addr; i2c0_buf[1] = mb_twi_controller_asctech_addr;
i2c0_buf[2] = 0; i2c0_buf[2] = 0;
i2c0_buf[3] = 234 + mb_twi_controller_asctech_addr; i2c0_buf[3] = 234 + mb_twi_controller_asctech_addr;
// mb_twi_i2c_done = FALSE; // mb_twi_i2c_done = FALSE;
i2c0_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done); i2c0_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
break; break;
case MB_TWI_CONTROLLER_COMMAND_SET_ADDR : case MB_TWI_CONTROLLER_COMMAND_SET_ADDR :
i2c0_buf[0] = 250; i2c0_buf[0] = 250;
i2c0_buf[1] = mb_twi_controller_asctech_addr; i2c0_buf[1] = mb_twi_controller_asctech_addr;
i2c0_buf[2] = mb_twi_controller_asctech_new_addr; i2c0_buf[2] = mb_twi_controller_asctech_new_addr;
i2c0_buf[3] = 230 + mb_twi_controller_asctech_addr + i2c0_buf[3] = 230 + mb_twi_controller_asctech_addr +
mb_twi_controller_asctech_new_addr; mb_twi_controller_asctech_new_addr;
mb_twi_controller_asctech_addr = mb_twi_controller_asctech_new_addr; mb_twi_controller_asctech_addr = mb_twi_controller_asctech_new_addr;
// mb_twi_i2c_done = FALSE; // mb_twi_i2c_done = FALSE;
i2c0_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done); i2c0_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
break; break;
} }
} }
sw/airborne/firmwares/non_ap/geiger_counter/geiger_counter.c
+70 -70
View File
@@ -84,86 +84,86 @@ void loop() {
if (Serial.available() > 0) { if (Serial.available() > 0) {
ser = Serial.read(); ser = Serial.read();
switch (stat) { switch (stat) {
case INIT: case INIT:
/* sync on the last byte of the prev message */ /* sync on the last byte of the prev message */
if (b == END_MSG) { if (b == END_MSG) {
count_geiger_1 = 0; count_geiger_1 = 0;
count_geiger_2 = 0; count_geiger_2 = 0;
volt_geiger = 0; volt_geiger = 0;
i = 0; i = 0;
stat = FOUND_SYNC; stat = FOUND_SYNC;
} }
break; break;
case FOUND_SYNC: case FOUND_SYNC:
if ((b <= '9') && (b >= '0')) { if ((b <= '9') && (b >= '0')) {
count_geiger_1 = count_geiger_1 * 10 + (b-'0'); count_geiger_1 = count_geiger_1 * 10 + (b-'0');
if (++i > 7) state = IDLE; if (++i > 7) state = IDLE;
} else if (b == ',')) { } else if (b == ',')) {
i = 0; i = 0;
stat = FOUND_1; stat = FOUND_1;
} else stat = INIT; } else stat = INIT;
break; break;
case FOUND_1: case FOUND_1:
/* read counter 1 */ /* read counter 1 */
if ((b <= '9') && (b >= '0')) { if ((b <= '9') && (b >= '0')) {
count_geiger_2 = count_geiger_2 * 10 + (b-'0'); count_geiger_2 = count_geiger_2 * 10 + (b-'0');
if (++i > 7) state = IDLE; if (++i > 7) state = IDLE;
} else if (b == ',')) { } else if (b == ',')) {
#ifdef DEBUG #ifdef DEBUG
Serial.println(count_geiger_1, DEC); Serial.println(count_geiger_1, DEC);
#endif #endif
i = 0; i = 0;
stat = FOUND_2; stat = FOUND_2;
} else stat = INIT; } else stat = INIT;
break; break;
case FOUND_2: case FOUND_2:
/* read counter 2 */ /* read counter 2 */
if ((b <= '9') && (b >= '0')) { if ((b <= '9') && (b >= '0')) {
count_geiger_2 = count_geiger_2 * 10 + (b-'0'); count_geiger_2 = count_geiger_2 * 10 + (b-'0');
if (++i > 7) state = IDLE; if (++i > 7) state = IDLE;
} else if (b == ',')) { } else if (b == ',')) {
#ifdef DEBUG #ifdef DEBUG
Serial.println(count_geiger_2, DEC); Serial.println(count_geiger_2, DEC);
#endif #endif
i = 0; i = 0;
stat = FOUND_3; stat = FOUND_3;
} else stat = INIT; } else stat = INIT;
break; break;
case FOUND_3: case FOUND_3:
/* ignore 3 */ /* ignore 3 */
if ((b <= '9') && (b >= '0')) { if ((b <= '9') && (b >= '0')) {
if (++i > 7) state = IDLE; if (++i > 7) state = IDLE;
} else if (b == ',')) { } else if (b == ',')) {
i = 0; i = 0;
stat = FOUND_4; stat = FOUND_4;
} else stat = INIT; } else stat = INIT;
break; break;
case FOUND_4: case FOUND_4:
/* ignore 4 */ /* ignore 4 */
if ((b <= '9') && (b >= '0')) { if ((b <= '9') && (b >= '0')) {
if (++i > 7) state = IDLE; if (++i > 7) state = IDLE;
} else if (b == ',')) { } else if (b == ',')) {
i = 0; i = 0;
stat = FOUND_5; stat = FOUND_5;
} else stat = INIT; } else stat = INIT;
break; break;
case FOUND_5: case FOUND_5:
/* read voltage */ /* read voltage */
if ((b <= '9') && (b >= '0')) { if ((b <= '9') && (b >= '0')) {
volt_geiger = volt_geiger * 10 + (b-'0'); volt_geiger = volt_geiger * 10 + (b-'0');
if (++i > 7) state = IDLE; if (++i > 7) state = IDLE;
} else if (b == 'V')) { } else if (b == 'V')) {
digitalWrite(LED_GR_PIN, HIGH); digitalWrite(LED_GR_PIN, HIGH);
#ifdef DEBUG #ifdef DEBUG
Serial.println(volt_geiger, DEC); Serial.println(volt_geiger, DEC);
#endif #endif
received_data = 0; received_data = 0;
stat = INIT; stat = INIT;
} else stat = INIT; } else stat = INIT;
break; break;
default: default:
stat = INIT; stat = INIT;
} }
} }
} }
sw/airborne/fms/fms_autopilot_msg.h
+5 -5
View File
@@ -72,7 +72,7 @@ struct __attribute__ ((packed)) AutopilotMessageTWDown
*/ */
struct __attribute__ ((packed)) ADCMessage { struct __attribute__ ((packed)) ADCMessage {
uint16_t channels[NB_ADC]; uint16_t channels[NB_ADC];
}; };
/* used to indicate parts of the message which actually represent a new measurement */ /* used to indicate parts of the message which actually represent a new measurement */
@@ -91,7 +91,7 @@ struct __attribute__ ((packed)) AutopilotMessagePTUp
struct Int32Rates gyro; struct Int32Rates gyro;
struct Int32Vect3 accel; struct Int32Vect3 accel;
struct Int32Vect3 mag; struct Int32Vect3 mag;
uint32_t imu_tick; uint32_t imu_tick;
int32_t pressure_absolute; int32_t pressure_absolute;
int32_t pressure_differential; int32_t pressure_differential;
int16_t rc_pitch; int16_t rc_pitch;
@@ -194,9 +194,9 @@ struct __attribute__ ((packed)) AutopilotMessageVIDown
#endif #endif
struct __attribute__ ((packed)) AutopilotMessagePTStream struct __attribute__ ((packed)) AutopilotMessagePTStream
{ {
uint8_t message_cnt; uint8_t message_cnt;
int8_t package_cntd; int8_t package_cntd;
uint8_t pkg_data[SPISTREAM_PACKAGE_SIZE]; uint8_t pkg_data[SPISTREAM_PACKAGE_SIZE];
}; };
/* Union for computing size of SPI transfer (largest of either up or down message) */ /* Union for computing size of SPI transfer (largest of either up or down message) */
sw/airborne/fms/fms_gs_com.c
+2 -2
View File
@@ -24,12 +24,12 @@ static void on_datalink_event(int fd, short event __attribute__((unused)), void
static void on_datalink_message(void); static void on_datalink_message(void);
uint8_t fms_gs_com_init(const char* gs_host, uint16_t gs_port, uint8_t fms_gs_com_init(const char* gs_host, uint16_t gs_port,
uint16_t datalink_port, uint8_t broadcast) { uint16_t datalink_port, uint8_t broadcast) {
fms_gs_com.network = network_new(gs_host, gs_port, datalink_port, broadcast); fms_gs_com.network = network_new(gs_host, gs_port, datalink_port, broadcast);
fms_gs_com.udp_transport = udp_transport_new(fms_gs_com.network); fms_gs_com.udp_transport = udp_transport_new(fms_gs_com.network);
event_set(&fms_gs_com.datalink_event, fms_gs_com.network->socket_in, EV_READ | EV_PERSIST, event_set(&fms_gs_com.datalink_event, fms_gs_com.network->socket_in, EV_READ | EV_PERSIST,
on_datalink_event, fms_gs_com.udp_transport); on_datalink_event, fms_gs_com.udp_transport);
event_add(&fms_gs_com.datalink_event, NULL); event_add(&fms_gs_com.datalink_event, NULL);
return 0; return 0;
sw/airborne/fms/fms_gs_com.h
+1 -1
View File
@@ -28,7 +28,7 @@ extern struct FmsGsCom fms_gs_com;
extern uint8_t telemetry_mode_Main_DefaultChannel; extern uint8_t telemetry_mode_Main_DefaultChannel;
extern uint8_t fms_gs_com_init(const char* gs_host, uint16_t gs_port, extern uint8_t fms_gs_com_init(const char* gs_host, uint16_t gs_port,
uint16_t datalink_port, uint8_t broadcast); uint16_t datalink_port, uint8_t broadcast);
extern void fms_gs_com_periodic(void); extern void fms_gs_com_periodic(void);
#endif /* FMS_GS_COM_H */ #endif /* FMS_GS_COM_H */
sw/airborne/fms/fms_spi_autopilot_msg.c
+11 -11
View File
@@ -58,8 +58,8 @@ void spi_ap_link_downlink_send(struct DownlinkTransport *tp)
{ {
uint32_t timestamp = 0; uint32_t timestamp = 0;
DOWNLINK_SEND_EKF7_Y(tp, &timestamp, &imuFloat.accel.x, &imuFloat.accel.y, &imuFloat.accel.z, DOWNLINK_SEND_EKF7_Y(tp, &timestamp, &imuFloat.accel.x, &imuFloat.accel.y, &imuFloat.accel.z,
&imuFloat.mag.x, &imuFloat.mag.y, &imuFloat.mag.z, &imuFloat.mag.x, &imuFloat.mag.y, &imuFloat.mag.z,
&imuFloat.gyro.p, &imuFloat.gyro.q, &imuFloat.gyro.r); &imuFloat.gyro.p, &imuFloat.gyro.q, &imuFloat.gyro.r);
} }
void spi_ap_link_set_vane_callback(void (* vane_cb)(uint8_t vane_id, float alpha, float beta)) void spi_ap_link_set_vane_callback(void (* vane_cb)(uint8_t vane_id, float alpha, float beta))
@@ -84,7 +84,7 @@ void spi_ap_link_set_radio_control_callback(void (* radio_control_cb)(void))
void spi_ap_link_set_adc_callback(void (* adc_callback_fun)(uint16_t * adc_channels)) void spi_ap_link_set_adc_callback(void (* adc_callback_fun)(uint16_t * adc_channels))
{ {
adc_callback = adc_callback_fun; adc_callback = adc_callback_fun;
} }
int spi_ap_link_init() int spi_ap_link_init()
@@ -99,13 +99,13 @@ int spi_ap_link_init()
imuFloat.sample_count = 0; imuFloat.sample_count = 0;
#ifdef IMU_ALIGN_BENCH #ifdef IMU_ALIGN_BENCH
// This code is for aligning body to imu rotation, turn this on, put the vehicle in hover, pointed north, read BOOZ2_AHRS_REF_QUAT as body to imu (in wing frame) // This code is for aligning body to imu rotation, turn this on, put the vehicle in hover, pointed north, read BOOZ2_AHRS_REF_QUAT as body to imu (in wing frame)
struct FloatVect3 x_axis = { 0.0, 1.0, 0.0 }; struct FloatVect3 x_axis = { 0.0, 1.0, 0.0 };
FLOAT_QUAT_OF_AXIS_ANGLE(imuFloat.body_to_imu_quat, x_axis, QUAT_SETPOINT_HOVER_PITCH); FLOAT_QUAT_OF_AXIS_ANGLE(imuFloat.body_to_imu_quat, x_axis, QUAT_SETPOINT_HOVER_PITCH);
#endif #endif
FLOAT_QUAT_NORMALIZE(imuFloat.body_to_imu_quat); FLOAT_QUAT_NORMALIZE(imuFloat.body_to_imu_quat);
FLOAT_EULERS_OF_QUAT(imuFloat.body_to_imu_eulers, imuFloat.body_to_imu_quat); FLOAT_EULERS_OF_QUAT(imuFloat.body_to_imu_eulers, imuFloat.body_to_imu_quat);
FLOAT_RMAT_OF_QUAT(imuFloat.body_to_imu_rmat, imuFloat.body_to_imu_quat); FLOAT_RMAT_OF_QUAT(imuFloat.body_to_imu_rmat, imuFloat.body_to_imu_quat);
struct FloatRates bias0 = { 0., 0., 0.}; struct FloatRates bias0 = { 0., 0., 0.};
@@ -127,11 +127,11 @@ static void passthrough_up_parse(struct AutopilotMessagePTUp *msg_up)
if (msg_up->valid.pressure_differential && pressure_differential_callback) if (msg_up->valid.pressure_differential && pressure_differential_callback)
pressure_differential_callback(0, (32768 + msg_up->pressure_differential)); pressure_differential_callback(0, (32768 + msg_up->pressure_differential));
if (msg_up->valid.adc) { if (msg_up->valid.adc) {
if(adc_callback) { if(adc_callback) {
adc_callback(msg_up->adc.channels); adc_callback(msg_up->adc.channels);
} }
} }
// Fill radio data // Fill radio data
if (msg_up->valid.rc && radio_control_callback) { if (msg_up->valid.rc && radio_control_callback) {
@@ -162,7 +162,7 @@ static void passthrough_up_parse(struct AutopilotMessagePTUp *msg_up)
imuFloat.mag.y = MAG_FLOAT_OF_BFP(msg_up->mag.y); imuFloat.mag.y = MAG_FLOAT_OF_BFP(msg_up->mag.y);
imuFloat.mag.z = MAG_FLOAT_OF_BFP(msg_up->mag.z); imuFloat.mag.z = MAG_FLOAT_OF_BFP(msg_up->mag.z);
imuFloat.sample_count = msg_up->imu_tick; imuFloat.sample_count = msg_up->imu_tick;
if (msg_up->valid.imu) if (msg_up->valid.imu)
rdyb_booz_imu_update(&imuFloat); rdyb_booz_imu_update(&imuFloat);
sw/airborne/fms/fms_spi_link.c
+24 -24
View File
@@ -43,30 +43,30 @@ int spi_link_init(void) {
int spi_link_send(void *buf_out, size_t count, void *buf_in, uint8_t* crc_valid) { int spi_link_send(void *buf_out, size_t count, void *buf_in, uint8_t* crc_valid) {
int ret; int ret;
struct spi_ioc_transfer tr = { struct spi_ioc_transfer tr = {
.tx_buf = (unsigned long)buf_out, .tx_buf = (unsigned long)buf_out,
.rx_buf = (unsigned long)buf_in, .rx_buf = (unsigned long)buf_in,
.len = count, .len = count,
.delay_usecs = spi_link.delay, .delay_usecs = spi_link.delay,
.speed_hz = spi_link.speed, .speed_hz = spi_link.speed,
.bits_per_word = spi_link.bits, .bits_per_word = spi_link.bits,
}; };
((uint8_t*)buf_out)[count-1] = crc_calc_block_crc8(buf_out, count-1); ((uint8_t*)buf_out)[count-1] = crc_calc_block_crc8(buf_out, count-1);
ret = ioctl(spi_link.fd, SPI_IOC_MESSAGE(1), &tr); ret = ioctl(spi_link.fd, SPI_IOC_MESSAGE(1), &tr);
spi_link.msg_cnt++; spi_link.msg_cnt++;
uint8_t computed_crc = crc_calc_block_crc8(buf_in, count-1); uint8_t computed_crc = crc_calc_block_crc8(buf_in, count-1);
if (computed_crc == ((uint8_t*)buf_in)[count-1]) if (computed_crc == ((uint8_t*)buf_in)[count-1])
*crc_valid = 1; *crc_valid = 1;
else { else {
*crc_valid = 0; *crc_valid = 0;
spi_link.crc_err_cnt++; spi_link.crc_err_cnt++;
} }
return ret; return ret;
} }
@@ -80,9 +80,9 @@ uint8_t crc_calc_block_crc8(const uint8_t buf[], uint32_t len) {
_remainder ^= (buf[byte] << (WIDTH - 8)); _remainder ^= (buf[byte] << (WIDTH - 8));
for (uint8_t bit = 8; bit > 0; --bit) { for (uint8_t bit = 8; bit > 0; --bit) {
if (_remainder & TOPBIT) if (_remainder & TOPBIT)
_remainder = (_remainder << 1) ^ POLYNOMIAL; _remainder = (_remainder << 1) ^ POLYNOMIAL;
else else
_remainder = (_remainder << 1); _remainder = (_remainder << 1);
} }
} }
return (_remainder); return (_remainder);
@@ -102,10 +102,10 @@ void crc__init(uint32_t polynomial) {
crc_remainder = crc_dividend << (CRC__WIDTH - 8); crc_remainder = crc_dividend << (CRC__WIDTH - 8);
for(bit = 8; bit > 0; bit--) { for(bit = 8; bit > 0; bit--) {
if(crc_remainder & top_bit) { if(crc_remainder & top_bit) {
crc_remainder = (crc_remainder << 1) ^ polynomial; crc_remainder = (crc_remainder << 1) ^ polynomial;
} }
else { else {
crc_remainder = (crc_remainder << 1); crc_remainder = (crc_remainder << 1);
} }
} }
crc__table[crc_dividend] = crc_remainder; crc__table[crc_dividend] = crc_remainder;
sw/airborne/fms/fms_spistream.h
+13 -13
View File
@@ -9,28 +9,28 @@
void print_message(char prefix[], uint8_t msg_id, uint8_t * data, uint16_t num_bytes); void print_message(char prefix[], uint8_t msg_id, uint8_t * data, uint16_t num_bytes);
void print_message(char prefix[], uint8_t msg_id, uint8_t * data, uint16_t num_bytes) { void print_message(char prefix[], uint8_t msg_id, uint8_t * data, uint16_t num_bytes) {
/* /*
struct tm * timeinfo; struct tm * timeinfo;
time_t c_time; time_t c_time;
char time_str[30]; char time_str[30];
*/ */
uint8_t cnt; uint8_t cnt;
uint8_t log_bytes = num_bytes; uint8_t log_bytes = num_bytes;
if(log_bytes > 16) { log_bytes = 16; } if(log_bytes > 16) { log_bytes = 16; }
/* /*
time(&c_time); time(&c_time);
timeinfo = localtime(&c_time); timeinfo = localtime(&c_time);
strftime(time_str, 30, " %X ", timeinfo); strftime(time_str, 30, " %X ", timeinfo);
printf("%s %s bytes: %3d | id: %3d | UART%d | ", printf("%s %s bytes: %3d | id: %3d | UART%d | ",
prefix, time_str, num_bytes, msg_id, data[0]); prefix, time_str, num_bytes, msg_id, data[0]);
*/ */
printf("%s bytes: %3d | id: %3d | UART%d | ", printf("%s bytes: %3d | id: %3d | UART%d | ",
prefix, num_bytes, msg_id, data[0]); prefix, num_bytes, msg_id, data[0]);
for(cnt = 1; cnt < log_bytes; cnt++) { for(cnt = 1; cnt < log_bytes; cnt++) {
printf("%02X ", data[cnt]); printf("%02X ", data[cnt]);
if(cnt >= 24 && cnt % 24 == 0 && cnt+1 < log_bytes) { if(cnt >= 24 && cnt % 24 == 0 && cnt+1 < log_bytes) {
printf("\n "); printf("\n ");
} }
} }
printf("\n"); printf("\n");
} }
sw/airborne/fms/fms_spistream_client.c
+79 -79
View File
@@ -75,12 +75,12 @@ int main(int argc, char *argv[]) {
TRACE(TRACE_DEBUG, "%s", "Entering mainloop\n"); TRACE(TRACE_DEBUG, "%s", "Entering mainloop\n");
/* Enter our mainloop */ /* Enter our mainloop */
event_dispatch(); event_dispatch();
while(1) { while(1) {
sleep(100); sleep(100);
} }
main_exit(); main_exit();
TRACE(TRACE_DEBUG, "%s", "leaving mainloop, goodbye!\n"); TRACE(TRACE_DEBUG, "%s", "leaving mainloop, goodbye!\n");
@@ -88,43 +88,43 @@ int main(int argc, char *argv[]) {
} }
static void main_periodic(int my_sig_num) { static void main_periodic(int my_sig_num) {
uint8_t fifo_idx; uint8_t fifo_idx;
uint8_t msg_id; uint8_t msg_id;
uint16_t num_bytes; uint16_t num_bytes;
int16_t ret; int16_t ret;
static uint8_t buf[SPISTREAM_MAX_MESSAGE_LENGTH*10]; static uint8_t buf[SPISTREAM_MAX_MESSAGE_LENGTH*10];
for(fifo_idx = 0; fifo_idx < 4; fifo_idx++) { for(fifo_idx = 0; fifo_idx < 4; fifo_idx++) {
// The periodic is triggered before fifo // The periodic is triggered before fifo
// connections have been initialized, so // connections have been initialized, so
// check for a valid fd first: // check for a valid fd first:
if(dfifo[fifo_idx] > 0) { if(dfifo[fifo_idx] > 0) {
ret = read(dfifo[fifo_idx], (uint8_t *)(&num_bytes), 2); ret = read(dfifo[fifo_idx], (uint8_t *)(&num_bytes), 2);
ret = read(dfifo[fifo_idx], (uint8_t *)(&msg_id), 1); ret = read(dfifo[fifo_idx], (uint8_t *)(&msg_id), 1);
memset(&buf, 0, SPISTREAM_MAX_MESSAGE_LENGTH); memset(&buf, 0, SPISTREAM_MAX_MESSAGE_LENGTH);
if(num_bytes > SPISTREAM_MAX_MESSAGE_LENGTH) { if(num_bytes > SPISTREAM_MAX_MESSAGE_LENGTH) {
fprintf(stderr, "Warning: Message has length %d, but limit " fprintf(stderr, "Warning: Message has length %d, but limit "
"is %d\n", "is %d\n",
num_bytes, SPISTREAM_MAX_MESSAGE_LENGTH); num_bytes, SPISTREAM_MAX_MESSAGE_LENGTH);
num_bytes = SPISTREAM_MAX_MESSAGE_LENGTH; num_bytes = SPISTREAM_MAX_MESSAGE_LENGTH;
} }
ret = read(dfifo[fifo_idx], &buf, num_bytes); ret = read(dfifo[fifo_idx], &buf, num_bytes);
if(ret > 0 && ret == num_bytes) { if(ret > 0 && ret == num_bytes) {
// Message received // Message received
print_message(">> Client", msg_id, buf, num_bytes); print_message(">> Client", msg_id, buf, num_bytes);
} }
else if(ret > 0 && ret < num_bytes) { else if(ret > 0 && ret < num_bytes) {
fprintf(stderr, "Tried to read %d bytes, but only got %d\n", fprintf(stderr, "Tried to read %d bytes, but only got %d\n",
num_bytes, ret); num_bytes, ret);
} }
} }
else { else {
// FIFO file descriptor is invalid, // FIFO file descriptor is invalid,
// retry to open it: // retry to open it:
dfifo[fifo_idx] = open(dfifo_files[fifo_idx], O_RDONLY | O_NONBLOCK); dfifo[fifo_idx] = open(dfifo_files[fifo_idx], O_RDONLY | O_NONBLOCK);
} }
} }
} }
@@ -141,18 +141,18 @@ static void main_init(void) {
return; return;
} }
signal(SIGKILL, on_kill); signal(SIGKILL, on_kill);
signal(SIGINT, on_kill); signal(SIGINT, on_kill);
signal(SIGILL, on_kill); signal(SIGILL, on_kill);
signal(SIGHUP, on_kill); signal(SIGHUP, on_kill);
signal(SIGQUIT, on_kill); signal(SIGQUIT, on_kill);
signal(SIGTERM, on_kill); signal(SIGTERM, on_kill);
signal(SIGSEGV, on_kill); signal(SIGSEGV, on_kill);
if(!open_stream()) { if(!open_stream()) {
fprintf(stderr, "Could not open stream, sorry\n"); fprintf(stderr, "Could not open stream, sorry\n");
exit(1); exit(1);
} }
TRACE(TRACE_DEBUG, "%s", "Initialization completed\n"); TRACE(TRACE_DEBUG, "%s", "Initialization completed\n");
} }
@@ -177,39 +177,39 @@ static void main_init(void) {
* *
*/ */
static int open_stream(void) { static int open_stream(void) {
uint8_t fifo_idx; uint8_t fifo_idx;
strcpy(dfifo_files[0], "/tmp/spistream_d0.fifo"); // FIFOs for data strcpy(dfifo_files[0], "/tmp/spistream_d0.fifo"); // FIFOs for data
strcpy(dfifo_files[1], "/tmp/spistream_d1.fifo"); // (STM -> daemon -> client) strcpy(dfifo_files[1], "/tmp/spistream_d1.fifo"); // (STM -> daemon -> client)
strcpy(dfifo_files[2], "/tmp/spistream_d2.fifo"); strcpy(dfifo_files[2], "/tmp/spistream_d2.fifo");
strcpy(dfifo_files[3], "/tmp/spistream_d3.fifo"); strcpy(dfifo_files[3], "/tmp/spistream_d3.fifo");
strcpy(cfifo_files[0], "/tmp/spistream_c0.fifo"); // FIFOs for commands strcpy(cfifo_files[0], "/tmp/spistream_c0.fifo"); // FIFOs for commands
strcpy(cfifo_files[1], "/tmp/spistream_c1.fifo"); // (client -> daemon -> STM) strcpy(cfifo_files[1], "/tmp/spistream_c1.fifo"); // (client -> daemon -> STM)
strcpy(cfifo_files[2], "/tmp/spistream_c2.fifo"); strcpy(cfifo_files[2], "/tmp/spistream_c2.fifo");
strcpy(cfifo_files[3], "/tmp/spistream_c3.fifo"); strcpy(cfifo_files[3], "/tmp/spistream_c3.fifo");
for(fifo_idx = 0; fifo_idx < 4; fifo_idx++) { for(fifo_idx = 0; fifo_idx < 4; fifo_idx++) {
fprintf(stderr, "Open data stream %s ... \n", dfifo_files[fifo_idx]); fprintf(stderr, "Open data stream %s ... \n", dfifo_files[fifo_idx]);
dfifo[fifo_idx] = open(dfifo_files[fifo_idx], O_RDONLY | O_NONBLOCK); dfifo[fifo_idx] = open(dfifo_files[fifo_idx], O_RDONLY | O_NONBLOCK);
fprintf(stderr, " ...\n"); fprintf(stderr, " ...\n");
} }
return 1; return 1;
for(fifo_idx = 0; fifo_idx < 3; fifo_idx++) { for(fifo_idx = 0; fifo_idx < 3; fifo_idx++) {
fprintf(stderr, "Open command stream %s ... \n", cfifo_files[fifo_idx]); fprintf(stderr, "Open command stream %s ... \n", cfifo_files[fifo_idx]);
cfifo[fifo_idx] = open(cfifo_files[fifo_idx], O_WRONLY); cfifo[fifo_idx] = open(cfifo_files[fifo_idx], O_WRONLY);
if(cfifo[fifo_idx] < 0) { if(cfifo[fifo_idx] < 0) {
fprintf(stderr, " failed\n"); fprintf(stderr, " failed\n");
return 0; return 0;
} }
} }
return 1; return 1;
} }
static void main_exit(void) static void main_exit(void)
{ {
fprintf(stderr, "Bye!\n"); fprintf(stderr, "Bye!\n");
} }
static void parse_command_line(int argc, char** argv) { static void parse_command_line(int argc, char** argv) {
@@ -217,7 +217,7 @@ static void parse_command_line(int argc, char** argv) {
static void on_kill(int signum) static void on_kill(int signum)
{ {
fprintf(stderr, "Exiting, got signal %d\n", signum); fprintf(stderr, "Exiting, got signal %d\n", signum);
main_exit(); main_exit();
exit(1); exit(1);
} }
sw/airborne/fms/fms_spistream_daemon.c
+146 -146
View File
@@ -90,12 +90,12 @@ int main(int argc, char *argv[]) {
TRACE(TRACE_DEBUG, "%s", "Entering mainloop\n"); TRACE(TRACE_DEBUG, "%s", "Entering mainloop\n");
/* Enter our mainloop */ /* Enter our mainloop */
event_dispatch(); event_dispatch();
while(1) { while(1) {
sleep(100); sleep(100);
} }
main_exit(); main_exit();
TRACE(TRACE_DEBUG, "%s", "leaving mainloop, goodbye!\n"); TRACE(TRACE_DEBUG, "%s", "leaving mainloop, goodbye!\n");
@@ -104,13 +104,13 @@ int main(int argc, char *argv[]) {
static void main_periodic(int my_sig_num) { static void main_periodic(int my_sig_num) {
static int32_t every_100 = 1000; static int32_t every_100 = 1000;
if(every_100-- == 0) { if(every_100-- == 0) {
every_100 = 1000; every_100 = 1000;
spistream_send_msg(spistream_msg, 21, SPISTREAM_NO_WAIT); spistream_send_msg(spistream_msg, 21, SPISTREAM_NO_WAIT);
} }
spistream_event(); spistream_event();
} }
static void spistream_event() { static void spistream_event() {
@@ -123,25 +123,25 @@ static void spistream_event() {
/* /*
uint8_t cnt; uint8_t cnt;
static uint8_t pkg_size = sizeof(msg_in.pkg_data); static uint8_t pkg_size = sizeof(msg_in.pkg_data);
if(msg_out.message_cnt != 0) { if(msg_out.message_cnt != 0) {
printf("Package out: Size: %3d MID: %3d PCNTD: %3d | ", printf("Package out: Size: %3d MID: %3d PCNTD: %3d | ",
pkg_size, msg_out.message_cnt, msg_out.package_cntd); pkg_size, msg_out.message_cnt, msg_out.package_cntd);
for(cnt = 0; cnt < pkg_size; cnt++) { for(cnt = 0; cnt < pkg_size; cnt++) {
printf("%3d ", msg_out.pkg_data[cnt]); printf("%3d ", msg_out.pkg_data[cnt]);
} }
printf("\n"); printf("\n");
} }
*/ */
spi_link_send(&msg_out, sizeof(struct AutopilotMessageCRCFrame), &msg_in, &crc_valid); spi_link_send(&msg_out, sizeof(struct AutopilotMessageCRCFrame), &msg_in, &crc_valid);
/* /*
if(msg_in.message_cnt != 0) { if(msg_in.message_cnt != 0) {
printf("PKG in (spi trx: %d): Size: %3d MID: %3d PCNTD: %3d | ", SPISTREAM_PACKAGE_SIZE, printf("PKG in (spi trx: %d): Size: %3d MID: %3d PCNTD: %3d | ", SPISTREAM_PACKAGE_SIZE,
pkg_size, msg_in.message_cnt, msg_in.package_cntd); pkg_size, msg_in.message_cnt, msg_in.package_cntd);
for(cnt = 0; cnt < pkg_size; cnt++) { for(cnt = 0; cnt < pkg_size; cnt++) {
printf("%02X ", msg_in.pkg_data[cnt]); printf("%02X ", msg_in.pkg_data[cnt]);
} }
printf("\n"); printf("\n");
} }
*/ */
spistream_write_pkg(&msg_out); spistream_write_pkg(&msg_out);
} }
@@ -149,47 +149,47 @@ static void spistream_event() {
static void on_spistream_msg_received(uint8_t msg_id, static void on_spistream_msg_received(uint8_t msg_id,
uint8_t * data, uint8_t * data,
uint16_t num_bytes) { uint16_t num_bytes) {
uint8_t uart; uint8_t uart;
uint8_t buf[SPISTREAM_MAX_MESSAGE_LENGTH+3]; uint8_t buf[SPISTREAM_MAX_MESSAGE_LENGTH+3];
print_message("<< Daemon", msg_id, data, num_bytes); print_message("<< Daemon", msg_id, data, num_bytes);
uart = data[0]; uart = data[0];
// Check for valid uart ID // Check for valid uart ID
if(uart >= 0 && uart <= 3) { if(uart >= 0 && uart <= 3) {
if(msg_id > 0) { if(msg_id > 0) {
buf[0] = (uint8_t)(num_bytes & 0x00ff); buf[0] = (uint8_t)(num_bytes & 0x00ff);
buf[1] = (uint8_t)((num_bytes << 8) & 0x00ff); buf[1] = (uint8_t)((num_bytes << 8) & 0x00ff);
buf[2] = msg_id; buf[2] = msg_id;
if(num_bytes > SPISTREAM_MAX_MESSAGE_LENGTH) { if(num_bytes > SPISTREAM_MAX_MESSAGE_LENGTH) {
fprintf(LOG_OUT, "Warning: Message has length %d, but limit " fprintf(LOG_OUT, "Warning: Message has length %d, but limit "
"is %d - truncating message\n", "is %d - truncating message\n",
num_bytes, SPISTREAM_MAX_MESSAGE_LENGTH); num_bytes, SPISTREAM_MAX_MESSAGE_LENGTH);
num_bytes = SPISTREAM_MAX_MESSAGE_LENGTH; num_bytes = SPISTREAM_MAX_MESSAGE_LENGTH;
} }
memcpy(buf+3, data, num_bytes); memcpy(buf+3, data, num_bytes);
send_to_client(buf, num_bytes+3, uart); send_to_client(buf, num_bytes+3, uart);
} }
} }
} }
static void send_to_client(uint8_t * data, uint16_t num_bytes, uint8_t fifo_idx) static void send_to_client(uint8_t * data, uint16_t num_bytes, uint8_t fifo_idx)
{ {
if(dfifo[fifo_idx] <= 0) { if(dfifo[fifo_idx] <= 0) {
// No client connected to this fifo, yet // No client connected to this fifo, yet
dfifo[fifo_idx] = open(dfifo_files[fifo_idx], O_WRONLY | O_NONBLOCK); dfifo[fifo_idx] = open(dfifo_files[fifo_idx], O_WRONLY | O_NONBLOCK);
if(dfifo[fifo_idx] <= 0) { if(dfifo[fifo_idx] <= 0) {
fprintf(LOG_OUT, "No client for data fifo %d (%s)\n", fprintf(LOG_OUT, "No client for data fifo %d (%s)\n",
fifo_idx, dfifo_files[fifo_idx]); fifo_idx, dfifo_files[fifo_idx]);
return; return;
} }
} }
else { else {
// Client connected to this fifo // Client connected to this fifo
if(write(dfifo[fifo_idx], data, num_bytes) == -1) { if(write(dfifo[fifo_idx], data, num_bytes) == -1) {
fprintf(LOG_OUT, "Write error on data fifo %d\n", fifo_idx); fprintf(LOG_OUT, "Write error on data fifo %d\n", fifo_idx);
} }
} }
} }
static void on_spistream_msg_sent(uint8_t msg_id) { static void on_spistream_msg_sent(uint8_t msg_id) {
@@ -222,124 +222,124 @@ static void main_init(void) {
return; return;
} }
signal(SIGKILL, on_kill); signal(SIGKILL, on_kill);
signal(SIGINT, on_kill); signal(SIGINT, on_kill);
signal(SIGILL, on_kill); signal(SIGILL, on_kill);
signal(SIGHUP, on_kill); signal(SIGHUP, on_kill);
signal(SIGQUIT, on_kill); signal(SIGQUIT, on_kill);
signal(SIGTERM, on_kill); signal(SIGTERM, on_kill);
signal(SIGSEGV, on_kill); signal(SIGSEGV, on_kill);
signal(SIGPIPE, on_dead_pipe); signal(SIGPIPE, on_dead_pipe);
if(!open_stream()) { if(!open_stream()) {
fprintf(LOG_OUT, "Could not open stream, sorry\n"); fprintf(LOG_OUT, "Could not open stream, sorry\n");
exit(1); exit(1);
} }
TRACE(TRACE_DEBUG, "%s", "Initialization completed\n"); TRACE(TRACE_DEBUG, "%s", "Initialization completed\n");
} }
static void main_exit(void) static void main_exit(void)
{ {
fprintf(LOG_OUT, "Closing socket\n"); fprintf(LOG_OUT, "Closing socket\n");
close_stream(); close_stream();
} }
static void parse_command_line(int argc, char** argv) { static void parse_command_line(int argc, char** argv) {
/* /*
while ((ch = getopt(argc, argv, "d:")) != -1) { while ((ch = getopt(argc, argv, "d:")) != -1) {
switch (ch) { switch (ch) {
case 'd': case 'd':
daemon_mode = 1; daemon_mode = 1;
break; break;
} }
} }
*/ */
} }
static int open_stream(void) { static int open_stream(void) {
uint8_t fifo_idx; uint8_t fifo_idx;
int ret; int ret;
strcpy(dfifo_files[0], "/tmp/spistream_d0.fifo"); // FIFOs for data strcpy(dfifo_files[0], "/tmp/spistream_d0.fifo"); // FIFOs for data
strcpy(dfifo_files[1], "/tmp/spistream_d1.fifo"); // (STM -> daemon -> client) strcpy(dfifo_files[1], "/tmp/spistream_d1.fifo"); // (STM -> daemon -> client)
strcpy(dfifo_files[2], "/tmp/spistream_d2.fifo"); strcpy(dfifo_files[2], "/tmp/spistream_d2.fifo");
strcpy(dfifo_files[3], "/tmp/spistream_d3.fifo"); strcpy(dfifo_files[3], "/tmp/spistream_d3.fifo");
strcpy(cfifo_files[0], "/tmp/spistream_c0.fifo"); // FIFOs for commands strcpy(cfifo_files[0], "/tmp/spistream_c0.fifo"); // FIFOs for commands
strcpy(cfifo_files[1], "/tmp/spistream_c1.fifo"); // (client -> daemon -> STM) strcpy(cfifo_files[1], "/tmp/spistream_c1.fifo"); // (client -> daemon -> STM)
strcpy(cfifo_files[2], "/tmp/spistream_c2.fifo"); strcpy(cfifo_files[2], "/tmp/spistream_c2.fifo");
strcpy(cfifo_files[3], "/tmp/spistream_c3.fifo"); strcpy(cfifo_files[3], "/tmp/spistream_c3.fifo");
for(fifo_idx = 0; fifo_idx < 4; fifo_idx++) { for(fifo_idx = 0; fifo_idx < 4; fifo_idx++) {
fprintf(LOG_OUT, "Creating data stream %s ...", dfifo_files[fifo_idx]); fprintf(LOG_OUT, "Creating data stream %s ...", dfifo_files[fifo_idx]);
if((ret = mkfifo(dfifo_files[fifo_idx], 0777)) < 0) { if((ret = mkfifo(dfifo_files[fifo_idx], 0777)) < 0) {
fprintf(LOG_OUT, " failed\n"); fprintf(LOG_OUT, " failed\n");
fprintf(LOG_OUT, "Could not create data fifo %d: %s\n", fprintf(LOG_OUT, "Could not create data fifo %d: %s\n",
fifo_idx, dfifo_files[fifo_idx]); fifo_idx, dfifo_files[fifo_idx]);
close_stream(); close_stream();
return 0; return 0;
} }
else { else {
fprintf(LOG_OUT, " ok\n"); fprintf(LOG_OUT, " ok\n");
dfifo[fifo_idx] = open(dfifo_files[fifo_idx], O_WRONLY | O_NONBLOCK); dfifo[fifo_idx] = open(dfifo_files[fifo_idx], O_WRONLY | O_NONBLOCK);
} }
} }
for(fifo_idx = 0; fifo_idx < 4; fifo_idx++) { for(fifo_idx = 0; fifo_idx < 4; fifo_idx++) {
fprintf(LOG_OUT, "Creating command stream %s ... ", cfifo_files[fifo_idx]); fprintf(LOG_OUT, "Creating command stream %s ... ", cfifo_files[fifo_idx]);
if((ret = mkfifo(cfifo_files[fifo_idx], 0777)) < 0) { if((ret = mkfifo(cfifo_files[fifo_idx], 0777)) < 0) {
fprintf(LOG_OUT, " failed\n"); fprintf(LOG_OUT, " failed\n");
fprintf(LOG_OUT, "Could not create command fifo %d: %s\n", fprintf(LOG_OUT, "Could not create command fifo %d: %s\n",
fifo_idx, cfifo_files[fifo_idx]); fifo_idx, cfifo_files[fifo_idx]);
close_stream(); close_stream();
return 0; return 0;
} }
else { else {
fprintf(LOG_OUT, " ok\n"); fprintf(LOG_OUT, " ok\n");
cfifo[fifo_idx] = open(cfifo_files[fifo_idx], O_RDONLY | O_NONBLOCK); cfifo[fifo_idx] = open(cfifo_files[fifo_idx], O_RDONLY | O_NONBLOCK);
} }
} }
return 1; return 1;
} }
static void close_stream(void) { static void close_stream(void) {
uint8_t fifo_idx; uint8_t fifo_idx;
fprintf(LOG_OUT, "Closing streams\n"); fprintf(LOG_OUT, "Closing streams\n");
for(fifo_idx = 0; fifo_idx < 4; fifo_idx++) for(fifo_idx = 0; fifo_idx < 4; fifo_idx++)
{ {
if(dfifo[fifo_idx] >= 0) { if(dfifo[fifo_idx] >= 0) {
close(dfifo[fifo_idx]); close(dfifo[fifo_idx]);
} }
unlink(dfifo_files[fifo_idx]); unlink(dfifo_files[fifo_idx]);
if(cfifo[fifo_idx] >= 0) { if(cfifo[fifo_idx] >= 0) {
close(cfifo[fifo_idx]); close(cfifo[fifo_idx]);
} }
unlink(cfifo_files[fifo_idx]); unlink(cfifo_files[fifo_idx]);
} }
} }
static void on_timeout(int signum) static void on_timeout(int signum)
{ {
fprintf(LOG_OUT, "Timeout, stopping spistream daemon\n"); fprintf(LOG_OUT, "Timeout, stopping spistream daemon\n");
exit(6); exit(6);
} }
static void on_kill(int signum) static void on_kill(int signum)
{ {
fprintf(LOG_OUT, "Exiting, got signal %d\n", signum); fprintf(LOG_OUT, "Exiting, got signal %d\n", signum);
main_exit(); main_exit();
exit(1); exit(1);
} }
static void on_dead_pipe(int signum) static void on_dead_pipe(int signum)
{ {
uint8_t fifo_idx; uint8_t fifo_idx;
fprintf(LOG_OUT, "Got SIGPIPE (signal %d)\n", signum); fprintf(LOG_OUT, "Got SIGPIPE (signal %d)\n", signum);
// *Pop* goes the pipe. Looks like our client got AWOL. // *Pop* goes the pipe. Looks like our client got AWOL.
// Let's be nice and invalidate the file descriptors: // Let's be nice and invalidate the file descriptors:
for(fifo_idx = 0; fifo_idx < 4; fifo_idx++) { for(fifo_idx = 0; fifo_idx < 4; fifo_idx++) {
close(dfifo[fifo_idx]); close(dfifo[fifo_idx]);
dfifo[fifo_idx] = -1; dfifo[fifo_idx] = -1;
} }
} }
sw/airborne/fms/libeknav/paparazzi_eigen_conversion.h
+44 -44
View File
@@ -54,24 +54,24 @@
MAT33_ELMT((_to),2,2) = MAT33_ELMT((_from),2,2); \ MAT33_ELMT((_to),2,2) = MAT33_ELMT((_from),2,2); \
} }
#define SWAP(a, b){ \ #define SWAP(a, b){ \
typeof (a) temp = a; \ typeof (a) temp = a; \
a = b; \ a = b; \
b = temp; \ b = temp; \
} }
#define MAT33_ROW(mat, row, value0, value1, value2){ \ #define MAT33_ROW(mat, row, value0, value1, value2){ \
mat[row*3+0] = value0; \ mat[row*3+0] = value0; \
mat[row*3+1] = value1; \ mat[row*3+1] = value1; \
mat[row*3+2] = value2; \ mat[row*3+2] = value2; \
} }
#define ENU_NED_transformation(mat){ \ #define ENU_NED_transformation(mat){ \
SWAP(mat.m[0], mat.m[3]); \ SWAP(mat.m[0], mat.m[3]); \
SWAP(mat.m[1], mat.m[4]); \ SWAP(mat.m[1], mat.m[4]); \
SWAP(mat.m[2], mat.m[5]); \ SWAP(mat.m[2], mat.m[5]); \
mat.m[6] = -mat.m[6]; \ mat.m[6] = -mat.m[6]; \
mat.m[7] = -mat.m[7]; \ mat.m[7] = -mat.m[7]; \
mat.m[8] = -mat.m[8]; \ mat.m[8] = -mat.m[8]; \
} }
#define DOUBLE_VECT3_NORM(_v) (sqrt((_v).x*(_v).x + (_v).y*(_v).y + (_v).z*(_v).z)) #define DOUBLE_VECT3_NORM(_v) (sqrt((_v).x*(_v).x + (_v).y*(_v).y + (_v).z*(_v).z))
@@ -89,41 +89,41 @@
} \ } \
else { \ else { \
if (RMAT_ELMT(_r, 0, 0) > RMAT_ELMT(_r, 1, 1) && \ if (RMAT_ELMT(_r, 0, 0) > RMAT_ELMT(_r, 1, 1) && \
RMAT_ELMT(_r, 0, 0) > RMAT_ELMT(_r, 2, 2)) { \ RMAT_ELMT(_r, 0, 0) > RMAT_ELMT(_r, 2, 2)) { \
const double two_qx = sqrt(RMAT_ELMT(_r, 0, 0) -RMAT_ELMT(_r, 1, 1) \ const double two_qx = sqrt(RMAT_ELMT(_r, 0, 0) -RMAT_ELMT(_r, 1, 1) \
-RMAT_ELMT(_r, 2, 2) + 1); \ -RMAT_ELMT(_r, 2, 2) + 1); \
const double four_qx = 2. * two_qx; \ const double four_qx = 2. * two_qx; \
_q.qi = (RMAT_ELMT(_r, 1, 2)-RMAT_ELMT(_r, 2, 1))/four_qx; \ _q.qi = (RMAT_ELMT(_r, 1, 2)-RMAT_ELMT(_r, 2, 1))/four_qx; \
_q.qx = 0.5 * two_qx; \ _q.qx = 0.5 * two_qx; \
_q.qy = (RMAT_ELMT(_r, 0, 1)+RMAT_ELMT(_r, 1, 0))/four_qx; \ _q.qy = (RMAT_ELMT(_r, 0, 1)+RMAT_ELMT(_r, 1, 0))/four_qx; \
_q.qz = (RMAT_ELMT(_r, 2, 0)+RMAT_ELMT(_r, 0, 2))/four_qx; \ _q.qz = (RMAT_ELMT(_r, 2, 0)+RMAT_ELMT(_r, 0, 2))/four_qx; \
/*printf("m00 largest\n");*/ \ /*printf("m00 largest\n");*/ \
} \ } \
else if (RMAT_ELMT(_r, 1, 1) > RMAT_ELMT(_r, 2, 2)) { \ else if (RMAT_ELMT(_r, 1, 1) > RMAT_ELMT(_r, 2, 2)) { \
const double two_qy = \ const double two_qy = \
sqrt(RMAT_ELMT(_r, 1, 1) - RMAT_ELMT(_r, 0, 0) - RMAT_ELMT(_r, 2, 2) + 1); \ sqrt(RMAT_ELMT(_r, 1, 1) - RMAT_ELMT(_r, 0, 0) - RMAT_ELMT(_r, 2, 2) + 1); \
const double four_qy = 2. * two_qy; \ const double four_qy = 2. * two_qy; \
_q.qi = (RMAT_ELMT(_r, 2, 0) - RMAT_ELMT(_r, 0, 2))/four_qy; \ _q.qi = (RMAT_ELMT(_r, 2, 0) - RMAT_ELMT(_r, 0, 2))/four_qy; \
_q.qx = (RMAT_ELMT(_r, 0, 1) + RMAT_ELMT(_r, 1, 0))/four_qy; \ _q.qx = (RMAT_ELMT(_r, 0, 1) + RMAT_ELMT(_r, 1, 0))/four_qy; \
_q.qy = 0.5 * two_qy; \ _q.qy = 0.5 * two_qy; \
_q.qz = (RMAT_ELMT(_r, 1, 2) + RMAT_ELMT(_r, 2, 1))/four_qy; \ _q.qz = (RMAT_ELMT(_r, 1, 2) + RMAT_ELMT(_r, 2, 1))/four_qy; \
/*printf("m11 largest\n");*/ \ /*printf("m11 largest\n");*/ \
} \ } \
else { \ else { \
const double two_qz = \ const double two_qz = \
sqrt(RMAT_ELMT(_r, 2, 2) - RMAT_ELMT(_r, 0, 0) - RMAT_ELMT(_r, 1, 1) + 1); \ sqrt(RMAT_ELMT(_r, 2, 2) - RMAT_ELMT(_r, 0, 0) - RMAT_ELMT(_r, 1, 1) + 1); \
const double four_qz = 2. * two_qz; \ const double four_qz = 2. * two_qz; \
_q.qi = (RMAT_ELMT(_r, 0, 1)- RMAT_ELMT(_r, 1, 0))/four_qz; \ _q.qi = (RMAT_ELMT(_r, 0, 1)- RMAT_ELMT(_r, 1, 0))/four_qz; \
_q.qx = (RMAT_ELMT(_r, 2, 0)+ RMAT_ELMT(_r, 0, 2))/four_qz; \ _q.qx = (RMAT_ELMT(_r, 2, 0)+ RMAT_ELMT(_r, 0, 2))/four_qz; \
_q.qy = (RMAT_ELMT(_r, 1, 2)+ RMAT_ELMT(_r, 2, 1))/four_qz; \ _q.qy = (RMAT_ELMT(_r, 1, 2)+ RMAT_ELMT(_r, 2, 1))/four_qz; \
_q.qz = 0.5 * two_qz; \ _q.qz = 0.5 * two_qz; \
/*printf("m22 largest\n");*/ \ /*printf("m22 largest\n");*/ \
} \ } \
} \ } \
} }
#define QUAT_ENU_FROM_TO_NED(from, to){ \ #define QUAT_ENU_FROM_TO_NED(from, to){ \
to.qi = - from.qx - from.qy; \ to.qi = - from.qx - from.qy; \
to.qy = + from.qi + from.qz; \ to.qy = + from.qi + from.qz; \
to.qx = + from.qi - from.qz; \ to.qx = + from.qi - from.qz; \
to.qz = - from.qx + from.qy; \ to.qz = - from.qx + from.qy; \
@@ -133,9 +133,9 @@
#define QUAT_IMAGINARY_PART(quat, vector) VECT3_ASSIGN(vector, (quat).qx, (quat).qy, (quat).qz) #define QUAT_IMAGINARY_PART(quat, vector) VECT3_ASSIGN(vector, (quat).qx, (quat).qy, (quat).qz)
#define VECT3_TO_EULERS(vector, eulers){ \ #define VECT3_TO_EULERS(vector, eulers){ \
(eulers).phi = (vector).x; \ (eulers).phi = (vector).x; \
(eulers).theta = (vector).y; \ (eulers).theta = (vector).y; \
(eulers).psi = (vector).z; \ (eulers).psi = (vector).z; \
} }
@@ -159,7 +159,7 @@
#define VECTOR_AS_VECT3(coords, vector) { coords.x = vector(0); coords.y = vector(1); coords.z = vector(2);} #define VECTOR_AS_VECT3(coords, vector) { coords.x = vector(0); coords.y = vector(1); coords.z = vector(2);}
#define QUATERNIOND_AS_DOUBLEQUAT(doublequat, quaterniond) {(doublequat).qi = (quaterniond).w(); (doublequat).qx = (quaterniond).x(); (doublequat).qy = (quaterniond).y(); (doublequat).qz = (quaterniond).z();} #define QUATERNIOND_AS_DOUBLEQUAT(doublequat, quaterniond) {(doublequat).qi = (quaterniond).w(); (doublequat).qx = (quaterniond).x(); (doublequat).qy = (quaterniond).y(); (doublequat).qz = (quaterniond).z();}
#define PPRZ_LLA_TO_EIGEN_ECEF(lla, ecef){ \ #define PPRZ_LLA_TO_EIGEN_ECEF(lla, ecef){ \
struct EcefCoor_f ecef_pprz; \ struct EcefCoor_f ecef_pprz; \
ecef_of_lla_f(&ecef_pprz, &lla); \ ecef_of_lla_f(&ecef_pprz, &lla); \
ecef = VECT3_AS_VECTOR3D(ecef_pprz); \ ecef = VECT3_AS_VECTOR3D(ecef_pprz); \
} }
sw/airborne/fms/libeknav/raw_log_to_ascii.c
+7 -7
View File
@@ -59,17 +59,17 @@ void print_raw_log_entry(struct raw_log_entry* e){
struct DoubleVect3 tempvect; struct DoubleVect3 tempvect;
struct DoubleRates temprates; struct DoubleRates temprates;
printf("%f\t", e->time); printf("%f\t", e->time);
printf("%i\t", e->message.valid_sensors); printf("%i\t", e->message.valid_sensors);
RATES_FLOAT_OF_BFP(temprates, e->message.gyro); RATES_FLOAT_OF_BFP(temprates, e->message.gyro);
printf("% f % f % f\t", temprates.p, temprates.q, temprates.r); printf("% f % f % f\t", temprates.p, temprates.q, temprates.r);
ACCELS_FLOAT_OF_BFP(tempvect, e->message.accel); ACCELS_FLOAT_OF_BFP(tempvect, e->message.accel);
printf("% f % f % f\t", tempvect.x, tempvect.y, tempvect.z); printf("% f % f % f\t", tempvect.x, tempvect.y, tempvect.z);
MAGS_FLOAT_OF_BFP(tempvect, e->message.mag); MAGS_FLOAT_OF_BFP(tempvect, e->message.mag);
printf("% f % f % f\t", tempvect.x, tempvect.y, tempvect.z); printf("% f % f % f\t", tempvect.x, tempvect.y, tempvect.z);
printf("% i % i % i\t", e->message.ecef_pos.x, e->message.ecef_pos.y, e->message.ecef_pos.z); printf("% i % i % i\t", e->message.ecef_pos.x, e->message.ecef_pos.y, e->message.ecef_pos.z);
printf("% i % i % i\t", e->message.ecef_vel.x, e->message.ecef_vel.y, e->message.ecef_vel.z); printf("% i % i % i\t", e->message.ecef_vel.x, e->message.ecef_vel.y, e->message.ecef_vel.z);
double baro_scaled = (double)(e->message.pressure_absolute)/256; double baro_scaled = (double)(e->message.pressure_absolute)/256;
printf("%f", baro_scaled); printf("%f", baro_scaled);
} }
/* /*
sw/airborne/fms/lpc_test_spi.c
+2 -2
View File
@@ -75,8 +75,8 @@ static inline void main_event_task( void ) {
#define S0SPCR_SPIE (1<<7) /* interrupt enable */ #define S0SPCR_SPIE (1<<7) /* interrupt enable */
#define S0SPCR_LSF_VAL (S0SPCR_bit_enable | S0SPCR_CPHA | \ #define S0SPCR_LSF_VAL (S0SPCR_bit_enable | S0SPCR_CPHA | \
S0SPCR_CPOL | S0SPCR_MSTR | \ S0SPCR_CPOL | S0SPCR_MSTR | \
S0SPCR_LSBF | S0SPCR_SPIE); S0SPCR_LSBF | S0SPCR_SPIE);
#define CPSDVSR 64 #define CPSDVSR 64
sw/airborne/fms/overo_blmc_calibrate.c
+5 -5
View File
@@ -70,16 +70,16 @@ static void main_init(void) {
return; return;
} }
printf("Starting at 2000us\n"); printf("Starting at 2000us\n");
/* Initialize blaaa */ /* Initialize blaaa */
for (uint8_t i=0; i<LISA_PWM_OUTPUT_NB; i++) blmc_calibrate.servos_outputs_usecs[i] = 2000; for (uint8_t i=0; i<LISA_PWM_OUTPUT_NB; i++) blmc_calibrate.servos_outputs_usecs[i] = 2000;
dialog_with_io_proc(); dialog_with_io_proc();
getchar(); getchar();
printf("At 1000us\n"); printf("At 1000us\n");
for (uint8_t i=0; i<LISA_PWM_OUTPUT_NB; i++) blmc_calibrate.servos_outputs_usecs[i] = 1000; for (uint8_t i=0; i<LISA_PWM_OUTPUT_NB; i++) blmc_calibrate.servos_outputs_usecs[i] = 1000;
dialog_with_io_proc(); dialog_with_io_proc();
getchar(); getchar();
printf("At 1500us\n"); printf("At 1500us\n");
for (uint8_t i=0; i<LISA_PWM_OUTPUT_NB; i++) blmc_calibrate.servos_outputs_usecs[i] = 1500; for (uint8_t i=0; i<LISA_PWM_OUTPUT_NB; i++) blmc_calibrate.servos_outputs_usecs[i] = 1500;
dialog_with_io_proc(); dialog_with_io_proc();
sw/airborne/fms/overo_test_gps_passthrough.c
+46 -46
View File
@@ -60,10 +60,10 @@ int main(int argc, char *argv[]) {
TRACE(TRACE_DEBUG, "%s", "Entering mainloop\n"); TRACE(TRACE_DEBUG, "%s", "Entering mainloop\n");
/* Enter our mainloop */ /* Enter our mainloop */
event_dispatch(); event_dispatch();
while(1) { while(1) {
sleep(100); sleep(100);
} }
TRACE(TRACE_DEBUG, "%s", "leaving mainloop, goodbye!\n"); TRACE(TRACE_DEBUG, "%s", "leaving mainloop, goodbye!\n");
@@ -72,17 +72,17 @@ int main(int argc, char *argv[]) {
static void main_periodic(int my_sig_num) { static void main_periodic(int my_sig_num) {
#if 0 #if 0
static int32_t every_100 = 1000; static int32_t every_100 = 1000;
if(every_100-- == 0) { if(every_100-- == 0) {
every_100 = 1000; every_100 = 1000;
spistream_send_msg(spistream_msg, 21, SPISTREAM_NO_WAIT); spistream_send_msg(spistream_msg, 21, SPISTREAM_NO_WAIT);
/* /*
spistream_send_msg(spistream_msg, 15); spistream_send_msg(spistream_msg, 15);
spistream_send_msg(spistream_msg, 25); spistream_send_msg(spistream_msg, 25);
*/ */
} }
#endif #endif
spistream_event(); spistream_event();
} }
static void spistream_event() { static void spistream_event() {
@@ -95,25 +95,25 @@ static void spistream_event() {
spistream_write_pkg(&msg_out); spistream_write_pkg(&msg_out);
if(msg_out.message_cnt != 0) { if(msg_out.message_cnt != 0) {
printf("Package out: Size: %3d MID: %3d PCNTD: %3d | ", printf("Package out: Size: %3d MID: %3d PCNTD: %3d | ",
pkg_size, msg_out.message_cnt, msg_out.package_cntd); pkg_size, msg_out.message_cnt, msg_out.package_cntd);
for(cnt = 0; cnt < pkg_size; cnt++) { for(cnt = 0; cnt < pkg_size; cnt++) {
printf("%3d ", msg_out.pkg_data[cnt]); printf("%3d ", msg_out.pkg_data[cnt]);
} }
printf("\n"); printf("\n");
} }
spi_link_send(&msg_out, sizeof(struct AutopilotMessageCRCFrame), &msg_in, &crc_valid); spi_link_send(&msg_out, sizeof(struct AutopilotMessageCRCFrame), &msg_in, &crc_valid);
/* /*
if(msg_in.message_cnt != 0) { if(msg_in.message_cnt != 0) {
printf("Package in: Size: %3d MID: %3d PCNTD: %3d | ", printf("Package in: Size: %3d MID: %3d PCNTD: %3d | ",
pkg_size, msg_in.message_cnt, msg_in.package_cntd); pkg_size, msg_in.message_cnt, msg_in.package_cntd);
for(cnt = 0; cnt < pkg_size; cnt++) { for(cnt = 0; cnt < pkg_size; cnt++) {
printf("%3d ", msg_in.pkg_data[cnt]); printf("%3d ", msg_in.pkg_data[cnt]);
} }
printf("\n"); printf("\n");
} }
*/ */
spistream_read_pkg(&msg_in); spistream_read_pkg(&msg_in);
} }
@@ -121,28 +121,28 @@ static void spistream_event() {
static void on_spistream_msg_received(uint8_t msg_id, static void on_spistream_msg_received(uint8_t msg_id,
uint8_t * data, uint8_t * data,
uint16_t num_bytes) { uint16_t num_bytes) {
static uint16_t plot_freq = 100; static uint16_t plot_freq = 100;
uint16_t log_bytes; uint16_t log_bytes;
uint8_t cnt; uint8_t cnt;
struct tm * timeinfo; struct tm * timeinfo;
time_t c_time; time_t c_time;
char time_str[30]; char time_str[30];
plot_freq = 100; plot_freq = 100;
time(&c_time); time(&c_time);
timeinfo = localtime(&c_time); timeinfo = localtime(&c_time);
strftime(time_str, 30, " %X ", timeinfo); strftime(time_str, 30, " %X ", timeinfo);
log_bytes = num_bytes; log_bytes = num_bytes;
if(log_bytes > 48) { log_bytes = 48; } if(log_bytes > 48) { log_bytes = 48; }
printf("SPI message received: "); printf("SPI message received: ");
printf("%s | Length: %3d | id: %3d | UART%d | ", time_str, num_bytes, msg_id, data[0]); printf("%s | Length: %3d | id: %3d | UART%d | ", time_str, num_bytes, msg_id, data[0]);
for(cnt = 1; cnt < log_bytes; cnt++) { for(cnt = 1; cnt < log_bytes; cnt++) {
printf("%02X ", data[cnt]); printf("%02X ", data[cnt]);
} }
printf("\n"); printf("\n");
} }
static void on_spistream_msg_sent(void) { static void on_spistream_msg_sent(void) {
@@ -163,7 +163,7 @@ static void main_init(void) {
spistream_init(&on_spistream_msg_received, spistream_init(&on_spistream_msg_received,
&on_spistream_msg_sent); &on_spistream_msg_sent);
/* /*
spistream_msg[0] = 0; spistream_msg[0] = 0;
for(byte_idx=1; byte_idx < 123; byte_idx += 4) { for(byte_idx=1; byte_idx < 123; byte_idx += 4) {
spistream_msg[byte_idx] = 0xDE; spistream_msg[byte_idx] = 0xDE;
spistream_msg[byte_idx+1] = 0xAD; spistream_msg[byte_idx+1] = 0xAD;
sw/airborne/fms/overo_test_passthrough_telemetry.h
+5 -5
View File
@@ -8,11 +8,11 @@
#include "fms/fms_gs_com.h" #include "fms/fms_gs_com.h"
#define PERIODIC_SEND_TEST_PASSTHROUGH_STATUS(_transport) \ #define PERIODIC_SEND_TEST_PASSTHROUGH_STATUS(_transport) \
DOWNLINK_SEND_TEST_PASSTHROUGH_STATUS(_transport, \ DOWNLINK_SEND_TEST_PASSTHROUGH_STATUS(_transport, \
&otp.io_proc_msg_cnt, \ &otp.io_proc_msg_cnt, \
&otp.io_proc_err_cnt, \ &otp.io_proc_err_cnt, \
&spi_link.msg_cnt, \ &spi_link.msg_cnt, \
&spi_link.crc_err_cnt, \ &spi_link.crc_err_cnt, \
&otp.rc_status) &otp.rc_status)
#define PERIODIC_SEND_IMU_GYRO(_transport) \ #define PERIODIC_SEND_IMU_GYRO(_transport) \
sw/airborne/fms/overo_test_spi_link.c
+25 -25
View File
@@ -77,22 +77,22 @@ int main(int argc, char *argv[]) {
/* check that received message is identical to the one previously sent */ /* check that received message is identical to the one previously sent */
if (!skip_buf_check && spi_link.msg_cnt > 1) { if (!skip_buf_check && spi_link.msg_cnt > 1) {
if (memcmp(&crc_msg_in.payload, &msg_out_prev.payload, sizeof(struct OVERO_LINK_MSG_DOWN))) { if (memcmp(&crc_msg_in.payload, &msg_out_prev.payload, sizeof(struct OVERO_LINK_MSG_DOWN))) {
printf("Compare failed: (received != expected): \n"); printf("Compare failed: (received != expected): \n");
print_up_msg(&crc_msg_in); print_up_msg(&crc_msg_in);
print_down_msg(&msg_out_prev); print_down_msg(&msg_out_prev);
buf_check_errors++; buf_check_errors++;
} }
} }
/* report crc error */ /* report crc error */
if (!skip_crc_check & !crc_valid) { if (!skip_crc_check & !crc_valid) {
printf("CRC checksum failed: received %04X != computed %04X\n", printf("CRC checksum failed: received %04X != computed %04X\n",
crc_msg_in.crc, crc_msg_in.crc,
crc_calc_block_crc8((uint8_t*)&crc_msg_in.payload, sizeof(struct OVERO_LINK_MSG_DOWN))); crc_calc_block_crc8((uint8_t*)&crc_msg_in.payload, sizeof(struct OVERO_LINK_MSG_DOWN)));
} }
/* report message count */ /* report message count */
if (!(spi_link.msg_cnt % 1000)) if (!(spi_link.msg_cnt % 1000))
printf("msg %d, buf err %d, CRC errors: %d\n", spi_link.msg_cnt, printf("msg %d, buf err %d, CRC errors: %d\n", spi_link.msg_cnt,
buf_check_errors, spi_link.crc_err_cnt); buf_check_errors, spi_link.crc_err_cnt);
/* give it some rest */ /* give it some rest */
if(us_delay > 0) if(us_delay > 0)
@@ -105,27 +105,27 @@ int main(int argc, char *argv[]) {
static void print_up_msg(struct AutopilotMessageCRCFrame * msg) { static void print_up_msg(struct AutopilotMessageCRCFrame * msg) {
printf("UP: %08X %08X %08X %08X %08X %08X %08X %08X CRC: %08X\n", printf("UP: %08X %08X %08X %08X %08X %08X %08X %08X CRC: %08X\n",
msg->payload.msg_up.foo, msg->payload.msg_up.foo,
msg->payload.msg_up.bar, msg->payload.msg_up.bar,
msg->payload.msg_up.bla, msg->payload.msg_up.bla,
msg->payload.msg_up.ble, msg->payload.msg_up.ble,
msg->payload.msg_up.bli, msg->payload.msg_up.bli,
msg->payload.msg_up.blo, msg->payload.msg_up.blo,
msg->payload.msg_up.blu, msg->payload.msg_up.blu,
msg->payload.msg_up.bly, msg->payload.msg_up.bly,
msg->crc); msg->crc);
} }
static void print_down_msg(struct AutopilotMessageCRCFrame * msg) { static void print_down_msg(struct AutopilotMessageCRCFrame * msg) {
printf("DW: %08X %08X %08X %08X %08X %08X %08X %08X CRC: %08X\n", printf("DW: %08X %08X %08X %08X %08X %08X %08X %08X CRC: %08X\n",
msg->payload.msg_down.foo, msg->payload.msg_down.foo,
msg->payload.msg_down.bar, msg->payload.msg_down.bar,
msg->payload.msg_down.bla, msg->payload.msg_down.bla,
msg->payload.msg_down.ble, msg->payload.msg_down.ble,
msg->payload.msg_down.bli, msg->payload.msg_down.bli,
msg->payload.msg_down.blo, msg->payload.msg_down.blo,
msg->payload.msg_up.blu, msg->payload.msg_up.blu,
msg->payload.msg_up.bly, msg->payload.msg_up.bly,
msg->crc); msg->crc);
} }
sw/airborne/fms/udp_transport.h
+1 -1
View File
@@ -38,7 +38,7 @@ extern uint8_t udpt_ck_a, udpt_ck_b;
downlink_nb_bytes += udpt_tx_buf_idx; \ downlink_nb_bytes += udpt_tx_buf_idx; \
downlink_nb_msgs++; \ downlink_nb_msgs++; \
if (len != udpt_tx_buf_idx) \ if (len != udpt_tx_buf_idx) \
downlink_nb_ovrn++; \ downlink_nb_ovrn++; \
udpt_tx_buf_idx = 0; \ udpt_tx_buf_idx = 0; \
} \ } \
} }
sw/airborne/fms/udp_transport2.c
+3 -3
View File
@@ -25,7 +25,7 @@ static void put_named_uint8_t(struct udp_transport *udp, char *name __attribute_
static void put_bytes(void *impl, enum DownlinkDataType data_type __attribute__((unused)), uint8_t len, const void *buf) static void put_bytes(void *impl, enum DownlinkDataType data_type __attribute__((unused)), uint8_t len, const void *buf)
{ {
struct udp_transport *udp = (struct udp_transport *) impl; struct udp_transport *udp = (struct udp_transport *) impl;
const uint8_t *bytes = (const uint8_t *) buf; const uint8_t *bytes = (const uint8_t *) buf;
for (int i = 0; i < len; i++) { for (int i = 0; i < len; i++) {
put_uint8_t(udp, bytes[i]); put_uint8_t(udp, bytes[i]);
} }
@@ -73,12 +73,12 @@ static void count_bytes(void *udp __attribute__((unused)), uint8_t bytes __attri
static int check_free_space(void *udp __attribute__((unused)), uint8_t bytes __attribute__((unused))) static int check_free_space(void *udp __attribute__((unused)), uint8_t bytes __attribute__((unused)))
{ {
return TRUE; return TRUE;
} }
static uint8_t size_of(void *udp __attribute__((unused)), uint8_t len) static uint8_t size_of(void *udp __attribute__((unused)), uint8_t len)
{ {
return len + 2; return len + 2;
} }
static void periodic(void *impl) static void periodic(void *impl)
sw/airborne/link_mcu_usart.c
+5 -5
View File
@@ -326,11 +326,11 @@ void link_mcu_periodic_task( void )
inter_mcu_fill_fbw_state(); /** Prepares the next message for AP */ inter_mcu_fill_fbw_state(); /** Prepares the next message for AP */
InterMcuSend_INTERMCU_FBW( InterMcuSend_INTERMCU_FBW(
fbw_state->ppm_cpt, fbw_state->ppm_cpt,
fbw_state->status, fbw_state->status,
fbw_state->nb_err, fbw_state->nb_err,
fbw_state->vsupply, fbw_state->vsupply,
fbw_state->current); fbw_state->current);
#if defined RADIO_CONTROL || RADIO_CONTROL_AUTO1 #if defined RADIO_CONTROL || RADIO_CONTROL_AUTO1
InterMcuSend_INTERMCU_RADIO( fbw_state->channels ); InterMcuSend_INTERMCU_RADIO( fbw_state->channels );
#endif #endif
sw/airborne/lisa/arch/stm32/lisa_overo_link_arch.h
+13 -13
View File
@@ -11,29 +11,29 @@
while(SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_BSY) ==SET); \ while(SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_BSY) ==SET); \
overo_link.timeout_cnt = 0; \ overo_link.timeout_cnt = 0; \
if((SPI_I2S_GetFlagStatus(SPI1, SPI_FLAG_CRCERR)) == RESET) { \ if((SPI_I2S_GetFlagStatus(SPI1, SPI_FLAG_CRCERR)) == RESET) { \
LED_ON(OVERO_LINK_LED_OK); \ LED_ON(OVERO_LINK_LED_OK); \
LED_OFF(OVERO_LINK_LED_KO); \ LED_OFF(OVERO_LINK_LED_KO); \
overo_link.msg_cnt++; \ overo_link.msg_cnt++; \
_data_received_handler(); \ _data_received_handler(); \
overo_link_arch_prepare_next_transfert(); \ overo_link_arch_prepare_next_transfert(); \
} \ } \
else { \ else { \
SPI_Cmd(SPI1, DISABLE); \ SPI_Cmd(SPI1, DISABLE); \
LED_OFF(OVERO_LINK_LED_OK); \ LED_OFF(OVERO_LINK_LED_OK); \
LED_ON(OVERO_LINK_LED_KO); \ LED_ON(OVERO_LINK_LED_KO); \
overo_link.crc_err_cnt++; \ overo_link.crc_err_cnt++; \
overo_link.crc_error = TRUE; \ overo_link.crc_error = TRUE; \
_crc_failed_handler(); \ _crc_failed_handler(); \
} \ } \
overo_link.status = IDLE; \ overo_link.status = IDLE; \
} \ } \
if (overo_link.crc_error && /* if we've had a bad crc */ \ if (overo_link.crc_error && /* if we've had a bad crc */ \
!GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_4)) { /* and we're not selected anymore */ \ !GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_4)) { /* and we're not selected anymore */ \
overo_link_arch_prepare_next_transfert(); \ overo_link_arch_prepare_next_transfert(); \
overo_link.crc_error = FALSE; \ overo_link.crc_error = FALSE; \
} \ } \
if (overo_link.timeout && /* if we've had a timeout */ \ if (overo_link.timeout && /* if we've had a timeout */ \
!GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_4)) { /* and we're not selected anymore */ \ !GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_4)) { /* and we're not selected anymore */ \
overo_link_arch_prepare_next_transfert(); \ overo_link_arch_prepare_next_transfert(); \
overo_link.timeout = FALSE; \ overo_link.timeout = FALSE; \
} \ } \
sw/airborne/lisa/lisa_overo_link.h
+11 -11
View File
@@ -46,12 +46,12 @@ extern void overo_link_arch_prepare_next_transfert(void);
overo_link.timeout_cnt++; \ overo_link.timeout_cnt++; \
else { \ else { \
if (overo_link.status != LOST && overo_link.status != DATA_AVAILABLE ) { \ if (overo_link.status != LOST && overo_link.status != DATA_AVAILABLE ) { \
SPI_Cmd(SPI1, DISABLE); \ SPI_Cmd(SPI1, DISABLE); \
overo_link.status = LOST; \ overo_link.status = LOST; \
LED_OFF(OVERO_LINK_LED_OK); \ LED_OFF(OVERO_LINK_LED_OK); \
LED_ON(OVERO_LINK_LED_KO); \ LED_ON(OVERO_LINK_LED_KO); \
overo_link.timeout = TRUE; \ overo_link.timeout = TRUE; \
_timeout_handler(); \ _timeout_handler(); \
} \ } \
} \ } \
} }
@@ -62,11 +62,11 @@ extern void overo_link_arch_prepare_next_transfert(void);
else { \ else { \
__disable_irq(); \ __disable_irq(); \
if (overo_link.status != LOST && overo_link.status != DATA_AVAILABLE ) { \ if (overo_link.status != LOST && overo_link.status != DATA_AVAILABLE ) { \
overo_link.status = LOST; \ overo_link.status = LOST; \
__enable_irq(); \ __enable_irq(); \
LED_OFF(OVERO_LINK_LED_OK); \ LED_OFF(OVERO_LINK_LED_OK); \
LED_ON(OVERO_LINK_LED_KO); \ LED_ON(OVERO_LINK_LED_KO); \
_timeout_handler(); \ _timeout_handler(); \
} \ } \
__enable_irq(); \ __enable_irq(); \
} \ } \
sw/airborne/lisa/lisa_spistream.h
+109 -109
View File
@@ -22,9 +22,9 @@
#endif #endif
#ifndef SPISTREAM_TX_MAX_BUFFER_PACKAGES #ifndef SPISTREAM_TX_MAX_BUFFER_PACKAGES
#define SPISTREAM_TX_MAX_BUFFER_PACKAGES ( \ #define SPISTREAM_TX_MAX_BUFFER_PACKAGES ( \
(SPISTREAM_MAX_TX_MESSAGE_LENGTH / \ (SPISTREAM_MAX_TX_MESSAGE_LENGTH / \
SPISTREAM_PACKAGE_SIZE) * \ SPISTREAM_PACKAGE_SIZE) * \
SPISTREAM_MAX_TX_PARALLEL_TRANSACTIONS) SPISTREAM_MAX_TX_PARALLEL_TRANSACTIONS)
#endif #endif
#define SPISTREAM_INVALID_MESSAGE_ID 0 #define SPISTREAM_INVALID_MESSAGE_ID 0
@@ -35,16 +35,16 @@ struct spistream_state_t {
}; };
struct spistream_message_range_t { struct spistream_message_range_t {
uint8_t index; uint8_t index;
uint8_t size; uint8_t size;
}; };
struct spistream_buffers_t { struct spistream_buffers_t {
uint16_t rx_num_packages; // number of packages in buffer uint16_t rx_num_packages; // number of packages in buffer
uint16_t tx_insert; // next index for package insertion uint16_t tx_insert; // next index for package insertion
uint16_t tx_read; // next index to read package from uint16_t tx_read; // next index to read package from
uint16_t tx_num_packages; // number of packages in buffer uint16_t tx_num_packages; // number of packages in buffer
// RX stores data as array // RX stores data as array
uint8_t rx[SPISTREAM_RX_BUFFER_SIZE]; uint8_t rx[SPISTREAM_RX_BUFFER_SIZE];
@@ -98,41 +98,41 @@ static inline void spistream_init(spistream_message_rx_handler_t message_rx_hand
*/ */
static inline void spistream_read_pkg(struct AutopilotMessagePTStream * pkg_in) static inline void spistream_read_pkg(struct AutopilotMessagePTStream * pkg_in)
{ {
uint8_t package_cntd; uint8_t package_cntd;
if(pkg_in->message_cnt == SPISTREAM_INVALID_MESSAGE_ID) { if(pkg_in->message_cnt == SPISTREAM_INVALID_MESSAGE_ID) {
return; return;
} }
// In the last package of every message, the package_cntd is expected to be // In the last package of every message, the package_cntd is expected to be
// negative or 0. It indicates the number of zero-bytes that are padded to // negative or 0. It indicates the number of zero-bytes that are padded to
// the end of the message to fill a package. // the end of the message to fill a package.
if(pkg_in->package_cntd <= 0) { package_cntd = 1; } if(pkg_in->package_cntd <= 0) { package_cntd = 1; }
else { package_cntd = pkg_in->package_cntd; } else { package_cntd = pkg_in->package_cntd; }
if(pkg_in->package_cntd >= spistream_buffers.rx_num_packages) { if(pkg_in->package_cntd >= spistream_buffers.rx_num_packages) {
spistream_buffers.rx_num_packages = pkg_in->package_cntd; spistream_buffers.rx_num_packages = pkg_in->package_cntd;
} }
if(spistream_state.rx_package_cntd == 0) { // Beginning of new message if(spistream_state.rx_package_cntd == 0) { // Beginning of new message
// Message length is first value of package countdown: // Message length is first value of package countdown:
spistream_buffers.rx_num_packages = package_cntd; spistream_buffers.rx_num_packages = package_cntd;
spistream_state.rx_package_cntd = package_cntd; spistream_state.rx_package_cntd = package_cntd;
} }
memcpy(spistream_buffers.rx + memcpy(spistream_buffers.rx +
((spistream_buffers.rx_num_packages - package_cntd) * ((spistream_buffers.rx_num_packages - package_cntd) *
SPISTREAM_PACKAGE_SIZE), SPISTREAM_PACKAGE_SIZE),
pkg_in->pkg_data, pkg_in->pkg_data,
SPISTREAM_PACKAGE_SIZE); SPISTREAM_PACKAGE_SIZE);
if(pkg_in->package_cntd <= 0) { if(pkg_in->package_cntd <= 0) {
// Message is ready, pass to handler: // Message is ready, pass to handler:
spistream.message_rx_handler(pkg_in->message_cnt, spistream.message_rx_handler(pkg_in->message_cnt,
(uint8_t *)(spistream_buffers.rx), (uint8_t *)(spistream_buffers.rx),
(spistream_buffers.rx_num_packages * (spistream_buffers.rx_num_packages *
SPISTREAM_PACKAGE_SIZE) + SPISTREAM_PACKAGE_SIZE) +
pkg_in->package_cntd); pkg_in->package_cntd);
spistream_state.rx_package_cntd = 0; spistream_state.rx_package_cntd = 0;
} }
} }
@@ -143,24 +143,24 @@ static inline void spistream_read_pkg(struct AutopilotMessagePTStream * pkg_in)
static inline void spistream_write_pkg(struct AutopilotMessagePTStream * pkg_out) static inline void spistream_write_pkg(struct AutopilotMessagePTStream * pkg_out)
{ {
if(spistream_buffers.tx_num_packages == 0) { if(spistream_buffers.tx_num_packages == 0) {
memset(pkg_out, 0, sizeof(struct AutopilotMessagePTStream)); memset(pkg_out, 0, sizeof(struct AutopilotMessagePTStream));
pkg_out->message_cnt = SPISTREAM_INVALID_MESSAGE_ID; pkg_out->message_cnt = SPISTREAM_INVALID_MESSAGE_ID;
return; return;
} }
memcpy(pkg_out, memcpy(pkg_out,
spistream_buffers.tx + spistream_buffers.tx_read, spistream_buffers.tx + spistream_buffers.tx_read,
sizeof(struct AutopilotMessagePTStream)); sizeof(struct AutopilotMessagePTStream));
if(pkg_out->package_cntd <= 0) { if(pkg_out->package_cntd <= 0) {
spistream.message_tx_handler(pkg_out->message_cnt); spistream.message_tx_handler(pkg_out->message_cnt);
} }
spistream_buffers.tx_read++; spistream_buffers.tx_read++;
if(spistream_buffers.tx_read >= SPISTREAM_TX_MAX_BUFFER_PACKAGES) { if(spistream_buffers.tx_read >= SPISTREAM_TX_MAX_BUFFER_PACKAGES) {
spistream_buffers.tx_read = 0; spistream_buffers.tx_read = 0;
} }
spistream_buffers.tx_num_packages--; spistream_buffers.tx_num_packages--;
} }
/** /**
@@ -169,84 +169,84 @@ static inline void spistream_write_pkg(struct AutopilotMessagePTStream * pkg_out
* at the moment. * at the moment.
*/ */
static inline uint8_t spistream_enqueue_msg(uint8_t * data, static inline uint8_t spistream_enqueue_msg(uint8_t * data,
uint16_t num_bytes, uint16_t num_bytes,
enum spistream_flag wait_for_read) enum spistream_flag wait_for_read)
{ {
uint16_t pkg_idx, num_packages, num_padding; uint16_t pkg_idx, num_packages, num_padding;
uint16_t idx; uint16_t idx;
// Enough space in buffer? // Enough space in buffer?
if(wait_for_read == SPISTREAM_NO_WAIT || if(wait_for_read == SPISTREAM_NO_WAIT ||
spistream_buffers.tx_num_packages+1 < SPISTREAM_TX_MAX_BUFFER_PACKAGES) spistream_buffers.tx_num_packages+1 < SPISTREAM_TX_MAX_BUFFER_PACKAGES)
{ {
spistream_state.tx_message_cnt++; spistream_state.tx_message_cnt++;
// Message id 0 is reserved for invalid packages: // Message id 0 is reserved for invalid packages:
if(spistream_state.tx_message_cnt == SPISTREAM_INVALID_MESSAGE_ID) { if(spistream_state.tx_message_cnt == SPISTREAM_INVALID_MESSAGE_ID) {
spistream_state.tx_message_cnt = 1; spistream_state.tx_message_cnt = 1;
} }
// How many packages we need for this message: // How many packages we need for this message:
num_packages = (num_bytes / SPISTREAM_PACKAGE_SIZE); num_packages = (num_bytes / SPISTREAM_PACKAGE_SIZE);
if(num_bytes % SPISTREAM_PACKAGE_SIZE != 0) { if(num_bytes % SPISTREAM_PACKAGE_SIZE != 0) {
num_packages++; num_packages++;
} }
// How many zero-bytes we will have at the end of the last package: // How many zero-bytes we will have at the end of the last package:
if(num_bytes > SPISTREAM_PACKAGE_SIZE) { if(num_bytes > SPISTREAM_PACKAGE_SIZE) {
num_padding = (num_packages * SPISTREAM_PACKAGE_SIZE) - num_bytes; num_padding = (num_packages * SPISTREAM_PACKAGE_SIZE) - num_bytes;
} }
else { else {
num_padding = SPISTREAM_PACKAGE_SIZE - num_bytes; num_padding = SPISTREAM_PACKAGE_SIZE - num_bytes;
} }
pkg_idx = spistream_buffers.tx_insert; pkg_idx = spistream_buffers.tx_insert;
// Convert data to packages and add them to TX buffer: // Convert data to packages and add them to TX buffer:
for(idx = 0; num_packages > 0; idx++) { for(idx = 0; num_packages > 0; idx++) {
if(idx < num_bytes) { if(idx < num_bytes) {
spistream_buffers.tx[pkg_idx].pkg_data[idx % SPISTREAM_PACKAGE_SIZE] = data[idx]; spistream_buffers.tx[pkg_idx].pkg_data[idx % SPISTREAM_PACKAGE_SIZE] = data[idx];
} }
else { // padding else { // padding
spistream_buffers.tx[pkg_idx].pkg_data[idx % SPISTREAM_PACKAGE_SIZE] = 0; spistream_buffers.tx[pkg_idx].pkg_data[idx % SPISTREAM_PACKAGE_SIZE] = 0;
} }
// Last byte in current package: // Last byte in current package:
if((idx % SPISTREAM_PACKAGE_SIZE) == SPISTREAM_PACKAGE_SIZE-1) { if((idx % SPISTREAM_PACKAGE_SIZE) == SPISTREAM_PACKAGE_SIZE-1) {
// Finish configuration of current package // Finish configuration of current package
// Last package uses field package_cntd to indicate the number // Last package uses field package_cntd to indicate the number
// of padding bytes it contains, as negative number: // of padding bytes it contains, as negative number:
if(num_packages == 1) { if(num_packages == 1) {
spistream_buffers.tx[pkg_idx].package_cntd = -num_padding; spistream_buffers.tx[pkg_idx].package_cntd = -num_padding;
} }
else { else {
spistream_buffers.tx[pkg_idx].package_cntd = num_packages; spistream_buffers.tx[pkg_idx].package_cntd = num_packages;
} }
spistream_buffers.tx[pkg_idx].message_cnt = spistream_state.tx_message_cnt; spistream_buffers.tx[pkg_idx].message_cnt = spistream_state.tx_message_cnt;
// Prepare next package: // Prepare next package:
num_packages--; num_packages--;
// Increment insert pointer with ring buffer overflow: // Increment insert pointer with ring buffer overflow:
spistream_buffers.tx_insert++; spistream_buffers.tx_insert++;
if(spistream_buffers.tx_insert >= SPISTREAM_TX_MAX_BUFFER_PACKAGES) { if(spistream_buffers.tx_insert >= SPISTREAM_TX_MAX_BUFFER_PACKAGES) {
spistream_buffers.tx_insert = 0; spistream_buffers.tx_insert = 0;
} }
// Continue with next package: // Continue with next package:
pkg_idx = spistream_buffers.tx_insert; pkg_idx = spistream_buffers.tx_insert;
spistream_buffers.tx_num_packages++; spistream_buffers.tx_num_packages++;
} }
} }
#if 0 #if 0
printf("Enqueue finished. Buffer: \n"); printf("Enqueue finished. Buffer: \n");
for(pkg_idx = 0; pkg_idx < spistream_buffers.tx_num_packages; pkg_idx++) { for(pkg_idx = 0; pkg_idx < spistream_buffers.tx_num_packages; pkg_idx++) {
printf("Package %2d | %3d |: ", pkg_idx, spistream_buffers.tx[pkg_idx].package_cntd); printf("Package %2d | %3d |: ", pkg_idx, spistream_buffers.tx[pkg_idx].package_cntd);
for(idx = 0; idx < SPISTREAM_PACKAGE_SIZE; idx++) { for(idx = 0; idx < SPISTREAM_PACKAGE_SIZE; idx++) {
printf("%3d ", spistream_buffers.tx[pkg_idx].pkg_data[idx]); printf("%3d ", spistream_buffers.tx[pkg_idx].pkg_data[idx]);
} }
printf("\n"); printf("\n");
} }
#endif #endif
return 1; return 1;
} }
return 0; return 0;
} }
static inline void spistream_dequeue_msg(uint8_t message_id) { static inline void spistream_dequeue_msg(uint8_t message_id) {
@@ -261,8 +261,8 @@ static inline void spistream_dequeue_msg(uint8_t message_id) {
* message and always return 1. * message and always return 1.
*/ */
static inline uint8_t spistream_send_msg(uint8_t * data, static inline uint8_t spistream_send_msg(uint8_t * data,
uint16_t num_bytes, uint16_t num_bytes,
enum spistream_flag wait_for_read) enum spistream_flag wait_for_read)
{ {
return spistream_enqueue_msg(data, num_bytes, wait_for_read); return spistream_enqueue_msg(data, num_bytes, wait_for_read);
} }
sw/airborne/lisa/lisa_stm_gps_passthrough_main.c
+1 -1
View File
@@ -125,7 +125,7 @@ static inline void on_spistream_msg_received(uint8_t msg_id,
static inline void on_spistream_msg_sent(uint8_t msg_id) { static inline void on_spistream_msg_sent(uint8_t msg_id) {
if(spistream_wait_for_num_transfers > 0) { if(spistream_wait_for_num_transfers > 0) {
spistream_wait_for_num_transfers--; spistream_wait_for_num_transfers--;
} }
} }

Some files were not shown because too many files have changed in this diff Show More