diff --git a/sw/airborne/arch/lpc21/mcu_periph/adc_arch.c b/sw/airborne/arch/lpc21/mcu_periph/adc_arch.c index 8faed22706..75d959204f 100644 --- a/sw/airborne/arch/lpc21/mcu_periph/adc_arch.c +++ b/sw/airborne/arch/lpc21/mcu_periph/adc_arch.c @@ -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 diff --git a/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.c b/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.c index 18a2a6549d..65d8614062 100644 --- a/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.c +++ b/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.c @@ -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 diff --git a/sw/airborne/arch/lpc21/modules/adcs/max11040_hw.c b/sw/airborne/arch/lpc21/modules/adcs/max11040_hw.c index 5fd4fe233a..bf1f95efe6 100644 --- a/sw/airborne/arch/lpc21/modules/adcs/max11040_hw.c +++ b/sw/airborne/arch/lpc21/modules/adcs/max11040_hw.c @@ -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; diff --git a/sw/airborne/arch/lpc21/modules/sensors/mag_micromag_fw_hw.h b/sw/airborne/arch/lpc21/modules/sensors/mag_micromag_fw_hw.h index 2ea2218424..7040e5b1af 100644 --- a/sw/airborne/arch/lpc21/modules/sensors/mag_micromag_fw_hw.h +++ b/sw/airborne/arch/lpc21/modules/sensors/mag_micromag_fw_hw.h @@ -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 ); diff --git a/sw/airborne/arch/lpc21/subsystems/actuators/servos_4017_hw.h b/sw/airborne/arch/lpc21/subsystems/actuators/servos_4017_hw.h index e6848f5d13..88d3ca37b5 100644 --- a/sw/airborne/arch/lpc21/subsystems/actuators/servos_4017_hw.h +++ b/sw/airborne/arch/lpc21/subsystems/actuators/servos_4017_hw.h @@ -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 */ \ diff --git a/sw/airborne/arch/lpc21/subsystems/datalink/audio_telemetry_hw.c b/sw/airborne/arch/lpc21/subsystems/datalink/audio_telemetry_hw.c index 42796a4a85..2ca81a3b06 100644 --- a/sw/airborne/arch/lpc21/subsystems/datalink/audio_telemetry_hw.c +++ b/sw/airborne/arch/lpc21/subsystems/datalink/audio_telemetry_hw.c @@ -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; diff --git a/sw/airborne/arch/lpc21/test/ledswitch.c b/sw/airborne/arch/lpc21/test/ledswitch.c index 1a4b2384d1..3f1bd21957 100644 --- a/sw/airborne/arch/lpc21/test/ledswitch.c +++ b/sw/airborne/arch/lpc21/test/ledswitch.c @@ -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 } diff --git a/sw/airborne/arch/omap/serial_port.c b/sw/airborne/arch/omap/serial_port.c index 13441f77b0..a0a4d2c8cf 100644 --- a/sw/airborne/arch/omap/serial_port.c +++ b/sw/airborne/arch/omap/serial_port.c @@ -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) { diff --git a/sw/airborne/arch/omap/serial_port.h b/sw/airborne/arch/omap/serial_port.h index f259114975..0187b8b149 100644 --- a/sw/airborne/arch/omap/serial_port.h +++ b/sw/airborne/arch/omap/serial_port.h @@ -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 */ diff --git a/sw/airborne/arch/sim/sim_ap.c b/sw/airborne/arch/sim/sim_ap.c index 20cb7ac5b4..5df27f6490 100644 --- a/sw/airborne/arch/sim/sim_ap.c +++ b/sw/airborne/arch/sim/sim_ap.c @@ -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))) { } diff --git a/sw/airborne/arch/stm32/peripherals/hmc5843_arch.c b/sw/airborne/arch/stm32/peripherals/hmc5843_arch.c index e64f5fffea..9cdf9087ec 100644 --- a/sw/airborne/arch/stm32/peripherals/hmc5843_arch.c +++ b/sw/airborne/arch/stm32/peripherals/hmc5843_arch.c @@ -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); diff --git a/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.c b/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.c index fa68953e0d..e1c34f4bec 100644 --- a/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.c +++ b/sw/airborne/arch/stm32/subsystems/imu/imu_aspirin_arch.c @@ -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); diff --git a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c index 7db39aa6e3..b93ee319a9 100644 --- a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c +++ b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c @@ -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) diff --git a/sw/airborne/boards/ardrone/at_com.c b/sw/airborne/boards/ardrone/at_com.c index eba448ee13..75112aae5f 100644 --- a/sw/airborne/boards/ardrone/at_com.c +++ b/sw/airborne/boards/ardrone/at_com.c @@ -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; } diff --git a/sw/airborne/boards/ardrone/gpio_ardrone.c b/sw/airborne/boards/ardrone/gpio_ardrone.c index 363ca87686..848c6909a1 100644 --- a/sw/airborne/boards/ardrone/gpio_ardrone.c +++ b/sw/airborne/boards/ardrone/gpio_ardrone.c @@ -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); */ } diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c index 6bb5020926..099034a73c 100644 --- a/sw/airborne/boards/ardrone/navdata.c +++ b/sw/airborne/boards/ardrone/navdata.c @@ -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 diff --git a/sw/airborne/boards/lisa_m/baro_board.c b/sw/airborne/boards/lisa_m/baro_board.c index 5a6aa9a489..6a23b1173d 100644 --- a/sw/airborne/boards/lisa_m/baro_board.c +++ b/sw/airborne/boards/lisa_m/baro_board.c @@ -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); diff --git a/sw/airborne/firmwares/beth/main_beth.c b/sw/airborne/firmwares/beth/main_beth.c index 4f1ff6ae6f..07e81ac89b 100644 --- a/sw/airborne/firmwares/beth/main_beth.c +++ b/sw/airborne/firmwares/beth/main_beth.c @@ -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); } diff --git a/sw/airborne/firmwares/beth/main_coders.c b/sw/airborne/firmwares/beth/main_coders.c index eb086428ab..67ff1ab189 100644 --- a/sw/airborne/firmwares/beth/main_coders.c +++ b/sw/airborne/firmwares/beth/main_coders.c @@ -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; diff --git a/sw/airborne/firmwares/beth/main_overo.c b/sw/airborne/firmwares/beth/main_overo.c index 8cabb82ec3..59edd04878 100644 --- a/sw/airborne/firmwares/beth/main_overo.c +++ b/sw/airborne/firmwares/beth/main_overo.c @@ -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()); diff --git a/sw/airborne/firmwares/beth/overo_controller.c b/sw/airborne/firmwares/beth/overo_controller.c index 61e70f0216..d548106640 100644 --- a/sw/airborne/firmwares/beth/overo_controller.c +++ b/sw/airborne/firmwares/beth/overo_controller.c @@ -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) + diff --git a/sw/airborne/firmwares/beth/overo_estimator.c b/sw/airborne/firmwares/beth/overo_estimator.c index 432df316b7..46efedf58b 100644 --- a/sw/airborne/firmwares/beth/overo_estimator.c +++ b/sw/airborne/firmwares/beth/overo_estimator.c @@ -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 diff --git a/sw/airborne/firmwares/beth/overo_test_uart.c b/sw/airborne/firmwares/beth/overo_test_uart.c index b8dc51ffe7..e7ad7d3eb6 100644 --- a/sw/airborne/firmwares/beth/overo_test_uart.c +++ b/sw/airborne/firmwares/beth/overo_test_uart.c @@ -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(); } diff --git a/sw/airborne/firmwares/beth/overo_twist_controller.c b/sw/airborne/firmwares/beth/overo_twist_controller.c index 845c5177a4..bc9b23fb20 100644 --- a/sw/airborne/firmwares/beth/overo_twist_controller.c +++ b/sw/airborne/firmwares/beth/overo_twist_controller.c @@ -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; } diff --git a/sw/airborne/firmwares/logger/main_logger.c b/sw/airborne/firmwares/logger/main_logger.c index 03a87f34a5..46388b126c 100644 --- a/sw/airborne/firmwares/logger/main_logger.c +++ b/sw/airborne/firmwares/logger/main_logger.c @@ -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>LOG_STOP_KEY) - { + /* write to SD until key is pressed */ + while ((IO0PIN & (1<>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) diff --git a/sw/airborne/firmwares/motor_bench/mb_scale.h b/sw/airborne/firmwares/motor_bench/mb_scale.h index ba359f6982..2f73bd13b8 100644 --- a/sw/airborne/firmwares/motor_bench/mb_scale.h +++ b/sw/airborne/firmwares/motor_bench/mb_scale.h @@ -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 */ diff --git a/sw/airborne/firmwares/motor_bench/mb_twi_controller_asctech.c b/sw/airborne/firmwares/motor_bench/mb_twi_controller_asctech.c index 0b6f4c1fed..4963dc3d85 100644 --- a/sw/airborne/firmwares/motor_bench/mb_twi_controller_asctech.c +++ b/sw/airborne/firmwares/motor_bench/mb_twi_controller_asctech.c @@ -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; } } diff --git a/sw/airborne/firmwares/non_ap/geiger_counter/geiger_counter.c b/sw/airborne/firmwares/non_ap/geiger_counter/geiger_counter.c index 86d22e8f56..770cfe3c5d 100644 --- a/sw/airborne/firmwares/non_ap/geiger_counter/geiger_counter.c +++ b/sw/airborne/firmwares/non_ap/geiger_counter/geiger_counter.c @@ -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; +} +} } diff --git a/sw/airborne/fms/fms_autopilot_msg.h b/sw/airborne/fms/fms_autopilot_msg.h index 03943029ca..37a94c06a3 100644 --- a/sw/airborne/fms/fms_autopilot_msg.h +++ b/sw/airborne/fms/fms_autopilot_msg.h @@ -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) */ diff --git a/sw/airborne/fms/fms_gs_com.c b/sw/airborne/fms/fms_gs_com.c index 688413d911..6475222a9e 100644 --- a/sw/airborne/fms/fms_gs_com.c +++ b/sw/airborne/fms/fms_gs_com.c @@ -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; diff --git a/sw/airborne/fms/fms_gs_com.h b/sw/airborne/fms/fms_gs_com.h index ea7bab7840..1732220b44 100644 --- a/sw/airborne/fms/fms_gs_com.h +++ b/sw/airborne/fms/fms_gs_com.h @@ -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 */ diff --git a/sw/airborne/fms/fms_spi_autopilot_msg.c b/sw/airborne/fms/fms_spi_autopilot_msg.c index 75043401ef..c0d2f41625 100644 --- a/sw/airborne/fms/fms_spi_autopilot_msg.c +++ b/sw/airborne/fms/fms_spi_autopilot_msg.c @@ -58,8 +58,8 @@ void spi_ap_link_downlink_send(struct DownlinkTransport *tp) { uint32_t timestamp = 0; DOWNLINK_SEND_EKF7_Y(tp, ×tamp, &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); diff --git a/sw/airborne/fms/fms_spi_link.c b/sw/airborne/fms/fms_spi_link.c index d32296ab2e..bae7177635 100644 --- a/sw/airborne/fms/fms_spi_link.c +++ b/sw/airborne/fms/fms_spi_link.c @@ -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; diff --git a/sw/airborne/fms/fms_spistream.h b/sw/airborne/fms/fms_spistream.h index 132846ec41..8c038bbeba 100644 --- a/sw/airborne/fms/fms_spistream.h +++ b/sw/airborne/fms/fms_spistream.h @@ -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"); } diff --git a/sw/airborne/fms/fms_spistream_client.c b/sw/airborne/fms/fms_spistream_client.c index f181675eb6..099f391473 100644 --- a/sw/airborne/fms/fms_spistream_client.c +++ b/sw/airborne/fms/fms_spistream_client.c @@ -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); } diff --git a/sw/airborne/fms/fms_spistream_daemon.c b/sw/airborne/fms/fms_spistream_daemon.c index 2c12ef4221..567fc02d07 100644 --- a/sw/airborne/fms/fms_spistream_daemon.c +++ b/sw/airborne/fms/fms_spistream_daemon.c @@ -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; + } } diff --git a/sw/airborne/fms/libeknav/paparazzi_eigen_conversion.h b/sw/airborne/fms/libeknav/paparazzi_eigen_conversion.h index b6a46b977d..09a49ed018 100644 --- a/sw/airborne/fms/libeknav/paparazzi_eigen_conversion.h +++ b/sw/airborne/fms/libeknav/paparazzi_eigen_conversion.h @@ -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); \ } diff --git a/sw/airborne/fms/libeknav/raw_log_to_ascii.c b/sw/airborne/fms/libeknav/raw_log_to_ascii.c index a8a360af21..e776993b16 100644 --- a/sw/airborne/fms/libeknav/raw_log_to_ascii.c +++ b/sw/airborne/fms/libeknav/raw_log_to_ascii.c @@ -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); } /* diff --git a/sw/airborne/fms/lpc_test_spi.c b/sw/airborne/fms/lpc_test_spi.c index f7f34ef0aa..3af04aa144 100644 --- a/sw/airborne/fms/lpc_test_spi.c +++ b/sw/airborne/fms/lpc_test_spi.c @@ -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 diff --git a/sw/airborne/fms/overo_blmc_calibrate.c b/sw/airborne/fms/overo_blmc_calibrate.c index 0163a61260..72e021cd87 100644 --- a/sw/airborne/fms/overo_blmc_calibrate.c +++ b/sw/airborne/fms/overo_blmc_calibrate.c @@ -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 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; diff --git a/sw/airborne/fms/overo_test_passthrough_telemetry.h b/sw/airborne/fms/overo_test_passthrough_telemetry.h index 494fa1b007..fc15027513 100644 --- a/sw/airborne/fms/overo_test_passthrough_telemetry.h +++ b/sw/airborne/fms/overo_test_passthrough_telemetry.h @@ -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) \ diff --git a/sw/airborne/fms/overo_test_spi_link.c b/sw/airborne/fms/overo_test_spi_link.c index 4d9124de58..52640f9fb7 100644 --- a/sw/airborne/fms/overo_test_spi_link.c +++ b/sw/airborne/fms/overo_test_spi_link.c @@ -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); } diff --git a/sw/airborne/fms/udp_transport.h b/sw/airborne/fms/udp_transport.h index 7c2ed36d96..2de1a8ef9f 100644 --- a/sw/airborne/fms/udp_transport.h +++ b/sw/airborne/fms/udp_transport.h @@ -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; \ } \ } diff --git a/sw/airborne/fms/udp_transport2.c b/sw/airborne/fms/udp_transport2.c index da5ae29e9a..1b041341b4 100644 --- a/sw/airborne/fms/udp_transport2.c +++ b/sw/airborne/fms/udp_transport2.c @@ -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) diff --git a/sw/airborne/link_mcu_usart.c b/sw/airborne/link_mcu_usart.c index ebbf2201ae..1fc29e325c 100644 --- a/sw/airborne/link_mcu_usart.c +++ b/sw/airborne/link_mcu_usart.c @@ -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 diff --git a/sw/airborne/lisa/arch/stm32/lisa_overo_link_arch.h b/sw/airborne/lisa/arch/stm32/lisa_overo_link_arch.h index a5887a4192..3020baf95d 100644 --- a/sw/airborne/lisa/arch/stm32/lisa_overo_link_arch.h +++ b/sw/airborne/lisa/arch/stm32/lisa_overo_link_arch.h @@ -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; \ } \ diff --git a/sw/airborne/lisa/lisa_overo_link.h b/sw/airborne/lisa/lisa_overo_link.h index 69e334d044..d3d9150fe5 100644 --- a/sw/airborne/lisa/lisa_overo_link.h +++ b/sw/airborne/lisa/lisa_overo_link.h @@ -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(); \ } \ diff --git a/sw/airborne/lisa/lisa_spistream.h b/sw/airborne/lisa/lisa_spistream.h index 48716eabd1..8ed2830148 100644 --- a/sw/airborne/lisa/lisa_spistream.h +++ b/sw/airborne/lisa/lisa_spistream.h @@ -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); } diff --git a/sw/airborne/lisa/lisa_stm_gps_passthrough_main.c b/sw/airborne/lisa/lisa_stm_gps_passthrough_main.c index 6e56777abb..0f654ba517 100644 --- a/sw/airborne/lisa/lisa_stm_gps_passthrough_main.c +++ b/sw/airborne/lisa/lisa_stm_gps_passthrough_main.c @@ -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--; } } diff --git a/sw/airborne/lisa/lisa_stm_passthrough_main.c b/sw/airborne/lisa/lisa_stm_passthrough_main.c index 582d3bf6b2..d87ff7ee0b 100644 --- a/sw/airborne/lisa/lisa_stm_passthrough_main.c +++ b/sw/airborne/lisa/lisa_stm_passthrough_main.c @@ -90,190 +90,190 @@ struct CscServoCmd csc_servo_cmd; int main(void) { - main_init(); + main_init(); - while (1) { - if (sys_time_check_and_ack_timer(0)) - main_periodic(); - main_event(); - } + while (1) { + if (sys_time_check_and_ack_timer(0)) + main_periodic(); + main_event(); + } - return 0; + return 0; } static inline void main_init(void) { - mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); - imu_init(); - baro_init(); - radio_control_init(); - actuators_init(); - overo_link_init(); - cscp_init(); + mcu_init(); + sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + imu_init(); + baro_init(); + radio_control_init(); + actuators_init(); + overo_link_init(); + cscp_init(); #ifdef PASSTHROUGH_CYGNUS - autopilot_init(); - nav_init(); - guidance_h_init(); - guidance_v_init(); - stabilization_init(); + autopilot_init(); + nav_init(); + guidance_h_init(); + guidance_v_init(); + stabilization_init(); - ahrs_aligner_init(); - ahrs_init(); + ahrs_aligner_init(); + ahrs_init(); - ins_init(); + ins_init(); #endif - adc_buf_channel(0, &adc0_buf, 8); - adc_buf_channel(1, &adc1_buf, 8); - adc_buf_channel(2, &adc2_buf, 8); - adc_buf_channel(3, &adc3_buf, 8); + adc_buf_channel(0, &adc0_buf, 8); + adc_buf_channel(1, &adc1_buf, 8); + adc_buf_channel(2, &adc2_buf, 8); + adc_buf_channel(3, &adc3_buf, 8); - cscp_register_callback(CSC_VANE_MSG_ID, on_vane_msg, (void *)&csc_vane_msg); - new_radio_msg = FALSE; - new_baro_diff = FALSE; - new_baro_abs = FALSE; - new_vane = FALSE; - new_adc = FALSE; + cscp_register_callback(CSC_VANE_MSG_ID, on_vane_msg, (void *)&csc_vane_msg); + new_radio_msg = FALSE; + new_baro_diff = FALSE; + new_baro_abs = FALSE; + new_vane = FALSE; + new_adc = FALSE; - overo_link.up.msg.imu_tick = 0; + overo_link.up.msg.imu_tick = 0; } static void check_radio_lost(void) { #ifdef PASSTHROUGH_CYGNUS - return; + return; #endif - if (radio_control.status == RC_REALLY_LOST) { - const static int32_t commands_failsafe[COMMANDS_NB] = COMMANDS_FAILSAFE; - SetActuatorsFromCommands(commands_failsafe); - } + if (radio_control.status == RC_REALLY_LOST) { + const static int32_t commands_failsafe[COMMANDS_NB] = COMMANDS_FAILSAFE; + SetActuatorsFromCommands(commands_failsafe); + } } static inline void main_periodic(void) { - uint16_t v1 = 123; - uint16_t v2 = 123; + uint16_t v1 = 123; + uint16_t v2 = 123; imu_periodic(); #ifdef PASSTHROUGH_CYGNUS - autopilot_periodic(); + autopilot_periodic(); #endif - OveroLinkPeriodic(on_overo_link_lost); + OveroLinkPeriodic(on_overo_link_lost); - RunOnceEvery(10, { - LED_PERIODIC(); - DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM); - radio_control_periodic(); - check_radio_lost(); - DOWNLINK_SEND_BARO_RAW(DefaultChannel, DefaultDevice, &baro.absolute, &baro.differential); - }); + RunOnceEvery(10, { + LED_PERIODIC(); + DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM); + radio_control_periodic(); + check_radio_lost(); + DOWNLINK_SEND_BARO_RAW(DefaultChannel, DefaultDevice, &baro.absolute, &baro.differential); + }); - RunOnceEvery(2, {baro_periodic();}); + RunOnceEvery(2, {baro_periodic();}); - if (adc_new_data_trigger) { - adc_new_data_trigger = 0; - new_adc = 1; - v1 = adc0_buf.sum / adc0_buf.av_nb_sample; - v2 = adc1_buf.values[0]; + if (adc_new_data_trigger) { + adc_new_data_trigger = 0; + new_adc = 1; + v1 = adc0_buf.sum / adc0_buf.av_nb_sample; + v2 = adc1_buf.values[0]; - RunOnceEvery(10, { DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, DefaultDevice, &v1, &v2) }); - } + RunOnceEvery(10, { DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, DefaultDevice, &v1, &v2) }); + } } static inline void on_rc_message(void) { - new_radio_msg = TRUE; - if (radio_control.values[RADIO_MODE] >= 150) { + new_radio_msg = TRUE; + if (radio_control.values[RADIO_MODE] >= 150) { #ifdef PASSTHROUGH_CYGNUS - autopilot_on_rc_frame(); + autopilot_on_rc_frame(); #else - static int32_t commands[COMMANDS_NB]; - SetCommandsFromRC(commands, radio_control.values); - SetActuatorsFromCommands(commands); + static int32_t commands[COMMANDS_NB]; + SetCommandsFromRC(commands, radio_control.values); + SetActuatorsFromCommands(commands); #endif - } + } #ifndef PASSTHROUGH_CYGNUS - if (radio_control.values[RADIO_KILL] > 150) { - actuators[SERVO_THROTTLE] = SERVO_THROTTLE_MIN; - ActuatorsCommit(); - } + if (radio_control.values[RADIO_KILL] > 150) { + actuators[SERVO_THROTTLE] = SERVO_THROTTLE_MIN; + ActuatorsCommit(); + } #endif } static inline void on_overo_link_msg_received(void) { - /* IMU up */ - overo_link.up.msg.valid.imu = 1; - RATES_COPY(overo_link.up.msg.gyro, imu.gyro); - VECT3_COPY(overo_link.up.msg.accel, imu.accel); - VECT3_COPY(overo_link.up.msg.mag, imu.mag); + /* IMU up */ + overo_link.up.msg.valid.imu = 1; + RATES_COPY(overo_link.up.msg.gyro, imu.gyro); + VECT3_COPY(overo_link.up.msg.accel, imu.accel); + VECT3_COPY(overo_link.up.msg.mag, imu.mag); - /* RC up */ - overo_link.up.msg.valid.rc = new_radio_msg; - new_radio_msg = FALSE; + /* RC up */ + overo_link.up.msg.valid.rc = new_radio_msg; + new_radio_msg = FALSE; - overo_link.up.msg.rc_pitch = radio_control.values[RADIO_PITCH]; - overo_link.up.msg.rc_roll = radio_control.values[RADIO_ROLL]; - overo_link.up.msg.rc_yaw = radio_control.values[RADIO_YAW]; - overo_link.up.msg.rc_thrust = radio_control.values[RADIO_THROTTLE]; - overo_link.up.msg.rc_mode = radio_control.values[RADIO_MODE]; + overo_link.up.msg.rc_pitch = radio_control.values[RADIO_PITCH]; + overo_link.up.msg.rc_roll = radio_control.values[RADIO_ROLL]; + overo_link.up.msg.rc_yaw = radio_control.values[RADIO_YAW]; + overo_link.up.msg.rc_thrust = radio_control.values[RADIO_THROTTLE]; + overo_link.up.msg.rc_mode = radio_control.values[RADIO_MODE]; #ifdef RADIO_CONTROL_KILL - overo_link.up.msg.rc_kill = radio_control.values[RADIO_KILL]; + overo_link.up.msg.rc_kill = radio_control.values[RADIO_KILL]; #endif #ifdef RADIO_CONTROL_GEAR - overo_link.up.msg.rc_gear = radio_control.values[RADIO_GEAR]; + overo_link.up.msg.rc_gear = radio_control.values[RADIO_GEAR]; #endif - overo_link.up.msg.rc_aux2 = radio_control.values[RADIO_AUX2]; - overo_link.up.msg.rc_aux3 = radio_control.values[RADIO_AUX3]; - overo_link.up.msg.rc_status = radio_control.status; + overo_link.up.msg.rc_aux2 = radio_control.values[RADIO_AUX2]; + overo_link.up.msg.rc_aux3 = radio_control.values[RADIO_AUX3]; + overo_link.up.msg.rc_status = radio_control.status; - overo_link.up.msg.stm_msg_cnt = overo_link.msg_cnt; - overo_link.up.msg.stm_crc_err_cnt = overo_link.crc_err_cnt; + overo_link.up.msg.stm_msg_cnt = overo_link.msg_cnt; + overo_link.up.msg.stm_crc_err_cnt = overo_link.crc_err_cnt; - /* baro up */ - overo_link.up.msg.valid.pressure_differential = new_baro_diff; - overo_link.up.msg.valid.pressure_absolute = new_baro_abs; - new_baro_diff = FALSE; - new_baro_abs = FALSE; - overo_link.up.msg.pressure_differential = baro.differential; - overo_link.up.msg.pressure_absolute = baro.absolute; + /* baro up */ + overo_link.up.msg.valid.pressure_differential = new_baro_diff; + overo_link.up.msg.valid.pressure_absolute = new_baro_abs; + new_baro_diff = FALSE; + new_baro_abs = FALSE; + overo_link.up.msg.pressure_differential = baro.differential; + overo_link.up.msg.pressure_absolute = baro.absolute; - /* vane up */ - overo_link.up.msg.valid.vane = new_vane; - new_vane = FALSE; - overo_link.up.msg.vane_angle1 = csc_vane_msg.vane_angle1; - overo_link.up.msg.vane_angle2 = csc_vane_msg.vane_angle2; + /* vane up */ + overo_link.up.msg.valid.vane = new_vane; + new_vane = FALSE; + overo_link.up.msg.vane_angle1 = csc_vane_msg.vane_angle1; + overo_link.up.msg.vane_angle2 = csc_vane_msg.vane_angle2; - /* adc up */ - overo_link.up.msg.adc.channels[0] = adc0_buf.sum / adc0_buf.av_nb_sample; - overo_link.up.msg.adc.channels[1] = adc1_buf.sum / adc1_buf.av_nb_sample; - overo_link.up.msg.adc.channels[2] = adc2_buf.sum / adc2_buf.av_nb_sample; - overo_link.up.msg.adc.channels[3] = adc3_buf.sum / adc3_buf.av_nb_sample; - overo_link.up.msg.valid.adc = new_adc; - new_adc = FALSE; + /* adc up */ + overo_link.up.msg.adc.channels[0] = adc0_buf.sum / adc0_buf.av_nb_sample; + overo_link.up.msg.adc.channels[1] = adc1_buf.sum / adc1_buf.av_nb_sample; + overo_link.up.msg.adc.channels[2] = adc2_buf.sum / adc2_buf.av_nb_sample; + overo_link.up.msg.adc.channels[3] = adc3_buf.sum / adc3_buf.av_nb_sample; + overo_link.up.msg.valid.adc = new_adc; + new_adc = FALSE; #ifdef PASSTHROUGH_CYGNUS - for (int i = 0; i < 6; i++) { - actuators_pwm_values[i] = overo_link.down.msg.pwm_outputs_usecs[i]; - } - actuators_pwm_commit(); + for (int i = 0; i < 6; i++) { + actuators_pwm_values[i] = overo_link.down.msg.pwm_outputs_usecs[i]; + } + actuators_pwm_commit(); - for (int i = 6; i < 10; i++) { - csc_servo_cmd.servos[i-6] = overo_link.down.msg.pwm_outputs_usecs[i]; - } - cscp_transmit(0, CSC_SERVO_CMD_ID, (uint8_t *)&csc_servo_cmd, sizeof(csc_servo_cmd)); + for (int i = 6; i < 10; i++) { + csc_servo_cmd.servos[i-6] = overo_link.down.msg.pwm_outputs_usecs[i]; + } + cscp_transmit(0, CSC_SERVO_CMD_ID, (uint8_t *)&csc_servo_cmd, sizeof(csc_servo_cmd)); #else - /* pwm acuators down */ - if (radio_control.values[RADIO_MODE] <= 150) { - for (int i = 0; i < 6; i++) { - actuators_pwm_values[i] = overo_link.down.msg.pwm_outputs_usecs[i]; - } - if (radio_control.values[RADIO_KILL] > 150) { - actuators[SERVO_THROTTLE] = SERVO_THROTTLE_MIN; - } - actuators_pwm_commit(); - } + /* pwm acuators down */ + if (radio_control.values[RADIO_MODE] <= 150) { + for (int i = 0; i < 6; i++) { + actuators_pwm_values[i] = overo_link.down.msg.pwm_outputs_usecs[i]; + } + if (radio_control.values[RADIO_KILL] > 150) { + actuators[SERVO_THROTTLE] = SERVO_THROTTLE_MIN; + } + actuators_pwm_commit(); + } #endif } @@ -287,61 +287,61 @@ static inline void on_accel_event(void) { } static inline void on_gyro_accel_event(void) { - ImuScaleGyro(imu); - ImuScaleAccel(imu); - overo_link.up.msg.imu_tick++; + ImuScaleGyro(imu); + ImuScaleAccel(imu); + overo_link.up.msg.imu_tick++; #ifdef PASSTHROUGH_CYGNUS - if (ahrs.status == AHRS_UNINIT) { - ahrs_aligner_run(); - if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) - ahrs_align(); - } else { - ahrs_propagate(); - ahrs_update_accel(); - ins_propagate(); - } + if (ahrs.status == AHRS_UNINIT) { + ahrs_aligner_run(); + if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) + ahrs_align(); + } else { + ahrs_propagate(); + ahrs_update_accel(); + ins_propagate(); + } #endif } static inline void on_mag_event(void) { - ImuScaleMag(imu); + ImuScaleMag(imu); #ifdef PASSTHROUGH_CYGNUS - if (ahrs.status == AHRS_RUNNING) - ahrs_update_mag(); + if (ahrs.status == AHRS_RUNNING) + ahrs_update_mag(); #endif } static inline void on_vane_msg(void *data) { - new_vane = TRUE; - int zero = 0; - DOWNLINK_SEND_VANE_SENSOR(DefaultChannel, DefaultDevice, - &(csc_vane_msg.vane_angle1), - &zero, - &zero, - &zero, - &zero, - &csc_vane_msg.vane_angle2, - &zero, - &zero, - &zero, - &zero); + new_vane = TRUE; + int zero = 0; + DOWNLINK_SEND_VANE_SENSOR(DefaultChannel, DefaultDevice, + &(csc_vane_msg.vane_angle1), + &zero, + &zero, + &zero, + &zero, + &csc_vane_msg.vane_angle2, + &zero, + &zero, + &zero, + &zero); } static inline void main_on_baro_diff(void) { - new_baro_diff = TRUE; + new_baro_diff = TRUE; } static inline void main_on_baro_abs(void) { - new_baro_abs = TRUE; + new_baro_abs = TRUE; } static inline void main_event(void) { - ImuEvent(on_gyro_accel_event, on_accel_event, on_mag_event); - BaroEvent(main_on_baro_abs, main_on_baro_diff); - OveroLinkEvent(on_overo_link_msg_received, on_overo_link_crc_failed); - RadioControlEvent(on_rc_message); - cscp_event(); + ImuEvent(on_gyro_accel_event, on_accel_event, on_mag_event); + BaroEvent(main_on_baro_abs, main_on_baro_diff); + OveroLinkEvent(on_overo_link_msg_received, on_overo_link_crc_failed); + RadioControlEvent(on_rc_message); + cscp_event(); } diff --git a/sw/airborne/lisa/stm_test_spi_link.c b/sw/airborne/lisa/stm_test_spi_link.c index 3051e338f7..8ddae94c5c 100644 --- a/sw/airborne/lisa/stm_test_spi_link.c +++ b/sw/airborne/lisa/stm_test_spi_link.c @@ -80,7 +80,7 @@ static inline void on_overo_link_msg_received(void) { DEBUG_S1_TOGGLE(); memcpy(&overo_link.up.msg, &overo_link.down.msg, - sizeof(union AutopilotMessage)); + sizeof(union AutopilotMessage)); } diff --git a/sw/airborne/lisa/test/hs_gyro.c b/sw/airborne/lisa/test/hs_gyro.c index bae2229930..0e9099e6c0 100644 --- a/sw/airborne/lisa/test/hs_gyro.c +++ b/sw/airborne/lisa/test/hs_gyro.c @@ -104,9 +104,9 @@ static inline void on_gyro_accel_event(void) { if (cnt == 10) { DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, - &imu.gyro_unscaled.p, - &imu.gyro_unscaled.q, - &imu.gyro_unscaled.r); + &imu.gyro_unscaled.p, + &imu.gyro_unscaled.q, + &imu.gyro_unscaled.r); } diff --git a/sw/airborne/lisa/test/lisa_test_aspirin.c b/sw/airborne/lisa/test/lisa_test_aspirin.c index abf82c5763..c528043e3f 100644 --- a/sw/airborne/lisa/test/lisa_test_aspirin.c +++ b/sw/airborne/lisa/test/lisa_test_aspirin.c @@ -78,10 +78,10 @@ static uint8_t foo=0; static inline void main_periodic_task( void ) { // LED_TOGGLE(6); RunOnceEvery(10, - { - DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM); - LED_PERIODIC(); - }); + { + DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM); + LED_PERIODIC(); + }); switch (foo) { diff --git a/sw/airborne/lisa/test/lisa_test_hmc5843.c b/sw/airborne/lisa/test/lisa_test_hmc5843.c index bb76fe423f..21a5929e34 100644 --- a/sw/airborne/lisa/test/lisa_test_hmc5843.c +++ b/sw/airborne/lisa/test/lisa_test_hmc5843.c @@ -179,7 +179,7 @@ static inline void main_event_task( void ) { // memcpy(tmp, i2c2.buf, 8); // DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice, 8, tmp); } - ); + ); reading_mag = FALSE; } diff --git a/sw/airborne/lisa/test/lisa_test_max1168.c b/sw/airborne/lisa/test/lisa_test_max1168.c index daa80f2760..af89ba6122 100644 --- a/sw/airborne/lisa/test/lisa_test_max1168.c +++ b/sw/airborne/lisa/test/lisa_test_max1168.c @@ -63,19 +63,19 @@ static inline void main_periodic_task( void ) { // LED_TOGGLE(6); max1168_read(); RunOnceEvery(10, - { - DOWNLINK_SEND_BOOT(DefaultChannel, DefaultDevice, &sys_time.nb_sec); - LED_PERIODIC(); - }); + { + DOWNLINK_SEND_BOOT(DefaultChannel, DefaultDevice, &sys_time.nb_sec); + LED_PERIODIC(); + }); } static inline void main_event_task( void ) { if (max1168_status == MAX1168_DATA_AVAILABLE) { RunOnceEvery(10, { - DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, &max1168_values[0], &max1168_values[1], &max1168_values[2]); - DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, &max1168_values[3], &max1168_values[4], &max1168_values[6]); - // DOWNLINK_SEND_BOOT(DefaultChannel, DefaultDevice, &max1168_values[7]); }); + DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, &max1168_values[0], &max1168_values[1], &max1168_values[2]); + DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, &max1168_values[3], &max1168_values[4], &max1168_values[6]); + // DOWNLINK_SEND_BOOT(DefaultChannel, DefaultDevice, &max1168_values[7]); }); }); max1168_status = MAX1168_IDLE; } diff --git a/sw/airborne/lisa/test/lisa_test_sc18is600.c b/sw/airborne/lisa/test/lisa_test_sc18is600.c index 4f77a15dd1..49f432073b 100644 --- a/sw/airborne/lisa/test/lisa_test_sc18is600.c +++ b/sw/airborne/lisa/test/lisa_test_sc18is600.c @@ -59,10 +59,10 @@ static inline void main_init( void ) { static inline void main_periodic_task( void ) { // LED_TOGGLE(6); RunOnceEvery(10, - { - DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM); - LED_PERIODIC(); - }); + { + DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM); + LED_PERIODIC(); + }); static uint8_t foo=0; switch (foo) { diff --git a/sw/airborne/lisa/test_can.c b/sw/airborne/lisa/test_can.c index 8d5d9af51b..f25782e366 100644 --- a/sw/airborne/lisa/test_can.c +++ b/sw/airborne/lisa/test_can.c @@ -53,74 +53,74 @@ int main(void) { new_can_data = false; while(1) { - if (sys_time_check_and_ack_timer(0)) - main_periodic_task(); - main_event_task(); + if (sys_time_check_and_ack_timer(0)) + main_periodic_task(); + main_event_task(); } return 0; } static inline void main_init( void ) { - mcu_init(); - sys_time_register_timer((0.5/PERIODIC_FREQUENCY), NULL); - ppz_can_init(main_on_can_msg); + mcu_init(); + sys_time_register_timer((0.5/PERIODIC_FREQUENCY), NULL); + ppz_can_init(main_on_can_msg); } static inline void main_periodic_task( void ) { - tx_data[0]+=1; - ppz_can_transmit(0, tx_data, 8); + tx_data[0]+=1; + ppz_can_transmit(0, tx_data, 8); - LED_PERIODIC(); - DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM); + LED_PERIODIC(); + DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM); } static inline void main_event_task( void ) { - if (new_can_data) { - if (rx_data[0] & 0x10) { - LED_ON(2); - } else { - LED_OFF(2); - } - } + if (new_can_data) { + if (rx_data[0] & 0x10) { + LED_ON(2); + } else { + LED_OFF(2); + } + } - if (new_can_data) { - if (rx_data[0] & 0x20) { - LED_ON(3); - } else { - LED_OFF(3); - } - } + if (new_can_data) { + if (rx_data[0] & 0x20) { + LED_ON(3); + } else { + LED_OFF(3); + } + } - if (new_can_data) { - if (rx_data[0] & 0x40) { - LED_ON(4); - } else { - LED_OFF(4); - } - } + if (new_can_data) { + if (rx_data[0] & 0x40) { + LED_ON(4); + } else { + LED_OFF(4); + } + } - if (new_can_data) { - if (rx_data[0] & 0x80) { - LED_ON(5); - } else { - LED_OFF(5); - } - } + if (new_can_data) { + if (rx_data[0] & 0x80) { + LED_ON(5); + } else { + LED_OFF(5); + } + } } void main_on_can_msg(uint32_t id, uint8_t *data, int len) { - for (int i = 0; i<8; i++) { - rx_data[i] = data[i]; - } + for (int i = 0; i<8; i++) { + rx_data[i] = data[i]; + } - new_can_data = true; + new_can_data = true; } diff --git a/sw/airborne/lisa/test_csc_servo.c b/sw/airborne/lisa/test_csc_servo.c index c5944db5b5..d237ae6b8e 100644 --- a/sw/airborne/lisa/test_csc_servo.c +++ b/sw/airborne/lisa/test_csc_servo.c @@ -50,80 +50,80 @@ int main(void) { servos[3] = 4; while(1) { - if (sys_time_check_and_ack_timer(0)) - main_periodic_task(); - main_event_task(); + if (sys_time_check_and_ack_timer(0)) + main_periodic_task(); + main_event_task(); } return 0; } static inline void main_init( void ) { - hw_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); - cscp_init(); - cscp_register_callback(CSC_VANE_MSG_ID, main_on_vane_msg, (void *)&csc_vane_msg); + hw_init(); + sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + cscp_init(); + cscp_register_callback(CSC_VANE_MSG_ID, main_on_vane_msg, (void *)&csc_vane_msg); } static inline void main_periodic_task( void ) { - servos[0]+=10; - servos[1]+=10; - servos[2]+=10; - servos[3]+=10; + servos[0]+=10; + servos[1]+=10; + servos[2]+=10; + servos[3]+=10; - if ((can_error_warning = CAN_GetFlagStatus(CAN1, CAN_FLAG_EWG)) == SET) { - LED_ON(2); - } else { - LED_OFF(2); - } - if ((can_error_passive = CAN_GetFlagStatus(CAN1, CAN_FLAG_EPV)) == SET) { - LED_ON(3); - } else { - LED_OFF(3); - } - if ((can_bus_off = CAN_GetFlagStatus(CAN1, CAN_FLAG_BOF)) == SET) { - LED_ON(0); - } else { - LED_OFF(0); - } + if ((can_error_warning = CAN_GetFlagStatus(CAN1, CAN_FLAG_EWG)) == SET) { + LED_ON(2); + } else { + LED_OFF(2); + } + if ((can_error_passive = CAN_GetFlagStatus(CAN1, CAN_FLAG_EPV)) == SET) { + LED_ON(3); + } else { + LED_OFF(3); + } + if ((can_bus_off = CAN_GetFlagStatus(CAN1, CAN_FLAG_BOF)) == SET) { + LED_ON(0); + } else { + LED_OFF(0); + } - cscp_transmit(0, 0, (uint8_t *)servos, 8); + cscp_transmit(0, 0, (uint8_t *)servos, 8); - LED_PERIODIC(); - DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM); + LED_PERIODIC(); + DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM); } static inline void main_event_task( void ) { - cscp_event(); + cscp_event(); - LED_OFF(0); - LED_OFF(1); - LED_OFF(2); - LED_OFF(3); + LED_OFF(0); + LED_OFF(1); + LED_OFF(2); + LED_OFF(3); // DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM); } void main_on_vane_msg(void *data) { - int zero = 0; + int zero = 0; // DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice); // RunOnceEvery(10, { - DOWNLINK_SEND_VANE_SENSOR(DefaultChannel, DefaultDevice, - &(csc_vane_msg.vane_angle1), - &zero, - &zero, - &zero, - &zero, - &csc_vane_msg.vane_angle2, - &zero, - &zero, - &zero, - &zero); + DOWNLINK_SEND_VANE_SENSOR(DefaultChannel, DefaultDevice, + &(csc_vane_msg.vane_angle1), + &zero, + &zero, + &zero, + &zero, + &csc_vane_msg.vane_angle2, + &zero, + &zero, + &zero, + &zero); // }); } diff --git a/sw/airborne/lisa/test_spi_slave2.c b/sw/airborne/lisa/test_spi_slave2.c index 45cee9d234..1f3e025de4 100644 --- a/sw/airborne/lisa/test_spi_slave2.c +++ b/sw/airborne/lisa/test_spi_slave2.c @@ -65,10 +65,10 @@ static inline void main_init( void ) { static inline void main_periodic_task( void ) { RunOnceEvery(10, - { - DOWNLINK_SEND_BOOT(DefaultChannel, DefaultDevice, &sys_time.nb_sec); - LED_PERIODIC(); - }); + { + DOWNLINK_SEND_BOOT(DefaultChannel, DefaultDevice, &sys_time.nb_sec); + LED_PERIODIC(); + }); } @@ -78,7 +78,7 @@ static inline void main_event_task( void ) { if (DMA_GetFlagStatus(DMA1_FLAG_TC2)) { LED_TOGGLE(3); RunOnceEvery(10, {DOWNLINK_SEND_DEBUG_MCU_LINK(DefaultChannel, DefaultDevice, &SPI_SLAVE_Buffer_Rx[0], - &SPI_SLAVE_Buffer_Rx[1], &SPI_SLAVE_Buffer_Rx[2]);}); + &SPI_SLAVE_Buffer_Rx[1], &SPI_SLAVE_Buffer_Rx[2]);}); memcpy(SPI_SLAVE_Buffer_Tx, SPI_SLAVE_Buffer_Rx, BufferSize); main_setup_dma(); main_enable_dma(); diff --git a/sw/airborne/lisa/test_uart_lisal.c b/sw/airborne/lisa/test_uart_lisal.c index 222dff8cab..7481caef12 100644 --- a/sw/airborne/lisa/test_uart_lisal.c +++ b/sw/airborne/lisa/test_uart_lisal.c @@ -52,45 +52,45 @@ static inline void main_periodic( void ) { uart_transmit(&uart3, 'c'); if (uart_char_available(&uart1)) { - ch = uart_getch(&uart1); - if (ch == 'a') { - LED_OFF(0); - LED_ON(1); - } else { - LED_ON(0); - LED_ON(1); - } + ch = uart_getch(&uart1); + if (ch == 'a') { + LED_OFF(0); + LED_ON(1); } else { - LED_ON(0); - LED_OFF(1); + LED_ON(0); + LED_ON(1); + } + } else { + LED_ON(0); + LED_OFF(1); } if (uart_char_available(&uart2)) { - ch = uart_getch(&uart2); - if (ch == 'b') { - LED_OFF(2); - LED_ON(3); - } else { - LED_ON(2); - LED_ON(3); - } + ch = uart_getch(&uart2); + if (ch == 'b') { + LED_OFF(2); + LED_ON(3); } else { - LED_ON(2); - LED_OFF(3); + LED_ON(2); + LED_ON(3); + } + } else { + LED_ON(2); + LED_OFF(3); } if (uart_char_available(&uart3)) { - ch = uart_getch(&uart3); - if (ch == 'c') { - LED_OFF(4); - LED_ON(5); - } else { - LED_ON(4); - LED_ON(5); - } + ch = uart_getch(&uart3); + if (ch == 'c') { + LED_OFF(4); + LED_ON(5); } else { - LED_ON(4); - LED_OFF(5); + LED_ON(4); + LED_ON(5); + } + } else { + LED_ON(4); + LED_OFF(5); } LED_PERIODIC(); diff --git a/sw/airborne/lisa/tunnel_hw.c b/sw/airborne/lisa/tunnel_hw.c index 510a48314f..75810aa701 100644 --- a/sw/airborne/lisa/tunnel_hw.c +++ b/sw/airborne/lisa/tunnel_hw.c @@ -63,16 +63,16 @@ int main(void) { /* Init GPIO for rx pins */ gpio_set(A_RX_PORT, A_RX_PIN); gpio_set_mode(A_RX_PORT, GPIO_MODE_INPUT, - GPIO_CNF_INPUT_PULL_UPDOWN, A_RX_PIN); + GPIO_CNF_INPUT_PULL_UPDOWN, A_RX_PIN); gpio_set(B_RX_PORT, B_RX_PIN); gpio_set_mode(B_RX_PORT, GPIO_MODE_INPUT, - GPIO_CNF_INPUT_PULL_UPDOWN, B_RX_PIN); + GPIO_CNF_INPUT_PULL_UPDOWN, B_RX_PIN); /* Init GPIO for tx pins */ gpio_set_mode(A_RX_PORT, GPIO_MODE_OUTPUT_50_MHZ, - GPIO_CNF_OUTPUT_PUSHPULL, A_TX_PIN); + GPIO_CNF_OUTPUT_PUSHPULL, A_TX_PIN); gpio_set_mode(B_RX_PORT, GPIO_MODE_OUTPUT_50_MHZ, - GPIO_CNF_OUTPUT_PUSHPULL, B_TX_PIN); + GPIO_CNF_OUTPUT_PUSHPULL, B_TX_PIN); gpio_clear(A_TX_PORT, A_TX_PIN); diff --git a/sw/airborne/math/pprz_algebra.h b/sw/airborne/math/pprz_algebra.h index f0680fb845..dbab039573 100644 --- a/sw/airborne/math/pprz_algebra.h +++ b/sw/airborne/math/pprz_algebra.h @@ -113,9 +113,9 @@ /* a = a * b */ #define VECT3_MUL( _v1, _v2){ \ - (_v1).x = (_v1).x * (_v2).x; \ - (_v1).y = (_v1).y * (_v2).y; \ - (_v1).z = (_v1).z * (_v2).z; \ + (_v1).x = (_v1).x * (_v2).x; \ + (_v1).y = (_v1).y * (_v2).y; \ + (_v1).z = (_v1).z * (_v2).z; \ } /* a = {abs(x), abs(y), abs(z)} */ diff --git a/sw/airborne/math/pprz_geodetic_wgs84.h b/sw/airborne/math/pprz_geodetic_wgs84.h index bc0c5c132f..a625f82c66 100644 --- a/sw/airborne/math/pprz_geodetic_wgs84.h +++ b/sw/airborne/math/pprz_geodetic_wgs84.h @@ -22,22 +22,22 @@ rows are from -180 to +170 starting north +90 to south-90 #define WGS84_H(x,y) ((float) pprz_geodetic_wgs84_int[(y)][(x)]) #define WGS84_ELLIPSOID_TO_GEOID(_Lat,_Lon,_diff) { \ - float x = (180.0f + DegOfRad(_Lon)) / 10.0f; \ - Bound(x,0.0f,35.99999f); \ - float y = (90.0f - DegOfRad(_Lat)) / 10.0f; \ - Bound(y,0.0f,17.99999f); \ - uint8_t ex1 = (uint8_t) x; \ - uint8_t ex2 = ex1 + 1; \ - if (ex2 >= 36) ex2 = 0; \ - uint8_t ey1 = (uint8_t) y; \ - uint8_t ey2 = ey1 + 1; \ - float lin_x = x - ((float) ex1); \ - float lin_y = y - ((float) ey1); \ - float h11 = (1.0f - lin_x) * (1.0f - lin_y) * WGS84_H(ex1,ey1); \ - float h12 = lin_x * (1.0f - lin_y) * WGS84_H(ex2,ey1); \ - float h21 = (1.0f - lin_x) * lin_y * WGS84_H(ex1,ey2); \ - float h22 = lin_x * lin_y * WGS84_H(ex2,ey2); \ - _diff = h11 + h12 + h21 + h22; \ + float x = (180.0f + DegOfRad(_Lon)) / 10.0f; \ + Bound(x,0.0f,35.99999f); \ + float y = (90.0f - DegOfRad(_Lat)) / 10.0f; \ + Bound(y,0.0f,17.99999f); \ + uint8_t ex1 = (uint8_t) x; \ + uint8_t ex2 = ex1 + 1; \ + if (ex2 >= 36) ex2 = 0; \ + uint8_t ey1 = (uint8_t) y; \ + uint8_t ey2 = ey1 + 1; \ + float lin_x = x - ((float) ex1); \ + float lin_y = y - ((float) ey1); \ + float h11 = (1.0f - lin_x) * (1.0f - lin_y) * WGS84_H(ex1,ey1); \ + float h12 = lin_x * (1.0f - lin_y) * WGS84_H(ex2,ey1); \ + float h21 = (1.0f - lin_x) * lin_y * WGS84_H(ex1,ey2); \ + float h22 = lin_x * lin_y * WGS84_H(ex2,ey2); \ + _diff = h11 + h12 + h21 + h22; \ } @@ -45,25 +45,25 @@ rows are from -180 to +170 starting north +90 to south-90 const int8_t pprz_geodetic_wgs84_int[19][36] = { - {13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13}, - {3,1,-2,-3,-3,-3,-1,3,1,5,9,11,19,27,31,34,33,34,33,34,28,23,17,13,9,4,4,1,-2,-2,0,2,3,2,1,1}, - {2,2,1,-1,-3,-7,-14,-24,-27,-25,-19,3,24,37,47,60,61,58,51,43,29,20,12,5,-2,-10,-14,-12,-10,-14,-12,-6,-2,3,6,4}, - {2,9,17,10,13,1,-14,-30,-39,-46,-42,-21,6,29,49,65,60,57,47,41,21,18,14,7,-3,-22,-29,-32,-32,-26,-15,-2,13,17,19,6}, - {-8,8,8,1,-11,-19,-16,-18,-22,-35,-40,-26,-12,24,45,63,62,59,47,48,42,28,12,-10,-19,-33,-43,-42,-43,-29,-2,17,23,22,6,2}, - {-12,-10,-13,-20,-31,-34,-21,-16,-26,-34,-33,-35,-26,2,33,59,52,51,52,48,35,40,33,-9,-28,-39,-48,-59,-50,-28,3,23,37,18,-1,-11}, - {-7,-5,-8,-15,-28,-40,-42,-29,-22,-26,-32,-51,-40,-17,17,31,34,44,36,28,29,17,12,-20,-15,-40,-33,-34,-34,-28,7,29,43,20,4,-6}, - {5,10,7,-7,-23,-39,-47,-34,-9,-10,-20,-45,-48,-32,-9,17,25,31,31,26,15,6,1,-29,-44,-61,-67,-59,-36,-11,21,39,49,39,22,10}, - {13,12,11,2,-11,-28,-38,-29,-10,3,1,-11,-41,-42,-16,3,17,33,22,23,2,-3,-7,-36,-59,-90,-95,-63,-24,12,53,60,58,46,36,26}, - {22,16,17,13,1,-12,-23,-20,-14,-3,14,10,-15,-27,-18,3,12,20,18,12,-13,-9,-28,-49,-62,-89,-102,-63,-9,33,58,73,74,63,50,32}, - {36,22,11,6,-1,-8,-10,-8,-11,-9,1,32,4,-18,-13,-9,4,14,12,13,-2,-14,-25,-32,-38,-60,-75,-63,-26,0,35,52,68,76,64,52}, - {51,27,10,0,-9,-11,-5,-2,-3,-1,9,35,20,-5,-6,-5,0,13,17,23,21,8,-9,-10,-11,-20,-40,-47,-45,-25,5,23,45,58,57,63}, - {46,22,5,-2,-8,-13,-10,-7,-4,1,9,32,16,4,-8,4,12,15,22,27,34,29,14,15,15,7,-9,-25,-37,-39,-23,-14,15,33,34,45}, - {21,6,1,-7,-12,-12,-12,-10,-7,-1,8,23,15,-2,-6,6,21,24,18,26,31,33,39,41,30,24,13,-2,-20,-32,-33,-27,-14,-2,5,20}, - {-15,-18,-18,-16,-17,-15,-10,-10,-8,-2,6,14,13,3,3,10,20,27,25,26,34,39,45,45,38,39,28,13,-1,-15,-22,-22,-18,-15,-14,-10}, - {-45,-43,-37,-32,-30,-26,-23,-22,-16,-10,-2,10,20,20,21,24,22,17,16,19,25,30,35,35,33,30,27,10,-2,-14,-23,-30,-33,-29,-35,-43}, - {-61,-60,-61,-55,-49,-44,-38,-31,-25,-16,-6,1,4,5,4,2,6,12,16,16,17,21,20,26,26,22,16,10,-1,-16,-29,-36,-46,-55,-54,-59}, - {-53,-54,-55,-52,-48,-42,-38,-38,-29,-26,-26,-24,-23,-21,-19,-16,-12,-8,-4,-1,1,4,4,6,5,4,2,-6,-15,-24,-33,-40,-48,-50,-53,-52}, - {-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30} + {13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13,13}, + {3,1,-2,-3,-3,-3,-1,3,1,5,9,11,19,27,31,34,33,34,33,34,28,23,17,13,9,4,4,1,-2,-2,0,2,3,2,1,1}, + {2,2,1,-1,-3,-7,-14,-24,-27,-25,-19,3,24,37,47,60,61,58,51,43,29,20,12,5,-2,-10,-14,-12,-10,-14,-12,-6,-2,3,6,4}, + {2,9,17,10,13,1,-14,-30,-39,-46,-42,-21,6,29,49,65,60,57,47,41,21,18,14,7,-3,-22,-29,-32,-32,-26,-15,-2,13,17,19,6}, + {-8,8,8,1,-11,-19,-16,-18,-22,-35,-40,-26,-12,24,45,63,62,59,47,48,42,28,12,-10,-19,-33,-43,-42,-43,-29,-2,17,23,22,6,2}, + {-12,-10,-13,-20,-31,-34,-21,-16,-26,-34,-33,-35,-26,2,33,59,52,51,52,48,35,40,33,-9,-28,-39,-48,-59,-50,-28,3,23,37,18,-1,-11}, + {-7,-5,-8,-15,-28,-40,-42,-29,-22,-26,-32,-51,-40,-17,17,31,34,44,36,28,29,17,12,-20,-15,-40,-33,-34,-34,-28,7,29,43,20,4,-6}, + {5,10,7,-7,-23,-39,-47,-34,-9,-10,-20,-45,-48,-32,-9,17,25,31,31,26,15,6,1,-29,-44,-61,-67,-59,-36,-11,21,39,49,39,22,10}, + {13,12,11,2,-11,-28,-38,-29,-10,3,1,-11,-41,-42,-16,3,17,33,22,23,2,-3,-7,-36,-59,-90,-95,-63,-24,12,53,60,58,46,36,26}, + {22,16,17,13,1,-12,-23,-20,-14,-3,14,10,-15,-27,-18,3,12,20,18,12,-13,-9,-28,-49,-62,-89,-102,-63,-9,33,58,73,74,63,50,32}, + {36,22,11,6,-1,-8,-10,-8,-11,-9,1,32,4,-18,-13,-9,4,14,12,13,-2,-14,-25,-32,-38,-60,-75,-63,-26,0,35,52,68,76,64,52}, + {51,27,10,0,-9,-11,-5,-2,-3,-1,9,35,20,-5,-6,-5,0,13,17,23,21,8,-9,-10,-11,-20,-40,-47,-45,-25,5,23,45,58,57,63}, + {46,22,5,-2,-8,-13,-10,-7,-4,1,9,32,16,4,-8,4,12,15,22,27,34,29,14,15,15,7,-9,-25,-37,-39,-23,-14,15,33,34,45}, + {21,6,1,-7,-12,-12,-12,-10,-7,-1,8,23,15,-2,-6,6,21,24,18,26,31,33,39,41,30,24,13,-2,-20,-32,-33,-27,-14,-2,5,20}, + {-15,-18,-18,-16,-17,-15,-10,-10,-8,-2,6,14,13,3,3,10,20,27,25,26,34,39,45,45,38,39,28,13,-1,-15,-22,-22,-18,-15,-14,-10}, + {-45,-43,-37,-32,-30,-26,-23,-22,-16,-10,-2,10,20,20,21,24,22,17,16,19,25,30,35,35,33,30,27,10,-2,-14,-23,-30,-33,-29,-35,-43}, + {-61,-60,-61,-55,-49,-44,-38,-31,-25,-16,-6,1,4,5,4,2,6,12,16,16,17,21,20,26,26,22,16,10,-1,-16,-29,-36,-46,-55,-54,-59}, + {-53,-54,-55,-52,-48,-42,-38,-38,-29,-26,-26,-24,-23,-21,-19,-16,-12,-8,-4,-1,1,4,4,6,5,4,2,-6,-15,-24,-33,-40,-48,-50,-53,-52}, + {-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30,-30} }; diff --git a/sw/airborne/mcu_periph/can.c b/sw/airborne/mcu_periph/can.c index 5fa03485c4..df04e8c3fc 100644 --- a/sw/airborne/mcu_periph/can.c +++ b/sw/airborne/mcu_periph/can.c @@ -31,17 +31,17 @@ void _can_run_rx_callback(uint32_t id, uint8_t *buf, uint8_t len); void ppz_can_init(can_rx_callback_t callback) { - can_rx_callback = callback; - can_hw_init(); + can_rx_callback = callback; + can_hw_init(); } int ppz_can_transmit(uint32_t id, const uint8_t *buf, uint8_t len) { - return can_hw_transmit(id, buf, len); + return can_hw_transmit(id, buf, len); } void _can_run_rx_callback(uint32_t id, uint8_t *buf, uint8_t len) { - if(can_rx_callback) - can_rx_callback(id, buf, len); + if(can_rx_callback) + can_rx_callback(id, buf, len); } diff --git a/sw/airborne/modules/benchmark/flight_benchmark.c b/sw/airborne/modules/benchmark/flight_benchmark.c index d58eb1153d..a2eaa74d57 100644 --- a/sw/airborne/modules/benchmark/flight_benchmark.c +++ b/sw/airborne/modules/benchmark/flight_benchmark.c @@ -35,89 +35,89 @@ bool_t benchm_go; void flight_benchmark_init( void ) { - SquareSumErr_airspeed = 0; - SquareSumErr_altitude = 0; - SquareSumErr_position = 0; - ToleranceAispeed = BENCHMARK_TOLERANCE_AIRSPEED; - ToleranceAltitude = BENCHMARK_TOLERANCE_ALTITUDE; - TolerancePosition = BENCHMARK_TOLERANCE_POSITION; - benchm_reset = 0; - benchm_go = 0; + SquareSumErr_airspeed = 0; + SquareSumErr_altitude = 0; + SquareSumErr_position = 0; + ToleranceAispeed = BENCHMARK_TOLERANCE_AIRSPEED; + ToleranceAltitude = BENCHMARK_TOLERANCE_ALTITUDE; + TolerancePosition = BENCHMARK_TOLERANCE_POSITION; + benchm_reset = 0; + benchm_go = 0; } void flight_benchmark_periodic( void ) { - float Err_airspeed = 0; - float Err_altitude = 0; - float Err_position = 0; + float Err_airspeed = 0; + float Err_altitude = 0; + float Err_position = 0; - if (benchm_reset){ - flight_benchmark_reset(); - benchm_reset = 0; - } + if (benchm_reset){ + flight_benchmark_reset(); + benchm_reset = 0; + } - if (benchm_go){ - #if USE_AIRSPEED && defined(BENCHMARK_AIRSPEED) - Err_airspeed = fabs(*stateGetAirspeed_f() - v_ctl_auto_airspeed_setpoint); - if (Err_airspeed>ToleranceAispeed){ - Err_airspeed = Err_airspeed-ToleranceAispeed; - SquareSumErr_airspeed += (Err_airspeed * Err_airspeed); - } - #endif + if (benchm_go){ + #if USE_AIRSPEED && defined(BENCHMARK_AIRSPEED) + Err_airspeed = fabs(*stateGetAirspeed_f() - v_ctl_auto_airspeed_setpoint); + if (Err_airspeed>ToleranceAispeed){ + Err_airspeed = Err_airspeed-ToleranceAispeed; + SquareSumErr_airspeed += (Err_airspeed * Err_airspeed); + } + #endif - #ifdef BENCHMARK_ALTITUDE - Err_altitude = fabs(stateGetPositionEnu_f()->z - v_ctl_altitude_setpoint); - if (Err_altitude>ToleranceAltitude){ - Err_altitude = Err_altitude-ToleranceAltitude; - SquareSumErr_altitude += (Err_altitude * Err_altitude); - } - #endif + #ifdef BENCHMARK_ALTITUDE + Err_altitude = fabs(stateGetPositionEnu_f()->z - v_ctl_altitude_setpoint); + if (Err_altitude>ToleranceAltitude){ + Err_altitude = Err_altitude-ToleranceAltitude; + SquareSumErr_altitude += (Err_altitude * Err_altitude); + } + #endif - #ifdef BENCHMARK_POSITION + #ifdef BENCHMARK_POSITION - //---------------This part is a waste of memory and calculation power - but it works - feel free to optimize it ;-) ----------------- + //---------------This part is a waste of memory and calculation power - but it works - feel free to optimize it ;-) ----------------- - // err_temp = waypoints[target].x - stateGetPositionEnu_f()->x; - float deltaPlaneX = 0; - float deltaPlaneY = 0; - float Err_position_segment = 0; - float Err_position_circle = 0; + // err_temp = waypoints[target].x - stateGetPositionEnu_f()->x; + float deltaPlaneX = 0; + float deltaPlaneY = 0; + float Err_position_segment = 0; + float Err_position_circle = 0; // if (nav_in_segment){ - float deltaX = nav_segment_x_2 - nav_segment_x_1; - float deltaY = nav_segment_y_2 - nav_segment_y_1; - float anglePath = atan2(deltaX,deltaY); - deltaPlaneX = nav_segment_x_2 - stateGetPositionEnu_f()->x; - deltaPlaneY = nav_segment_y_2 - stateGetPositionEnu_f()->y; - float anglePlane = atan2(deltaPlaneX,deltaPlaneY); - float angleDiff = fabs(anglePlane - anglePath); - Err_position_segment = fabs(sin(angleDiff)*sqrt(deltaPlaneX*deltaPlaneX+deltaPlaneY*deltaPlaneY)); + float deltaX = nav_segment_x_2 - nav_segment_x_1; + float deltaY = nav_segment_y_2 - nav_segment_y_1; + float anglePath = atan2(deltaX,deltaY); + deltaPlaneX = nav_segment_x_2 - stateGetPositionEnu_f()->x; + deltaPlaneY = nav_segment_y_2 - stateGetPositionEnu_f()->y; + float anglePlane = atan2(deltaPlaneX,deltaPlaneY); + float angleDiff = fabs(anglePlane - anglePath); + Err_position_segment = fabs(sin(angleDiff)*sqrt(deltaPlaneX*deltaPlaneX+deltaPlaneY*deltaPlaneY)); // } // if (nav_in_circle){ - deltaPlaneX = nav_circle_x - stateGetPositionEnu_f()->x; - deltaPlaneY = nav_circle_y - stateGetPositionEnu_f()->y; - Err_position_circle = fabs(sqrt(deltaPlaneX*deltaPlaneX+deltaPlaneY*deltaPlaneY)-nav_circle_radius); + deltaPlaneX = nav_circle_x - stateGetPositionEnu_f()->x; + deltaPlaneY = nav_circle_y - stateGetPositionEnu_f()->y; + Err_position_circle = fabs(sqrt(deltaPlaneX*deltaPlaneX+deltaPlaneY*deltaPlaneY)-nav_circle_radius); // } - if (Err_position_circle < Err_position_segment){ - Err_position = Err_position_circle; - } - else - Err_position = Err_position_segment; + if (Err_position_circle < Err_position_segment){ + Err_position = Err_position_circle; + } + else + Err_position = Err_position_segment; - if (Err_position>TolerancePosition){ - SquareSumErr_position += (Err_position * Err_position); - } - #endif - } + if (Err_position>TolerancePosition){ + SquareSumErr_position += (Err_position * Err_position); + } + #endif + } - DOWNLINK_SEND_FLIGHT_BENCHMARK(DefaultChannel, DefaultDevice, &SquareSumErr_airspeed, &SquareSumErr_altitude, &SquareSumErr_position, &Err_airspeed, &Err_altitude, &Err_position) + DOWNLINK_SEND_FLIGHT_BENCHMARK(DefaultChannel, DefaultDevice, &SquareSumErr_airspeed, &SquareSumErr_altitude, &SquareSumErr_position, &Err_airspeed, &Err_altitude, &Err_position) } void flight_benchmark_reset( void ) { - SquareSumErr_airspeed = 0; - SquareSumErr_altitude = 0; - SquareSumErr_position = 0; + SquareSumErr_airspeed = 0; + SquareSumErr_altitude = 0; + SquareSumErr_position = 0; } diff --git a/sw/airborne/modules/benchmark/i2c_abuse_test.c b/sw/airborne/modules/benchmark/i2c_abuse_test.c index fab971083f..268fcf302a 100644 --- a/sw/airborne/modules/benchmark/i2c_abuse_test.c +++ b/sw/airborne/modules/benchmark/i2c_abuse_test.c @@ -193,45 +193,45 @@ void event_i2c_abuse_test(void) // Wait for I2C transaction object to be released by the I2C driver before changing anything if ((i2c_abuse_test_counter < 12) && (i2c_abuse_test_counter > 3)) { - if ((i2c_test2.status == I2CTransFailed) || (i2c_test2.status == I2CTransSuccess)) - { - //i2c_test2.slave_addr = 0x90; - i2c_test2.type = I2CTransRx; - i2c_test2.slave_addr = 0x92; - i2c_test2.len_r = 2; - i2c_submit(&I2C_ABUSE_PORT,&i2c_test2); - } + if ((i2c_test2.status == I2CTransFailed) || (i2c_test2.status == I2CTransSuccess)) + { + //i2c_test2.slave_addr = 0x90; + i2c_test2.type = I2CTransRx; + i2c_test2.slave_addr = 0x92; + i2c_test2.len_r = 2; + i2c_submit(&I2C_ABUSE_PORT,&i2c_test2); + } } if ((i2c_test1.status == I2CTransFailed) || (i2c_test1.status == I2CTransSuccess)) { - if (i2c_abuse_test_counter < 16) - { - i2c_abuse_test_counter++; - } - else - { - // wait until ready: - if (i2c_idle(&I2C_ABUSE_PORT)) - { - i2c_abuse_test_counter = 1; + if (i2c_abuse_test_counter < 16) + { + i2c_abuse_test_counter++; + } + else + { + // wait until ready: + if (i2c_idle(&I2C_ABUSE_PORT)) + { + i2c_abuse_test_counter = 1; - i2c_setbitrate(&I2C_ABUSE_PORT, i2c_abuse_test_bitrate); + i2c_setbitrate(&I2C_ABUSE_PORT, i2c_abuse_test_bitrate); - i2c_abuse_test_bitrate += 17000; - if (i2c_abuse_test_bitrate > 410000) - { - i2c_abuse_test_bitrate -= 410000; - } - } - } + i2c_abuse_test_bitrate += 17000; + if (i2c_abuse_test_bitrate > 410000) + { + i2c_abuse_test_bitrate -= 410000; + } + } + } - if (i2c_abuse_test_counter < 16) - { - RunOnceEvery(100,LED_TOGGLE(I2C_ABUSE_LED)); - i2c_abuse_send_transaction( i2c_abuse_test_counter ); - } + if (i2c_abuse_test_counter < 16) + { + RunOnceEvery(100,LED_TOGGLE(I2C_ABUSE_LED)); + i2c_abuse_send_transaction( i2c_abuse_test_counter ); + } } } diff --git a/sw/airborne/modules/cartography/cartography.c b/sw/airborne/modules/cartography/cartography.c index 64a3c1a0b3..d8a31b2121 100644 --- a/sw/airborne/modules/cartography/cartography.c +++ b/sw/airborne/modules/cartography/cartography.c @@ -147,7 +147,7 @@ void init_carto(void) { } void periodic_downlink_carto(void) { - DOWNLINK_SEND_CAMERA_SNAPSHOT(DefaultChannel, DefaultDevice,&camera_snapshot_image_number); + DOWNLINK_SEND_CAMERA_SNAPSHOT(DefaultChannel, DefaultDevice,&camera_snapshot_image_number); } void start_carto(void) { @@ -161,47 +161,47 @@ void stop_carto(void) { /////////////////////////////////////////////////////////////////////////////////////////////// bool_t nav_survey_Inc_railnumberSinceBoot(void) { - railnumberSinceBoot++; - return FALSE; + railnumberSinceBoot++; + return FALSE; } /////////////////////////////////////////////////////////////////////////////////////////////// bool_t nav_survey_Snapshoot(void) { - camera_snapshot_image_number=railnumberSinceBoot; - PRTDEBSTR(SNAPSHOT) + camera_snapshot_image_number=railnumberSinceBoot; + PRTDEBSTR(SNAPSHOT) cartography_periodic_downlink_carto_status = MODULES_START; - return FALSE; + return FALSE; } /////////////////////////////////////////////////////////////////////////////////////////////// bool_t nav_survey_Snapshoot_Continu(void) { - camera_snapshot_image_number=railnumberSinceBoot; - PRTDEBSTR(SNAPSHOT) + camera_snapshot_image_number=railnumberSinceBoot; + PRTDEBSTR(SNAPSHOT) cartography_periodic_downlink_carto_status = MODULES_START; - return TRUE; + return TRUE; } /////////////////////////////////////////////////////////////////////////////////////////////// bool_t nav_survey_StopSnapshoot(void) { - camera_snapshot_image_number=0; - PRTDEBSTR(STOP SNAPSHOT) + camera_snapshot_image_number=0; + PRTDEBSTR(STOP SNAPSHOT) cartography_periodic_downlink_carto_status = MODULES_START; - return FALSE; + return FALSE; } /////////////////////////////////////////////////////////////////////////////////////////////// bool_t nav_survey_computefourth_corner(uint8_t wp1, uint8_t wp2, uint8_t wp3, uint8_t wp4 ) { - waypoints[wp4].x=waypoints[wp2].x+waypoints[wp3].x-waypoints[wp1].x; - waypoints[wp4].y=waypoints[wp2].y+waypoints[wp3].y-waypoints[wp1].y; + waypoints[wp4].x=waypoints[wp2].x+waypoints[wp3].x-waypoints[wp1].x; + waypoints[wp4].y=waypoints[wp2].y+waypoints[wp3].y-waypoints[wp1].y; - PRTDEBSTR(nav_survey_computefourth_corner) - PRTDEB(f,waypoints[wp4].x) - PRTDEB(f,waypoints[wp4].y) - return FALSE; + PRTDEBSTR(nav_survey_computefourth_corner) + PRTDEB(f,waypoints[wp4].x) + PRTDEB(f,waypoints[wp4].y) + return FALSE; } //////////////////////////////////////////////////////////////////////////////////////////////// @@ -211,513 +211,513 @@ bool_t nav_survey_ComputeProjectionOnLine(struct point pointAf,struct point poi bool_t nav_survey_ComputeProjectionOnLine(struct point pointAf,struct point pointBf,float pos_xf,float pos_yf,float *normAMf,float *normBMf,float *distancefromrailf) //return if the projection of the estimator on the AB line is located inside the AB interval { - float a,b,c,xa,xb,xc,ya,yb,yc; - float f; - float AA1; - float BB1; - float YP; - float XP; + float a,b,c,xa,xb,xc,ya,yb,yc; + float f; + float AA1; + float BB1; + float YP; + float XP; - float AMx,AMy,BMx,BMy; - //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!! + float AMx,AMy,BMx,BMy; + //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!! - xb=pointAf.x; - yb=pointAf.y; + xb=pointAf.x; + yb=pointAf.y; - xc=pointBf.x; - yc=pointBf.y; + xc=pointBf.x; + yc=pointBf.y; - xa=pos_xf; - ya=pos_yf; + xa=pos_xf; + ya=pos_yf; - //calcul des parametres de la droite pointAf pointBf - a = yc - yb; - b = xb - xc; - c = (yb - yc) * xb + (xc - xb) * yb ; + //calcul des parametres de la droite pointAf pointBf + a = yc - yb; + b = xb - xc; + c = (yb - yc) * xb + (xc - xb) * yb ; - //calcul de la distance de la droite à l'avion + //calcul de la distance de la droite à l'avion - if (fabs(a)>1e-10) - *distancefromrailf = fabs((a * xa + b * ya + c) / sqrt(a * a + b * b)); //denominateur =0 iniquement si a=b=0 //peut arriver si 2 waypoints sont confondus - else - return 0; + if (fabs(a)>1e-10) + *distancefromrailf = fabs((a * xa + b * ya + c) / sqrt(a * a + b * b)); //denominateur =0 iniquement si a=b=0 //peut arriver si 2 waypoints sont confondus + else + return 0; - PRTDEB(f,a) - PRTDEB(f,b) - PRTDEB(f,c) - PRTDEB(f,*distancefromrailf) + PRTDEB(f,a) + PRTDEB(f,b) + PRTDEB(f,c) + PRTDEB(f,*distancefromrailf) - // calcul des coordonnées du projeté orthogonal M(xx,y) de A sur (BC) - AA1 = (xc - xb); - BB1 = (yc - yb); - if (fabs(AA1)>1e-10) - { - f=(b - (a * BB1 / AA1)); - if (fabs(f)>1e-10) - YP = (-(a * xa) - (a * BB1 * ya / AA1) - c) / f; - else - return 0; - } - else - return 0; + // calcul des coordonnées du projeté orthogonal M(xx,y) de A sur (BC) + AA1 = (xc - xb); + BB1 = (yc - yb); + if (fabs(AA1)>1e-10) + { + f=(b - (a * BB1 / AA1)); + if (fabs(f)>1e-10) + YP = (-(a * xa) - (a * BB1 * ya / AA1) - c) / f; + else + return 0; + } + else + return 0; - XP = (-c - b * YP) / a ; //a !=0 deja testé avant - //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!! - //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!! - //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!! + XP = (-c - b * YP) / a ; //a !=0 deja testé avant + //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!! + //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!! + //+++++++++++++++++++++++++ATTENTION AUX DIVISIONS PAR 0!!!!!!!!!!!!!!! - PRTDEB(f,AA1) - PRTDEB(f,BB1) - PRTDEB(f,YP) - PRTDEB(f,XP) + PRTDEB(f,AA1) + PRTDEB(f,BB1) + PRTDEB(f,YP) + PRTDEB(f,XP) - AMx=XP-pointAf.x; - AMy=YP-pointAf.y; - BMx=XP-pointBf.x; - BMy=YP-pointBf.y; + AMx=XP-pointAf.x; + AMy=YP-pointAf.y; + BMx=XP-pointBf.x; + BMy=YP-pointBf.y; - *normAMf=NORMXY(AMx,AMy); - *normBMf=NORMXY(BMx,BMy); + *normAMf=NORMXY(AMx,AMy); + *normBMf=NORMXY(BMx,BMy); - PRTDEB(f,*normAMf) - PRTDEB(f,*normBMf) + PRTDEB(f,*normAMf) + PRTDEB(f,*normBMf) - if ( ( (*normAMf) + (*normBMf) ) >1.05*DISTXY(pointBf.x,pointBf.y,pointAf.x,pointAf.y)) - { - PRTDEBSTR(NOT INSIDE) - return 0; - } - else - { - PRTDEBSTR(INSIDE) - return 1; - } + if ( ( (*normAMf) + (*normBMf) ) >1.05*DISTXY(pointBf.x,pointBf.y,pointAf.x,pointAf.y)) + { + PRTDEBSTR(NOT INSIDE) + return 0; + } + else + { + PRTDEBSTR(INSIDE) + return 1; + } } /////////////////////////////////////////////////////////////////////////// //if distrailinit = 0, the aircraft travel from wp1 -> wp2 then do the inverse travel passing through the wp3, //This mode could be use to register bands of images aquired in a first nav_survey_losange_carto, done perpendicularly bool_t nav_survey_losange_carto_init(uint8_t wp1, uint8_t wp2, uint8_t wp3, float distrailinit, float distplusinit) { - //PRTDEBSTR(nav_survey_losange_carto_init) - survey_losange_uturn=FALSE; + //PRTDEBSTR(nav_survey_losange_carto_init) + survey_losange_uturn=FALSE; - point1.x=waypoints[wp1].x; //the coordinates are in meter units, taken from the flight plan, in float type - point1.y=waypoints[wp1].y; - point2.x=waypoints[wp2].x; - point2.y=waypoints[wp2].y; - point3.x=waypoints[wp3].x; - point3.y=waypoints[wp3].y; + point1.x=waypoints[wp1].x; //the coordinates are in meter units, taken from the flight plan, in float type + point1.y=waypoints[wp1].y; + point2.x=waypoints[wp2].x; + point2.y=waypoints[wp2].y; + point3.x=waypoints[wp3].x; + point3.y=waypoints[wp3].y; - PRTDEB(u,wp1) - PRTDEB(f,point1.x) - PRTDEB(f,point1.y) + PRTDEB(u,wp1) + PRTDEB(f,point1.x) + PRTDEB(f,point1.y) - PRTDEB(u,wp2) - PRTDEB(f,point2.x) - PRTDEB(f,point2.y) + PRTDEB(u,wp2) + PRTDEB(f,point2.x) + PRTDEB(f,point2.y) - PRTDEB(u,wp3) - PRTDEB(f,point3.x) - PRTDEB(f,point3.y) + PRTDEB(u,wp3) + PRTDEB(f,point3.x) + PRTDEB(f,point3.y) - vec12.x=point2.x-point1.x; - vec12.y=point2.y-point1.y; - PRTDEB(f,vec12.x) - PRTDEB(f,vec12.y) + vec12.x=point2.x-point1.x; + vec12.y=point2.y-point1.y; + PRTDEB(f,vec12.x) + PRTDEB(f,vec12.y) - //TODO gerer le cas ou un golio met les points à la meme position -> norm=0 > /0 - norm12=NORMXY(vec12.x,vec12.y); + //TODO gerer le cas ou un golio met les points à la meme position -> norm=0 > /0 + norm12=NORMXY(vec12.x,vec12.y); - PRTDEB(f,norm12) + PRTDEB(f,norm12) - vec13.x=point3.x-point1.x; - vec13.y=point3.y-point1.y; - PRTDEB(f,vec13.x) - PRTDEB(f,vec13.y) + vec13.x=point3.x-point1.x; + vec13.y=point3.y-point1.y; + PRTDEB(f,vec13.x) + PRTDEB(f,vec13.y) - norm13=NORMXY(vec13.x,vec13.y); - PRTDEB(f,norm13) + norm13=NORMXY(vec13.x,vec13.y); + PRTDEB(f,norm13) - //if (distrail<1e-15) //inutile distrail=0 pour recollage et dans ce cas, il prend la valeur norm13 - // return FALSE; + //if (distrail<1e-15) //inutile distrail=0 pour recollage et dans ce cas, il prend la valeur norm13 + // return FALSE; - if (fabs(distrailinit)<=1) - { //is distrailinit==0, then the aircraft should do 2 passes to register the bands - distrail=norm13; - numberofrailtodo=1; - } - else - {//no, so normal trajectory - distrail=fabs(distrailinit); - numberofrailtodo=ceil( norm13 / distrail);//round to the upper integer - } + if (fabs(distrailinit)<=1) + { //is distrailinit==0, then the aircraft should do 2 passes to register the bands + distrail=norm13; + numberofrailtodo=1; + } + else + {//no, so normal trajectory + distrail=fabs(distrailinit); + numberofrailtodo=ceil( norm13 / distrail);//round to the upper integer + } - distplus=fabs(distplusinit); + distplus=fabs(distplusinit); - PRTDEB(f,distrail) - PRTDEB(f,distplus) - PRTDEB(d,numberofrailtodo) - PRTDEB(d,railnumber) - PRTDEB(d,railnumberSinceBoot) + PRTDEB(f,distrail) + PRTDEB(f,distplus) + PRTDEB(d,numberofrailtodo) + PRTDEB(d,railnumber) + PRTDEB(d,railnumberSinceBoot) - railnumber=-1; // the state is before the first rail, which is numbered 0 + railnumber=-1; // the state is before the first rail, which is numbered 0 - if (norm12<1e-15) - return FALSE; - if (norm13<1e-15) - return FALSE; + if (norm12<1e-15) + return FALSE; + if (norm13<1e-15) + return FALSE; - angle1213=(180/3.14159) * acos( ( ((vec12.x*vec13.x ) + (vec12.y*vec13.y) ))/(norm12*norm13));//oriented angle between 12 and 13 vectors + angle1213=(180/3.14159) * acos( ( ((vec12.x*vec13.x ) + (vec12.y*vec13.y) ))/(norm12*norm13));//oriented angle between 12 and 13 vectors - angle1213 = atan2f(vec13.y, vec13.x) - atan2f(vec12.y,vec12.x); - while ( angle1213 >= M_PI ) angle1213 -= 2*M_PI; - while ( angle1213 <= -M_PI ) angle1213 += 2*M_PI; + angle1213 = atan2f(vec13.y, vec13.x) - atan2f(vec12.y,vec12.x); + while ( angle1213 >= M_PI ) angle1213 -= 2*M_PI; + while ( angle1213 <= -M_PI ) angle1213 += 2*M_PI; - PRTDEB(f,angle1213) + PRTDEB(f,angle1213) - if (angle1213 >0) - signforturn=-1; - else - signforturn=1; + if (angle1213 >0) + signforturn=-1; + else + signforturn=1; - return FALSE; //Init function must return false, so that the next function in the flight plan is automatically executed - //dans le flight_plan.h - // if (! (nav_survey_losange_carto())) - // NextStageAndBreak(); + return FALSE; //Init function must return false, so that the next function in the flight plan is automatically executed + //dans le flight_plan.h + // if (! (nav_survey_losange_carto())) + // NextStageAndBreak(); } //////////////////////////////////////////////////////////////////////////////////////////////// bool_t nav_survey_losange_carto(void) { - //test pour modifier en vol la valeur distrail + //test pour modifier en vol la valeur distrail - //distrail=distrailinteractif; + //distrail=distrailinteractif; - //by default, a 0 is sent in the message DOWNLINK_SEND_CAMERA_SNAPSHOT, - //if the aircraft is inside the region to map, camera_snapshot_image_number will be equal to the number of rail since the last boot (not since the nav_survey_losange_carto_init, in order to get different values for differents calls to the cartography function (this number is used to name the images on the hard drive - camera_snapshot_image_number=0; + //by default, a 0 is sent in the message DOWNLINK_SEND_CAMERA_SNAPSHOT, + //if the aircraft is inside the region to map, camera_snapshot_image_number will be equal to the number of rail since the last boot (not since the nav_survey_losange_carto_init, in order to get different values for differents calls to the cartography function (this number is used to name the images on the hard drive + camera_snapshot_image_number=0; - PRTDEB(f,distrail) + PRTDEB(f,distrail) - PRTDEBSTR(nav_survey_losange_carto) - PRTDEB(d,railnumber) + PRTDEBSTR(nav_survey_losange_carto) + PRTDEB(d,railnumber) - PRTDEB(d,railnumberSinceBoot) + PRTDEB(d,railnumberSinceBoot) - //PRTDEB(f,stateGetPositionEnu_f()->x) - //PRTDEB(f,stateGetPositionEnu_f()->y) + //PRTDEB(f,stateGetPositionEnu_f()->x) + //PRTDEB(f,stateGetPositionEnu_f()->y) - //sortir du bloc si données abhérantes - if (norm13<1e-15) - { - PRTDEBSTR(norm13<1e-15) - return FALSE; - } - if (norm12<1e-15) - { - PRTDEBSTR(norm13<1e-15) - return FALSE; - } - if (distrail<1e-15) - { - PRTDEBSTR(distrail<1e-15) - return FALSE; - } + //sortir du bloc si données abhérantes + if (norm13<1e-15) + { + PRTDEBSTR(norm13<1e-15) + return FALSE; + } + if (norm12<1e-15) + { + PRTDEBSTR(norm13<1e-15) + return FALSE; + } + if (distrail<1e-15) + { + PRTDEBSTR(distrail<1e-15) + return FALSE; + } - if (survey_losange_uturn==FALSE) - { + if (survey_losange_uturn==FALSE) + { - if (railnumber==-1) - { //se diriger vers le début du 1°rail - PRTDEBSTR(approche debut rail 0) - pointA.x=point1.x-(vec12.x/norm12)*distplus*1.2; //on prend une marge plus grande pour arriver en ce point - pointA.y=point1.y-(vec12.y/norm12)*distplus*1.2; //car le virage n'est pas tres bien géré + if (railnumber==-1) + { //se diriger vers le début du 1°rail + PRTDEBSTR(approche debut rail 0) + pointA.x=point1.x-(vec12.x/norm12)*distplus*1.2; //on prend une marge plus grande pour arriver en ce point + pointA.y=point1.y-(vec12.y/norm12)*distplus*1.2; //car le virage n'est pas tres bien géré - pointB.x=point2.x+(vec12.x/norm12)*distplus*1.2; //on prend une marge plus grande pour arriver en ce point - pointB.y=point2.y+(vec12.y/norm12)*distplus*1.2; //car le virage n'est pas tres bien géré + pointB.x=point2.x+(vec12.x/norm12)*distplus*1.2; //on prend une marge plus grande pour arriver en ce point + pointB.y=point2.y+(vec12.y/norm12)*distplus*1.2; //car le virage n'est pas tres bien géré - //PRTDEB(f,pointA.x) - //PRTDEB(f,pointA.y) + //PRTDEB(f,pointA.x) + //PRTDEB(f,pointA.y) - //the following test can cause problem when the aircraft is quite close to the entry point, as it can turn around for infinte time - //if ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointA.x,pointA.y) >DISTLIMIT) - //if ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointA.x,pointA.y) >DISTLIMIT) + //the following test can cause problem when the aircraft is quite close to the entry point, as it can turn around for infinte time + //if ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointA.x,pointA.y) >DISTLIMIT) + //if ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointA.x,pointA.y) >DISTLIMIT) - nav_survey_ComputeProjectionOnLine(pointA,pointB,stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,&normAM,&normBM,&distancefromrail); + nav_survey_ComputeProjectionOnLine(pointA,pointB,stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,&normAM,&normBM,&distancefromrail); - if ((DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointA.x,pointA.y) >2* DISTLIMIT) || (normBM<(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y)))) - { - nav_route_xy(stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y,pointA.x,pointA.y); - //nav_route_xy(pointB.x, pointB.y,pointA.x,pointA.y); - } - else - { - PRTDEBSTR(debut rail 0) - //un fois arrivé, on commence le 1° rail; - railnumber=0; - railnumberSinceBoot++; + if ((DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointA.x,pointA.y) >2* DISTLIMIT) || (normBM<(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y)))) + { + nav_route_xy(stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y,pointA.x,pointA.y); + //nav_route_xy(pointB.x, pointB.y,pointA.x,pointA.y); + } + else + { + PRTDEBSTR(debut rail 0) + //un fois arrivé, on commence le 1° rail; + railnumber=0; + railnumberSinceBoot++; - } - } + } + } - if (railnumber>=0) - { - pointA.x=(point1.x - ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail); - pointA.y=(point1.y - ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail); + if (railnumber>=0) + { + pointA.x=(point1.x - ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail); + pointA.y=(point1.y - ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail); - pointB.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail); - pointB.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail); + pointB.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail); + pointB.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail); - if ((railnumber %2)==0) //rail n0, 2, 4, donc premiere direction, de wp1 vers wp2 - { - //rien a faire - } - else //if ((railnumber %2)==1) //rail n1, 3, 5, donc seconde direction, de wp2 vers wp1 - { - //echange pointA et B - tempx=pointA.x; - tempy=pointA.y; - pointA.x=pointB.x; - pointA.y=pointB.y; - pointB.x=tempx; - pointB.y=tempy; + if ((railnumber %2)==0) //rail n0, 2, 4, donc premiere direction, de wp1 vers wp2 + { + //rien a faire + } + else //if ((railnumber %2)==1) //rail n1, 3, 5, donc seconde direction, de wp2 vers wp1 + { + //echange pointA et B + tempx=pointA.x; + tempy=pointA.y; + pointA.x=pointB.x; + pointA.y=pointB.y; + pointB.x=tempx; + pointB.y=tempy; - } + } - // PRTDEB(f,pointA.x) - // PRTDEB(f,pointA.y) - // PRTDEB(f,pointB.x) - // PRTDEB(f,pointB.y) - ProjectionInsideLimitOfRail=nav_survey_ComputeProjectionOnLine(pointA,pointB,stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,&normAM,&normBM,&distancefromrail); + // PRTDEB(f,pointA.x) + // PRTDEB(f,pointA.y) + // PRTDEB(f,pointB.x) + // PRTDEB(f,pointB.y) + ProjectionInsideLimitOfRail=nav_survey_ComputeProjectionOnLine(pointA,pointB,stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,&normAM,&normBM,&distancefromrail); - // if ( ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) >DISTLIMIT) && - // (normBM>(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y)))) + // if ( ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) >DISTLIMIT) && + // (normBM>(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y)))) - if (! ( ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) (DISTXY(pointB.x,pointB.y,pointA.x,pointA.y)))))) - // (normBM>(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y)))) - { - nav_route_xy(pointA.x,pointA.y,pointB.x,pointB.y); - PRTDEBSTR(NAVROUTE) - - - //est ce que l'avion est dans la zone ou il doit prendre des images? - //DEJA APPELE AVANT LE IF - // nav_survey_ComputeProjectionOnLine(pointA,pointB,stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,&normAM,&normBM,&distancefromrail); - - if ( (normAM> distplus) && (normBM> distplus) && (distancefromrailx,stateGetPositionEnu_f()->y,pointB.x,pointB.y) (DISTXY(pointB.x,pointB.y,pointA.x,pointA.y)))))) + // (normBM>(DISTXY(pointB.x,pointB.y,pointA.x,pointA.y)))) + { + nav_route_xy(pointA.x,pointA.y,pointB.x,pointB.y); + PRTDEBSTR(NAVROUTE) + + + //est ce que l'avion est dans la zone ou il doit prendre des images? + //DEJA APPELE AVANT LE IF + // nav_survey_ComputeProjectionOnLine(pointA,pointB,stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,&normAM,&normBM,&distancefromrail); + + if ( (normAM> distplus) && (normBM> distplus) && (distancefromrailnumberofrailtodo) - { - PRTDEBSTR(fin nav_survey_losange_carto) - return FALSE; //apparament, l'avion va au bloc suivant lorsque la fonction renvoie false - } - - } - } - else // (survey_losange_uturn==TRUE) - { - - - if (distrail<200) - { - //tourne autour d'un point à mi chemin entre les 2 rails - - //attention railnumber a été incrémenté en fin du rail précédent - - if ((railnumber %2)==1) //rail précédent n0, 2, 4, donc premiere direction, de wp1 vers wp2 - { - PRTDEBSTR(UTURN-IMPAIR) - //fin du rail précédent - pointA.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber-1)*(vec13.x/norm13)* distrail); - pointA.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber-1)*(vec13.y/norm13)* distrail); - //début du rail suivant - pointB.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail); - pointB.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail); - //milieu - waypoints[0].x=(pointA.x+pointB.x)/2; - waypoints[0].y=(pointA.y+pointB.y)/2; - - tempcircleradius=distrail/2; - if(tempcircleradius M_PI) angle_between -= 2 * M_PI; - while (angle_between < -M_PI) angle_between += 2 * M_PI; - - angle_between= DegOfRad(angle_between); - PRTDEB(f,angle_between ) - //if (angle_between> -10 && angle_between< 10) PRTDEBSTR(ON SE CASSE) - - NavCircleWaypoint(0,signforturn*tempcircleradius); - if ( ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) -10 && angle_between< 10) ) - { - //l'avion fait le rail suivant - survey_losange_uturn=FALSE; - PRTDEBSTR(FIN UTURN-IMPAIR) - } - } - else //if ((railnumber %2)==0) //rail précédent n1, 3, 5, donc seconde direction, de wp2 vers wp1 - { - PRTDEBSTR(UTURN-PAIR) - //fin du rail précédent - pointA.x=(point1.x - ((vec12.x/norm12)*distplus) ) + ((railnumber-1)*(vec13.x/norm13)* distrail); - pointA.y=(point1.y - ((vec12.y/norm12)*distplus) ) + ((railnumber-1)*(vec13.y/norm13)* distrail); - //début du rail suivant - pointB.x=(point1.x - ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail); - pointB.y=(point1.y - ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail); - //milieu - waypoints[0].x=(pointA.x+pointB.x)/2; - waypoints[0].y=(pointA.y+pointB.y)/2; - - tempcircleradius=distrail/2; - if(tempcircleradius M_PI) angle_between -= 2 * M_PI; - while (angle_between < -M_PI) angle_between += 2 * M_PI; - - angle_between= DegOfRad(angle_between); - PRTDEB(f,angle_between ) - //if (angle_between> -10 && angle_between< 10) PRTDEBSTR(ON SE CASSE) - - NavCircleWaypoint(0,signforturn*(-1)*tempcircleradius); - if (( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) -10 && angle_between< 10) ) - { - //l'avion fait le rail suivant - survey_losange_uturn=FALSE; - PRTDEBSTR(FIN UTURN-PAIR) - } - } - } - else - { //Le virage serait trop grand, on va en ligne droite pour ne pas trop éloigner l'avion - - if ((railnumber %2)==1) //rail précédent n0, 2, 4, donc premiere direction, de wp1 vers wp2 - { - PRTDEBSTR(TRANSIT-IMPAIR) - //fin du rail précédent - pointA.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber-1)*(vec13.x/norm13)* distrail); - pointA.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber-1)*(vec13.y/norm13)* distrail); - //début du rail suivant - pointB.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail); - pointB.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail); - nav_route_xy(pointA.x,pointA.y,pointB.x,pointB.y); - if ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) numberofrailtodo) + { + PRTDEBSTR(fin nav_survey_losange_carto) + return FALSE; //apparament, l'avion va au bloc suivant lorsque la fonction renvoie false + } + + } + } + else // (survey_losange_uturn==TRUE) + { + + + if (distrail<200) + { + //tourne autour d'un point à mi chemin entre les 2 rails + + //attention railnumber a été incrémenté en fin du rail précédent + + if ((railnumber %2)==1) //rail précédent n0, 2, 4, donc premiere direction, de wp1 vers wp2 + { + PRTDEBSTR(UTURN-IMPAIR) + //fin du rail précédent + pointA.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber-1)*(vec13.x/norm13)* distrail); + pointA.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber-1)*(vec13.y/norm13)* distrail); + //début du rail suivant + pointB.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail); + pointB.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail); + //milieu + waypoints[0].x=(pointA.x+pointB.x)/2; + waypoints[0].y=(pointA.y+pointB.y)/2; + + tempcircleradius=distrail/2; + if(tempcircleradius M_PI) angle_between -= 2 * M_PI; + while (angle_between < -M_PI) angle_between += 2 * M_PI; + + angle_between= DegOfRad(angle_between); + PRTDEB(f,angle_between ) + //if (angle_between> -10 && angle_between< 10) PRTDEBSTR(ON SE CASSE) + + NavCircleWaypoint(0,signforturn*tempcircleradius); + if ( ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) -10 && angle_between< 10) ) + { + //l'avion fait le rail suivant + survey_losange_uturn=FALSE; + PRTDEBSTR(FIN UTURN-IMPAIR) + } + } + else //if ((railnumber %2)==0) //rail précédent n1, 3, 5, donc seconde direction, de wp2 vers wp1 + { + PRTDEBSTR(UTURN-PAIR) + //fin du rail précédent + pointA.x=(point1.x - ((vec12.x/norm12)*distplus) ) + ((railnumber-1)*(vec13.x/norm13)* distrail); + pointA.y=(point1.y - ((vec12.y/norm12)*distplus) ) + ((railnumber-1)*(vec13.y/norm13)* distrail); + //début du rail suivant + pointB.x=(point1.x - ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail); + pointB.y=(point1.y - ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail); + //milieu + waypoints[0].x=(pointA.x+pointB.x)/2; + waypoints[0].y=(pointA.y+pointB.y)/2; + + tempcircleradius=distrail/2; + if(tempcircleradius M_PI) angle_between -= 2 * M_PI; + while (angle_between < -M_PI) angle_between += 2 * M_PI; + + angle_between= DegOfRad(angle_between); + PRTDEB(f,angle_between ) + //if (angle_between> -10 && angle_between< 10) PRTDEBSTR(ON SE CASSE) + + NavCircleWaypoint(0,signforturn*(-1)*tempcircleradius); + if (( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) -10 && angle_between< 10) ) + { + //l'avion fait le rail suivant + survey_losange_uturn=FALSE; + PRTDEBSTR(FIN UTURN-PAIR) + } + } + } + else + { //Le virage serait trop grand, on va en ligne droite pour ne pas trop éloigner l'avion + + if ((railnumber %2)==1) //rail précédent n0, 2, 4, donc premiere direction, de wp1 vers wp2 + { + PRTDEBSTR(TRANSIT-IMPAIR) + //fin du rail précédent + pointA.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber-1)*(vec13.x/norm13)* distrail); + pointA.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber-1)*(vec13.y/norm13)* distrail); + //début du rail suivant + pointB.x=(point2.x + ((vec12.x/norm12)*distplus) ) + ((railnumber)*(vec13.x/norm13)* distrail); + pointB.y=(point2.y + ((vec12.y/norm12)*distplus) ) + ((railnumber)*(vec13.y/norm13)* distrail); + nav_route_xy(pointA.x,pointA.y,pointB.x,pointB.y); + if ( DISTXY(stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) x,stateGetPositionEnu_f()->y,pointB.x,pointB.y) true + return TRUE; //apparament pour les fonctions de tache=> true } //////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/sw/airborne/modules/enose/enose.c b/sw/airborne/modules/enose/enose.c index 26fe69eb06..d5828070ca 100644 --- a/sw/airborne/modules/enose/enose.c +++ b/sw/airborne/modules/enose/enose.c @@ -74,13 +74,13 @@ void enose_periodic( void ) { else if (enose_status == ENOSE_MEASURING_RD) { uint16_t val = (i2c0_buf[0]<<8) | i2c0_buf[1]; if (val < 5000) - enose_val[0] = val; + enose_val[0] = val; val = (i2c0_buf[2]<<8) | i2c0_buf[3]; if (val < 5000) - enose_val[1] = val; + enose_val[1] = val; val = (i2c0_buf[4]<<8) | i2c0_buf[5]; if (val < 5000) - enose_val[2] = val; + enose_val[2] = val; enose_status = ENOSE_IDLE; } } diff --git a/sw/airborne/modules/ins/ins_xsens700.c b/sw/airborne/modules/ins/ins_xsens700.c index 70354bbea5..f8965ee6e6 100644 --- a/sw/airborne/modules/ins/ins_xsens700.c +++ b/sw/airborne/modules/ins/ins_xsens700.c @@ -200,63 +200,63 @@ void ins_periodic_task( void ) { { switch (xsens_configured) { - case 25: - /* send mode and settings to MT */ - XSENS_GoToConfig(); - //XSENS_SetOutputMode(xsens_output_mode); - //XSENS_SetOutputSettings(xsens_output_settings); - break; - case 18: - // Give pulses on SyncOut - //XSENS_SetSyncOutSettings(0,0x0002); - break; - case 17: + case 25: + /* send mode and settings to MT */ + XSENS_GoToConfig(); + //XSENS_SetOutputMode(xsens_output_mode); + //XSENS_SetOutputSettings(xsens_output_settings); + break; + case 18: + // Give pulses on SyncOut + //XSENS_SetSyncOutSettings(0,0x0002); + break; + case 17: - XsensHeader(XSENS_SetOutputConfiguration_ID, 44); - xsens_ask_message_rate( 0x10, 0x10, 4); // UTC - xsens_ask_message_rate( 0x20, 0x30, 100); // Attitude Euler - xsens_ask_message_rate( 0x40, 0x10, 100); // Delta-V - xsens_ask_message_rate( 0x80, 0x20, 100); // Rate of turn - xsens_ask_message_rate( 0xE0, 0x20, 50); // Status - xsens_ask_message_rate( 0x30, 0x10, 50); // Baro Pressure - xsens_ask_message_rate( 0x88, 0x40, 1); // NavSol - xsens_ask_message_rate( 0x88, 0xA0, 1); // SV Info - xsens_ask_message_rate( 0x50, 0x20, 50); // GPS Altitude Ellipsiod - xsens_ask_message_rate( 0x50, 0x40, 50); // GPS Position - xsens_ask_message_rate( 0xD0, 0x10, 50); // GPS Speed - XsensTrailer(); - break; - case 2: - //XSENS_ReqLeverArmGps(); - break; + XsensHeader(XSENS_SetOutputConfiguration_ID, 44); + xsens_ask_message_rate( 0x10, 0x10, 4); // UTC + xsens_ask_message_rate( 0x20, 0x30, 100); // Attitude Euler + xsens_ask_message_rate( 0x40, 0x10, 100); // Delta-V + xsens_ask_message_rate( 0x80, 0x20, 100); // Rate of turn + xsens_ask_message_rate( 0xE0, 0x20, 50); // Status + xsens_ask_message_rate( 0x30, 0x10, 50); // Baro Pressure + xsens_ask_message_rate( 0x88, 0x40, 1); // NavSol + xsens_ask_message_rate( 0x88, 0xA0, 1); // SV Info + xsens_ask_message_rate( 0x50, 0x20, 50); // GPS Altitude Ellipsiod + xsens_ask_message_rate( 0x50, 0x40, 50); // GPS Position + xsens_ask_message_rate( 0xD0, 0x10, 50); // GPS Speed + XsensTrailer(); + break; + case 2: + //XSENS_ReqLeverArmGps(); + break; - case 6: - //XSENS_ReqMagneticDeclination(); - break; + case 6: + //XSENS_ReqMagneticDeclination(); + break; - case 13: - //#ifdef AHRS_H_X - //#pragma message "Sending XSens Magnetic Declination." - //xsens_declination = atan2(AHRS_H_Y, AHRS_H_X); - //XSENS_SetMagneticDeclination(xsens_declination); - //#endif - break; - case 12: - #ifdef GPS_IMU_LEVER_ARM_X - #pragma message "Sending XSens GPS Arm." - XSENS_SetLeverArmGps(GPS_IMU_LEVER_ARM_X,GPS_IMU_LEVER_ARM_Y,GPS_IMU_LEVER_ARM_Z); - #endif - break; - case 10: - { - uint8_t baud = 1; - XSENS_SetBaudrate(baud); - } - break; + case 13: + //#ifdef AHRS_H_X + //#pragma message "Sending XSens Magnetic Declination." + //xsens_declination = atan2(AHRS_H_Y, AHRS_H_X); + //XSENS_SetMagneticDeclination(xsens_declination); + //#endif + break; + case 12: + #ifdef GPS_IMU_LEVER_ARM_X + #pragma message "Sending XSens GPS Arm." + XSENS_SetLeverArmGps(GPS_IMU_LEVER_ARM_X,GPS_IMU_LEVER_ARM_Y,GPS_IMU_LEVER_ARM_Z); + #endif + break; + case 10: + { + uint8_t baud = 1; + XSENS_SetBaudrate(baud); + } + break; - case 1: - XSENS_GoToMeasurment(); - break; + case 1: + XSENS_GoToMeasurment(); + break; } xsens_configured--; return; @@ -451,9 +451,9 @@ void parse_ins_msg( void ) { // Altitude Elipsoid gps.utm_pos.alt = XSENS_DATA_Altitude_h(xsens_msg_buf,offset)* 1000.0f; - // Compute geoid (MSL) height + // Compute geoid (MSL) height float geoid_h; - WGS84_ELLIPSOID_TO_GEOID(lla_f.lat,lla_f.lon,geoid_h); + WGS84_ELLIPSOID_TO_GEOID(lla_f.lat,lla_f.lon,geoid_h); gps.hmsl = gps.utm_pos.alt - (geoid_h * 1000.0f); //gps.tow = geoid_h * 1000.0f; //gps.utm_pos.alt; diff --git a/sw/airborne/modules/light/light.c b/sw/airborne/modules/light/light.c index 540cb5ebda..9a2a94a500 100644 --- a/sw/airborne/modules/light/light.c +++ b/sw/airborne/modules/light/light.c @@ -57,67 +57,67 @@ void periodic_light(void) switch (strobe_light_mode) { default: // Always off - LED_OFF(LIGHT_LED_STROBE); - break; + LED_OFF(LIGHT_LED_STROBE); + break; case 1: // Always on - LED_ON(LIGHT_LED_STROBE); - break; + LED_ON(LIGHT_LED_STROBE); + break; case 2: // Blink case 3: case 4: - if (counter == (strobe_light_mode*5 - 4)) - { - LED_OFF(LIGHT_LED_STROBE); - } - else if (counter >= 20) - { - LED_ON(LIGHT_LED_STROBE); - counter = 0; - } - break; + if (counter == (strobe_light_mode*5 - 4)) + { + LED_OFF(LIGHT_LED_STROBE); + } + else if (counter >= 20) + { + LED_ON(LIGHT_LED_STROBE); + counter = 0; + } + break; case 5: // Complex Blinking - if (counter == 3) - { - LED_OFF(LIGHT_LED_STROBE); - } - else if (counter == 4) - { - LED_ON(LIGHT_LED_STROBE); - } - else if (counter == 6) - { - LED_OFF(LIGHT_LED_STROBE); - } - else if (counter == 7) - { - LED_ON(LIGHT_LED_STROBE); - } - else if (counter == 8) - { - LED_OFF(LIGHT_LED_STROBE); - } - else if (counter >= 25) - { - LED_ON(LIGHT_LED_STROBE); - counter = 0; - } - break; + if (counter == 3) + { + LED_OFF(LIGHT_LED_STROBE); + } + else if (counter == 4) + { + LED_ON(LIGHT_LED_STROBE); + } + else if (counter == 6) + { + LED_OFF(LIGHT_LED_STROBE); + } + else if (counter == 7) + { + LED_ON(LIGHT_LED_STROBE); + } + else if (counter == 8) + { + LED_OFF(LIGHT_LED_STROBE); + } + else if (counter >= 25) + { + LED_ON(LIGHT_LED_STROBE); + counter = 0; + } + break; case 6: - if (counter <= 18) - { - if ((counter % 2) == 0) - { - LED_ON(LIGHT_LED_STROBE); - } + if (counter <= 18) + { + if ((counter % 2) == 0) + { + LED_ON(LIGHT_LED_STROBE); + } else - { - LED_OFF(LIGHT_LED_STROBE); - } - } - else if (counter == 35) - { - counter = 0; - } + { + LED_OFF(LIGHT_LED_STROBE); + } + } + else if (counter == 35) + { + counter = 0; + } break; } @@ -125,24 +125,24 @@ void periodic_light(void) switch (nav_light_mode) { default: // Always off - LED_OFF(LIGHT_LED_NAV); - break; + LED_OFF(LIGHT_LED_NAV); + break; case 1: // Always on - LED_ON(LIGHT_LED_NAV); - break; + LED_ON(LIGHT_LED_NAV); + break; case 2: // Blink case 3: case 4: - if (counter_nav == (nav_light_mode*5 - 4)) - { - LED_OFF(LIGHT_LED_NAV); - } - else if (counter_nav >= 20) - { - LED_ON(LIGHT_LED_NAV); - counter_nav = 0; - } - break; + if (counter_nav == (nav_light_mode*5 - 4)) + { + LED_OFF(LIGHT_LED_NAV); + } + else if (counter_nav >= 20) + { + LED_ON(LIGHT_LED_NAV); + counter_nav = 0; + } + break; } counter_nav++; #endif diff --git a/sw/airborne/modules/max7456/max7456_regs.h b/sw/airborne/modules/max7456/max7456_regs.h old mode 100755 new mode 100644 diff --git a/sw/airborne/modules/meteo/humid_pcap01.c b/sw/airborne/modules/meteo/humid_pcap01.c index e38e5de2a9..31815dd2c6 100644 --- a/sw/airborne/modules/meteo/humid_pcap01.c +++ b/sw/airborne/modules/meteo/humid_pcap01.c @@ -50,24 +50,24 @@ PCAP01VALUE pcap01Value; void writePCAP01_SRAM(uint8_t data, uint16_t s_add) { - while (pcap01_trans.status == I2CTransPending); + while (pcap01_trans.status == I2CTransPending); - pcap01_trans.buf[0] = 0x90+(unsigned char)(s_add>>8); - pcap01_trans.buf[1] = (unsigned char)(s_add); - pcap01_trans.buf[2] = data; - i2c_transmit(&PCAP01_I2C_DEV, &pcap01_trans, PCAP01_ADDR, 3); + pcap01_trans.buf[0] = 0x90+(unsigned char)(s_add>>8); + pcap01_trans.buf[1] = (unsigned char)(s_add); + pcap01_trans.buf[2] = data; + i2c_transmit(&PCAP01_I2C_DEV, &pcap01_trans, PCAP01_ADDR, 3); } uint8_t readPCAP01_SRAM(uint16_t s_add) { - while (pcap01_trans.status == I2CTransPending); + while (pcap01_trans.status == I2CTransPending); - pcap01_trans.buf[0] = 0x10+(unsigned char)(s_add>>8); - pcap01_trans.buf[1] = (unsigned char)(s_add); - i2c_transceive(&PCAP01_I2C_DEV, &pcap01_trans, PCAP01_ADDR, 2, 1); - while (pcap01_trans.status == I2CTransPending); + pcap01_trans.buf[0] = 0x10+(unsigned char)(s_add>>8); + pcap01_trans.buf[1] = (unsigned char)(s_add); + i2c_transceive(&PCAP01_I2C_DEV, &pcap01_trans, PCAP01_ADDR, 2, 1); + while (pcap01_trans.status == I2CTransPending); - return pcap01_trans.buf[0]; + return pcap01_trans.buf[0]; } /** * \brief PCAP01_Control @@ -84,110 +84,110 @@ uint8_t readPCAP01_SRAM(uint16_t s_add) */ void PCAP01_Control(uint8_t control) { - while (pcap01_trans.status == I2CTransPending); + while (pcap01_trans.status == I2CTransPending); - pcap01_trans.buf[0] = control; - pcap01_trans.buf[1] = 0; - pcap01_trans.buf[2] = 0; - pcap01_trans.buf[3] = 0; - i2c_transmit(&PCAP01_I2C_DEV, &pcap01_trans, PCAP01_ADDR, 4); + pcap01_trans.buf[0] = control; + pcap01_trans.buf[1] = 0; + pcap01_trans.buf[2] = 0; + pcap01_trans.buf[3] = 0; + i2c_transmit(&PCAP01_I2C_DEV, &pcap01_trans, PCAP01_ADDR, 4); } void pcap01writeRegister(uint8_t reg,uint32_t value) { - while (pcap01_trans.status == I2CTransPending); + while (pcap01_trans.status == I2CTransPending); - pcap01_trans.buf[0] = PCAP01_WRITE_REG + reg; - pcap01_trans.buf[1] = (unsigned char) (value>>16); - pcap01_trans.buf[2] = (unsigned char) (value>>8); - pcap01_trans.buf[3] = (unsigned char) (value); - i2c_transmit(&PCAP01_I2C_DEV, &pcap01_trans, PCAP01_ADDR, 4); + pcap01_trans.buf[0] = PCAP01_WRITE_REG + reg; + pcap01_trans.buf[1] = (unsigned char) (value>>16); + pcap01_trans.buf[2] = (unsigned char) (value>>8); + pcap01_trans.buf[3] = (unsigned char) (value); + i2c_transmit(&PCAP01_I2C_DEV, &pcap01_trans, PCAP01_ADDR, 4); } #ifdef PCAP01_LOAD_FIRMWARE void writePCAP01_firmware(void) { - int i = 0; - char testbyte = 44; - uint16_t testaddress = 71; - // Hard reset - PCAP01_Control(PCAP01_PU_RESET); - sys_time_usleep(50000); - //write testbyte - writePCAP01_SRAM(testbyte, testaddress); + int i = 0; + char testbyte = 44; + uint16_t testaddress = 71; + // Hard reset + PCAP01_Control(PCAP01_PU_RESET); + sys_time_usleep(50000); + //write testbyte + writePCAP01_SRAM(testbyte, testaddress); - //check testbyte - if (readPCAP01_SRAM(testaddress) != testbyte) - return; - else - { + //check testbyte + if (readPCAP01_SRAM(testaddress) != testbyte) + return; + else + { LED_ON(3); - //Hard reset - PCAP01_Control(PCAP01_PU_RESET); - //write firmware - for (i = 0;i< sizeof(firmware); i++) - { - writePCAP01_SRAM(firmware[i],i); - } - //fill with ffs - for (;i< 4029; i++) - { - writePCAP01_SRAM(0xff,i); - } - i++; + //Hard reset + PCAP01_Control(PCAP01_PU_RESET); + //write firmware + for (i = 0;i< sizeof(firmware); i++) + { + writePCAP01_SRAM(firmware[i],i); + } + //fill with ffs + for (;i< 4029; i++) + { + writePCAP01_SRAM(0xff,i); + } + i++; #ifdef PCAP01_200HZ - //write end bytes of sram - writePCAP01_SRAM(0x04,i++); - writePCAP01_SRAM(0x01,i++); - writePCAP01_SRAM(0x01,i++); + //write end bytes of sram + writePCAP01_SRAM(0x04,i++); + writePCAP01_SRAM(0x01,i++); + writePCAP01_SRAM(0x01,i++); #endif #ifdef PCAP01_STANDARD - //write end bytes of sram - writePCAP01_SRAM(0x02,i++); - writePCAP01_SRAM(0x01,i++); - writePCAP01_SRAM(0x03,i++); + //write end bytes of sram + writePCAP01_SRAM(0x02,i++); + writePCAP01_SRAM(0x01,i++); + writePCAP01_SRAM(0x03,i++); #endif - } + } } #endif // PCAP01_LOAD_FIRMWARE void pcap01_init(void) { - pcap01_trans.status = I2CTransDone; - pcap01Value.status = PCAP01_IDLE; + pcap01_trans.status = I2CTransDone; + pcap01Value.status = PCAP01_IDLE; #ifdef PCAP01_LOAD_FIRMWARE - writePCAP01_firmware(); + writePCAP01_firmware(); #endif - pcap01writeRegister(PCAP01_REG0, PCAP01_REG0_VALUE); - pcap01writeRegister(PCAP01_REG1, PCAP01_REG1_VALUE); - pcap01writeRegister(PCAP01_REG2, PCAP01_REG2_VALUE); - pcap01writeRegister(PCAP01_REG3, PCAP01_REG3_VALUE); - pcap01writeRegister(PCAP01_REG4, PCAP01_REG4_VALUE); - pcap01writeRegister(PCAP01_REG5, PCAP01_REG5_VALUE); - pcap01writeRegister(PCAP01_REG6, PCAP01_REG6_VALUE); - pcap01writeRegister(PCAP01_REG7, PCAP01_REG7_VALUE); - pcap01writeRegister(PCAP01_REG8, PCAP01_REG8_VALUE); - pcap01writeRegister(PCAP01_REG9, PCAP01_REG9_VALUE); - pcap01writeRegister(PCAP01_REG10, PCAP01_REG10_VALUE); - pcap01writeRegister(PCAP01_REG11, PCAP01_REG11_VALUE); - pcap01writeRegister(PCAP01_REG12, PCAP01_REG12_VALUE); - pcap01writeRegister(PCAP01_REG13, PCAP01_REG13_VALUE); - pcap01writeRegister(PCAP01_REG14, PCAP01_REG14_VALUE); - pcap01writeRegister(PCAP01_REG15, PCAP01_REG15_VALUE); - pcap01writeRegister(PCAP01_REG16, PCAP01_REG16_VALUE); - pcap01writeRegister(PCAP01_REG17, PCAP01_REG17_VALUE); - pcap01writeRegister(PCAP01_REG18, PCAP01_REG18_VALUE); - pcap01writeRegister(PCAP01_REG19, PCAP01_REG19_VALUE); - pcap01writeRegister(PCAP01_REG20, PCAP01_REG20_VALUE); - PCAP01_Control(PCAP01_IN_RESET); - sys_time_usleep(500000); - PCAP01_Control(PCAP01_START); + pcap01writeRegister(PCAP01_REG0, PCAP01_REG0_VALUE); + pcap01writeRegister(PCAP01_REG1, PCAP01_REG1_VALUE); + pcap01writeRegister(PCAP01_REG2, PCAP01_REG2_VALUE); + pcap01writeRegister(PCAP01_REG3, PCAP01_REG3_VALUE); + pcap01writeRegister(PCAP01_REG4, PCAP01_REG4_VALUE); + pcap01writeRegister(PCAP01_REG5, PCAP01_REG5_VALUE); + pcap01writeRegister(PCAP01_REG6, PCAP01_REG6_VALUE); + pcap01writeRegister(PCAP01_REG7, PCAP01_REG7_VALUE); + pcap01writeRegister(PCAP01_REG8, PCAP01_REG8_VALUE); + pcap01writeRegister(PCAP01_REG9, PCAP01_REG9_VALUE); + pcap01writeRegister(PCAP01_REG10, PCAP01_REG10_VALUE); + pcap01writeRegister(PCAP01_REG11, PCAP01_REG11_VALUE); + pcap01writeRegister(PCAP01_REG12, PCAP01_REG12_VALUE); + pcap01writeRegister(PCAP01_REG13, PCAP01_REG13_VALUE); + pcap01writeRegister(PCAP01_REG14, PCAP01_REG14_VALUE); + pcap01writeRegister(PCAP01_REG15, PCAP01_REG15_VALUE); + pcap01writeRegister(PCAP01_REG16, PCAP01_REG16_VALUE); + pcap01writeRegister(PCAP01_REG17, PCAP01_REG17_VALUE); + pcap01writeRegister(PCAP01_REG18, PCAP01_REG18_VALUE); + pcap01writeRegister(PCAP01_REG19, PCAP01_REG19_VALUE); + pcap01writeRegister(PCAP01_REG20, PCAP01_REG20_VALUE); + PCAP01_Control(PCAP01_IN_RESET); + sys_time_usleep(500000); + PCAP01_Control(PCAP01_START); } void pcap01readRegister(uint8_t reg) { - uint16_t byte1 = 0x40 | reg; - pcap01_trans.buf[0] = byte1; - i2c_transceive(&PCAP01_I2C_DEV, &pcap01_trans, PCAP01_ADDR, 1, 3); + uint16_t byte1 = 0x40 | reg; + pcap01_trans.buf[0] = byte1; + i2c_transceive(&PCAP01_I2C_DEV, &pcap01_trans, PCAP01_ADDR, 1, 3); } /** @@ -198,54 +198,54 @@ void pcap01readRegister(uint8_t reg) */ void pcap01_periodic(void) { - pcap01Value.status = PCAP01_GET_HUMID; + pcap01Value.status = PCAP01_GET_HUMID; #ifdef PCAP01_STANDARD - pcap01readRegister(PCAP01_REG1); + pcap01readRegister(PCAP01_REG1); #endif #ifdef PCAP01_200HZ - pcap01readRegister(PCAP01_REG2); + pcap01readRegister(PCAP01_REG2); #endif } void pcap01_event(void) { - float humidity; - float temperature; + float humidity; + float temperature; if (pcap01_trans.status == I2CTransSuccess) { switch (pcap01Value.status) { - case PCAP01_GET_HUMID: - pcap01Value.C_ratio = pcap01_trans.buf[0] << 16; - pcap01Value.C_ratio |= (pcap01_trans.buf[1] << 8); - pcap01Value.C_ratio |= pcap01_trans.buf[2]; - pcap01Value.status = PCAP01_GET_TEMP; + case PCAP01_GET_HUMID: + pcap01Value.C_ratio = pcap01_trans.buf[0] << 16; + pcap01Value.C_ratio |= (pcap01_trans.buf[1] << 8); + pcap01Value.C_ratio |= pcap01_trans.buf[2]; + pcap01Value.status = PCAP01_GET_TEMP; #ifdef PCAP01_STANDARD - pcap01readRegister(PCAP01_REG13); + pcap01readRegister(PCAP01_REG13); #endif #ifdef PCAP01_200HZ - pcap01readRegister(PCAP01_REG3); + pcap01readRegister(PCAP01_REG3); #endif - break; + break; - case PCAP01_GET_TEMP: - pcap01Value.R_ratio = pcap01_trans.buf[0] << 16; - pcap01Value.R_ratio |= (pcap01_trans.buf[1] << 8); - pcap01Value.R_ratio |= pcap01_trans.buf[2]; - humidity = pcap01Value.C_ratio * (-0.0023959245437) + 516.4124438673063; - temperature = pcap01Value.R_ratio * 61.927 - 259.74; - DOWNLINK_SEND_PCAP01_STATUS(DefaultChannel, DefaultDevice, - &pcap01Value.C_ratio, - &pcap01Value.R_ratio, - &humidity, - &temperature); - pcap01_trans.status = I2CTransDone; - pcap01Value.status = PCAP01_IDLE; - break; + case PCAP01_GET_TEMP: + pcap01Value.R_ratio = pcap01_trans.buf[0] << 16; + pcap01Value.R_ratio |= (pcap01_trans.buf[1] << 8); + pcap01Value.R_ratio |= pcap01_trans.buf[2]; + humidity = pcap01Value.C_ratio * (-0.0023959245437) + 516.4124438673063; + temperature = pcap01Value.R_ratio * 61.927 - 259.74; + DOWNLINK_SEND_PCAP01_STATUS(DefaultChannel, DefaultDevice, + &pcap01Value.C_ratio, + &pcap01Value.R_ratio, + &humidity, + &temperature); + pcap01_trans.status = I2CTransDone; + pcap01Value.status = PCAP01_IDLE; + break; - default: - pcap01_trans.status = I2CTransDone; - break; - } + default: + pcap01_trans.status = I2CTransDone; + break; + } } } diff --git a/sw/airborne/modules/meteo/humid_pcap01.h b/sw/airborne/modules/meteo/humid_pcap01.h index 1f6af61590..dcb80f1fa3 100644 --- a/sw/airborne/modules/meteo/humid_pcap01.h +++ b/sw/airborne/modules/meteo/humid_pcap01.h @@ -37,14 +37,14 @@ #define PCAP01_200HZ typedef struct { - uint32_t temp; - uint32_t hum_t; - uint32_t hum; - uint32_t R_ratio; - uint32_t C_ratio; - uint32_t NV_temp; - uint32_t V_rh; - uint32_t status; + uint32_t temp; + uint32_t hum_t; + uint32_t hum; + uint32_t R_ratio; + uint32_t C_ratio; + uint32_t NV_temp; + uint32_t V_rh; + uint32_t status; }PCAP01VALUE; #define PCAP01_ADDR 0xA0 diff --git a/sw/airborne/modules/poles/nav_poles.c b/sw/airborne/modules/poles/nav_poles.c index 6d9bb74e88..cd75107e77 100644 --- a/sw/airborne/modules/poles/nav_poles.c +++ b/sw/airborne/modules/poles/nav_poles.c @@ -11,7 +11,7 @@ int8_t nav_poles_land = 1; waypoints wp1 and wp2 */ bool nav_poles_init(uint8_t wp1, uint8_t wp2, uint8_t wp1c, uint8_t wp2c, - float radius) { + float radius) { float x = WaypointX(wp2) - WaypointX(wp1); float y = WaypointY(wp2) - WaypointY(wp1); float d = sqrt(x*x+y*y); diff --git a/sw/airborne/modules/poles/nav_poles.h b/sw/airborne/modules/poles/nav_poles.h index 186e490cba..c9f6ad3aa2 100644 --- a/sw/airborne/modules/poles/nav_poles.h +++ b/sw/airborne/modules/poles/nav_poles.h @@ -10,7 +10,7 @@ extern int8_t nav_poles_land; bool nav_poles_init(uint8_t wp1, uint8_t wp2, uint8_t wp1c, uint8_t wp2c, - float radius ); + float radius ); #define nav_poles_SetLandDir(_d) { if (_d < 0) _d = -1; else _d = 1; } diff --git a/sw/airborne/modules/sensors/airspeed_amsys.c b/sw/airborne/modules/sensors/airspeed_amsys.c index f2e4ef2e7f..0a52383c6e 100644 --- a/sw/airborne/modules/sensors/airspeed_amsys.c +++ b/sw/airborne/modules/sensors/airspeed_amsys.c @@ -107,12 +107,12 @@ void airspeed_amsys_read_periodic( void ) { } #if USE_AIRSPEED - stateSetAirspeed_f(&airspeed_amsys); + stateSetAirspeed_f(&airspeed_amsys); #endif #elif !defined USE_NPS - extern float sim_air_speed; - stateSetAirspeed_f(&sim_air_speed); + extern float sim_air_speed; + stateSetAirspeed_f(&sim_air_speed); #endif //SITL diff --git a/sw/airborne/modules/vehicle_interface/vi_datalink.h b/sw/airborne/modules/vehicle_interface/vi_datalink.h index a64ebfae4c..9363549ca6 100644 --- a/sw/airborne/modules/vehicle_interface/vi_datalink.h +++ b/sw/airborne/modules/vehicle_interface/vi_datalink.h @@ -59,17 +59,17 @@ extern void vi_update_wp(uint8_t wp_id); break; \ case GUIDANCE_H_MODE_ATTITUDE : \ { \ - vi.input.h_sp.attitude.phi = DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer); \ - vi.input.h_sp.attitude.theta = DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer); \ - vi.input.h_sp.attitude.psi = DL_BOOZ2_FMS_COMMAND_h_sp_3(_dl_buffer); \ - ANGLE_REF_NORMALIZE(vi.input.h_sp.attitude.psi); \ + vi.input.h_sp.attitude.phi = DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer); \ + vi.input.h_sp.attitude.theta = DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer); \ + vi.input.h_sp.attitude.psi = DL_BOOZ2_FMS_COMMAND_h_sp_3(_dl_buffer); \ + ANGLE_REF_NORMALIZE(vi.input.h_sp.attitude.psi); \ VI_LIMIT_ATTITUDE(vi.input.h_sp.attitude); \ } \ break; \ case GUIDANCE_H_MODE_HOVER : \ { \ - vi.input.h_sp.pos.x = DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer); \ - vi.input.h_sp.pos.y = DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer); \ + vi.input.h_sp.pos.x = DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer); \ + vi.input.h_sp.pos.y = DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer); \ } \ break; \ case GUIDANCE_H_MODE_NAV : \ diff --git a/sw/airborne/modules/vehicle_interface/vi_test_signal.c b/sw/airborne/modules/vehicle_interface/vi_test_signal.c index de29c7d25e..2bf57f4bca 100644 --- a/sw/airborne/modules/vehicle_interface/vi_test_signal.c +++ b/sw/airborne/modules/vehicle_interface/vi_test_signal.c @@ -70,8 +70,8 @@ void booz_fms_impl_periodic(void) { booz_fms_test_signal_start_z = ins_impl.ltp_pos.z; else { booz_fms_input.v_sp.height = (booz_fms_test_signal_counter < booz_fms_test_signal_period) ? - booz_fms_test_signal_start_z : - booz_fms_test_signal_start_z - 256; + booz_fms_test_signal_start_z : + booz_fms_test_signal_start_z - 256; //BOOZ_INT_OF_FLOAT(-0.5, IPOS_FRAC) } } diff --git a/sw/airborne/peripherals/ami601.h b/sw/airborne/peripherals/ami601.h index d6d9e58193..4bfc851d93 100644 --- a/sw/airborne/peripherals/ami601.h +++ b/sw/airborne/peripherals/ami601.h @@ -39,7 +39,7 @@ extern volatile uint32_t ami601_nb_err; switch (ami601_status) { \ case AMI601_SENDING_REQ : \ if ( ami601_i2c_trans.status == I2CTransSuccess ) { \ - ami601_status = AMI601_WAITING_MEASURE; \ + ami601_status = AMI601_WAITING_MEASURE; \ } \ break; \ case AMI601_WAITING_MEASURE : \ @@ -50,16 +50,16 @@ extern volatile uint32_t ami601_nb_err; break; \ case AMI601_READING_MEASURE : \ if ( ami601_i2c_trans.status == I2CTransSuccess ) { \ - ami601_foo1 = ami601_i2c_trans.buf[0]; /* AA ? */ \ - ami601_foo2 = ami601_i2c_trans.buf[1]; /* 55 ? */ \ - ami601_foo3 = ami601_i2c_trans.buf[2]; /* ERR ? */ \ - uint8_t i; \ - for (i=0; i< AMI601_NB_CHAN; i++) { \ - ami601_values[i] = ami601_i2c_trans.buf[3 + 2 * i]; \ - ami601_values[i] += ami601_i2c_trans.buf[3 + 2 * i + 1] * 256; \ - } \ - ami601_status = AMI601_DATA_AVAILABLE; \ - _handler(); \ + ami601_foo1 = ami601_i2c_trans.buf[0]; /* AA ? */ \ + ami601_foo2 = ami601_i2c_trans.buf[1]; /* 55 ? */ \ + ami601_foo3 = ami601_i2c_trans.buf[2]; /* ERR ? */ \ + uint8_t i; \ + for (i=0; i< AMI601_NB_CHAN; i++) { \ + ami601_values[i] = ami601_i2c_trans.buf[3 + 2 * i]; \ + ami601_values[i] += ami601_i2c_trans.buf[3 + 2 * i + 1] * 256; \ + } \ + ami601_status = AMI601_DATA_AVAILABLE; \ + _handler(); \ } \ break; \ default: \ diff --git a/sw/airborne/rc_settings.h b/sw/airborne/rc_settings.h index 3d10935226..901eb53edb 100644 --- a/sw/airborne/rc_settings.h +++ b/sw/airborne/rc_settings.h @@ -58,8 +58,8 @@ void rc_settings(bool_t mode_changed); #define RcSettingsOff() (rc_settings_mode==RC_SETTINGS_MODE_NONE) #define RC_SETTINGS_MODE_OF_PULSE(pprz) (pprz < THRESHOLD1 ? RC_SETTINGS_MODE_DOWN : \ - (pprz < THRESHOLD2 ? RC_SETTINGS_MODE_NONE : \ - RC_SETTINGS_MODE_UP)) + (pprz < THRESHOLD2 ? RC_SETTINGS_MODE_NONE : \ + RC_SETTINGS_MODE_UP)) #define RcSettingsModeUpdate(_rc_channels) \ ModeUpdate(rc_settings_mode, RC_SETTINGS_MODE_OF_PULSE(_rc_channels[RADIO_CALIB])) diff --git a/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.c b/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.c index d0ca6b723d..4d6f443085 100644 --- a/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.c +++ b/sw/airborne/subsystems/chibios-libopencm3/chibios_sdlog.c @@ -63,7 +63,7 @@ static void launchBatterySurveyThread (void) bool_t chibios_logInit(const bool_t binaryFile) { nvicSetSystemHandlerPriority(HANDLER_PENDSV, - CORTEX_PRIORITY_MASK(15)); + CORTEX_PRIORITY_MASK(15)); if (sdLogInit (NULL) != SDLOG_OK) goto error; @@ -112,7 +112,7 @@ static msg_t batterySurveyThd(void *arg) chThdSleepMilliseconds (2000); register_adc_watchdog((uint32_t) ADC1, 4, - V_ALERT, 0xfff, &powerOutageIsr); + V_ALERT, 0xfff, &powerOutageIsr); chEvtWaitOne(EVENT_MASK(1)); chibios_logFinish (); diff --git a/sw/airborne/subsystems/chibios-libopencm3/sdLog.c b/sw/airborne/subsystems/chibios-libopencm3/sdLog.c index f7545ed940..27d759bb94 100644 --- a/sw/airborne/subsystems/chibios-libopencm3/sdLog.c +++ b/sw/airborne/subsystems/chibios-libopencm3/sdLog.c @@ -38,7 +38,7 @@ static size_t logRawLen (const size_t len); static SdioError getNextFileName(const char* prefix, const char* directoryName, - char* nextFileName, const size_t nameLength); + char* nextFileName, const size_t nameLength); static uint32_t uiGetIndexOfLogFile (const char* prefix, const char* fileName) ; static msg_t thdSdLog(void *arg) ; static Thread *sdLogThd = NULL; @@ -198,7 +198,7 @@ SdioError sdLoglaunchThread (const bool_t binaryLog) chThdSleepMilliseconds(100); sdLogThd = chThdCreateStatic(waThdSdLog, sizeof(waThdSdLog), - NORMALPRIO, thdSdLog, (void *) binaryLog); + NORMALPRIO, thdSdLog, (void *) binaryLog); if (sdLogThd == NULL) return SDLOG_INTERNAL_ERROR; else @@ -239,7 +239,7 @@ SdioError sdLogStopThread (void) static SdioError getNextFileName(const char* prefix, const char* directoryName, - char* nextFileName, const size_t nameLength) + char* nextFileName, const size_t nameLength) { DIR dir; /* Directory object */ FRESULT rc; /* Result code */ @@ -290,7 +290,7 @@ static SdioError getNextFileName(const char* prefix, const char* directoryName, } chsnprintf (nextFileName, nameLength, "%s\\%s%.03d.LOG", - directoryName, prefix, maxCurrentIndex+1); + directoryName, prefix, maxCurrentIndex+1); return SDLOG_OK; } @@ -333,9 +333,9 @@ static msg_t thdSdLog(void *arg) if (curBufFill) { f_write(foSaved, perfBuffer, curBufFill, &bw); } - if (appendCloseLogMsg) { - f_write(foSaved, "\r\nEND_OF_LOG\r\n", 14, &bw); - } + if (appendCloseLogMsg) { + f_write(foSaved, "\r\nEND_OF_LOG\r\n", 14, &bw); + } f_sync (foSaved); } chThdExit(SDLOG_OK); diff --git a/sw/airborne/subsystems/chibios-libopencm3/usb_msd.h b/sw/airborne/subsystems/chibios-libopencm3/usb_msd.h index c8ca2954d1..249db80418 100644 --- a/sw/airborne/subsystems/chibios-libopencm3/usb_msd.h +++ b/sw/airborne/subsystems/chibios-libopencm3/usb_msd.h @@ -33,30 +33,30 @@ * @brief Command Block Wrapper structure */ PACK_STRUCT_BEGIN typedef struct { - uint32_t signature; - uint32_t tag; - uint32_t data_len; - uint8_t flags; - uint8_t lun; - uint8_t scsi_cmd_len; - uint8_t scsi_cmd_data[16]; + uint32_t signature; + uint32_t tag; + uint32_t data_len; + uint8_t flags; + uint8_t lun; + uint8_t scsi_cmd_len; + uint8_t scsi_cmd_data[16]; } PACK_STRUCT_STRUCT msd_cbw_t PACK_STRUCT_END; /** * @brief Command Status Wrapper structure */ PACK_STRUCT_BEGIN typedef struct { - uint32_t signature; - uint32_t tag; - uint32_t data_residue; - uint8_t status; + uint32_t signature; + uint32_t tag; + uint32_t data_residue; + uint8_t status; } PACK_STRUCT_STRUCT msd_csw_t PACK_STRUCT_END; /** * @brief Structure holding sense data (status/error information) */ PACK_STRUCT_BEGIN typedef struct { - uint8_t byte[18]; + uint8_t byte[18]; } PACK_STRUCT_STRUCT msd_scsi_sense_response_t PACK_STRUCT_END; /** @@ -140,16 +140,16 @@ typedef struct { */ typedef struct { const USBMassStorageConfig* config; - BinarySemaphore bsem; - Thread* thread; - EventSource evt_connected, evt_ejected; - BlockDeviceInfo block_dev_info; - msd_state_t state; - msd_cbw_t cbw; - msd_csw_t csw; - msd_scsi_sense_response_t sense; - msd_scsi_inquiry_response_t inquiry; - bool_t result; + BinarySemaphore bsem; + Thread* thread; + EventSource evt_connected, evt_ejected; + BlockDeviceInfo block_dev_info; + msd_state_t state; + msd_cbw_t cbw; + msd_csw_t csw; + msd_scsi_sense_response_t sense; + msd_scsi_inquiry_response_t inquiry; + bool_t result; } USBMassStorageDriver; #ifdef __cplusplus diff --git a/sw/airborne/subsystems/datalink/downlink_transport.h b/sw/airborne/subsystems/datalink/downlink_transport.h index 6959bc54f2..5c1d6c1f5e 100644 --- a/sw/airborne/subsystems/datalink/downlink_transport.h +++ b/sw/airborne/subsystems/datalink/downlink_transport.h @@ -31,34 +31,34 @@ #include enum DownlinkDataType { - DL_TYPE_ARRAY_LENGTH = 1, - DL_TYPE_UINT8, - DL_TYPE_INT8, - DL_TYPE_UINT16, - DL_TYPE_INT16, - DL_TYPE_UINT32, - DL_TYPE_INT32, - DL_TYPE_UINT64, - DL_TYPE_INT64, - DL_TYPE_FLOAT, - DL_TYPE_DOUBLE, - DL_TYPE_TIMESTAMP, + DL_TYPE_ARRAY_LENGTH = 1, + DL_TYPE_UINT8, + DL_TYPE_INT8, + DL_TYPE_UINT16, + DL_TYPE_INT16, + DL_TYPE_UINT32, + DL_TYPE_INT32, + DL_TYPE_UINT64, + DL_TYPE_INT64, + DL_TYPE_FLOAT, + DL_TYPE_DOUBLE, + DL_TYPE_TIMESTAMP, }; struct DownlinkTransport { - uint8_t (*SizeOf)(void *impl, uint8_t size); - int (*CheckFreeSpace)(void *impl, uint8_t size); + uint8_t (*SizeOf)(void *impl, uint8_t size); + int (*CheckFreeSpace)(void *impl, uint8_t size); - void (*PutBytes)(void *impl, enum DownlinkDataType data_type, uint8_t len, const void *bytes); + void (*PutBytes)(void *impl, enum DownlinkDataType data_type, uint8_t len, const void *bytes); - void (*StartMessage)(void *impl, char *name, uint8_t msg_id, uint8_t payload_len); - void (*EndMessage)(void *impl); - void (*Overrun)(void *impl); - void (*CountBytes)(void *impl, uint8_t len); - void (*Periodic)(void *impl); + void (*StartMessage)(void *impl, char *name, uint8_t msg_id, uint8_t payload_len); + void (*EndMessage)(void *impl); + void (*Overrun)(void *impl); + void (*CountBytes)(void *impl, uint8_t len); + void (*Periodic)(void *impl); - void *impl; + void *impl; }; #endif /* DOWNLINK_TRANSPORT_H */ diff --git a/sw/airborne/subsystems/datalink/uart_print.h b/sw/airborne/subsystems/datalink/uart_print.h index b8da58accb..e61c7e4491 100644 --- a/sw/airborne/subsystems/datalink/uart_print.h +++ b/sw/airborne/subsystems/datalink/uart_print.h @@ -35,7 +35,7 @@ #define _PrintHex(out_fun, c) { \ const uint8_t hex[16] = { '0', '1', '2', '3', '4', '5', '6', '7', \ - '8', '9', 'A', 'B', 'C', 'D', 'E', 'F' }; \ + '8', '9', 'A', 'B', 'C', 'D', 'E', 'F' }; \ uint8_t high = (c & 0xF0)>>4; \ uint8_t low = c & 0x0F; \ out_fun(hex[high]); \ diff --git a/sw/airborne/subsystems/gps/gps_sirf.c b/sw/airborne/subsystems/gps/gps_sirf.c index 96d0e165b1..600391ef39 100644 --- a/sw/airborne/subsystems/gps/gps_sirf.c +++ b/sw/airborne/subsystems/gps/gps_sirf.c @@ -49,7 +49,7 @@ void gps_impl_init( void ) { void sirf_parse_char(uint8_t c) { switch(gps_sirf.read_state) { - case UNINIT: + case UNINIT: if(c == 0xA0) { gps_sirf.msg_len = 0; gps_sirf.msg_buf[gps_sirf.msg_len] = c; @@ -57,7 +57,7 @@ void sirf_parse_char(uint8_t c) { gps_sirf.read_state = GOT_A0; } break; - case GOT_A0: + case GOT_A0: if(c == 0xA2) { gps_sirf.msg_buf[gps_sirf.msg_len] = c; gps_sirf.msg_len++; @@ -66,13 +66,13 @@ void sirf_parse_char(uint8_t c) { else goto restart; break; - case GOT_A2: + case GOT_A2: gps_sirf.msg_buf[gps_sirf.msg_len] = c; gps_sirf.msg_len++; if(c == 0xB0) gps_sirf.read_state = GOT_B0; break; - case GOT_B0: + case GOT_B0: if(c == 0xB3) { gps_sirf.msg_buf[gps_sirf.msg_len] = c; gps_sirf.msg_len++; @@ -179,10 +179,10 @@ void sirf_parse_msg(void) { //Check the message id and parse the message uint8_t message_id = gps_sirf.msg_buf[4]; switch(message_id) { - case 0x29: + case 0x29: sirf_parse_41(); break; - case 0x02: + case 0x02: sirf_parse_2(); break; } diff --git a/sw/airborne/subsystems/imu.c b/sw/airborne/subsystems/imu.c index 0f99b76edc..a826660222 100644 --- a/sw/airborne/subsystems/imu.c +++ b/sw/airborne/subsystems/imu.c @@ -155,7 +155,7 @@ void imu_float_init(void) { for conversions between body and imu frame */ EULERS_ASSIGN(imuf.body_to_imu_eulers, - IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI); + IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI); FLOAT_QUAT_OF_EULERS(imuf.body_to_imu_quat, imuf.body_to_imu_eulers); FLOAT_QUAT_NORMALIZE(imuf.body_to_imu_quat); FLOAT_RMAT_OF_EULERS(imuf.body_to_imu_rmat, imuf.body_to_imu_eulers); diff --git a/sw/airborne/subsystems/ins/hf_float.c b/sw/airborne/subsystems/ins/hf_float.c index ac9511bb71..27cc1c5de3 100644 --- a/sw/airborne/subsystems/ins/hf_float.c +++ b/sw/airborne/subsystems/ins/hf_float.c @@ -39,9 +39,9 @@ #include #define DBG_LEVEL 1 #define PRINT_DBG(_l, _p) { \ - if (DBG_LEVEL >= _l) \ - printf _p; \ -} + if (DBG_LEVEL >= _l) \ + printf _p; \ + } #else #define PRINT_DBG(_l, _p) {} #endif @@ -121,9 +121,9 @@ void b2_hff_store_accel_body(void) { /* once the buffer is full it always has the last acc_body.size accel measurements */ if (acc_body.n < acc_body.size) { - acc_body.n++; + acc_body.n++; } else { - acc_body.r = (acc_body.r + 1) < acc_body.size ? (acc_body.r + 1) : 0; + acc_body.r = (acc_body.r + 1) < acc_body.size ? (acc_body.r + 1) : 0; } } @@ -142,9 +142,9 @@ static inline void b2_hff_compute_accel_body_mean(uint8_t n) { j = (acc_body.w - i) > 0 ? (acc_body.w - i) : (acc_body.w - i + acc_body.size); VECT3_ADD(sum, acc_body.buf[j]); } - VECT3_SDIV(acc_body_mean, sum, n); + VECT3_SDIV(acc_body_mean, sum, n); } else { - VECT3_COPY(acc_body_mean, sum); + VECT3_COPY(acc_body_mean, sum); } } @@ -188,10 +188,10 @@ unsigned int acc_buf_n; /* number of elements in buffer */ */ #define HFF_RB_MAXN ((int) (GPS_LAG * 4)) #define INC_RB_POINTER(ptr) { \ - if (ptr == &b2_hff_rb[HFF_RB_MAXN-1]) \ - ptr = b2_hff_rb; \ - else \ - ptr++; \ + if (ptr == &b2_hff_rb[HFF_RB_MAXN-1]) \ + ptr = b2_hff_rb; \ + else \ + ptr++; \ } struct HfilterFloat b2_hff_rb[HFF_RB_MAXN]; /* ringbuffer with state and covariance when GPS was valid */ @@ -241,32 +241,32 @@ static inline void b2_hff_update_ydot(struct HfilterFloat* hff_work, float vel, static void send_hff(void) { DOWNLINK_SEND_HFF(DefaultChannel, DefaultDevice, - &b2_hff_state.x, - &b2_hff_state.y, - &b2_hff_state.xdot, - &b2_hff_state.ydot, - &b2_hff_state.xdotdot, - &b2_hff_state.ydotdot); + &b2_hff_state.x, + &b2_hff_state.y, + &b2_hff_state.xdot, + &b2_hff_state.ydot, + &b2_hff_state.xdotdot, + &b2_hff_state.ydotdot); } static void send_hff_debug(void) { DOWNLINK_SEND_HFF_DBG(DefaultChannel, DefaultDevice, - &b2_hff_x_meas, - &b2_hff_y_meas, - &b2_hff_xd_meas, - &b2_hff_yd_meas, - &b2_hff_state.xP[0][0], - &b2_hff_state.yP[0][0], - &b2_hff_state.xP[1][1], - &b2_hff_state.yP[1][1]); + &b2_hff_x_meas, + &b2_hff_y_meas, + &b2_hff_xd_meas, + &b2_hff_yd_meas, + &b2_hff_state.xP[0][0], + &b2_hff_state.yP[0][0], + &b2_hff_state.xP[1][1], + &b2_hff_state.yP[1][1]); } #ifdef GPS_LAG static void send_hff_gps(void) { DOWNLINK_SEND_HFF_GPS(DefaultChannel, DefaultDevice, - &(b2_hff_rb_last->lag_counter), - &lag_counter_err, - &save_counter); + &(b2_hff_rb_last->lag_counter), + &lag_counter_err, + &save_counter); } #endif @@ -328,7 +328,7 @@ static inline void b2_hff_init_x(float init_x, float init_xdot) { for (i=0; i acc_buf_n) { - PRINT_DBG(1, ("Cannot go back %d steps, going back only %d instead!\n", back_n, acc_buf_n)); - back_n = acc_buf_n; + PRINT_DBG(1, ("Cannot go back %d steps, going back only %d instead!\n", back_n, acc_buf_n)); + back_n = acc_buf_n; } else if (back_n == 0) { - PRINT_DBG(1, ("Cannot go back zero steps!\n")); - return; + PRINT_DBG(1, ("Cannot go back zero steps!\n")); + return; } if ((int)(acc_buf_w - back_n) < 0) - i = acc_buf_w - back_n + ACC_BUF_MAXN; + i = acc_buf_w - back_n + ACC_BUF_MAXN; else - i = acc_buf_w - back_n; + i = acc_buf_w - back_n; b2_hff_xdd_meas = past_accel[i].x; b2_hff_ydd_meas = past_accel[i].y; PRINT_DBG(3, ("get past accel. buf_n: %2d \tbuf_w: %2d \tback_n: %2d \ti: %2d \txdd: %f \tydd: %f\n", acc_buf_n, acc_buf_w, back_n, i, b2_hff_xdd_meas, b2_hff_ydd_meas)); @@ -387,21 +387,21 @@ static inline void b2_hff_rb_put_state(struct HfilterFloat* source) { /* increase fill count and forward last pointer if neccessary */ if (b2_hff_rb_n < HFF_RB_MAXN) { - b2_hff_rb_n++; + b2_hff_rb_n++; } else { - INC_RB_POINTER(b2_hff_rb_last); + INC_RB_POINTER(b2_hff_rb_last); } PRINT_DBG(2, ("put state. fill count now: %d\n", b2_hff_rb_n)); } static inline void b2_hff_rb_drop_last(void) { if (b2_hff_rb_n > 0) { - INC_RB_POINTER(b2_hff_rb_last); - b2_hff_rb_n--; + INC_RB_POINTER(b2_hff_rb_last); + b2_hff_rb_n--; } else { - PRINT_DBG(2, ("hff ringbuffer empty!\n")); - b2_hff_rb_last->lag_counter = 0; - b2_hff_rb_last->rollback = FALSE; + PRINT_DBG(2, ("hff ringbuffer empty!\n")); + b2_hff_rb_last->lag_counter = 0; + b2_hff_rb_last->rollback = FALSE; } PRINT_DBG(2, ("drop last state. fill count now: %d\n", b2_hff_rb_n)); } @@ -416,10 +416,10 @@ static inline void b2_hff_set_state(struct HfilterFloat* dest, struct HfilterFlo dest->ydot = source->ydot; dest->ydotdot = source->ydotdot; for (int i=0; i < HFF_STATE_SIZE; i++) { - for (int j=0; j < HFF_STATE_SIZE; j++) { - dest->xP[i][j] = source->xP[i][j]; - dest->yP[i][j] = source->yP[i][j]; - } + for (int j=0; j < HFF_STATE_SIZE; j++) { + dest->xP[i][j] = source->xP[i][j]; + dest->yP[i][j] = source->yP[i][j]; + } } } @@ -427,37 +427,37 @@ static inline void b2_hff_propagate_past(struct HfilterFloat* hff_past) { PRINT_DBG(1, ("enter propagate past: %d\n", hff_past->lag_counter)); /* run max MAX_PP_STEPS propagation steps */ for (int i=0; i < MAX_PP_STEPS; i++) { - if (hff_past->lag_counter > 0) { - b2_hff_get_past_accel(hff_past->lag_counter); - PRINT_DBG(2, ("propagate past: %d\n", hff_past->lag_counter)); - b2_hff_propagate_x(hff_past); - b2_hff_propagate_y(hff_past); - hff_past->lag_counter--; + if (hff_past->lag_counter > 0) { + b2_hff_get_past_accel(hff_past->lag_counter); + PRINT_DBG(2, ("propagate past: %d\n", hff_past->lag_counter)); + b2_hff_propagate_x(hff_past); + b2_hff_propagate_y(hff_past); + hff_past->lag_counter--; - if (past_save_counter > 0) { - past_save_counter--; - PRINT_DBG(2, ("dec past_save_counter: %d\n", past_save_counter)); - } else if (past_save_counter == SAVE_NOW) { - /* next GPS measurement valid at this state -> save */ - PRINT_DBG(2, ("save past state\n")); - b2_hff_rb_put_state(hff_past); - past_save_counter = SAVING; - } else if (past_save_counter == SAVING) { - /* increase lag counter on if next state is already saved */ - if (hff_past == &b2_hff_rb[HFF_RB_MAXN-1]) - b2_hff_rb[0].lag_counter++; - else - (hff_past+1)->lag_counter++; - } - } + if (past_save_counter > 0) { + past_save_counter--; + PRINT_DBG(2, ("dec past_save_counter: %d\n", past_save_counter)); + } else if (past_save_counter == SAVE_NOW) { + /* next GPS measurement valid at this state -> save */ + PRINT_DBG(2, ("save past state\n")); + b2_hff_rb_put_state(hff_past); + past_save_counter = SAVING; + } else if (past_save_counter == SAVING) { + /* increase lag counter on if next state is already saved */ + if (hff_past == &b2_hff_rb[HFF_RB_MAXN-1]) + b2_hff_rb[0].lag_counter++; + else + (hff_past+1)->lag_counter++; + } + } - /* finished re-propagating the past values */ - if (hff_past->lag_counter == 0) { - b2_hff_set_state(&b2_hff_state, hff_past); - b2_hff_rb_drop_last(); + /* finished re-propagating the past values */ + if (hff_past->lag_counter == 0) { + b2_hff_set_state(&b2_hff_state, hff_past); + b2_hff_rb_drop_last(); past_save_counter = SAVE_DONE; - break; - } + break; + } } } #endif /* GPS_LAG */ @@ -471,7 +471,7 @@ void b2_hff_propagate(void) { #ifdef GPS_LAG /* continue re-propagating to catch up with the present */ if (b2_hff_rb_last->rollback) { - b2_hff_propagate_past(b2_hff_rb_last); + b2_hff_propagate_past(b2_hff_rb_last); } #endif @@ -480,7 +480,7 @@ void b2_hff_propagate(void) { /* propagate current state if it is time */ if (b2_hff_ps_counter == HFF_PRESCALER) { - b2_hff_ps_counter = 1; + b2_hff_ps_counter = 1; if (b2_hff_lost_counter < b2_hff_lost_limit) { /* compute float ltp mean acceleration */ @@ -588,7 +588,7 @@ void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 vel) { b2_hff_state.ydot = vel.y; #ifdef GPS_LAG while (b2_hff_rb_n > 0) { - b2_hff_rb_drop_last(); + b2_hff_rb_drop_last(); } save_counter = -1; past_save_counter = SAVE_DONE; @@ -602,17 +602,17 @@ void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 vel) { * * - F = [ 1 dt - 0 1 ]; + F = [ 1 dt + 0 1 ]; - B = [ dt^2/2 dt]'; + B = [ dt^2/2 dt]'; - Q = [ 0.01 0 - 0 0.01]; + Q = [ 0.01 0 + 0 0.01]; - Xk1 = F * Xk0 + B * accel; + Xk1 = F * Xk0 + B * accel; - Pk1 = F * Pk0 * F' + Q; + Pk1 = F * Pk0 * F' + Q; */ static inline void b2_hff_propagate_x(struct HfilterFloat* hff_work) { @@ -656,18 +656,18 @@ static inline void b2_hff_propagate_y(struct HfilterFloat* hff_work) { * * - H = [1 0]; - R = 0.1; - // state residual - y = pos_measurement - H * Xm; - // covariance residual - S = H*Pm*H' + R; - // kalman gain - K = Pm*H'*inv(S); - // update state - Xp = Xm + K*y; - // update covariance - Pp = Pm - K*H*Pm; + H = [1 0]; + R = 0.1; + // state residual + y = pos_measurement - H * Xm; + // covariance residual + S = H*Pm*H' + R; + // kalman gain + K = Pm*H'*inv(S); + // update state + Xp = Xm + K*y; + // update covariance + Pp = Pm - K*H*Pm; */ void b2_hff_update_pos (struct FloatVect2 pos, struct FloatVect2 Rpos) { b2_hff_update_x(&b2_hff_state, pos.x, Rpos.x); @@ -727,18 +727,18 @@ static inline void b2_hff_update_y(struct HfilterFloat* hff_work, float y_meas, * * - H = [0 1]; - R = 0.1; - // state residual - yd = vx - H * Xm; - // covariance residual - S = H*Pm*H' + R; - // kalman gain - K = Pm*H'*inv(S); - // update state - Xp = Xm + K*yd; - // update covariance - Pp = Pm - K*H*Pm; + H = [0 1]; + R = 0.1; + // state residual + yd = vx - H * Xm; + // covariance residual + S = H*Pm*H' + R; + // kalman gain + K = Pm*H'*inv(S); + // update state + Xp = Xm + K*yd; + // update covariance + Pp = Pm - K*H*Pm; */ void b2_hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel) { b2_hff_update_xdot(&b2_hff_state, vel.x, Rvel.x); diff --git a/sw/airborne/subsystems/nav.h b/sw/airborne/subsystems/nav.h index 1f01b29eaa..74ce373275 100644 --- a/sw/airborne/subsystems/nav.h +++ b/sw/airborne/subsystems/nav.h @@ -187,7 +187,7 @@ bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float ap #define NavAttitude(_roll) { \ lateral_mode = LATERAL_MODE_ROLL; \ if(pprz_mode != PPRZ_MODE_AUTO1) \ - {h_ctl_roll_setpoint = _roll;} \ + {h_ctl_roll_setpoint = _roll;} \ } #define nav_IncreaseShift(x) { if (x==0) nav_shift = 0; else nav_shift += x; } diff --git a/sw/airborne/subsystems/navigation/OSAMNav.c b/sw/airborne/subsystems/navigation/OSAMNav.c index 33a10f76a6..3705f8a79e 100644 --- a/sw/airborne/subsystems/navigation/OSAMNav.c +++ b/sw/airborne/subsystems/navigation/OSAMNav.c @@ -34,8 +34,8 @@ /************** Flower Navigation **********************************************/ /** Makes a flower pattern. - CenterWP is the center of the flower. The Navigation Height is taken from this waypoint. - EdgeWP defines the radius of the flower (distance from CenterWP to EdgeWP) + CenterWP is the center of the flower. The Navigation Height is taken from this waypoint. + EdgeWP defines the radius of the flower (distance from CenterWP to EdgeWP) */ enum FlowerStatus { Outside, FlowerLine, Circle }; diff --git a/sw/airborne/subsystems/navigation/snav.c b/sw/airborne/subsystems/navigation/snav.c index 0acf313152..253e35021b 100644 --- a/sw/airborne/subsystems/navigation/snav.c +++ b/sw/airborne/subsystems/navigation/snav.c @@ -155,8 +155,8 @@ static inline float ground_speed_of_course(float x) { /* Compute the ground speed for courses 0, 360/NB_ANGLES, ... (NB_ANGLES-1)360/NB_ANGLES */ static void compute_ground_speed(float airspeed, - float wind_x, - float wind_y) { + float wind_x, + float wind_y) { uint8_t i; float alpha = 0; float c = wind_x*wind_x+wind_y*wind_y-airspeed*airspeed; diff --git a/sw/airborne/subsystems/navigation/spiral.h b/sw/airborne/subsystems/navigation/spiral.h index 72356d1ee7..d49257a171 100644 --- a/sw/airborne/subsystems/navigation/spiral.h +++ b/sw/airborne/subsystems/navigation/spiral.h @@ -33,7 +33,7 @@ extern bool_t SpiralNav(void); extern bool_t InitializeSpiral(uint8_t CenterWP, uint8_t EdgeWP, float StartRad, float IncRad, - float Segments, float ZKoord ); + float Segments, float ZKoord ); #endif // SPIRAL_H diff --git a/sw/airborne/subsystems/radio_control/superbitrf_rc.h b/sw/airborne/subsystems/radio_control/superbitrf_rc.h index de410a56d3..f25cd37c21 100644 --- a/sw/airborne/subsystems/radio_control/superbitrf_rc.h +++ b/sw/airborne/subsystems/radio_control/superbitrf_rc.h @@ -61,11 +61,11 @@ uint8_t i; \ for(i = 0; i < _count; i++) { \ if(i == RADIO_THROTTLE) { \ - _out[i] = (_in[i] + MAX_PPRZ) / 2; \ - Bound(_out[i], 0, MAX_PPRZ); \ + _out[i] = (_in[i] + MAX_PPRZ) / 2; \ + Bound(_out[i], 0, MAX_PPRZ); \ } else { \ - _out[i] = -_in[i]; \ - Bound(_out[i], MIN_PPRZ, MAX_PPRZ); \ + _out[i] = -_in[i]; \ + Bound(_out[i], MIN_PPRZ, MAX_PPRZ); \ } \ } \ } @@ -75,7 +75,7 @@ cyrf6936_event(&superbitrf.cyrf6936); \ superbitrf_event(); \ if(superbitrf.rc_frame_available) { \ - radio_control.frame_cpt++; \ + radio_control.frame_cpt++; \ radio_control.time_since_last_frame = 0; \ radio_control.radio_ok_cpt = 0; \ radio_control.status = RC_OK; \ diff --git a/sw/airborne/test/ahrs/run_ahrs_on_flight_log.c b/sw/airborne/test/ahrs/run_ahrs_on_flight_log.c index b8e2234ea0..3ee4a36c2a 100644 --- a/sw/airborne/test/ahrs/run_ahrs_on_flight_log.c +++ b/sw/airborne/test/ahrs/run_ahrs_on_flight_log.c @@ -88,17 +88,17 @@ int main(int argc, char** argv) { if (ahrs.status == AHRS_UNINIT) { ahrs_aligner_run(); if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) - ahrs_align(); + ahrs_align(); } else { ahrs_propagate(); #ifdef ENABLE_MAG_UPDATE if (MAG_AVAILABLE(samples[i].flag)) - ahrs_update_mag(); + ahrs_update_mag(); #endif #ifdef ENABLE_ACCEL_UPDATE if (IMU_AVAILABLE(samples[i].flag) && (!MAG_AVAILABLE(samples[i].flag))) - ahrs_update_accel(); + ahrs_update_accel(); #endif } store_filter_output(i); @@ -118,13 +118,13 @@ static void read_ascii_flight_log(const char* filename) { do { struct test_sample* s = &samples[nb_samples]; ret = fscanf(fd, "%lf\t%hhu\t%lf %lf %lf\t%lf %lf %lf\t%lf %lf %lf\t%lf %lf %lf\t%lf %lf %lf\t%lf", - &s->time, &s->flag, - &s->gyro.p, &s->gyro.q, &s->gyro.r, - &s->accel.x, &s->accel.y, &s->accel.z, - &s->mag.x, &s->mag.y, &s->mag.z, - &s->gps_pecef.x, &s->gps_pecef.y, &s->gps_pecef.z, - &s->gps_vecef.x, &s->gps_vecef.y, &s->gps_vecef.z, - &s->baro); + &s->time, &s->flag, + &s->gyro.p, &s->gyro.q, &s->gyro.r, + &s->accel.x, &s->accel.y, &s->accel.z, + &s->mag.x, &s->mag.y, &s->mag.z, + &s->gps_pecef.x, &s->gps_pecef.y, &s->gps_pecef.z, + &s->gps_vecef.x, &s->gps_vecef.y, &s->gps_vecef.z, + &s->baro); nb_samples++; } while (ret == 18 && nb_samples < MAX_SAMPLE); diff --git a/sw/airborne/test/ahrs/run_ahrs_on_synth.c b/sw/airborne/test/ahrs/run_ahrs_on_synth.c index 57f7deef13..5f951047e8 100644 --- a/sw/airborne/test/ahrs/run_ahrs_on_synth.c +++ b/sw/airborne/test/ahrs/run_ahrs_on_synth.c @@ -40,16 +40,16 @@ static void report(void) { printf("%f ", aos.time); printf("%f %f %f ", DegOfRad(aos.ltp_to_imu_euler.phi), - DegOfRad(aos.ltp_to_imu_euler.theta), - DegOfRad(aos.ltp_to_imu_euler.psi)); + DegOfRad(aos.ltp_to_imu_euler.theta), + DegOfRad(aos.ltp_to_imu_euler.psi)); printf("%f %f %f ", DegOfRad(aos.imu_rates.p), - DegOfRad(aos.imu_rates.q), - DegOfRad(aos.imu_rates.r)); + DegOfRad(aos.imu_rates.q), + DegOfRad(aos.imu_rates.r)); printf("%f %f %f ", DegOfRad(aos.gyro_bias.p), - DegOfRad(aos.gyro_bias.q), - DegOfRad(aos.gyro_bias.r)); + DegOfRad(aos.gyro_bias.q), + DegOfRad(aos.gyro_bias.r)); #if AHRS_TYPE == AHRS_TYPE_ICQ struct Int32Eulers ltp_to_imu_euler_i; @@ -69,8 +69,8 @@ static void report(void) { struct FloatRates bias; RATES_FLOAT_OF_BFP(bias, ahrs_impl.gyro_bias); printf("%f %f %f ", DegOfRad(bias.p), - DegOfRad(bias.q), - DegOfRad(bias.r)); + DegOfRad(bias.q), + DegOfRad(bias.r)); #elif AHRS_TYPE == AHRS_TYPE_FCR2 || AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FCR @@ -83,14 +83,14 @@ static void report(void) { DegOfRad(ahrs_impl.imu_rate.r)); printf("%f %f %f ", DegOfRad(ahrs_impl.gyro_bias.p), - DegOfRad(ahrs_impl.gyro_bias.q), - DegOfRad(ahrs_impl.gyro_bias.r)); + DegOfRad(ahrs_impl.gyro_bias.q), + DegOfRad(ahrs_impl.gyro_bias.r)); #endif if (output_pos) { printf("%f %f %f ", aos.ltp_pos.x, aos.ltp_pos.y, - aos.ltp_pos.z); + aos.ltp_pos.z); } if (output_sensors) { diff --git a/sw/airborne/test/ahrs/run_ahrs_on_synth_ivy.c b/sw/airborne/test/ahrs/run_ahrs_on_synth_ivy.c index 0bd0851989..e96d0d6e52 100644 --- a/sw/airborne/test/ahrs/run_ahrs_on_synth_ivy.c +++ b/sw/airborne/test/ahrs/run_ahrs_on_synth_ivy.c @@ -41,36 +41,36 @@ gboolean timeout_callback(gpointer data) { #if AHRS_TYPE == AHRS_TYPE_ICQ IvySendMsg("183 AHRS_GYRO_BIAS_INT %d %d %d", - ahrs_impl.gyro_bias.p, - ahrs_impl.gyro_bias.q, - ahrs_impl.gyro_bias.r); + ahrs_impl.gyro_bias.p, + ahrs_impl.gyro_bias.q, + ahrs_impl.gyro_bias.r); #endif #if AHRS_TYPE == AHRS_TYPE_FLQ || AHRS_TYPE == AHRS_TYPE_FCR2 struct Int32Rates bias_i; RATES_BFP_OF_REAL(bias_i, ahrs_impl.gyro_bias); IvySendMsg("183 AHRS_GYRO_BIAS_INT %d %d %d", - bias_i.p, - bias_i.q, - bias_i.r); + bias_i.p, + bias_i.q, + bias_i.r); #endif IvySendMsg("183 AHRS_EULER %f %f %f", - ahrs_float.ltp_to_imu_euler.phi, - ahrs_float.ltp_to_imu_euler.theta, - ahrs_float.ltp_to_imu_euler.psi); + ahrs_float.ltp_to_imu_euler.phi, + ahrs_float.ltp_to_imu_euler.theta, + ahrs_float.ltp_to_imu_euler.psi); IvySendMsg("183 NPS_RATE_ATTITUDE %f %f %f %f %f %f", - DegOfRad(aos.imu_rates.p), - DegOfRad(aos.imu_rates.q), - DegOfRad(aos.imu_rates.r), - DegOfRad(aos.ltp_to_imu_euler.phi), - DegOfRad(aos.ltp_to_imu_euler.theta), - DegOfRad(aos.ltp_to_imu_euler.psi)); + DegOfRad(aos.imu_rates.p), + DegOfRad(aos.imu_rates.q), + DegOfRad(aos.imu_rates.r), + DegOfRad(aos.ltp_to_imu_euler.phi), + DegOfRad(aos.ltp_to_imu_euler.theta), + DegOfRad(aos.ltp_to_imu_euler.psi)); IvySendMsg("183 NPS_GYRO_BIAS %f %f %f", - DegOfRad(aos.gyro_bias.p), - DegOfRad(aos.gyro_bias.q), - DegOfRad(aos.gyro_bias.r)); + DegOfRad(aos.gyro_bias.p), + DegOfRad(aos.gyro_bias.q), + DegOfRad(aos.gyro_bias.r)); return TRUE; } diff --git a/sw/airborne/test/pprz_algebra_print.h b/sw/airborne/test/pprz_algebra_print.h index 2f67df9cc5..1d06d01e86 100644 --- a/sw/airborne/test/pprz_algebra_print.h +++ b/sw/airborne/test/pprz_algebra_print.h @@ -18,8 +18,8 @@ #define DISPLAY_FLOAT_RMAT(text, mat) { \ printf("%s\n %f %f %f\n %f %f %f\n %f %f %f\n",text, \ - mat.m[0], mat.m[1], mat.m[2], mat.m[3], mat.m[4], mat.m[5], \ - mat.m[6], mat.m[7], mat.m[8]); \ + mat.m[0], mat.m[1], mat.m[2], mat.m[3], mat.m[4], mat.m[5], \ + mat.m[6], mat.m[7], mat.m[8]); \ } #define DISPLAY_FLOAT_EULERS(text, _e) { \ @@ -28,7 +28,7 @@ #define DISPLAY_FLOAT_EULERS_DEG(text, _e) { \ printf("%s %f %f %f\n",text, DegOfRad((_e).phi), \ - DegOfRad((_e).theta), DegOfRad((_e).psi)); \ + DegOfRad((_e).theta), DegOfRad((_e).psi)); \ } @@ -118,11 +118,11 @@ int32_t quat_norm; \ INT32_QUAT_NORM(quat_norm, quat); \ printf("%s %d %d %d %d (%d) (%f %f %f %f)\n",text, \ - quat.qi, quat.qx, quat.qy, quat.qz, quat_norm, \ - (float)quat.qi/(1<queue_full_cnt; uint16_t i2c2_ack_fail_cnt = i2c2.errors->ack_fail_cnt; @@ -183,21 +183,21 @@ static inline void main_report(void) { &i2c2_last_unexpected_event, &_bus2); #endif - }, - { - DOWNLINK_SEND_AHRS_EULER_INT(DefaultChannel, DefaultDevice, - &ahrs.ltp_to_imu_euler.phi, - &ahrs.ltp_to_imu_euler.theta, - &ahrs.ltp_to_imu_euler.psi, - &ahrs.ltp_to_body_euler.phi, - &ahrs.ltp_to_body_euler.theta, - &ahrs.ltp_to_body_euler.psi); - }, - { - DOWNLINK_SEND_AHRS_GYRO_BIAS_INT(DefaultChannel, DefaultDevice, - &ahrs_impl.gyro_bias.p, - &ahrs_impl.gyro_bias.q, - &ahrs_impl.gyro_bias.r); + }, + { + DOWNLINK_SEND_AHRS_EULER_INT(DefaultChannel, DefaultDevice, + &ahrs.ltp_to_imu_euler.phi, + &ahrs.ltp_to_imu_euler.theta, + &ahrs.ltp_to_imu_euler.psi, + &ahrs.ltp_to_body_euler.phi, + &ahrs.ltp_to_body_euler.theta, + &ahrs.ltp_to_body_euler.psi); + }, + { + DOWNLINK_SEND_AHRS_GYRO_BIAS_INT(DefaultChannel, DefaultDevice, + &ahrs_impl.gyro_bias.p, + &ahrs_impl.gyro_bias.q, + &ahrs_impl.gyro_bias.r); - }); + }); } diff --git a/sw/airborne/test/subsystems/test_radio_control.c b/sw/airborne/test/subsystems/test_radio_control.c index 98b9afe951..3f0fcfd0bb 100644 --- a/sw/airborne/test/subsystems/test_radio_control.c +++ b/sw/airborne/test/subsystems/test_radio_control.c @@ -67,16 +67,16 @@ static inline void main_periodic_task( void ) { int16_t foo = 0; RunOnceEvery(10, {DOWNLINK_SEND_ROTORCRAFT_RADIO_CONTROL(DefaultChannel, DefaultDevice, \ - &radio_control.values[RADIO_ROLL], \ - &radio_control.values[RADIO_PITCH], \ - &radio_control.values[RADIO_YAW], \ - &radio_control.values[RADIO_THROTTLE], \ - &radio_control.values[RADIO_MODE], \ - &foo, \ - &radio_control.status);}); + &radio_control.values[RADIO_ROLL], \ + &radio_control.values[RADIO_PITCH], \ + &radio_control.values[RADIO_YAW], \ + &radio_control.values[RADIO_THROTTLE], \ + &radio_control.values[RADIO_MODE], \ + &foo, \ + &radio_control.status);}); #ifdef RADIO_CONTROL_TYPE_PPM RunOnceEvery(10, - {uint8_t blaa = 0; DOWNLINK_SEND_PPM(DefaultChannel, DefaultDevice,&blaa, 8, ppm_pulses);}); + {uint8_t blaa = 0; DOWNLINK_SEND_PPM(DefaultChannel, DefaultDevice,&blaa, 8, ppm_pulses);}); #endif LED_PERIODIC(); diff --git a/sw/airborne/test/test_adcs.c b/sw/airborne/test/test_adcs.c index 39d2b76d5f..00b77816d2 100644 --- a/sw/airborne/test/test_adcs.c +++ b/sw/airborne/test/test_adcs.c @@ -56,7 +56,7 @@ int main (int argc, char** argv) { uint16_t values[NB_ADC]; uint8_t i; for(i = 0; i < NB_ADC; i++) - values[i] = buf_adc[i].sum / ADC_NB_SAMPLES; + values[i] = buf_adc[i].sum / ADC_NB_SAMPLES; uint8_t id = 42; DOWNLINK_SEND_ADC(&id, NB_ADC, values); diff --git a/sw/airborne/test/test_matrix.c b/sw/airborne/test/test_matrix.c index 4964c59707..13f503f0b9 100644 --- a/sw/airborne/test/test_matrix.c +++ b/sw/airborne/test/test_matrix.c @@ -8,20 +8,20 @@ int main(int argc, char** argv) { float A[2][3] = {{ 1., 2., 3.}, - { 4., 5., 6.}}; + { 4., 5., 6.}}; float B[3][2] = {{ 1., 2.}, - { 3., 4.}, - { 5., 6.}}; + { 3., 4.}, + { 5., 6.}}; float C[2][3] = {{ 1., 2., 3.}, - { 4., 5., 6.}}; + { 4., 5., 6.}}; float D[2][2]; float E[3][3] = {{ 1., 2., 3.}, - { 5., 1., 3.}, - { 3., 4., 6.}}; + { 5., 1., 3.}, + { 3., 4., 6.}}; float F[3][3];