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 28 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
/* 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
lpc_vor_convertions.c for right timing code */
but we are a lot slower (e.g. 58.6kHz with PCLK = 15MHz), see
lpc_vor_convertions.c for right timing code */
/* 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 ;
/* AD0 selected as IRQ */
@@ -214,8 +214,8 @@ void adc_init( void ) {
#ifdef USE_AD1
/* 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
lpc_vor_convertions.c for right timing code */
but we are a lot slower (e.g. 58.6kHz with PCLK = 15MHz), see
lpc_vor_convertions.c for right timing code */
/* 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 ;
/* AD1 selected as IRQ */
@@ -239,12 +239,12 @@ void adcISR0 ( void ) {
struct adc_buf* buf = buffers[channel];
if (buf) {
uint8_t new_head = buf->head + 1;
if (new_head >= buf->av_nb_sample) new_head = 0;
buf->sum -= buf->values[new_head];
buf->values[new_head] = value;
buf->sum += value;
buf->head = new_head;
uint8_t new_head = buf->head + 1;
if (new_head >= buf->av_nb_sample) new_head = 0;
buf->sum -= buf->values[new_head];
buf->values[new_head] = value;
buf->sum += value;
buf->head = new_head;
}
VICVectAddr = 0x00000000; // clear this interrupt from the VIC
@@ -259,12 +259,12 @@ void adcISR1 ( void ) {
adc1_val[channel] = value;
struct adc_buf* buf = buffers[channel+NB_ADC];
if (buf) {
uint8_t new_head = buf->head + 1;
if (new_head >= buf->av_nb_sample) new_head = 0;
buf->sum -= buf->values[new_head];
buf->values[new_head] = value;
buf->sum += value;
buf->head = new_head;
uint8_t new_head = buf->head + 1;
if (new_head >= buf->av_nb_sample) new_head = 0;
buf->sum -= buf->values[new_head];
buf->values[new_head] = value;
buf->sum += value;
buf->head = new_head;
}
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)
{
U0THR = uart0_tx_buffer[uart0_tx_extract_idx];
uart0_tx_extract_idx++;
uart0_tx_extract_idx++;
uart0_tx_extract_idx %= UART0_TX_BUFFER_SIZE;
}
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_out) {
max11040_buf_in = i;
} else {
} else {
//throw error;
}
}
}
}
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 MmSet() SetBit(MM_RESET_IOSET,MM_RESET_PIN)
#define MmOnSpiIt() { \
switch (micromag_status) { \
case MM_SENDING_REQ: \
{ \
/* read dummy control byte reply */ \
uint8_t foo __attribute__ ((unused)) = SSPDR; \
micromag_status = MM_WAITING_EOC; \
MmUnselect(); \
SpiClearRti(); \
SpiDisableRti(); \
SpiDisable(); \
} \
break; \
case MM_READING_RES: \
{ \
int16_t new_val; \
new_val = SSPDR << 8; \
new_val += SSPDR; \
if (abs(new_val) < 2000) \
micromag_values[micromag_cur_axe] = new_val; \
MmUnselect(); \
SpiClearRti(); \
SpiDisableRti(); \
SpiDisable(); \
micromag_cur_axe++; \
if (micromag_cur_axe > 2) { \
micromag_cur_axe = 0; \
micromag_status = MM_DATA_AVAILABLE; \
} \
else \
micromag_status = MM_IDLE; \
} \
break; \
} \
#define MmOnSpiIt() { \
switch (micromag_status) { \
case MM_SENDING_REQ: \
{ \
/* read dummy control byte reply */ \
uint8_t foo __attribute__ ((unused)) = SSPDR; \
micromag_status = MM_WAITING_EOC; \
MmUnselect(); \
SpiClearRti(); \
SpiDisableRti(); \
SpiDisable(); \
} \
break; \
case MM_READING_RES: \
{ \
int16_t new_val; \
new_val = SSPDR << 8; \
new_val += SSPDR; \
if (abs(new_val) < 2000) \
micromag_values[micromag_cur_axe] = new_val; \
MmUnselect(); \
SpiClearRti(); \
SpiDisableRti(); \
SpiDisable(); \
micromag_cur_axe++; \
if (micromag_cur_axe > 2) { \
micromag_cur_axe = 0; \
micromag_status = MM_DATA_AVAILABLE; \
} \
else \
micromag_status = MM_IDLE; \
} \
break; \
} \
}
#define MmSendReq() { \
MmSelect(); \
micromag_status = MM_SENDING_REQ; \
MmSet(); \
SpiClearRti(); \
SpiEnableRti(); \
MmReset(); \
#define MmSendReq() { \
MmSelect(); \
micromag_status = MM_SENDING_REQ; \
MmSet(); \
SpiClearRti(); \
SpiEnableRti(); \
MmReset(); \
uint8_t control_byte = (micromag_cur_axe+1) << 0 | MM_DIVISOR_1024 << 4; \
SSPDR = control_byte; \
SpiEnable(); \
SSPDR = control_byte; \
SpiEnable(); \
}
#define MmReadRes() { \
micromag_status = MM_READING_RES; \
MmSelect(); \
/* trigger 2 bytes read */ \
SSPDR = 0; \
SSPDR = 0; \
SpiEnable(); \
SpiClearRti(); \
SpiEnableRti(); \
micromag_status = MM_READING_RES; \
MmSelect(); \
/* trigger 2 bytes read */ \
SSPDR = 0; \
SSPDR = 0; \
SpiEnable(); \
SpiClearRti(); \
SpiEnableRti(); \
}
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; \
SetBit(IO1CLR, SERVO_RESET_PIN); \
} \
\
\
/* request clock high on next match */ \
T0MR1 += servos_values[servos_4017_idx]; \
/* 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_tail++;
if( tx_tail >= TX_BUF_SIZE )
tx_tail = 0;
tx_tail = 0;
}
}
return ret;
sw/airborne/arch/lpc21/test/ledswitch.c
+14 -14
View File
@@ -47,21 +47,21 @@ int main(void)
}
while (1)
{
if (BUTTTON1_OFF()) {
YELLOW_LED_ON();
}
else {
YELLOW_LED_OFF();
}
if (BUTTTON2_OFF()) {
GREEN_LED_ON();
}
else {
GREEN_LED_OFF();
}
{
if (BUTTTON1_OFF()) {
YELLOW_LED_ON();
}
else {
YELLOW_LED_OFF();
}
if (BUTTTON2_OFF()) {
GREEN_LED_ON();
}
else {
GREEN_LED_OFF();
}
}
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,
void(*term_conf_callback)(struct termios*, speed_t*)) {
void(*term_conf_callback)(struct termios*, speed_t*)) {
speed_t speed;
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 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,
void(*term_conf_callback)(struct termios*, speed_t*));
void(*term_conf_callback)(struct termios*, speed_t*));
extern void serial_port_close(struct SerialPort* me);
#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 */
void adc_buf_channel(void* a __attribute__ ((unused)),
void* b __attribute__ ((unused)),
void* c __attribute__ ((unused))) {
void* b __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_AFIO);
gpio_set_mode(GPIOB, GPIO_MODE_INPUT,
GPIO_CNF_INPUT_FLOAT, GPIO5);
GPIO_CNF_INPUT_FLOAT, GPIO5);
#ifdef HMC5843_USE_INT
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 */
rcc_periph_clock_enable(RCC_GPIOC);
gpio_set_mode(GPIOC, GPIO_MODE_INPUT,
GPIO_CNF_INPUT_FLOAT, GPIO12 | GPIO13);
GPIO_CNF_INPUT_FLOAT, GPIO12 | GPIO13);
#endif
/* Gyro --------------------------------------------------------------------*/
@@ -63,7 +63,7 @@ void imu_aspirin_arch_init(void) {
rcc_periph_clock_enable(RCC_GPIOC);
rcc_periph_clock_enable(RCC_AFIO);
gpio_set_mode(GPIOC, GPIO_MODE_INPUT,
GPIO_CNF_INPUT_FLOAT, GPIO14);
GPIO_CNF_INPUT_FLOAT, GPIO14);
#ifdef ASPIRIN_USE_GYRO_INT
exti_select_source(EXTI14, GPIOC);
@@ -74,7 +74,7 @@ void imu_aspirin_arch_init(void) {
/* configure external interrupt exti2 on PB2( accel int ) */
rcc_periph_clock_enable(RCC_GPIOB);
gpio_set_mode(GPIOB, GPIO_MODE_INPUT,
GPIO_CNF_INPUT_FLOAT, GPIO2);
GPIO_CNF_INPUT_FLOAT, GPIO2);
exti_select_source(EXTI2, GPIOB);
exti_set_trigger(EXTI2, EXTI_TRIGGER_FALLING);
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)
{
static uint32_t previous_led_hw_values = 0x00;
if (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) );
}
static uint32_t previous_led_hw_values = 0x00;
if (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) );
}
}
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;
// FIXME(ben): not clear why recvfrom() and not recv() is used.
n = recvfrom(navdata_socket, buffer, ARDRONE_NAVDATA_BUFFER_SIZE, 0x0,
(struct sockaddr *) &from, (socklen_t *) &l);
(struct sockaddr *) &from, (socklen_t *) &l);
return n;
}
sw/airborne/boards/ardrone/gpio_ardrone.c
+16 -16
View File
@@ -42,14 +42,14 @@ struct gpio_data {
};
enum gpio_mode {
GPIO_INPUT = 0, //!< Pin configured for input
GPIO_OUTPUT_LOW, //!< Pin configured for output with low level
GPIO_OUTPUT_HIGH, //!< Pin configured for output with high level
GPIO_INPUT = 0, //!< Pin configured for input
GPIO_OUTPUT_LOW, //!< Pin configured for output with low level
GPIO_OUTPUT_HIGH, //!< Pin configured for output with high level
};
struct gpio_direction {
int pin;
enum gpio_mode mode;
int pin;
enum gpio_mode mode;
};
@@ -59,7 +59,7 @@ void gpio_set(uint32_t port, uint16_t pin)
struct gpio_data data;
// Open the device if not open
if (gpiofp == 0)
gpiofp = open("/dev/gpio",O_RDWR);
gpiofp = open("/dev/gpio",O_RDWR);
// Read the GPIO value
data.pin = pin;
@@ -74,7 +74,7 @@ void gpio_clear(uint32_t port, uint16_t pin)
struct gpio_data data;
// Open the device if not open
if (gpiofp == 0)
gpiofp = open("/dev/gpio",O_RDWR);
gpiofp = open("/dev/gpio",O_RDWR);
// Read the GPIO value
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)
{
/*
if (port != 0x32524) return; // protect ardrone board from unauthorized use
struct gpio_direction dir;
// Open the device if not open
if (gpiofp == 0)
/*
if (port != 0x32524) return; // protect ardrone board from unauthorized use
struct gpio_direction dir;
// Open the device if not open
if (gpiofp == 0)
gpiofp = open("/dev/gpio",O_RDWR);
// Read the GPIO value
dir.pin = pin;
dir.mode = GPIO_OUTPUT_LOW;
ioctl(gpiofp, GPIO_DIRECTION, &dir);
// Read the GPIO value
dir.pin = pin;
dir.mode = GPIO_OUTPUT_LOW;
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 (errno == EAGAIN || errno == EWOULDBLOCK)
continue;
continue;
return n;
}
written += n;
@@ -85,7 +85,7 @@ ssize_t full_read(int fd, uint8_t *buf, size_t count)
if (n < 0)
{
if (errno == EAGAIN || errno == EWOULDBLOCK)
continue;
continue;
return n;
}
readed += n;
@@ -469,8 +469,8 @@ void navdata_update()
// Check if there is a new sonar measurement and update the sonar
if (navdata.ultrasound >> 15)
{
sonar_meas = (navdata.ultrasound & 0x7FFF);
ins_update_sonar();
sonar_meas = (navdata.ultrasound & 0x7FFF);
ins_update_sonar();
}
#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_set_mode(GPIOB, GPIO_MODE_INPUT,
GPIO_CNF_INPUT_PULL_UPDOWN, GPIO0);
GPIO_CNF_INPUT_PULL_UPDOWN, GPIO0);
#ifdef 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 ) {
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 */
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-*/
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);
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.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.thrust_out,&msg_in.payload.msg_up.pitch_out);});
&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.can_errs,&msg_in.payload.msg_up.spi_errs,
&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,
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",
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",
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.
//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) ) {
controller.armed = 0; last_state=1;
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();
/* 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)
&imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.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);});
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
&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.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);})
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)
&imu.gyro.p,&imu.gyro.q,&imu.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);});
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,
//&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);});*/
//&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);});*/
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) {
RunOnceEvery(15, {DOWNLINK_SEND_BETH_CONTROLLER(gcs_com.udp_transport,
&controller.cmd_pitch,&controller.cmd_thrust,
&controller.cmd_pitch_ff,&controller.cmd_pitch_fb,
&controller.cmd_thrust_ff,&controller.cmd_thrust_fb,
&controller.tilt_sp,&controller.tilt_ref,&controller.tilt_dot_ref,
&controller.elevation_sp,&controller.elevation_ref,&controller.elevation_dot_ref,
&controller.azimuth_sp);});
&controller.cmd_pitch,&controller.cmd_thrust,
&controller.cmd_pitch_ff,&controller.cmd_pitch_fb,
&controller.cmd_thrust_ff,&controller.cmd_thrust_fb,
&controller.tilt_sp,&controller.tilt_ref,&controller.tilt_dot_ref,
&controller.elevation_sp,&controller.elevation_ref,&controller.elevation_dot_ref,
&controller.azimuth_sp);});
}
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_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_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_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) {
RunOnceEvery(25, {DOWNLINK_SEND_BETH_ESTIMATOR(gcs_com.udp_transport,
&estimator.tilt,&estimator.tilt_dot,
&estimator.elevation,&estimator.elevation_dot,
&estimator.azimuth,&estimator.azimuth_dot);});
&estimator.tilt,&estimator.tilt_dot,
&estimator.elevation,&estimator.elevation_dot,
&estimator.azimuth,&estimator.azimuth_dot);});
}
//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){ \
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)); \
DownlinkStartMessage(_chan, "HITL_UBX", DL_HITL_UBX, 0+1+1+1+1+nb_ubx_payload*1) \
DownlinkPutUint8ByAddr(_chan, (class)); \
DownlinkPutUint8ByAddr(_chan, (id)); \
DownlinkPutUint8ByAddr(_chan, (ac_id)); \
DownlinkPutUint8Array(_chan, nb_ubx_payload, ubx_payload); \
DownlinkEndMessage(_chan ) \
} else \
DownlinkOverrun(_chan ); \
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)); \
DownlinkStartMessage(_chan, "HITL_UBX", DL_HITL_UBX, 0+1+1+1+1+nb_ubx_payload*1) \
DownlinkPutUint8ByAddr(_chan, (class)); \
DownlinkPutUint8ByAddr(_chan, (id)); \
DownlinkPutUint8ByAddr(_chan, (ac_id)); \
DownlinkPutUint8Array(_chan, nb_ubx_payload, ubx_payload); \
DownlinkEndMessage(_chan ) \
} else \
DownlinkOverrun(_chan ); \
}
#endif
@@ -204,7 +204,7 @@ void check_gps(void){
else {
if (!donegpsconf) {
printf("Finished GPS configuration.\n");
donegpsconf=1;
donegpsconf=1;
}
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) {
RunOnceEvery(15, {DOWNLINK_SEND_BETH_CONTROLLER(gcs_com.udp_transport,
&controller.cmd_pitch,&controller.cmd_thrust,
&controller.cmd_pitch_ff,&controller.cmd_pitch_fb,
&controller.cmd_thrust_ff,&controller.cmd_thrust_fb,
&controller.tilt_sp,&controller.tilt_ref,&controller.tilt_dot_ref,
&controller.elevation_sp,&controller.elevation_ref,&controller.elevation_dot_ref,
&controller.azimuth_sp);});
&controller.cmd_pitch,&controller.cmd_thrust,
&controller.cmd_pitch_ff,&controller.cmd_pitch_fb,
&controller.cmd_thrust_ff,&controller.cmd_thrust_fb,
&controller.tilt_sp,&controller.tilt_ref,&controller.tilt_dot_ref,
&controller.elevation_sp,&controller.elevation_ref,&controller.elevation_dot_ref,
&controller.azimuth_sp);});
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_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_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.mass * (controller.omega_cl * controller.omega_cl * err_elevation);
controller.mass * (controller.omega_cl * controller.omega_cl * err_elevation);
#if USE_AZIMUTH
controller.cmd_azimuth_ff = controller.one_over_J * controller.azimuth_ddot_ref;
@@ -188,184 +188,184 @@ void control_run(void) {
float get_U_twt()
{
/**Definition des constantes du modèle**/
const float Gain = -65;
const float Te = 1/512.;
/**Definition des constantes du modèle**/
const float Gain = -65;
const float Te = 1/512.;
/**Variables utilisés par la loi de commande**/
static volatile float yd[2] = {0.0,0.0};
static volatile float y[2] = {0.,0.};
//static float emax = 0.035; // en rad, initialement 2
/**Variables utilisés par la loi de commande**/
static volatile float yd[2] = {0.0,0.0};
static volatile float y[2] = {0.,0.};
//static float emax = 0.035; // en rad, initialement 2
/**Variables pour l'algorithme**/
float udot;
float sens;
/**Variables pour l'algorithme**/
float udot;
float sens;
//Acquisition consigne
yd[1] = controller.tilt_ref;
//Acquisition mesure
y[1] = estimator.tilt;
//Acquisition consigne
yd[1] = controller.tilt_ref;
//Acquisition mesure
y[1] = estimator.tilt;
/***************************/
/***************************/
/**Calcul Surface et derive Surface**/
// S[1],y[1],yd[1] new value
// S[0],y[0],yd[0] last value
/**Calcul Surface et derive Surface**/
// S[1],y[1],yd[1] new value
// S[0],y[0],yd[0] last value
//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]) - estimator.tilt_dot ) * 0.8 ) ;
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]);
/*************************************/
//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]) - estimator.tilt_dot ) * 0.8 ) ;
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]);
/*************************************/
//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 ) {
U_twt[1] = U_twt[0];
} else {*/
/**Algorithme twisting**/
if ( controller.S[1] < 0.0 )
sens = -1.0;
else
sens = 1.0;
U_twt[1] = U_twt[0];
} else {*/
/**Algorithme twisting**/
if ( controller.S[1] < 0.0 )
sens = -1.0;
else
sens = 1.0;
if ( abs(controller.U_twt[1]) < controller.ulim ) {
if ( (controller.S[1] * controller.S_dot) > 0) {
udot = -controller.VM * sens;
}
else {
udot = -controller.Vm * sens;
}
}
else {
udot = -controller.U_twt[1];
}
if ( abs(controller.U_twt[1]) < controller.ulim ) {
if ( (controller.S[1] * controller.S_dot) > 0) {
udot = -controller.VM * sens;
}
else {
udot = -controller.Vm * sens;
}
}
else {
udot = -controller.U_twt[1];
}
// Integration de u, qu'avec 2 valeurs, penser à faire plus
// u[1] new , u[0] old
controller.U_twt[1] = controller.U_twt[0] + (Te * udot);
//}
/**********************/
// Integration de u, qu'avec 2 valeurs, penser à faire plus
// u[1] new , u[0] old
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) ){
Bound(controller.U_twt[1],-controller.satval1,controller.satval1);
}
else {
Bound(controller.U_twt[1],-controller.satval2,controller.satval2);
}
/********************************/
if ( (controller.S[1] > -controller.satval1) && (controller.S[1] < controller.satval1) ){
Bound(controller.U_twt[1],-controller.satval1,controller.satval1);
}
else {
Bound(controller.U_twt[1],-controller.satval2,controller.satval2);
}
/********************************/
/**Mises à jour**/
controller.U_twt[0] = controller.U_twt[1];
yd[0] = yd[1];
y[0] = y[1];
/**Mises à jour**/
controller.U_twt[0] = controller.U_twt[1];
yd[0] = yd[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()
{
/**Definition des constantes du modèle**/
const float Gain = 800.;
const float Te = 1/512.;
/**Definition des constantes du modèle**/
const float Gain = 800.;
const float Te = 1/512.;
/**Variables utilisés par la loi de commande**/
static volatile float yd2[2] = {0.0,0.0};
static volatile float y2[2] = {0.,0.};
//static float emax = 0.035; // en rad, initialement 2
/**Variables utilisés par la loi de commande**/
static volatile float yd2[2] = {0.0,0.0};
static volatile float y2[2] = {0.,0.};
//static float emax = 0.035; // en rad, initialement 2
/**Variables pour l'algorithme**/
float udot2;
float sens2;
static float S2[2]= {0.0,0.0};
static float S2_dot=0;
static float U2_twt[2]= {0.0,0.0};
/**Variables pour l'algorithme**/
float udot2;
float sens2;
static float S2[2]= {0.0,0.0};
static float S2_dot=0;
static float U2_twt[2]= {0.0,0.0};
//Acquisition consigne
yd2[1] = controller.elevation_ref;
//Acquisition mesure
y2[1] = estimator.elevation;
//Acquisition consigne
yd2[1] = controller.elevation_ref;
//Acquisition mesure
y2[1] = estimator.elevation;
/***************************/
/***************************/
/**Calcul Surface et derive Surface**/
// S[1],y[1],yd[1] new value
// S[0],y[0],yd[0] last value
/**Calcul Surface et derive Surface**/
// S[1],y[1],yd[1] new value
// S[0],y[0],yd[0] last value
//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]) - estimator.tilt_dot ) * 0.8 ) ;
S2[1] = (float)( controller.c * (y2[1]-yd2[1]) + estimator.elevation_dot - controller.elevation_dot_ref );
S2_dot = (S2[1] - S2[0]);
/*************************************/
//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]) - estimator.tilt_dot ) * 0.8 ) ;
S2[1] = (float)( controller.c * (y2[1]-yd2[1]) + estimator.elevation_dot - controller.elevation_dot_ref );
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 ) {
U_twt[1] = U_twt[0];
} else {*/
/**Algorithme twisting**/
if ( S2[1] < 0.0 )
sens2 = -1.0;
else
sens2 = 1.0;
U_twt[1] = U_twt[0];
} else {*/
/**Algorithme twisting**/
if ( S2[1] < 0.0 )
sens2 = -1.0;
else
sens2 = 1.0;
if ( abs(U2_twt[1]) < controller.ulim ) {
if ( (S2[1] * S2_dot) > 0) {
udot2 = -controller.VM * sens2;
}
else {
udot2 = -controller.Vm * sens2;
}
}
else {
udot2 = -U2_twt[1];
}
if ( abs(U2_twt[1]) < controller.ulim ) {
if ( (S2[1] * S2_dot) > 0) {
udot2 = -controller.VM * sens2;
}
else {
udot2 = -controller.Vm * sens2;
}
}
else {
udot2 = -U2_twt[1];
}
// Integration de u, qu'avec 2 valeurs, penser à faire plus
// u[1] new , u[0] old
U2_twt[1] = U2_twt[0] + (Te * udot2);
//}
/**********************/
// Integration de u, qu'avec 2 valeurs, penser à faire plus
// u[1] new , u[0] old
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) ){
Bound(U2_twt[1],-controller.satval1,controller.satval1);
}
else {
Bound(U2_twt[1],-controller.satval2,controller.satval2);
}
/********************************/
if ( (S2[1] > -controller.satval1) && (S2[1] < controller.satval1) ){
Bound(U2_twt[1],-controller.satval1,controller.satval1);
}
else {
Bound(U2_twt[1],-controller.satval2,controller.satval2);
}
/********************************/
/**Mises à jour**/
U2_twt[0] = U2_twt[1];
yd2[0] = yd2[1];
y2[0] = y2[1];
/**Mises à jour**/
U2_twt[0] = U2_twt[1];
yd2[0] = yd2[1];
y2[0] = y2[1];
S2[0] = S2[1];
S2[0] = S2[1];
#define NUMSAMPS (1000)
float retval = Gain * U2_twt[1];
float retval = Gain * U2_twt[1];
static int sum = 0;
static int i = 0;
static int sum = 0;
static int i = 0;
sum = sum + retval;
sum = sum + retval;
if (i == (NUMSAMPS-1)) {
i = 0;
printf("avg: %f\n",sum/(float)NUMSAMPS);
sum = 0;
} else {
i++;
}
i = 0;
printf("avg: %f\n",sum/(float)NUMSAMPS);
sum = 0;
} else {
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)
{
/* do not use sprintf or similar */
int i;
/* do not use sprintf or similar */
int i;
for (i=7; i>=0; i--) {
name[i] = (local % 10) + '0';
local /= 10;
}
name[8]='.';name[9]='t';name[10]='l';name[11]='m';name[12]=0;
for (i=7; i>=0; i--) {
name[i] = (local % 10) + '0';
local /= 10;
}
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)
{
int i;
unsigned char retval = start;
for (i=0;i<length;i++) retval += data[i];
int i;
unsigned char retval = start;
for (i=0;i<length;i++) retval += data[i];
return retval;
return retval;
}
unsigned int getclock(void)
{
uint64_t clock;
uint32_t clock_lsb;
uint64_t clock;
uint32_t clock_lsb;
clock_lsb = T0TC;
clock_lsb = T0TC;
if (clock_lsb < clock_lsb_last) clock_msb++;
clock_lsb_last = clock_lsb;
if (clock_lsb < clock_lsb_last) clock_msb++;
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 */
@@ -286,49 +286,49 @@ void log_xbee(unsigned char c, unsigned char source)
static unsigned char cs, payload_idx, i;
switch (xbeel_status) {
case XBEE_UNINIT:
if (c == XBEE_START)
{
// serial receive broken with MAX
case XBEE_UNINIT:
if (c == XBEE_START)
{
// serial receive broken with MAX
#ifndef USE_MAX11040
xbeel_timestamp = getclock();
xbeel_timestamp = getclock();
#endif
xbeel_status++;
}
break;
case XBEE_GOT_START:
xbeel_payload_len = c<<8;
xbeel_status++;
}
break;
case XBEE_GOT_START:
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)
break;
case XBEE_GOT_LENGTH_MSB:
xbeel_payload_len |= c;
xbeel_status++;
break;
case XBEE_GOT_PAYLOAD:
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
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++;
break;
case XBEE_GOT_PAYLOAD:
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
log_payload(xbeel_payload_len-XBEE_RFDATA_OFFSET, source, xbeel_timestamp);
log_payload(xbeel_payload_len-XBEE_RFDATA_OFFSET, source, xbeel_timestamp);
#endif
LED_TOGGLE(LED_GREEN);
goto restart;
LED_TOGGLE(LED_GREEN);
goto restart;
}
return;
error:
@@ -344,45 +344,45 @@ void log_pprz(unsigned char c, unsigned char source)
static unsigned char _ck_a, _ck_b, payload_idx, i;
switch (pprzl_status) {
case UNINIT:
if (c == STX)
// serial receive broken with MAX
case UNINIT:
if (c == STX)
// serial receive broken with MAX
#ifndef USE_MAX11040
pprzl_timestamp = getclock();
pprzl_timestamp = getclock();
#endif
pprzl_status++;
break;
case GOT_STX:
pprzl_payload_len = c-4; /* Counting STX, LENGTH and CRC1 and CRC2 */
_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)
break;
case GOT_STX:
pprzl_payload_len = c-4; /* Counting STX, LENGTH and CRC1 and CRC2 */
_ck_a = _ck_b = c;
pprzl_status++;
break;
case GOT_PAYLOAD:
if (c != _ck_a)
goto error;
pprzl_status++;
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
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++;
break;
case GOT_PAYLOAD:
if (c != _ck_a)
goto error;
pprzl_status++;
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
log_payload(pprzl_payload_len, source, pprzl_timestamp);
log_payload(pprzl_payload_len, source, pprzl_timestamp);
#endif
LED_TOGGLE(LED_GREEN);
goto restart;
LED_TOGGLE(LED_GREEN);
goto restart;
}
return;
error:
@@ -394,100 +394,100 @@ void log_pprz(unsigned char c, unsigned char source)
int do_log(void)
{
unsigned int count;
unsigned char name[13];
unsigned char inc;
int temp;
unsigned int count;
unsigned char name[13];
unsigned char inc;
int temp;
if(efs_init(&efs, 0) != 0) {
return(-1);
}
if(efs_init(&efs, 0) != 0) {
return(-1);
}
/* find an unused file number the dumb way */
for (count = 1; count < 0xFFFFFFF; count++)
{
set_filename(count, name);
if(file_fopen(&filer, &efs.myFs, name,'r')!=0) break;
file_fclose(&filer);
}
/* find an unused file number the dumb way */
for (count = 1; count < 0xFFFFFFF; count++)
{
set_filename(count, name);
if(file_fopen(&filer, &efs.myFs, name,'r')!=0) break;
file_fclose(&filer);
}
if (file_fopen(&filew, &efs.myFs, name, 'w' ) != 0)
{
return(-1);
}
if (file_fopen(&filew, &efs.myFs, name, 'w' ) != 0)
{
return(-1);
}
/* write to SD until key is pressed */
while ((IO0PIN & (1<<LOG_STOP_KEY))>>LOG_STOP_KEY)
{
/* write to SD until key is pressed */
while ((IO0PIN & (1<<LOG_STOP_KEY))>>LOG_STOP_KEY)
{
#ifdef USE_MAX11040
if ((max11040_data == MAX11040_DATA_AVAILABLE) &&
(max11040_buf_in != max11040_buf_out)) {
// LED_TOGGLE(LED_GREEN);
int i;
if ((max11040_data == MAX11040_DATA_AVAILABLE) &&
(max11040_buf_in != max11040_buf_out)) {
// LED_TOGGLE(LED_GREEN);
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+1] = 61; // message_id (DL_TURB_PRESSURE_RAW)
log_buffer[LOG_DATA_OFFSET+0] = AC_ID; // sender_id
log_buffer[LOG_DATA_OFFSET+1] = 61; // message_id (DL_TURB_PRESSURE_RAW)
while(max11040_buf_in != max11040_buf_out) {
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 + 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 + 3] = (max11040_values[max11040_buf_out][i] >> 24) & 0xFF;
while(max11040_buf_in != max11040_buf_out) {
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 + 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 + 3] = (max11040_values[max11040_buf_out][i] >> 24) & 0xFF;
}
log_payload(2 + 16 * 4, LOG_SOURCE_UART0, max11040_timestamp[max11040_buf_out]);
i = max11040_buf_out+1;
if (i >= MAX11040_BUF_SIZE) i=0;
max11040_buf_out = i;
}
}
log_payload(2 + 16 * 4, LOG_SOURCE_UART0, max11040_timestamp[max11040_buf_out]);
i = max11040_buf_out+1;
if (i >= MAX11040_BUF_SIZE) i=0;
max11040_buf_out = i;
}
}
#endif
#if USE_UART0
temp = 0;
while (uart_char_available(&uart0) && (temp++ < 128))
{
// LED_TOGGLE(LED_GREEN);
inc = uart_getch(&uart1);
temp = 0;
while (uart_char_available(&uart0) && (temp++ < 128))
{
// LED_TOGGLE(LED_GREEN);
inc = uart_getch(&uart1);
#ifdef LOG_XBEE
log_xbee(inc, LOG_SOURCE_UART0);
log_xbee(inc, LOG_SOURCE_UART0);
#else
#ifdef LOG_PPRZ
log_pprz(inc, LOG_SOURCE_UART0);
log_pprz(inc, LOG_SOURCE_UART0);
#else
#error no log transport protocol selected
#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
}
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 );
fs_umount( &efs.myFs ) ;
file_fclose( &filew );
fs_umount( &efs.myFs ) ;
return 0;
return 0;
}
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 );
#define MB_SCALE_IT TIR_CR3I
#define MB_SCALE_ICP_ISR() { \
static uint32_t last; \
uint32_t now = T0CR3; \
mb_scale_pulse_len = now - last; \
last = now; \
if (mb_scale_calib > 0) { \
mb_scale_thrust += mb_scale_pulse_len; \
if (mb_scale_calib >= MB_SCALE_NB_CALIB) { \
mb_scale_neutral = mb_scale_thrust / MB_SCALE_NB_CALIB; \
mb_scale_calib = 0; \
} \
else \
mb_scale_calib++; \
} \
else { \
#define MB_SCALE_ICP_ISR() { \
static uint32_t last; \
uint32_t now = T0CR3; \
mb_scale_pulse_len = now - last; \
last = now; \
if (mb_scale_calib > 0) { \
mb_scale_thrust += mb_scale_pulse_len; \
if (mb_scale_calib >= MB_SCALE_NB_CALIB) { \
mb_scale_neutral = mb_scale_thrust / MB_SCALE_NB_CALIB; \
mb_scale_calib = 0; \
} \
else \
mb_scale_calib++; \
} \
else { \
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) { \
mb_scale_calib = 1; \
mb_scale_thrust = 0.; \
#define mb_scale_Calib(_val) { \
mb_scale_calib = 1; \
mb_scale_thrust = 0.; \
}
#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;
switch (mb_twi_controller_asctech_command_type) {
case MB_TWI_CONTROLLER_COMMAND_TEST :
i2c0_buf[0] = 251;
i2c0_buf[1] = mb_twi_controller_asctech_addr;
i2c0_buf[2] = 0;
i2c0_buf[3] = 231 + mb_twi_controller_asctech_addr;
// mb_twi_i2c_done = FALSE;
i2c0_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
break;
case MB_TWI_CONTROLLER_COMMAND_TEST :
i2c0_buf[0] = 251;
i2c0_buf[1] = mb_twi_controller_asctech_addr;
i2c0_buf[2] = 0;
i2c0_buf[3] = 231 + mb_twi_controller_asctech_addr;
// mb_twi_i2c_done = FALSE;
i2c0_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
break;
case MB_TWI_CONTROLLER_COMMAND_REVERSE :
i2c0_buf[0] = 254;
i2c0_buf[1] = mb_twi_controller_asctech_addr;
i2c0_buf[2] = 0;
i2c0_buf[3] = 234 + mb_twi_controller_asctech_addr;
// mb_twi_i2c_done = FALSE;
i2c0_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
break;
case MB_TWI_CONTROLLER_COMMAND_REVERSE :
i2c0_buf[0] = 254;
i2c0_buf[1] = mb_twi_controller_asctech_addr;
i2c0_buf[2] = 0;
i2c0_buf[3] = 234 + mb_twi_controller_asctech_addr;
// mb_twi_i2c_done = FALSE;
i2c0_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
break;
case MB_TWI_CONTROLLER_COMMAND_SET_ADDR :
i2c0_buf[0] = 250;
i2c0_buf[1] = mb_twi_controller_asctech_addr;
i2c0_buf[2] = mb_twi_controller_asctech_new_addr;
i2c0_buf[3] = 230 + 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;
i2c0_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
break;
case MB_TWI_CONTROLLER_COMMAND_SET_ADDR :
i2c0_buf[0] = 250;
i2c0_buf[1] = mb_twi_controller_asctech_addr;
i2c0_buf[2] = mb_twi_controller_asctech_new_addr;
i2c0_buf[3] = 230 + 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;
i2c0_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done);
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) {
ser = Serial.read();
switch (stat) {
case INIT:
/* sync on the last byte of the prev message */
if (b == END_MSG) {
count_geiger_1 = 0;
count_geiger_2 = 0;
volt_geiger = 0;
i = 0;
stat = FOUND_SYNC;
}
break;
case FOUND_SYNC:
if ((b <= '9') && (b >= '0')) {
count_geiger_1 = count_geiger_1 * 10 + (b-'0');
if (++i > 7) state = IDLE;
} else if (b == ',')) {
i = 0;
stat = FOUND_1;
} else stat = INIT;
break;
case INIT:
/* sync on the last byte of the prev message */
if (b == END_MSG) {
count_geiger_1 = 0;
count_geiger_2 = 0;
volt_geiger = 0;
i = 0;
stat = FOUND_SYNC;
}
break;
case FOUND_SYNC:
if ((b <= '9') && (b >= '0')) {
count_geiger_1 = count_geiger_1 * 10 + (b-'0');
if (++i > 7) state = IDLE;
} else if (b == ',')) {
i = 0;
stat = FOUND_1;
} else stat = INIT;
break;
case FOUND_1:
/* read counter 1 */
if ((b <= '9') && (b >= '0')) {
count_geiger_2 = count_geiger_2 * 10 + (b-'0');
if (++i > 7) state = IDLE;
if (++i > 7) state = IDLE;
} else if (b == ',')) {
#ifdef DEBUG
Serial.println(count_geiger_1, DEC);
Serial.println(count_geiger_1, DEC);
#endif
i = 0;
stat = FOUND_2;
} else stat = INIT;
break;
case FOUND_2:
/* read counter 2 */
if ((b <= '9') && (b >= '0')) {
count_geiger_2 = count_geiger_2 * 10 + (b-'0');
if (++i > 7) state = IDLE;
} else if (b == ',')) {
i = 0;
stat = FOUND_2;
} else stat = INIT;
break;
case FOUND_2:
/* read counter 2 */
if ((b <= '9') && (b >= '0')) {
count_geiger_2 = count_geiger_2 * 10 + (b-'0');
if (++i > 7) state = IDLE;
} else if (b == ',')) {
#ifdef DEBUG
Serial.println(count_geiger_2, DEC);
Serial.println(count_geiger_2, DEC);
#endif
i = 0;
stat = FOUND_3;
} else stat = INIT;
break;
case FOUND_3:
/* ignore 3 */
if ((b <= '9') && (b >= '0')) {
if (++i > 7) state = IDLE;
} else if (b == ',')) {
i = 0;
stat = FOUND_4;
} else stat = INIT;
break;
case FOUND_4:
/* ignore 4 */
if ((b <= '9') && (b >= '0')) {
if (++i > 7) state = IDLE;
} else if (b == ',')) {
i = 0;
stat = FOUND_5;
} else stat = INIT;
break;
case FOUND_5:
/* read voltage */
if ((b <= '9') && (b >= '0')) {
volt_geiger = volt_geiger * 10 + (b-'0');
if (++i > 7) state = IDLE;
} else if (b == 'V')) {
digitalWrite(LED_GR_PIN, HIGH);
i = 0;
stat = FOUND_3;
} else stat = INIT;
break;
case FOUND_3:
/* ignore 3 */
if ((b <= '9') && (b >= '0')) {
if (++i > 7) state = IDLE;
} else if (b == ',')) {
i = 0;
stat = FOUND_4;
} else stat = INIT;
break;
case FOUND_4:
/* ignore 4 */
if ((b <= '9') && (b >= '0')) {
if (++i > 7) state = IDLE;
} else if (b == ',')) {
i = 0;
stat = FOUND_5;
} else stat = INIT;
break;
case FOUND_5:
/* read voltage */
if ((b <= '9') && (b >= '0')) {
volt_geiger = volt_geiger * 10 + (b-'0');
if (++i > 7) state = IDLE;
} else if (b == 'V')) {
digitalWrite(LED_GR_PIN, HIGH);
#ifdef DEBUG
Serial.println(volt_geiger, DEC);
Serial.println(volt_geiger, DEC);
#endif
received_data = 0;
stat = INIT;
} else stat = INIT;
break;
default:
stat = INIT;
}
}
received_data = 0;
stat = INIT;
} else stat = INIT;
break;
default:
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 {
uint16_t channels[NB_ADC];
uint16_t channels[NB_ADC];
};
/* 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 Int32Vect3 accel;
struct Int32Vect3 mag;
uint32_t imu_tick;
uint32_t imu_tick;
int32_t pressure_absolute;
int32_t pressure_differential;
int16_t rc_pitch;
@@ -194,9 +194,9 @@ struct __attribute__ ((packed)) AutopilotMessageVIDown
#endif
struct __attribute__ ((packed)) AutopilotMessagePTStream
{
uint8_t message_cnt;
int8_t package_cntd;
uint8_t pkg_data[SPISTREAM_PACKAGE_SIZE];
uint8_t message_cnt;
int8_t package_cntd;
uint8_t pkg_data[SPISTREAM_PACKAGE_SIZE];
};
/* 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);
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.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,
on_datalink_event, fms_gs_com.udp_transport);
on_datalink_event, fms_gs_com.udp_transport);
event_add(&fms_gs_com.datalink_event, NULL);
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 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);
#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;
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.gyro.p, &imuFloat.gyro.q, &imuFloat.gyro.r);
&imuFloat.mag.x, &imuFloat.mag.y, &imuFloat.mag.z,
&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))
@@ -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))
{
adc_callback = adc_callback_fun;
adc_callback = adc_callback_fun;
}
int spi_ap_link_init()
@@ -99,13 +99,13 @@ int spi_ap_link_init()
imuFloat.sample_count = 0;
#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 };
FLOAT_QUAT_OF_AXIS_ANGLE(imuFloat.body_to_imu_quat, x_axis, QUAT_SETPOINT_HOVER_PITCH);
#endif
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);
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)
pressure_differential_callback(0, (32768 + msg_up->pressure_differential));
if (msg_up->valid.adc) {
if(adc_callback) {
adc_callback(msg_up->adc.channels);
}
}
if (msg_up->valid.adc) {
if(adc_callback) {
adc_callback(msg_up->adc.channels);
}
}
// Fill radio data
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.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)
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 ret;
int ret;
struct spi_ioc_transfer tr = {
.tx_buf = (unsigned long)buf_out,
.rx_buf = (unsigned long)buf_in,
.len = count,
.delay_usecs = spi_link.delay,
.speed_hz = spi_link.speed,
.bits_per_word = spi_link.bits,
};
struct spi_ioc_transfer tr = {
.tx_buf = (unsigned long)buf_out,
.rx_buf = (unsigned long)buf_in,
.len = count,
.delay_usecs = spi_link.delay,
.speed_hz = spi_link.speed,
.bits_per_word = spi_link.bits,
};
((uint8_t*)buf_out)[count-1] = crc_calc_block_crc8(buf_out, count-1);
ret = ioctl(spi_link.fd, SPI_IOC_MESSAGE(1), &tr);
spi_link.msg_cnt++;
((uint8_t*)buf_out)[count-1] = crc_calc_block_crc8(buf_out, count-1);
ret = ioctl(spi_link.fd, SPI_IOC_MESSAGE(1), &tr);
spi_link.msg_cnt++;
uint8_t computed_crc = crc_calc_block_crc8(buf_in, count-1);
if (computed_crc == ((uint8_t*)buf_in)[count-1])
*crc_valid = 1;
else {
*crc_valid = 0;
spi_link.crc_err_cnt++;
}
uint8_t computed_crc = crc_calc_block_crc8(buf_in, count-1);
if (computed_crc == ((uint8_t*)buf_in)[count-1])
*crc_valid = 1;
else {
*crc_valid = 0;
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));
for (uint8_t bit = 8; bit > 0; --bit) {
if (_remainder & TOPBIT)
_remainder = (_remainder << 1) ^ POLYNOMIAL;
_remainder = (_remainder << 1) ^ POLYNOMIAL;
else
_remainder = (_remainder << 1);
_remainder = (_remainder << 1);
}
}
return (_remainder);
@@ -102,10 +102,10 @@ void crc__init(uint32_t polynomial) {
crc_remainder = crc_dividend << (CRC__WIDTH - 8);
for(bit = 8; bit > 0; bit--) {
if(crc_remainder & top_bit) {
crc_remainder = (crc_remainder << 1) ^ polynomial;
crc_remainder = (crc_remainder << 1) ^ polynomial;
}
else {
crc_remainder = (crc_remainder << 1);
crc_remainder = (crc_remainder << 1);
}
}
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) {
/*
struct tm * timeinfo;
time_t c_time;
char time_str[30];
struct tm * timeinfo;
time_t c_time;
char time_str[30];
*/
uint8_t cnt;
uint8_t log_bytes = num_bytes;
if(log_bytes > 16) { log_bytes = 16; }
uint8_t log_bytes = num_bytes;
if(log_bytes > 16) { log_bytes = 16; }
/*
time(&c_time);
timeinfo = localtime(&c_time);
strftime(time_str, 30, " %X ", timeinfo);
time(&c_time);
timeinfo = localtime(&c_time);
strftime(time_str, 30, " %X ", timeinfo);
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 | ",
prefix, num_bytes, msg_id, data[0]);
prefix, num_bytes, msg_id, data[0]);
for(cnt = 1; cnt < log_bytes; cnt++) {
printf("%02X ", data[cnt]);
if(cnt >= 24 && cnt % 24 == 0 && cnt+1 < log_bytes) {
printf("\n ");
}
if(cnt >= 24 && cnt % 24 == 0 && cnt+1 < log_bytes) {
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");
/* Enter our mainloop */
event_dispatch();
while(1) {
sleep(100);
}
event_dispatch();
while(1) {
sleep(100);
}
main_exit();
main_exit();
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) {
uint8_t fifo_idx;
uint8_t msg_id;
uint16_t num_bytes;
int16_t ret;
static uint8_t buf[SPISTREAM_MAX_MESSAGE_LENGTH*10];
uint8_t fifo_idx;
uint8_t msg_id;
uint16_t num_bytes;
int16_t ret;
static uint8_t buf[SPISTREAM_MAX_MESSAGE_LENGTH*10];
for(fifo_idx = 0; fifo_idx < 4; fifo_idx++) {
// The periodic is triggered before fifo
// connections have been initialized, so
// check for a valid fd first:
if(dfifo[fifo_idx] > 0) {
ret = read(dfifo[fifo_idx], (uint8_t *)(&num_bytes), 2);
ret = read(dfifo[fifo_idx], (uint8_t *)(&msg_id), 1);
for(fifo_idx = 0; fifo_idx < 4; fifo_idx++) {
// The periodic is triggered before fifo
// connections have been initialized, so
// check for a valid fd first:
if(dfifo[fifo_idx] > 0) {
ret = read(dfifo[fifo_idx], (uint8_t *)(&num_bytes), 2);
ret = read(dfifo[fifo_idx], (uint8_t *)(&msg_id), 1);
memset(&buf, 0, SPISTREAM_MAX_MESSAGE_LENGTH);
if(num_bytes > SPISTREAM_MAX_MESSAGE_LENGTH) {
fprintf(stderr, "Warning: Message has length %d, but limit "
"is %d\n",
num_bytes, SPISTREAM_MAX_MESSAGE_LENGTH);
num_bytes = SPISTREAM_MAX_MESSAGE_LENGTH;
}
ret = read(dfifo[fifo_idx], &buf, num_bytes);
if(ret > 0 && ret == num_bytes) {
// Message received
print_message(">> Client", msg_id, buf, num_bytes);
}
else if(ret > 0 && ret < num_bytes) {
fprintf(stderr, "Tried to read %d bytes, but only got %d\n",
num_bytes, ret);
}
}
else {
// FIFO file descriptor is invalid,
// retry to open it:
dfifo[fifo_idx] = open(dfifo_files[fifo_idx], O_RDONLY | O_NONBLOCK);
}
}
memset(&buf, 0, SPISTREAM_MAX_MESSAGE_LENGTH);
if(num_bytes > SPISTREAM_MAX_MESSAGE_LENGTH) {
fprintf(stderr, "Warning: Message has length %d, but limit "
"is %d\n",
num_bytes, SPISTREAM_MAX_MESSAGE_LENGTH);
num_bytes = SPISTREAM_MAX_MESSAGE_LENGTH;
}
ret = read(dfifo[fifo_idx], &buf, num_bytes);
if(ret > 0 && ret == num_bytes) {
// Message received
print_message(">> Client", msg_id, buf, num_bytes);
}
else if(ret > 0 && ret < num_bytes) {
fprintf(stderr, "Tried to read %d bytes, but only got %d\n",
num_bytes, ret);
}
}
else {
// FIFO file descriptor is invalid,
// retry to open it:
dfifo[fifo_idx] = open(dfifo_files[fifo_idx], O_RDONLY | O_NONBLOCK);
}
}
}
@@ -141,18 +141,18 @@ static void main_init(void) {
return;
}
signal(SIGKILL, on_kill);
signal(SIGINT, on_kill);
signal(SIGILL, on_kill);
signal(SIGHUP, on_kill);
signal(SIGQUIT, on_kill);
signal(SIGTERM, on_kill);
signal(SIGSEGV, on_kill);
signal(SIGKILL, on_kill);
signal(SIGINT, on_kill);
signal(SIGILL, on_kill);
signal(SIGHUP, on_kill);
signal(SIGQUIT, on_kill);
signal(SIGTERM, on_kill);
signal(SIGSEGV, on_kill);
if(!open_stream()) {
fprintf(stderr, "Could not open stream, sorry\n");
exit(1);
}
if(!open_stream()) {
fprintf(stderr, "Could not open stream, sorry\n");
exit(1);
}
TRACE(TRACE_DEBUG, "%s", "Initialization completed\n");
}
@@ -177,39 +177,39 @@ static void main_init(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[1], "/tmp/spistream_d1.fifo"); // (STM -> daemon -> client)
strcpy(dfifo_files[2], "/tmp/spistream_d2.fifo");
strcpy(dfifo_files[3], "/tmp/spistream_d3.fifo");
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[2], "/tmp/spistream_c2.fifo");
strcpy(cfifo_files[3], "/tmp/spistream_c3.fifo");
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[2], "/tmp/spistream_d2.fifo");
strcpy(dfifo_files[3], "/tmp/spistream_d3.fifo");
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[2], "/tmp/spistream_c2.fifo");
strcpy(cfifo_files[3], "/tmp/spistream_c3.fifo");
for(fifo_idx = 0; fifo_idx < 4; 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);
fprintf(stderr, " ...\n");
}
for(fifo_idx = 0; fifo_idx < 4; 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);
fprintf(stderr, " ...\n");
}
return 1;
return 1;
for(fifo_idx = 0; fifo_idx < 3; fifo_idx++) {
fprintf(stderr, "Open command stream %s ... \n", cfifo_files[fifo_idx]);
cfifo[fifo_idx] = open(cfifo_files[fifo_idx], O_WRONLY);
if(cfifo[fifo_idx] < 0) {
fprintf(stderr, " failed\n");
return 0;
}
}
return 1;
for(fifo_idx = 0; fifo_idx < 3; fifo_idx++) {
fprintf(stderr, "Open command stream %s ... \n", cfifo_files[fifo_idx]);
cfifo[fifo_idx] = open(cfifo_files[fifo_idx], O_WRONLY);
if(cfifo[fifo_idx] < 0) {
fprintf(stderr, " failed\n");
return 0;
}
}
return 1;
}
static void main_exit(void)
{
fprintf(stderr, "Bye!\n");
fprintf(stderr, "Bye!\n");
}
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)
{
fprintf(stderr, "Exiting, got signal %d\n", signum);
main_exit();
exit(1);
fprintf(stderr, "Exiting, got signal %d\n", signum);
main_exit();
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");
/* Enter our mainloop */
event_dispatch();
while(1) {
sleep(100);
}
event_dispatch();
while(1) {
sleep(100);
}
main_exit();
main_exit();
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 int32_t every_100 = 1000;
if(every_100-- == 0) {
every_100 = 1000;
spistream_send_msg(spistream_msg, 21, SPISTREAM_NO_WAIT);
}
static int32_t every_100 = 1000;
if(every_100-- == 0) {
every_100 = 1000;
spistream_send_msg(spistream_msg, 21, SPISTREAM_NO_WAIT);
}
spistream_event();
spistream_event();
}
static void spistream_event() {
@@ -123,25 +123,25 @@ static void spistream_event() {
/*
uint8_t cnt;
static uint8_t pkg_size = sizeof(msg_in.pkg_data);
if(msg_out.message_cnt != 0) {
printf("Package out: Size: %3d MID: %3d PCNTD: %3d | ",
pkg_size, msg_out.message_cnt, msg_out.package_cntd);
for(cnt = 0; cnt < pkg_size; cnt++) {
printf("%3d ", msg_out.pkg_data[cnt]);
}
printf("\n");
}
if(msg_out.message_cnt != 0) {
printf("Package out: Size: %3d MID: %3d PCNTD: %3d | ",
pkg_size, msg_out.message_cnt, msg_out.package_cntd);
for(cnt = 0; cnt < pkg_size; cnt++) {
printf("%3d ", msg_out.pkg_data[cnt]);
}
printf("\n");
}
*/
spi_link_send(&msg_out, sizeof(struct AutopilotMessageCRCFrame), &msg_in, &crc_valid);
/*
if(msg_in.message_cnt != 0) {
printf("PKG in (spi trx: %d): Size: %3d MID: %3d PCNTD: %3d | ", SPISTREAM_PACKAGE_SIZE,
if(msg_in.message_cnt != 0) {
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);
for(cnt = 0; cnt < pkg_size; cnt++) {
printf("%02X ", msg_in.pkg_data[cnt]);
}
printf("\n");
}
for(cnt = 0; cnt < pkg_size; cnt++) {
printf("%02X ", msg_in.pkg_data[cnt]);
}
printf("\n");
}
*/
spistream_write_pkg(&msg_out);
}
@@ -149,47 +149,47 @@ static void spistream_event() {
static void on_spistream_msg_received(uint8_t msg_id,
uint8_t * data,
uint16_t num_bytes) {
uint8_t uart;
uint8_t buf[SPISTREAM_MAX_MESSAGE_LENGTH+3];
uint8_t uart;
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];
// Check for valid uart ID
if(uart >= 0 && uart <= 3) {
if(msg_id > 0) {
buf[0] = (uint8_t)(num_bytes & 0x00ff);
buf[1] = (uint8_t)((num_bytes << 8) & 0x00ff);
buf[2] = msg_id;
if(num_bytes > SPISTREAM_MAX_MESSAGE_LENGTH) {
fprintf(LOG_OUT, "Warning: Message has length %d, but limit "
uart = data[0];
// Check for valid uart ID
if(uart >= 0 && uart <= 3) {
if(msg_id > 0) {
buf[0] = (uint8_t)(num_bytes & 0x00ff);
buf[1] = (uint8_t)((num_bytes << 8) & 0x00ff);
buf[2] = msg_id;
if(num_bytes > SPISTREAM_MAX_MESSAGE_LENGTH) {
fprintf(LOG_OUT, "Warning: Message has length %d, but limit "
"is %d - truncating message\n",
num_bytes, SPISTREAM_MAX_MESSAGE_LENGTH);
num_bytes = SPISTREAM_MAX_MESSAGE_LENGTH;
}
memcpy(buf+3, data, num_bytes);
send_to_client(buf, num_bytes+3, uart);
}
}
num_bytes, SPISTREAM_MAX_MESSAGE_LENGTH);
num_bytes = SPISTREAM_MAX_MESSAGE_LENGTH;
}
memcpy(buf+3, data, num_bytes);
send_to_client(buf, num_bytes+3, uart);
}
}
}
static void send_to_client(uint8_t * data, uint16_t num_bytes, uint8_t fifo_idx)
{
if(dfifo[fifo_idx] <= 0) {
// No client connected to this fifo, yet
dfifo[fifo_idx] = open(dfifo_files[fifo_idx], O_WRONLY | O_NONBLOCK);
if(dfifo[fifo_idx] <= 0) {
fprintf(LOG_OUT, "No client for data fifo %d (%s)\n",
fifo_idx, dfifo_files[fifo_idx]);
return;
}
}
else {
// Client connected to this fifo
if(write(dfifo[fifo_idx], data, num_bytes) == -1) {
fprintf(LOG_OUT, "Write error on data fifo %d\n", fifo_idx);
}
}
if(dfifo[fifo_idx] <= 0) {
// No client connected to this fifo, yet
dfifo[fifo_idx] = open(dfifo_files[fifo_idx], O_WRONLY | O_NONBLOCK);
if(dfifo[fifo_idx] <= 0) {
fprintf(LOG_OUT, "No client for data fifo %d (%s)\n",
fifo_idx, dfifo_files[fifo_idx]);
return;
}
}
else {
// Client connected to this fifo
if(write(dfifo[fifo_idx], data, num_bytes) == -1) {
fprintf(LOG_OUT, "Write error on data fifo %d\n", fifo_idx);
}
}
}
static void on_spistream_msg_sent(uint8_t msg_id) {
@@ -222,124 +222,124 @@ static void main_init(void) {
return;
}
signal(SIGKILL, on_kill);
signal(SIGINT, on_kill);
signal(SIGILL, on_kill);
signal(SIGHUP, on_kill);
signal(SIGQUIT, on_kill);
signal(SIGTERM, on_kill);
signal(SIGSEGV, on_kill);
signal(SIGPIPE, on_dead_pipe);
signal(SIGKILL, on_kill);
signal(SIGINT, on_kill);
signal(SIGILL, on_kill);
signal(SIGHUP, on_kill);
signal(SIGQUIT, on_kill);
signal(SIGTERM, on_kill);
signal(SIGSEGV, on_kill);
signal(SIGPIPE, on_dead_pipe);
if(!open_stream()) {
fprintf(LOG_OUT, "Could not open stream, sorry\n");
exit(1);
}
if(!open_stream()) {
fprintf(LOG_OUT, "Could not open stream, sorry\n");
exit(1);
}
TRACE(TRACE_DEBUG, "%s", "Initialization completed\n");
}
static void main_exit(void)
{
fprintf(LOG_OUT, "Closing socket\n");
close_stream();
fprintf(LOG_OUT, "Closing socket\n");
close_stream();
}
static void parse_command_line(int argc, char** argv) {
/*
while ((ch = getopt(argc, argv, "d:")) != -1) {
switch (ch) {
case 'd':
daemon_mode = 1;
break;
}
}
while ((ch = getopt(argc, argv, "d:")) != -1) {
switch (ch) {
case 'd':
daemon_mode = 1;
break;
}
}
*/
}
static int open_stream(void) {
uint8_t fifo_idx;
int ret;
uint8_t fifo_idx;
int ret;
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[2], "/tmp/spistream_d2.fifo");
strcpy(dfifo_files[3], "/tmp/spistream_d3.fifo");
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[2], "/tmp/spistream_c2.fifo");
strcpy(cfifo_files[3], "/tmp/spistream_c3.fifo");
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[2], "/tmp/spistream_d2.fifo");
strcpy(dfifo_files[3], "/tmp/spistream_d3.fifo");
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[2], "/tmp/spistream_c2.fifo");
strcpy(cfifo_files[3], "/tmp/spistream_c3.fifo");
for(fifo_idx = 0; fifo_idx < 4; fifo_idx++) {
fprintf(LOG_OUT, "Creating data stream %s ...", dfifo_files[fifo_idx]);
if((ret = mkfifo(dfifo_files[fifo_idx], 0777)) < 0) {
fprintf(LOG_OUT, " failed\n");
fprintf(LOG_OUT, "Could not create data fifo %d: %s\n",
fifo_idx, dfifo_files[fifo_idx]);
close_stream();
return 0;
}
else {
fprintf(LOG_OUT, " ok\n");
dfifo[fifo_idx] = open(dfifo_files[fifo_idx], O_WRONLY | O_NONBLOCK);
}
}
for(fifo_idx = 0; fifo_idx < 4; fifo_idx++) {
fprintf(LOG_OUT, "Creating data stream %s ...", dfifo_files[fifo_idx]);
if((ret = mkfifo(dfifo_files[fifo_idx], 0777)) < 0) {
fprintf(LOG_OUT, " failed\n");
fprintf(LOG_OUT, "Could not create data fifo %d: %s\n",
fifo_idx, dfifo_files[fifo_idx]);
close_stream();
return 0;
}
else {
fprintf(LOG_OUT, " ok\n");
dfifo[fifo_idx] = open(dfifo_files[fifo_idx], O_WRONLY | O_NONBLOCK);
}
}
for(fifo_idx = 0; fifo_idx < 4; fifo_idx++) {
fprintf(LOG_OUT, "Creating command stream %s ... ", cfifo_files[fifo_idx]);
if((ret = mkfifo(cfifo_files[fifo_idx], 0777)) < 0) {
fprintf(LOG_OUT, " failed\n");
fprintf(LOG_OUT, "Could not create command fifo %d: %s\n",
fifo_idx, cfifo_files[fifo_idx]);
close_stream();
return 0;
}
else {
fprintf(LOG_OUT, " ok\n");
cfifo[fifo_idx] = open(cfifo_files[fifo_idx], O_RDONLY | O_NONBLOCK);
}
}
return 1;
for(fifo_idx = 0; fifo_idx < 4; fifo_idx++) {
fprintf(LOG_OUT, "Creating command stream %s ... ", cfifo_files[fifo_idx]);
if((ret = mkfifo(cfifo_files[fifo_idx], 0777)) < 0) {
fprintf(LOG_OUT, " failed\n");
fprintf(LOG_OUT, "Could not create command fifo %d: %s\n",
fifo_idx, cfifo_files[fifo_idx]);
close_stream();
return 0;
}
else {
fprintf(LOG_OUT, " ok\n");
cfifo[fifo_idx] = open(cfifo_files[fifo_idx], O_RDONLY | O_NONBLOCK);
}
}
return 1;
}
static void close_stream(void) {
uint8_t fifo_idx;
fprintf(LOG_OUT, "Closing streams\n");
for(fifo_idx = 0; fifo_idx < 4; fifo_idx++)
{
if(dfifo[fifo_idx] >= 0) {
close(dfifo[fifo_idx]);
}
unlink(dfifo_files[fifo_idx]);
if(cfifo[fifo_idx] >= 0) {
close(cfifo[fifo_idx]);
}
unlink(cfifo_files[fifo_idx]);
}
uint8_t fifo_idx;
fprintf(LOG_OUT, "Closing streams\n");
for(fifo_idx = 0; fifo_idx < 4; fifo_idx++)
{
if(dfifo[fifo_idx] >= 0) {
close(dfifo[fifo_idx]);
}
unlink(dfifo_files[fifo_idx]);
if(cfifo[fifo_idx] >= 0) {
close(cfifo[fifo_idx]);
}
unlink(cfifo_files[fifo_idx]);
}
}
static void on_timeout(int signum)
{
fprintf(LOG_OUT, "Timeout, stopping spistream daemon\n");
exit(6);
fprintf(LOG_OUT, "Timeout, stopping spistream daemon\n");
exit(6);
}
static void on_kill(int signum)
{
fprintf(LOG_OUT, "Exiting, got signal %d\n", signum);
main_exit();
exit(1);
fprintf(LOG_OUT, "Exiting, got signal %d\n", signum);
main_exit();
exit(1);
}
static void on_dead_pipe(int signum)
{
uint8_t fifo_idx;
fprintf(LOG_OUT, "Got SIGPIPE (signal %d)\n", signum);
// *Pop* goes the pipe. Looks like our client got AWOL.
// Let's be nice and invalidate the file descriptors:
for(fifo_idx = 0; fifo_idx < 4; fifo_idx++) {
close(dfifo[fifo_idx]);
dfifo[fifo_idx] = -1;
}
uint8_t fifo_idx;
fprintf(LOG_OUT, "Got SIGPIPE (signal %d)\n", signum);
// *Pop* goes the pipe. Looks like our client got AWOL.
// Let's be nice and invalidate the file descriptors:
for(fifo_idx = 0; fifo_idx < 4; fifo_idx++) {
close(dfifo[fifo_idx]);
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); \
}
#define SWAP(a, b){ \
typeof (a) temp = a; \
a = b; \
b = temp; \
typeof (a) temp = a; \
a = b; \
b = temp; \
}
#define MAT33_ROW(mat, row, value0, value1, value2){ \
mat[row*3+0] = value0; \
mat[row*3+1] = value1; \
mat[row*3+2] = value2; \
mat[row*3+0] = value0; \
mat[row*3+1] = value1; \
mat[row*3+2] = value2; \
}
#define ENU_NED_transformation(mat){ \
SWAP(mat.m[0], mat.m[3]); \
SWAP(mat.m[1], mat.m[4]); \
SWAP(mat.m[2], mat.m[5]); \
mat.m[6] = -mat.m[6]; \
mat.m[7] = -mat.m[7]; \
mat.m[8] = -mat.m[8]; \
SWAP(mat.m[0], mat.m[3]); \
SWAP(mat.m[1], mat.m[4]); \
SWAP(mat.m[2], mat.m[5]); \
mat.m[6] = -mat.m[6]; \
mat.m[7] = -mat.m[7]; \
mat.m[8] = -mat.m[8]; \
}
#define DOUBLE_VECT3_NORM(_v) (sqrt((_v).x*(_v).x + (_v).y*(_v).y + (_v).z*(_v).z))
@@ -89,41 +89,41 @@
} \
else { \
if (RMAT_ELMT(_r, 0, 0) > RMAT_ELMT(_r, 1, 1) && \
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) \
-RMAT_ELMT(_r, 2, 2) + 1); \
const double four_qx = 2. * two_qx; \
_q.qi = (RMAT_ELMT(_r, 1, 2)-RMAT_ELMT(_r, 2, 1))/four_qx; \
_q.qx = 0.5 * two_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; \
/*printf("m00 largest\n");*/ \
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) \
-RMAT_ELMT(_r, 2, 2) + 1); \
const double four_qx = 2. * two_qx; \
_q.qi = (RMAT_ELMT(_r, 1, 2)-RMAT_ELMT(_r, 2, 1))/four_qx; \
_q.qx = 0.5 * two_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; \
/*printf("m00 largest\n");*/ \
} \
else if (RMAT_ELMT(_r, 1, 1) > RMAT_ELMT(_r, 2, 2)) { \
const double two_qy = \
sqrt(RMAT_ELMT(_r, 1, 1) - RMAT_ELMT(_r, 0, 0) - RMAT_ELMT(_r, 2, 2) + 1); \
const double four_qy = 2. * two_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.qy = 0.5 * two_qy; \
_q.qz = (RMAT_ELMT(_r, 1, 2) + RMAT_ELMT(_r, 2, 1))/four_qy; \
/*printf("m11 largest\n");*/ \
const double two_qy = \
sqrt(RMAT_ELMT(_r, 1, 1) - RMAT_ELMT(_r, 0, 0) - RMAT_ELMT(_r, 2, 2) + 1); \
const double four_qy = 2. * two_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.qy = 0.5 * two_qy; \
_q.qz = (RMAT_ELMT(_r, 1, 2) + RMAT_ELMT(_r, 2, 1))/four_qy; \
/*printf("m11 largest\n");*/ \
} \
else { \
const double two_qz = \
sqrt(RMAT_ELMT(_r, 2, 2) - RMAT_ELMT(_r, 0, 0) - RMAT_ELMT(_r, 1, 1) + 1); \
const double four_qz = 2. * two_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.qy = (RMAT_ELMT(_r, 1, 2)+ RMAT_ELMT(_r, 2, 1))/four_qz; \
_q.qz = 0.5 * two_qz; \
/*printf("m22 largest\n");*/ \
const double two_qz = \
sqrt(RMAT_ELMT(_r, 2, 2) - RMAT_ELMT(_r, 0, 0) - RMAT_ELMT(_r, 1, 1) + 1); \
const double four_qz = 2. * two_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.qy = (RMAT_ELMT(_r, 1, 2)+ RMAT_ELMT(_r, 2, 1))/four_qz; \
_q.qz = 0.5 * two_qz; \
/*printf("m22 largest\n");*/ \
} \
} \
}
#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.qx = + from.qi - from.qz; \
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 VECT3_TO_EULERS(vector, eulers){ \
(eulers).phi = (vector).x; \
(eulers).theta = (vector).y; \
(eulers).psi = (vector).z; \
(eulers).phi = (vector).x; \
(eulers).theta = (vector).y; \
(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 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){ \
struct EcefCoor_f ecef_pprz; \
ecef_of_lla_f(&ecef_pprz, &lla); \
ecef = VECT3_AS_VECTOR3D(ecef_pprz); \
struct EcefCoor_f ecef_pprz; \
ecef_of_lla_f(&ecef_pprz, &lla); \
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 DoubleRates temprates;
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);
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);
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);
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_vel.x, e->message.ecef_vel.y, e->message.ecef_vel.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_vel.x, e->message.ecef_vel.y, e->message.ecef_vel.z);
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_LSF_VAL (S0SPCR_bit_enable | S0SPCR_CPHA | \
S0SPCR_CPOL | S0SPCR_MSTR | \
S0SPCR_LSBF | S0SPCR_SPIE);
S0SPCR_CPOL | S0SPCR_MSTR | \
S0SPCR_LSBF | S0SPCR_SPIE);
#define CPSDVSR 64
sw/airborne/fms/overo_blmc_calibrate.c
+5 -5
View File
@@ -70,16 +70,16 @@ static void main_init(void) {
return;
}
printf("Starting at 2000us\n");
printf("Starting at 2000us\n");
/* Initialize blaaa */
for (uint8_t i=0; i<LISA_PWM_OUTPUT_NB; i++) blmc_calibrate.servos_outputs_usecs[i] = 2000;
dialog_with_io_proc();
getchar();
printf("At 1000us\n");
getchar();
printf("At 1000us\n");
for (uint8_t i=0; i<LISA_PWM_OUTPUT_NB; i++) blmc_calibrate.servos_outputs_usecs[i] = 1000;
dialog_with_io_proc();
getchar();
printf("At 1500us\n");
getchar();
printf("At 1500us\n");
for (uint8_t i=0; i<LISA_PWM_OUTPUT_NB; i++) blmc_calibrate.servos_outputs_usecs[i] = 1500;
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");
/* Enter our mainloop */
event_dispatch();
while(1) {
sleep(100);
}
event_dispatch();
while(1) {
sleep(100);
}
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) {
#if 0
static int32_t every_100 = 1000;
if(every_100-- == 0) {
every_100 = 1000;
spistream_send_msg(spistream_msg, 21, SPISTREAM_NO_WAIT);
static int32_t every_100 = 1000;
if(every_100-- == 0) {
every_100 = 1000;
spistream_send_msg(spistream_msg, 21, SPISTREAM_NO_WAIT);
/*
spistream_send_msg(spistream_msg, 15);
spistream_send_msg(spistream_msg, 25);
spistream_send_msg(spistream_msg, 15);
spistream_send_msg(spistream_msg, 25);
*/
}
}
#endif
spistream_event();
spistream_event();
}
static void spistream_event() {
@@ -95,25 +95,25 @@ static void spistream_event() {
spistream_write_pkg(&msg_out);
if(msg_out.message_cnt != 0) {
printf("Package out: Size: %3d MID: %3d PCNTD: %3d | ",
pkg_size, msg_out.message_cnt, msg_out.package_cntd);
for(cnt = 0; cnt < pkg_size; cnt++) {
printf("%3d ", msg_out.pkg_data[cnt]);
}
printf("\n");
}
if(msg_out.message_cnt != 0) {
printf("Package out: Size: %3d MID: %3d PCNTD: %3d | ",
pkg_size, msg_out.message_cnt, msg_out.package_cntd);
for(cnt = 0; cnt < pkg_size; cnt++) {
printf("%3d ", msg_out.pkg_data[cnt]);
}
printf("\n");
}
spi_link_send(&msg_out, sizeof(struct AutopilotMessageCRCFrame), &msg_in, &crc_valid);
/*
if(msg_in.message_cnt != 0) {
printf("Package in: Size: %3d MID: %3d PCNTD: %3d | ",
pkg_size, msg_in.message_cnt, msg_in.package_cntd);
for(cnt = 0; cnt < pkg_size; cnt++) {
printf("%3d ", msg_in.pkg_data[cnt]);
}
printf("\n");
}
if(msg_in.message_cnt != 0) {
printf("Package in: Size: %3d MID: %3d PCNTD: %3d | ",
pkg_size, msg_in.message_cnt, msg_in.package_cntd);
for(cnt = 0; cnt < pkg_size; cnt++) {
printf("%3d ", msg_in.pkg_data[cnt]);
}
printf("\n");
}
*/
spistream_read_pkg(&msg_in);
}
@@ -121,28 +121,28 @@ static void spistream_event() {
static void on_spistream_msg_received(uint8_t msg_id,
uint8_t * data,
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;
struct tm * timeinfo;
time_t c_time;
char time_str[30];
struct tm * timeinfo;
time_t c_time;
char time_str[30];
plot_freq = 100;
time(&c_time);
plot_freq = 100;
time(&c_time);
timeinfo = localtime(&c_time);
strftime(time_str, 30, " %X ", timeinfo);
timeinfo = localtime(&c_time);
strftime(time_str, 30, " %X ", timeinfo);
log_bytes = num_bytes;
if(log_bytes > 48) { log_bytes = 48; }
printf("SPI message received: ");
printf("%s | Length: %3d | id: %3d | UART%d | ", time_str, num_bytes, msg_id, data[0]);
for(cnt = 1; cnt < log_bytes; cnt++) {
printf("%02X ", data[cnt]);
}
printf("\n");
log_bytes = num_bytes;
if(log_bytes > 48) { log_bytes = 48; }
printf("SPI message received: ");
printf("%s | Length: %3d | id: %3d | UART%d | ", time_str, num_bytes, msg_id, data[0]);
for(cnt = 1; cnt < log_bytes; cnt++) {
printf("%02X ", data[cnt]);
}
printf("\n");
}
static void on_spistream_msg_sent(void) {
@@ -163,7 +163,7 @@ static void main_init(void) {
spistream_init(&on_spistream_msg_received,
&on_spistream_msg_sent);
/*
spistream_msg[0] = 0;
spistream_msg[0] = 0;
for(byte_idx=1; byte_idx < 123; byte_idx += 4) {
spistream_msg[byte_idx] = 0xDE;
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"
#define PERIODIC_SEND_TEST_PASSTHROUGH_STATUS(_transport) \
DOWNLINK_SEND_TEST_PASSTHROUGH_STATUS(_transport, \
&otp.io_proc_msg_cnt, \
&otp.io_proc_err_cnt, \
&spi_link.msg_cnt, \
&spi_link.crc_err_cnt, \
&otp.rc_status)
&otp.io_proc_msg_cnt, \
&otp.io_proc_err_cnt, \
&spi_link.msg_cnt, \
&spi_link.crc_err_cnt, \
&otp.rc_status)
#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 */
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))) {
printf("Compare failed: (received != expected): \n");
print_up_msg(&crc_msg_in);
print_down_msg(&msg_out_prev);
buf_check_errors++;
printf("Compare failed: (received != expected): \n");
print_up_msg(&crc_msg_in);
print_down_msg(&msg_out_prev);
buf_check_errors++;
}
}
/* report crc error */
if (!skip_crc_check & !crc_valid) {
printf("CRC checksum failed: received %04X != computed %04X\n",
crc_msg_in.crc,
crc_calc_block_crc8((uint8_t*)&crc_msg_in.payload, sizeof(struct OVERO_LINK_MSG_DOWN)));
crc_msg_in.crc,
crc_calc_block_crc8((uint8_t*)&crc_msg_in.payload, sizeof(struct OVERO_LINK_MSG_DOWN)));
}
/* report message count */
if (!(spi_link.msg_cnt % 1000))
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 */
if(us_delay > 0)
@@ -105,27 +105,27 @@ int main(int argc, char *argv[]) {
static void print_up_msg(struct AutopilotMessageCRCFrame * msg) {
printf("UP: %08X %08X %08X %08X %08X %08X %08X %08X CRC: %08X\n",
msg->payload.msg_up.foo,
msg->payload.msg_up.bar,
msg->payload.msg_up.bla,
msg->payload.msg_up.ble,
msg->payload.msg_up.bli,
msg->payload.msg_up.blo,
msg->payload.msg_up.blu,
msg->payload.msg_up.bly,
msg->crc);
msg->payload.msg_up.foo,
msg->payload.msg_up.bar,
msg->payload.msg_up.bla,
msg->payload.msg_up.ble,
msg->payload.msg_up.bli,
msg->payload.msg_up.blo,
msg->payload.msg_up.blu,
msg->payload.msg_up.bly,
msg->crc);
}
static void print_down_msg(struct AutopilotMessageCRCFrame * msg) {
printf("DW: %08X %08X %08X %08X %08X %08X %08X %08X CRC: %08X\n",
msg->payload.msg_down.foo,
msg->payload.msg_down.bar,
msg->payload.msg_down.bla,
msg->payload.msg_down.ble,
msg->payload.msg_down.bli,
msg->payload.msg_down.blo,
msg->payload.msg_up.blu,
msg->payload.msg_up.bly,
msg->crc);
msg->payload.msg_down.foo,
msg->payload.msg_down.bar,
msg->payload.msg_down.bla,
msg->payload.msg_down.ble,
msg->payload.msg_down.bli,
msg->payload.msg_down.blo,
msg->payload.msg_up.blu,
msg->payload.msg_up.bly,
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_msgs++; \
if (len != udpt_tx_buf_idx) \
downlink_nb_ovrn++; \
downlink_nb_ovrn++; \
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)
{
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++) {
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)))
{
return TRUE;
return TRUE;
}
static uint8_t size_of(void *udp __attribute__((unused)), uint8_t len)
{
return len + 2;
return len + 2;
}
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 */
InterMcuSend_INTERMCU_FBW(
fbw_state->ppm_cpt,
fbw_state->status,
fbw_state->nb_err,
fbw_state->vsupply,
fbw_state->current);
fbw_state->ppm_cpt,
fbw_state->status,
fbw_state->nb_err,
fbw_state->vsupply,
fbw_state->current);
#if defined RADIO_CONTROL || RADIO_CONTROL_AUTO1
InterMcuSend_INTERMCU_RADIO( fbw_state->channels );
#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); \
overo_link.timeout_cnt = 0; \
if((SPI_I2S_GetFlagStatus(SPI1, SPI_FLAG_CRCERR)) == RESET) { \
LED_ON(OVERO_LINK_LED_OK); \
LED_OFF(OVERO_LINK_LED_KO); \
overo_link.msg_cnt++; \
_data_received_handler(); \
overo_link_arch_prepare_next_transfert(); \
LED_ON(OVERO_LINK_LED_OK); \
LED_OFF(OVERO_LINK_LED_KO); \
overo_link.msg_cnt++; \
_data_received_handler(); \
overo_link_arch_prepare_next_transfert(); \
} \
else { \
SPI_Cmd(SPI1, DISABLE); \
LED_OFF(OVERO_LINK_LED_OK); \
LED_ON(OVERO_LINK_LED_KO); \
overo_link.crc_err_cnt++; \
overo_link.crc_error = TRUE; \
_crc_failed_handler(); \
SPI_Cmd(SPI1, DISABLE); \
LED_OFF(OVERO_LINK_LED_OK); \
LED_ON(OVERO_LINK_LED_KO); \
overo_link.crc_err_cnt++; \
overo_link.crc_error = TRUE; \
_crc_failed_handler(); \
} \
overo_link.status = IDLE; \
} \
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.crc_error = FALSE; \
} \
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.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++; \
else { \
if (overo_link.status != LOST && overo_link.status != DATA_AVAILABLE ) { \
SPI_Cmd(SPI1, DISABLE); \
overo_link.status = LOST; \
LED_OFF(OVERO_LINK_LED_OK); \
LED_ON(OVERO_LINK_LED_KO); \
overo_link.timeout = TRUE; \
_timeout_handler(); \
SPI_Cmd(SPI1, DISABLE); \
overo_link.status = LOST; \
LED_OFF(OVERO_LINK_LED_OK); \
LED_ON(OVERO_LINK_LED_KO); \
overo_link.timeout = TRUE; \
_timeout_handler(); \
} \
} \
}
@@ -62,11 +62,11 @@ extern void overo_link_arch_prepare_next_transfert(void);
else { \
__disable_irq(); \
if (overo_link.status != LOST && overo_link.status != DATA_AVAILABLE ) { \
overo_link.status = LOST; \
__enable_irq(); \
LED_OFF(OVERO_LINK_LED_OK); \
LED_ON(OVERO_LINK_LED_KO); \
_timeout_handler(); \
overo_link.status = LOST; \
__enable_irq(); \
LED_OFF(OVERO_LINK_LED_OK); \
LED_ON(OVERO_LINK_LED_KO); \
_timeout_handler(); \
} \
__enable_irq(); \
} \
sw/airborne/lisa/lisa_spistream.h
+109 -109
View File
@@ -22,9 +22,9 @@
#endif
#ifndef SPISTREAM_TX_MAX_BUFFER_PACKAGES
#define SPISTREAM_TX_MAX_BUFFER_PACKAGES ( \
(SPISTREAM_MAX_TX_MESSAGE_LENGTH / \
SPISTREAM_PACKAGE_SIZE) * \
SPISTREAM_MAX_TX_PARALLEL_TRANSACTIONS)
(SPISTREAM_MAX_TX_MESSAGE_LENGTH / \
SPISTREAM_PACKAGE_SIZE) * \
SPISTREAM_MAX_TX_PARALLEL_TRANSACTIONS)
#endif
#define SPISTREAM_INVALID_MESSAGE_ID 0
@@ -35,16 +35,16 @@ struct spistream_state_t {
};
struct spistream_message_range_t {
uint8_t index;
uint8_t size;
uint8_t index;
uint8_t size;
};
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_read; // next index to read package from
uint16_t tx_num_packages; // number of packages in buffer
uint16_t tx_insert; // next index for package insertion
uint16_t tx_read; // next index to read package from
uint16_t tx_num_packages; // number of packages in buffer
// RX stores data as array
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)
{
uint8_t package_cntd;
uint8_t package_cntd;
if(pkg_in->message_cnt == SPISTREAM_INVALID_MESSAGE_ID) {
return;
}
// 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
// the end of the message to fill a package.
if(pkg_in->package_cntd <= 0) { package_cntd = 1; }
else { package_cntd = pkg_in->package_cntd; }
// 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
// the end of the message to fill a package.
if(pkg_in->package_cntd <= 0) { package_cntd = 1; }
else { package_cntd = pkg_in->package_cntd; }
if(pkg_in->package_cntd >= spistream_buffers.rx_num_packages) {
spistream_buffers.rx_num_packages = pkg_in->package_cntd;
}
if(pkg_in->package_cntd >= spistream_buffers.rx_num_packages) {
spistream_buffers.rx_num_packages = pkg_in->package_cntd;
}
if(spistream_state.rx_package_cntd == 0) { // Beginning of new message
// Message length is first value of package countdown:
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 +
((spistream_buffers.rx_num_packages - package_cntd) *
SPISTREAM_PACKAGE_SIZE),
((spistream_buffers.rx_num_packages - package_cntd) *
SPISTREAM_PACKAGE_SIZE),
pkg_in->pkg_data,
SPISTREAM_PACKAGE_SIZE);
if(pkg_in->package_cntd <= 0) {
// Message is ready, pass to handler:
spistream.message_rx_handler(pkg_in->message_cnt,
(uint8_t *)(spistream_buffers.rx),
(spistream_buffers.rx_num_packages *
SPISTREAM_PACKAGE_SIZE) +
pkg_in->package_cntd);
spistream_state.rx_package_cntd = 0;
spistream.message_rx_handler(pkg_in->message_cnt,
(uint8_t *)(spistream_buffers.rx),
(spistream_buffers.rx_num_packages *
SPISTREAM_PACKAGE_SIZE) +
pkg_in->package_cntd);
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)
{
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;
return;
}
memcpy(pkg_out,
spistream_buffers.tx + spistream_buffers.tx_read,
sizeof(struct AutopilotMessagePTStream));
if(pkg_out->package_cntd <= 0) {
spistream.message_tx_handler(pkg_out->message_cnt);
}
memcpy(pkg_out,
spistream_buffers.tx + spistream_buffers.tx_read,
sizeof(struct AutopilotMessagePTStream));
if(pkg_out->package_cntd <= 0) {
spistream.message_tx_handler(pkg_out->message_cnt);
}
spistream_buffers.tx_read++;
if(spistream_buffers.tx_read >= SPISTREAM_TX_MAX_BUFFER_PACKAGES) {
spistream_buffers.tx_read = 0;
}
spistream_buffers.tx_read++;
if(spistream_buffers.tx_read >= SPISTREAM_TX_MAX_BUFFER_PACKAGES) {
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.
*/
static inline uint8_t spistream_enqueue_msg(uint8_t * data,
uint16_t num_bytes,
enum spistream_flag wait_for_read)
uint16_t num_bytes,
enum spistream_flag wait_for_read)
{
uint16_t pkg_idx, num_packages, num_padding;
uint16_t idx;
// Enough space in buffer?
uint16_t pkg_idx, num_packages, num_padding;
uint16_t idx;
// Enough space in buffer?
if(wait_for_read == SPISTREAM_NO_WAIT ||
spistream_buffers.tx_num_packages+1 < SPISTREAM_TX_MAX_BUFFER_PACKAGES)
{
spistream_state.tx_message_cnt++;
// Message id 0 is reserved for invalid packages:
if(spistream_state.tx_message_cnt == SPISTREAM_INVALID_MESSAGE_ID) {
spistream_state.tx_message_cnt = 1;
}
// How many packages we need for this message:
num_packages = (num_bytes / SPISTREAM_PACKAGE_SIZE);
if(num_bytes % SPISTREAM_PACKAGE_SIZE != 0) {
num_packages++;
}
// How many zero-bytes we will have at the end of the last package:
if(num_bytes > SPISTREAM_PACKAGE_SIZE) {
num_padding = (num_packages * SPISTREAM_PACKAGE_SIZE) - num_bytes;
}
else {
num_padding = SPISTREAM_PACKAGE_SIZE - num_bytes;
}
if(wait_for_read == SPISTREAM_NO_WAIT ||
spistream_buffers.tx_num_packages+1 < SPISTREAM_TX_MAX_BUFFER_PACKAGES)
{
spistream_state.tx_message_cnt++;
// Message id 0 is reserved for invalid packages:
if(spistream_state.tx_message_cnt == SPISTREAM_INVALID_MESSAGE_ID) {
spistream_state.tx_message_cnt = 1;
}
// How many packages we need for this message:
num_packages = (num_bytes / SPISTREAM_PACKAGE_SIZE);
if(num_bytes % SPISTREAM_PACKAGE_SIZE != 0) {
num_packages++;
}
// How many zero-bytes we will have at the end of the last package:
if(num_bytes > SPISTREAM_PACKAGE_SIZE) {
num_padding = (num_packages * SPISTREAM_PACKAGE_SIZE) - num_bytes;
}
else {
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:
for(idx = 0; num_packages > 0; idx++) {
if(idx < num_bytes) {
spistream_buffers.tx[pkg_idx].pkg_data[idx % SPISTREAM_PACKAGE_SIZE] = data[idx];
}
else { // padding
spistream_buffers.tx[pkg_idx].pkg_data[idx % SPISTREAM_PACKAGE_SIZE] = 0;
}
// Last byte in current package:
if((idx % SPISTREAM_PACKAGE_SIZE) == SPISTREAM_PACKAGE_SIZE-1) {
// Convert data to packages and add them to TX buffer:
for(idx = 0; num_packages > 0; idx++) {
if(idx < num_bytes) {
spistream_buffers.tx[pkg_idx].pkg_data[idx % SPISTREAM_PACKAGE_SIZE] = data[idx];
}
else { // padding
spistream_buffers.tx[pkg_idx].pkg_data[idx % SPISTREAM_PACKAGE_SIZE] = 0;
}
// Last byte in current package:
if((idx % SPISTREAM_PACKAGE_SIZE) == SPISTREAM_PACKAGE_SIZE-1) {
// Finish configuration of current package
// Last package uses field package_cntd to indicate the number
// of padding bytes it contains, as negative number:
if(num_packages == 1) {
spistream_buffers.tx[pkg_idx].package_cntd = -num_padding;
}
else {
spistream_buffers.tx[pkg_idx].package_cntd = num_packages;
}
spistream_buffers.tx[pkg_idx].message_cnt = spistream_state.tx_message_cnt;
// Finish configuration of current package
// Last package uses field package_cntd to indicate the number
// of padding bytes it contains, as negative number:
if(num_packages == 1) {
spistream_buffers.tx[pkg_idx].package_cntd = -num_padding;
}
else {
spistream_buffers.tx[pkg_idx].package_cntd = num_packages;
}
spistream_buffers.tx[pkg_idx].message_cnt = spistream_state.tx_message_cnt;
// Prepare next package:
num_packages--;
// Increment insert pointer with ring buffer overflow:
spistream_buffers.tx_insert++;
if(spistream_buffers.tx_insert >= SPISTREAM_TX_MAX_BUFFER_PACKAGES) {
spistream_buffers.tx_insert = 0;
}
// Continue with next package:
pkg_idx = spistream_buffers.tx_insert;
spistream_buffers.tx_num_packages++;
}
}
// Prepare next package:
num_packages--;
// Increment insert pointer with ring buffer overflow:
spistream_buffers.tx_insert++;
if(spistream_buffers.tx_insert >= SPISTREAM_TX_MAX_BUFFER_PACKAGES) {
spistream_buffers.tx_insert = 0;
}
// Continue with next package:
pkg_idx = spistream_buffers.tx_insert;
spistream_buffers.tx_num_packages++;
}
}
#if 0
printf("Enqueue finished. Buffer: \n");
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);
for(idx = 0; idx < SPISTREAM_PACKAGE_SIZE; idx++) {
printf("%3d ", spistream_buffers.tx[pkg_idx].pkg_data[idx]);
}
printf("\n");
}
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);
for(idx = 0; idx < SPISTREAM_PACKAGE_SIZE; idx++) {
printf("%3d ", spistream_buffers.tx[pkg_idx].pkg_data[idx]);
}
printf("\n");
}
#endif
return 1;
}
return 0;
return 1;
}
return 0;
}
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.
*/
static inline uint8_t spistream_send_msg(uint8_t * data,
uint16_t num_bytes,
enum spistream_flag wait_for_read)
uint16_t num_bytes,
enum spistream_flag 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) {
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