diff --git a/sw/airborne/arch/linux/mcu_periph/spi_arch.c b/sw/airborne/arch/linux/mcu_periph/spi_arch.c index 04116afc6a..35238b9c26 100644 --- a/sw/airborne/arch/linux/mcu_periph/spi_arch.c +++ b/sw/airborne/arch/linux/mcu_periph/spi_arch.c @@ -42,7 +42,7 @@ void spi_init_slaves(void) */ } -bool_t spi_submit(struct spi_periph* p, struct spi_transaction* t) +bool_t spi_submit(struct spi_periph *p, struct spi_transaction *t) { int fd = (int)p->reg_addr; @@ -57,10 +57,9 @@ bool_t spi_submit(struct spi_periph* p, struct spi_transaction* t) uint8_t tx_buf[buf_len]; memset(tx_buf, 0, sizeof tx_buf); /* copy bytes to transmit to larger buffer, rest filled with zero */ - memcpy(tx_buf, (void*)t->output_buf, t->output_length); + memcpy(tx_buf, (void *)t->output_buf, t->output_length); xfer.tx_buf = (unsigned long)tx_buf; - } - else { + } else { xfer.tx_buf = (unsigned long)t->output_buf; } @@ -68,8 +67,7 @@ bool_t spi_submit(struct spi_periph* p, struct spi_transaction* t) uint8_t rx_buf[buf_len]; memset(rx_buf, 0, sizeof rx_buf); xfer.rx_buf = (unsigned long)rx_buf; - } - else { + } else { xfer.rx_buf = (unsigned long)t->input_buf; } @@ -93,19 +91,21 @@ bool_t spi_submit(struct spi_periph* p, struct spi_transaction* t) /* copy recieved data if we had to use an extra rx_buffer */ if (buf_len > t->input_length) { - memcpy((void*)t->input_buf, (void*)xfer.rx_buf, t->input_length); + memcpy((void *)t->input_buf, (void *)xfer.rx_buf, t->input_length); } t->status = SPITransSuccess; return TRUE; } -bool_t spi_lock(struct spi_periph* p, uint8_t slave) { +bool_t spi_lock(struct spi_periph *p, uint8_t slave) +{ // not implemented return FALSE; } -bool_t spi_resume(struct spi_periph* p, uint8_t slave) { +bool_t spi_resume(struct spi_periph *p, uint8_t slave) +{ // not implemented return FALSE; } @@ -121,10 +121,10 @@ void spi0_arch_init(void) spi0.reg_addr = NULL; return; } - spi0.reg_addr = (void*)fd; + spi0.reg_addr = (void *)fd; /* spi mode */ - if (ioctl(fd, SPI_IOC_WR_MODE, (SPI_CPOL|SPI_CPHA)) < 0) { + if (ioctl(fd, SPI_IOC_WR_MODE, (SPI_CPOL | SPI_CPHA)) < 0) { perror("SPI0: can't set spi mode"); } @@ -155,10 +155,10 @@ void spi1_arch_init(void) spi1.reg_addr = NULL; return; } - spi1.reg_addr = (void*)fd; + spi1.reg_addr = (void *)fd; /* spi mode */ - if (ioctl(fd, SPI_IOC_WR_MODE, (SPI_CPOL|SPI_CPHA)) < 0) { + if (ioctl(fd, SPI_IOC_WR_MODE, (SPI_CPOL | SPI_CPHA)) < 0) { perror("SPI1: can't set spi mode"); } diff --git a/sw/airborne/arch/linux/mcu_periph/sys_time_arch.c b/sw/airborne/arch/linux/mcu_periph/sys_time_arch.c index f4fe4a8c5a..5fdda3a4a8 100644 --- a/sw/airborne/arch/linux/mcu_periph/sys_time_arch.c +++ b/sw/airborne/arch/linux/mcu_periph/sys_time_arch.c @@ -34,7 +34,8 @@ #endif -void sys_time_arch_init( void ) { +void sys_time_arch_init(void) +{ sys_time.cpu_ticks_per_sec = 1e6; sys_time.resolution_cpu_ticks = (uint32_t)(sys_time.resolution * sys_time.cpu_ticks_per_sec + 0.5); @@ -56,7 +57,8 @@ void sys_time_arch_init( void ) { setitimer(ITIMER_REAL, &timer, NULL); } -void sys_tick_handler( int signum ) { +void sys_tick_handler(int signum) +{ sys_time.nb_tick++; sys_time.nb_sec_rem += sys_time.resolution_cpu_ticks;; @@ -68,7 +70,7 @@ void sys_tick_handler( int signum ) { #endif } - for (unsigned int i=0; i= sys_time.timer[i].end_time) { sys_time.timer[i].end_time += sys_time.timer[i].duration; diff --git a/sw/airborne/arch/linux/mcu_periph/sys_time_arch.h b/sw/airborne/arch/linux/mcu_periph/sys_time_arch.h index 0b498ee6e5..f7b5a612b2 100644 --- a/sw/airborne/arch/linux/mcu_periph/sys_time_arch.h +++ b/sw/airborne/arch/linux/mcu_periph/sys_time_arch.h @@ -38,21 +38,24 @@ extern void sys_tick_handler(int signum); * WARNING: overflows after 71min34seconds! * @return current system time as uint32_t */ -static inline uint32_t get_sys_time_usec(void) { +static inline uint32_t get_sys_time_usec(void) +{ return sys_time.nb_sec * 1000000 + - usec_of_cpu_ticks(sys_time.nb_sec_rem); + usec_of_cpu_ticks(sys_time.nb_sec_rem); } /** * Get the time in milliseconds since startup. * @return milliseconds since startup as uint32_t */ -static inline uint32_t get_sys_time_msec(void) { +static inline uint32_t get_sys_time_msec(void) +{ return sys_time.nb_sec * 1000 + - msec_of_cpu_ticks(sys_time.nb_sec_rem); + msec_of_cpu_ticks(sys_time.nb_sec_rem); } -static inline void sys_time_usleep(uint32_t us) { +static inline void sys_time_usleep(uint32_t us) +{ usleep(us); } diff --git a/sw/airborne/arch/linux/mcu_periph/uart_arch.c b/sw/airborne/arch/linux/mcu_periph/uart_arch.c index 3ff18110c3..afe3c5aef0 100644 --- a/sw/airborne/arch/linux/mcu_periph/uart_arch.c +++ b/sw/airborne/arch/linux/mcu_periph/uart_arch.c @@ -58,87 +58,88 @@ static inline void uart5_handler(void); static inline void uart6_handler(void); #endif -void uart_periph_set_baudrate(struct uart_periph* periph, uint32_t baud) { - struct SerialPort* port; +void uart_periph_set_baudrate(struct uart_periph *periph, uint32_t baud) +{ + struct SerialPort *port; // close serial port if already open if (periph->reg_addr != NULL) { - port = (struct SerialPort*)(periph->reg_addr); + port = (struct SerialPort *)(periph->reg_addr); serial_port_close(port); serial_port_free(port); } // open serial port port = serial_port_new(); // use register address to store SerialPort structure pointer... - periph->reg_addr = (void*)port; + periph->reg_addr = (void *)port; //TODO: set device name in application and pass as argument // FIXME: paparazzi baud is 9600 for B9600 while open_raw needs 12 for B9600 // /printf("opening %s on uart0 at termios.h baud value=%d\n", periph->dev, baud); - int ret = serial_port_open_raw(port,periph->dev, baud); - if (ret != 0) - { - TRACE("Error opening %s code %d\n",periph->dev,ret); + int ret = serial_port_open_raw(port, periph->dev, baud); + if (ret != 0) { + TRACE("Error opening %s code %d\n", periph->dev, ret); } } -void uart_transmit(struct uart_periph* periph, uint8_t data) { +void uart_transmit(struct uart_periph *periph, uint8_t data) +{ uint16_t temp = (periph->tx_insert_idx + 1) % UART_TX_BUFFER_SIZE; - if (temp == periph->tx_extract_idx) - return; // no room + if (temp == periph->tx_extract_idx) { + return; // no room + } // check if in process of sending data if (periph->tx_running) { // yes, add to queue periph->tx_buf[periph->tx_insert_idx] = data; periph->tx_insert_idx = temp; - } - else { // no, set running flag and write to output register + } else { // no, set running flag and write to output register periph->tx_running = TRUE; - struct SerialPort* port = (struct SerialPort*)(periph->reg_addr); + struct SerialPort *port = (struct SerialPort *)(periph->reg_addr); int ret = write((int)(port->fd), &data, 1); - if (ret < 1) - { - TRACE("w %x [%d]\n",data,ret); + if (ret < 1) { + TRACE("w %x [%d]\n", data, ret); } } } #include -static inline void uart_handler(struct uart_periph* periph) { - unsigned char c='D'; +static inline void uart_handler(struct uart_periph *periph) +{ + unsigned char c = 'D'; - if (periph->reg_addr == NULL) return; // device not initialized ? + if (periph->reg_addr == NULL) { return; } // device not initialized ? - struct SerialPort* port = (struct SerialPort*)(periph->reg_addr); + struct SerialPort *port = (struct SerialPort *)(periph->reg_addr); int fd = port->fd; // check if more data to send if (periph->tx_insert_idx != periph->tx_extract_idx) { int ret = write(fd, &(periph->tx_buf[periph->tx_extract_idx]), 1); - if (ret < 1) - { + if (ret < 1) { TRACE("w %x [%d: %s]\n", periph->tx_buf[periph->tx_extract_idx], ret, strerror(errno)); } periph->tx_extract_idx++; periph->tx_extract_idx %= UART_TX_BUFFER_SIZE; - } - else { + } else { periph->tx_running = FALSE; // clear running flag } - if(read(fd,&c,1) > 0){ + if (read(fd, &c, 1) > 0) { //printf("r %x %c\n",c,c); uint16_t temp = (periph->rx_insert_idx + 1) % UART_RX_BUFFER_SIZE; periph->rx_buf[periph->rx_insert_idx] = c; // check for more room in queue - if (temp != periph->rx_extract_idx) - periph->rx_insert_idx = temp; // update insert index + if (temp != periph->rx_extract_idx) { + periph->rx_insert_idx = temp; // update insert index + } } } -void uart_event(void) { +void uart_event(void) +{ #if USE_UART0 uart0_handler(); #endif @@ -163,85 +164,99 @@ void uart_event(void) { } #if USE_UART0 -void uart0_init( void ) { +void uart0_init(void) +{ uart_periph_init(&uart0); strcpy(uart0.dev, UART0_DEV); uart_periph_set_baudrate(&uart0, UART0_BAUD); } -static inline void uart0_handler(void) { +static inline void uart0_handler(void) +{ uart_handler(&uart0); } #endif /* USE_UART0 */ #if USE_UART1 -void uart1_init( void ) { +void uart1_init(void) +{ uart_periph_init(&uart1); strcpy(uart1.dev, UART1_DEV); uart_periph_set_baudrate(&uart1, UART1_BAUD); } -static inline void uart1_handler(void) { +static inline void uart1_handler(void) +{ uart_handler(&uart1); } #endif /* USE_UART1 */ #if USE_UART2 -void uart2_init(void) { +void uart2_init(void) +{ uart_periph_init(&uart2); strcpy(uart2.dev, UART2_DEV); uart_periph_set_baudrate(&uart2, UART2_BAUD); } -static inline void uart2_handler(void) { +static inline void uart2_handler(void) +{ uart_handler(&uart2); } #endif /* USE_UART2 */ #if USE_UART3 -void uart3_init(void) { +void uart3_init(void) +{ uart_periph_init(&uart3); strcpy(uart3.dev, UART3_DEV); uart_periph_set_baudrate(&uart3, UART3_BAUD); } -static inline void uart3_handler(void) { +static inline void uart3_handler(void) +{ uart_handler(&uart3); } #endif /* USE_UART3 */ #if USE_UART4 -void uart4_init(void) { +void uart4_init(void) +{ uart_periph_init(&uart4); strcpy(uart4.dev, UART4_DEV); uart_periph_set_baudrate(&uart4, UART4_BAUD); } -static inline void uart4_handler(void) { +static inline void uart4_handler(void) +{ uart_handler(&uart4); } #endif /* USE_UART4 */ #if USE_UART5 -void uart5_init(void) { +void uart5_init(void) +{ uart_periph_init(&uart5); strcpy(uart5.dev, UART5_DEV); uart_periph_set_baudrate(&uart5, UART5_BAUD); } -static inline void uart5_handler(void) { +static inline void uart5_handler(void) +{ uart_handler(&uart5); } #endif /* USE_UART5 */ #if USE_UART6 -void uart6_init(void) { +void uart6_init(void) +{ uart_periph_init(&uart6); strcpy(uart6.dev, UART6_DEV); uart_periph_set_baudrate(&uart6, UART6_BAUD); } -static inline void uart6_handler(void) { +static inline void uart6_handler(void) +{ uart_handler(&uart6); } #endif /* USE_UART6 */ diff --git a/sw/airborne/arch/linux/mcu_periph/uart_arch.h b/sw/airborne/arch/linux/mcu_periph/uart_arch.h index e269428480..e0b0ada648 100644 --- a/sw/airborne/arch/linux/mcu_periph/uart_arch.h +++ b/sw/airborne/arch/linux/mcu_periph/uart_arch.h @@ -32,7 +32,8 @@ #include // for conversion between linux baud rate definition and actual speed -static inline int uart_speed(int def) { +static inline int uart_speed(int def) +{ switch (def) { case B1200: return 1200; case B2400: return 2400; diff --git a/sw/airborne/arch/linux/mcu_periph/udp_arch.c b/sw/airborne/arch/linux/mcu_periph/udp_arch.c index 5149c90f5a..e3375bbfff 100644 --- a/sw/airborne/arch/linux/mcu_periph/udp_arch.c +++ b/sw/airborne/arch/linux/mcu_periph/udp_arch.c @@ -29,15 +29,15 @@ #include #include -static inline void udp_create_socket(int* sock, const int protocol, const bool_t reuse_addr, const bool_t broadcast); +static inline void udp_create_socket(int *sock, const int protocol, const bool_t reuse_addr, const bool_t broadcast); /** * Initialize the UDP stream */ -void udp_arch_periph_init(struct udp_periph* p, char* host, int port_out, int port_in, bool_t broadcast) +void udp_arch_periph_init(struct udp_periph *p, char *host, int port_out, int port_in, bool_t broadcast) { - struct UdpNetwork* network = malloc(sizeof(struct UdpNetwork)); + struct UdpNetwork *network = malloc(sizeof(struct UdpNetwork)); if (port_out >= 0) { // Create the output socket (enable reuse of the address, and broadcast if necessary) @@ -58,21 +58,21 @@ void udp_arch_periph_init(struct udp_periph* p, char* host, int port_out, int po network->addr_in.sin_port = htons(port_in); network->addr_in.sin_addr.s_addr = htonl(INADDR_ANY); - bind(network->socket_in, (struct sockaddr*)&network->addr_in, sizeof(network->addr_in)); + bind(network->socket_in, (struct sockaddr *)&network->addr_in, sizeof(network->addr_in)); } - p->network = (void*)network; + p->network = (void *)network; } /** * Read bytes from UDP */ -void udp_receive(struct udp_periph* p) +void udp_receive(struct udp_periph *p) { int16_t i; int16_t available = UDP_RX_BUFFER_SIZE - udp_char_available(p); uint8_t buf[UDP_RX_BUFFER_SIZE]; - struct UdpNetwork* network = (struct UdpNetwork*) p->network; + struct UdpNetwork *network = (struct UdpNetwork *) p->network; if (available <= 0) { return; // No space @@ -80,7 +80,7 @@ void udp_receive(struct udp_periph* p) socklen_t slen = sizeof(struct sockaddr_in); ssize_t byte_read = recvfrom(network->socket_in, buf, UDP_RX_BUFFER_SIZE, MSG_DONTWAIT, - (struct sockaddr*)&network->addr_in, &slen); + (struct sockaddr *)&network->addr_in, &slen); if (byte_read > 0) { for (i = 0; i < byte_read; i++) { @@ -93,13 +93,13 @@ void udp_receive(struct udp_periph* p) /** * Send a message */ -void udp_send_message(struct udp_periph* p) +void udp_send_message(struct udp_periph *p) { - struct UdpNetwork* network = (struct UdpNetwork*) p->network; + struct UdpNetwork *network = (struct UdpNetwork *) p->network; if (p->tx_insert_idx > 0) { ssize_t test __attribute__((unused)) = sendto(network->socket_out, p->tx_buf, p->tx_insert_idx, MSG_DONTWAIT, - (struct sockaddr*)&network->addr_out, sizeof(network->addr_out)); + (struct sockaddr *)&network->addr_out, sizeof(network->addr_out)); p->tx_insert_idx = 0; } } @@ -107,7 +107,7 @@ void udp_send_message(struct udp_periph* p) /** * Create a new udp socket */ -static inline void udp_create_socket(int* sock, const int protocol, const bool_t reuse_addr, const bool_t broadcast) +static inline void udp_create_socket(int *sock, const int protocol, const bool_t reuse_addr, const bool_t broadcast) { // Create the socket with the correct protocl *sock = socket(PF_INET, SOCK_DGRAM, protocol); diff --git a/sw/airborne/arch/linux/serial_port.c b/sw/airborne/arch/linux/serial_port.c index a0a4d2c8cf..9412f45896 100644 --- a/sw/airborne/arch/linux/serial_port.c +++ b/sw/airborne/arch/linux/serial_port.c @@ -13,64 +13,69 @@ #define TRACE(type,fmt,args...) #define TRACE_ERROR 1 -struct SerialPort* serial_port_new(void) { - struct SerialPort* me = malloc(sizeof(struct SerialPort)); +struct SerialPort *serial_port_new(void) +{ + struct SerialPort *me = malloc(sizeof(struct SerialPort)); return me; } -void serial_port_free(struct SerialPort* me) { +void serial_port_free(struct SerialPort *me) +{ free(me); } -void serial_port_flush(struct SerialPort* me) { +void serial_port_flush(struct SerialPort *me) +{ /* * flush any input that might be on the port so we start fresh. */ if (tcflush(me->fd, TCIFLUSH)) { - TRACE(TRACE_ERROR,"%s, set term attr failed: %s (%d)\n", "", strerror(errno), errno); + TRACE(TRACE_ERROR, "%s, set term attr failed: %s (%d)\n", "", strerror(errno), errno); fprintf(stderr, "flush (%d) failed: %s (%d)\n", me->fd, strerror(errno), errno); } } -void serial_port_flush_output(struct SerialPort* me) { +void serial_port_flush_output(struct SerialPort *me) +{ /* * flush any input that might be on the port so we start fresh. */ if (tcflush(me->fd, TCOFLUSH)) { - TRACE(TRACE_ERROR,"%s, set term attr failed: %s (%d)\n", "", strerror(errno), errno); + TRACE(TRACE_ERROR, "%s, set term attr failed: %s (%d)\n", "", strerror(errno), errno); fprintf(stderr, "flush (%d) failed: %s (%d)\n", me->fd, strerror(errno), errno); } } -int serial_port_open_raw(struct SerialPort* me, const char* device, speed_t speed) { +int serial_port_open_raw(struct SerialPort *me, const char *device, speed_t speed) +{ if ((me->fd = open(device, O_RDWR | O_NONBLOCK | O_NOCTTY)) < 0) { - TRACE(TRACE_ERROR,"%s, open failed: %s (%d)\n", device, strerror(errno), errno); + TRACE(TRACE_ERROR, "%s, open failed: %s (%d)\n", device, strerror(errno), errno); return -1; } if (tcgetattr(me->fd, &me->orig_termios) < 0) { - TRACE(TRACE_ERROR,"%s, get term settings failed: %s (%d)\n", device, strerror(errno), errno); + TRACE(TRACE_ERROR, "%s, get term settings failed: %s (%d)\n", device, strerror(errno), errno); close(me->fd); return -1; } me->cur_termios = me->orig_termios; /* input modes */ - me->cur_termios.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|INPCK|ISTRIP|INLCR|IGNCR - |ICRNL |IUCLC|IXON|IXANY|IXOFF|IMAXBEL); + me->cur_termios.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | INPCK | ISTRIP | INLCR | IGNCR + | ICRNL | IUCLC | IXON | IXANY | IXOFF | IMAXBEL); me->cur_termios.c_iflag |= IGNPAR; /* control modes*/ - me->cur_termios.c_cflag &= ~(CSIZE|PARENB|CRTSCTS|PARODD|HUPCL|CSTOPB); - me->cur_termios.c_cflag |= CREAD|CS8|CLOCAL; + me->cur_termios.c_cflag &= ~(CSIZE | PARENB | CRTSCTS | PARODD | HUPCL | CSTOPB); + me->cur_termios.c_cflag |= CREAD | CS8 | CLOCAL; /* local modes */ - me->cur_termios.c_lflag &= ~(ISIG|ICANON|IEXTEN|ECHO|FLUSHO|PENDIN); + me->cur_termios.c_lflag &= ~(ISIG | ICANON | IEXTEN | ECHO | FLUSHO | PENDIN); me->cur_termios.c_lflag |= NOFLSH; if (cfsetispeed(&me->cur_termios, speed)) { - TRACE(TRACE_ERROR,"%s, set term speed failed: %s (%d)\n", device, strerror(errno), errno); + TRACE(TRACE_ERROR, "%s, set term speed failed: %s (%d)\n", device, strerror(errno), errno); close(me->fd); return -1; } if (tcsetattr(me->fd, TCSADRAIN, &me->cur_termios)) { - TRACE(TRACE_ERROR,"%s, set term attr failed: %s (%d)\n", device, strerror(errno), errno); + TRACE(TRACE_ERROR, "%s, set term attr failed: %s (%d)\n", device, strerror(errno), errno); close(me->fd); return -1; } @@ -78,28 +83,29 @@ int serial_port_open_raw(struct SerialPort* me, const char* device, speed_t spe return 0; } -int serial_port_open(struct SerialPort* me, const char* device, - void(*term_conf_callback)(struct termios*, speed_t*)) { +int serial_port_open(struct SerialPort *me, const char *device, + void(*term_conf_callback)(struct termios *, speed_t *)) +{ speed_t speed; if ((me->fd = open(device, O_RDWR | O_NONBLOCK)) < 0) { - TRACE(TRACE_ERROR,"%s, open failed: %s (%d)\n", device, strerror(errno), errno); + TRACE(TRACE_ERROR, "%s, open failed: %s (%d)\n", device, strerror(errno), errno); return -1; } if (tcgetattr(me->fd, &me->orig_termios) < 0) { - TRACE(TRACE_ERROR,"%s, get term settings failed: %s (%d)\n", device, strerror(errno), errno); + TRACE(TRACE_ERROR, "%s, get term settings failed: %s (%d)\n", device, strerror(errno), errno); close(me->fd); return -1; } me->cur_termios = me->orig_termios; term_conf_callback(&me->cur_termios, &speed); if (cfsetispeed(&me->cur_termios, speed)) { - TRACE(TRACE_ERROR,"%s, set term speed failed: %s (%d)\n", device, strerror(errno), errno); + TRACE(TRACE_ERROR, "%s, set term speed failed: %s (%d)\n", device, strerror(errno), errno); close(me->fd); return -1; } if (tcsetattr(me->fd, TCSADRAIN, &me->cur_termios)) { - TRACE(TRACE_ERROR,"%s, set term attr failed: %s (%d)\n", device, strerror(errno), errno); + TRACE(TRACE_ERROR, "%s, set term attr failed: %s (%d)\n", device, strerror(errno), errno); close(me->fd); return -1; } @@ -108,23 +114,25 @@ int serial_port_open(struct SerialPort* me, const char* device, } -void serial_port_close(struct SerialPort* me) { +void serial_port_close(struct SerialPort *me) +{ /* if null pointer or file descriptor indicates error just bail */ - if (!me || me->fd < 0) + if (!me || me->fd < 0) { return; + } if (tcflush(me->fd, TCIOFLUSH)) { - TRACE(TRACE_ERROR,"flushing (%s) (%d)\n", strerror(errno), errno); + TRACE(TRACE_ERROR, "flushing (%s) (%d)\n", strerror(errno), errno); close(me->fd); return; } if (tcsetattr(me->fd, TCSADRAIN, &me->orig_termios)) { // Restore modes. - TRACE(TRACE_ERROR,"restoring term attributes (%s) (%d)\n", strerror(errno), errno); + TRACE(TRACE_ERROR, "restoring term attributes (%s) (%d)\n", strerror(errno), errno); close(me->fd); return; } if (close(me->fd)) { - TRACE(TRACE_ERROR,"closing %s (%d)\n", strerror(errno), errno); + TRACE(TRACE_ERROR, "closing %s (%d)\n", strerror(errno), errno); return; } return; diff --git a/sw/airborne/arch/linux/serial_port.h b/sw/airborne/arch/linux/serial_port.h index 0187b8b149..c72c4a4ce0 100644 --- a/sw/airborne/arch/linux/serial_port.h +++ b/sw/airborne/arch/linux/serial_port.h @@ -31,13 +31,13 @@ struct SerialPort { struct termios cur_termios; /* tty state structure */ }; -extern struct SerialPort* serial_port_new(void); -extern void serial_port_free(struct SerialPort* me); -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*)); -extern void serial_port_close(struct SerialPort* me); +extern struct SerialPort *serial_port_new(void); +extern void serial_port_free(struct SerialPort *me); +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 *)); +extern void serial_port_close(struct SerialPort *me); #endif /* SERIAL_PORT_H */ diff --git a/sw/airborne/arch/linux/subsystems/settings_arch.c b/sw/airborne/arch/linux/subsystems/settings_arch.c index 5f45389286..6ac485ceaf 100644 --- a/sw/airborne/arch/linux/subsystems/settings_arch.c +++ b/sw/airborne/arch/linux/subsystems/settings_arch.c @@ -28,14 +28,16 @@ #include "subsystems/settings.h" -int32_t persistent_write(uint32_t ptr, uint32_t size) { - ptr=ptr; - size=size; +int32_t persistent_write(uint32_t ptr, uint32_t size) +{ + ptr = ptr; + size = size; return -1; } -int32_t persistent_read(uint32_t ptr, uint32_t size) { - ptr=ptr; - size=size; +int32_t persistent_read(uint32_t ptr, uint32_t size) +{ + ptr = ptr; + size = size; return -1; } diff --git a/sw/airborne/arch/lpc21/ADS8344.c b/sw/airborne/arch/lpc21/ADS8344.c index 6e9c593396..685094a546 100644 --- a/sw/airborne/arch/lpc21/ADS8344.c +++ b/sw/airborne/arch/lpc21/ADS8344.c @@ -85,7 +85,8 @@ static uint8_t channel; #define SPI1_VIC_SLOT 7 #endif -void ADS8344_init( void ) { +void ADS8344_init(void) +{ channel = 0; ADS8344_available = FALSE; @@ -105,19 +106,21 @@ void ADS8344_init( void ) { /* setup slave select */ /* configure SS pin */ - SetBit( ADS8344_SS_IODIR, ADS8344_SS_PIN); /* pin is output */ + SetBit(ADS8344_SS_IODIR, ADS8344_SS_PIN); /* pin is output */ ADS8344Unselect(); /* pin low */ } -static inline void read_values( void ) { - uint8_t foo __attribute__ ((unused)) = SSPDR; +static inline void read_values(void) +{ + uint8_t foo __attribute__((unused)) = SSPDR; uint8_t msb = SSPDR; uint8_t lsb = SSPDR; uint8_t llsb = SSPDR; ADS8344_values[channel] = (msb << 8 | lsb) << 1 | llsb >> 7; } -static inline void send_request( void ) { +static inline void send_request(void) +{ uint8_t control = 1 << 7 | channel << 4 | SGL_DIF << 2 | POWER_MODE; SSPDR = control; @@ -126,7 +129,8 @@ static inline void send_request( void ) { SSPDR = 0; } -void ADS8344_start( void ) { +void ADS8344_start(void) +{ ADS8344Select(); SpiClearRti(); SpiEnableRti(); @@ -134,18 +138,19 @@ void ADS8344_start( void ) { send_request(); } -void SPI1_ISR(void) { - ISR_ENTRY(); - LED_TOGGLE(2); - read_values(); - channel++; - if (channel > 7) { - channel = 0; - ADS8344_available = TRUE; - } - send_request(); - SpiClearRti(); +void SPI1_ISR(void) +{ + ISR_ENTRY(); + LED_TOGGLE(2); + read_values(); + channel++; + if (channel > 7) { + channel = 0; + ADS8344_available = TRUE; + } + send_request(); + SpiClearRti(); - VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */ - ISR_EXIT(); + VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */ + ISR_EXIT(); } diff --git a/sw/airborne/arch/lpc21/ADS8344.h b/sw/airborne/arch/lpc21/ADS8344.h index fb755ce7f3..186b00802c 100644 --- a/sw/airborne/arch/lpc21/ADS8344.h +++ b/sw/airborne/arch/lpc21/ADS8344.h @@ -30,7 +30,7 @@ extern uint16_t ADS8344_values[NB_CHANNELS]; extern bool_t ADS8344_available; -void ADS8344_init( void ); -void ADS8344_start( void ); +void ADS8344_init(void); +void ADS8344_start(void); #endif // ADS8344_H diff --git a/sw/airborne/arch/lpc21/armVIC.c b/sw/airborne/arch/lpc21/armVIC.c index edf6b46946..46284cfe87 100644 --- a/sw/airborne/arch/lpc21/armVIC.c +++ b/sw/airborne/arch/lpc21/armVIC.c @@ -21,13 +21,13 @@ static inline unsigned __get_cpsr(void) { unsigned long retval; - asm volatile (" mrs %0, cpsr" : "=r" (retval) : /* no inputs */ ); + asm volatile(" mrs %0, cpsr" : "=r"(retval) : /* no inputs */); return retval; } static inline void __set_cpsr(unsigned val) { - asm volatile (" msr cpsr, %0" : /* no outputs */ : "r" (val) ); + asm volatile(" msr cpsr, %0" : /* no outputs */ : "r"(val)); } unsigned disableIRQ(void) diff --git a/sw/airborne/arch/lpc21/icp_scale.h b/sw/airborne/arch/lpc21/icp_scale.h index 18c10de965..086e9d4a47 100644 --- a/sw/airborne/arch/lpc21/icp_scale.h +++ b/sw/airborne/arch/lpc21/icp_scale.h @@ -10,7 +10,8 @@ volatile uint32_t pulse_len; -static inline void icp_scale_init ( void ) { +static inline void icp_scale_init(void) +{ /* select pin for capture */ ICP_PINSEL |= ICP_PINSEL_VAL << ICP_PINSEL_BIT; /* enable capture 0.2 on falling edge + trigger interrupt */ @@ -18,12 +19,12 @@ static inline void icp_scale_init ( void ) { } -#define ICP_ISR() { \ - static uint32_t last; \ - uint32_t now = T0CR2; \ - pulse_len = now - last; \ - last = now; \ - LED_TOGGLE(2); \ +#define ICP_ISR() { \ + static uint32_t last; \ + uint32_t now = T0CR2; \ + pulse_len = now - last; \ + last = now; \ + LED_TOGGLE(2); \ } diff --git a/sw/airborne/arch/lpc21/led_hw.h b/sw/airborne/arch/lpc21/led_hw.h index 158c5a8d5c..0e5cc2cef4 100644 --- a/sw/airborne/arch/lpc21/led_hw.h +++ b/sw/airborne/arch/lpc21/led_hw.h @@ -27,11 +27,11 @@ #define LED_ON(i) LED_CLR(i) = _BV(LED_PIN(i)); #define LED_OFF(i) LED_SET(i) = _BV(LED_PIN(i)); -#define LED_TOGGLE(i) { \ - if (LED_PIN_REG(i) & _BV(LED_PIN(i))) \ - LED_ON(i) \ - else \ - LED_OFF(i) \ -} +#define LED_TOGGLE(i) { \ + if (LED_PIN_REG(i) & _BV(LED_PIN(i))) \ + LED_ON(i) \ + else \ + LED_OFF(i) \ + } #endif /* LED_HW_H */ diff --git a/sw/airborne/arch/lpc21/link_mcu_hw.h b/sw/airborne/arch/lpc21/link_mcu_hw.h index 0e49ab3ed5..d350fb9d62 100644 --- a/sw/airborne/arch/lpc21/link_mcu_hw.h +++ b/sw/airborne/arch/lpc21/link_mcu_hw.h @@ -32,7 +32,8 @@ #define CrcLow(x) ((x)&0xff) #define CrcHigh(x) ((x)>>8) -static inline uint16_t CrcUpdate(uint16_t crc, uint8_t data) { +static inline uint16_t CrcUpdate(uint16_t crc, uint8_t data) +{ uint8_t a = ((uint8_t)CrcHigh(crc)) + data; uint8_t b = ((uint8_t)CrcLow(crc)) + a; crc = b | a << 8; diff --git a/sw/airborne/arch/lpc21/mcu_arch.c b/sw/airborne/arch/lpc21/mcu_arch.c index cae484595e..f01e1ac3b8 100644 --- a/sw/airborne/arch/lpc21/mcu_arch.c +++ b/sw/airborne/arch/lpc21/mcu_arch.c @@ -37,7 +37,8 @@ /* declare functions and values from crt0.S & the linker control file */ extern void reset(void); -void mcu_arch_init(void) { +void mcu_arch_init(void) +{ /* set PLL multiplier & divisor. */ /* values computed from config.h */ @@ -49,8 +50,9 @@ void mcu_arch_init(void) { PLLFEED = 0x55; /* wait for PLL lock */ - while (!(PLLSTAT & PLLSTAT_LOCK)) + while (!(PLLSTAT & PLLSTAT_LOCK)) { continue; + } /* enable & connect PLL */ PLLCON = PLLCON_PLLE | PLLCON_PLLC; diff --git a/sw/airborne/arch/lpc21/mcu_periph/adc_arch.c b/sw/airborne/arch/lpc21/mcu_periph/adc_arch.c index 75d959204f..6510627fd8 100644 --- a/sw/airborne/arch/lpc21/mcu_periph/adc_arch.c +++ b/sw/airborne/arch/lpc21/mcu_periph/adc_arch.c @@ -48,15 +48,16 @@ /** First NB_ADC for bank 0, others for bank 2 */ -static struct adc_buf* buffers[NB_ADC*2]; +static struct adc_buf *buffers[NB_ADC * 2]; volatile uint16_t adc0_val[NB_ADC] = {1, 2, 3, 4, 5, 6, 7, 8}; volatile uint16_t adc1_val[NB_ADC] = {9, 10, 11, 12, 13, 14, 15, 16}; -void adcISR0 ( void ) __attribute__((naked)); -void adcISR1 ( void ) __attribute__((naked)); +void adcISR0(void) __attribute__((naked)); +void adcISR1(void) __attribute__((naked)); -void adc_buf_channel(uint8_t adc_channel, struct adc_buf* s, uint8_t av_nb_sample) { +void adc_buf_channel(uint8_t adc_channel, struct adc_buf *s, uint8_t av_nb_sample) +{ buffers[adc_channel] = s; s->av_nb_sample = av_nb_sample; } @@ -85,113 +86,114 @@ pin2 AD1.7 P0.22 PINSEL1 1 << 12 static const uint32_t ADC_PINSEL0_ONES = 0 #if defined USE_AD0_6 - | 3 << 8 + | 3 << 8 #endif #if defined USE_AD0_7 - | 3 << 10 + | 3 << 10 #endif #if defined USE_AD1_0 - | 3 << 12 + | 3 << 12 #endif #if defined USE_AD1_1 - | 3 << 16 + | 3 << 16 #endif #if defined USE_AD1_2 - | 3 << 20 + | 3 << 20 #endif #if defined USE_AD1_3 - | 3 << 24 + | 3 << 24 #endif #if defined USE_AD1_4 - | 3 << 26 + | 3 << 26 #endif #if defined USE_AD1_5 - | 3 << 30 + | 3 << 30 #endif -; + ; static const uint32_t ADC_PINSEL1_ONES = 0 #if defined USE_AD0_0 - | 1 << 22 + | 1 << 22 #endif #if defined USE_AD0_1 - | 1 << 24 + | 1 << 24 #endif #if defined USE_AD0_2 - | 1 << 26 + | 1 << 26 #endif #if defined USE_AD0_3 - | 1 << 28 + | 1 << 28 #endif #if defined USE_AD0_4 - | 1 << 18 + | 1 << 18 #endif #if defined USE_AD0_5 - | 1 << 20 + | 1 << 20 #endif #if defined USE_AD1_6 - | 2 << 10 + | 2 << 10 #endif #if defined USE_AD1_7 - | 1 << 12 + | 1 << 12 #endif -; + ; static const uint32_t ADC_AD0CR_SEL_HW_SCAN = 0 #if defined USE_AD0_0 - | 1 << 0 + | 1 << 0 #endif #if defined USE_AD0_1 - | 1 << 1 + | 1 << 1 #endif #if defined USE_AD0_2 - | 1 << 2 + | 1 << 2 #endif #if defined USE_AD0_3 - | 1 << 3 + | 1 << 3 #endif #if defined USE_AD0_4 - | 1 << 4 + | 1 << 4 #endif #if defined USE_AD0_5 - | 1 << 5 + | 1 << 5 #endif #if defined USE_AD0_6 - | 1 << 6 + | 1 << 6 #endif #if defined USE_AD0_7 - | 1 << 7 + | 1 << 7 #endif -; + ; static const uint32_t ADC_AD1CR_SEL_HW_SCAN = 0 #if defined USE_AD1_0 - | 1 << 0 + | 1 << 0 #endif #if defined USE_AD1_1 - | 1 << 1 + | 1 << 1 #endif #if defined USE_AD1_2 - | 1 << 2 + | 1 << 2 #endif #if defined USE_AD1_3 - | 1 << 3 + | 1 << 3 #endif #if defined USE_AD1_4 - | 1 << 4 + | 1 << 4 #endif #if defined USE_AD1_5 - | 1 << 5 + | 1 << 5 #endif #if defined USE_AD1_6 - | 1 << 6 + | 1 << 6 #endif #if defined USE_AD1_7 - | 1 << 7 + | 1 << 7 #endif -; + ; -void adc_init( void ) { +void adc_init(void) +{ /* connect pins for selected ADCs */ PINSEL0 |= ADC_PINSEL0_ONES; @@ -230,17 +232,18 @@ void adc_init( void ) { } -void adcISR0 ( void ) { +void adcISR0(void) +{ ISR_ENTRY(); uint32_t tmp = AD0GDR; uint8_t channel = (uint8_t)(tmp >> 24) & 0x07; uint16_t value = (uint16_t)(tmp >> 6) & 0x03FF; adc0_val[channel] = value; - struct adc_buf* buf = buffers[channel]; + 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; + if (new_head >= buf->av_nb_sample) { new_head = 0; } buf->sum -= buf->values[new_head]; buf->values[new_head] = value; buf->sum += value; @@ -251,16 +254,17 @@ void adcISR0 ( void ) { ISR_EXIT(); // recover registers and return } -void adcISR1 ( void ) { +void adcISR1(void) +{ ISR_ENTRY(); uint32_t tmp = AD1GDR; uint8_t channel = (uint8_t)(tmp >> 24) & 0x07; uint16_t value = (uint16_t)(tmp >> 6) & 0x03FF; adc1_val[channel] = value; - struct adc_buf* buf = buffers[channel+NB_ADC]; + 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; + if (new_head >= buf->av_nb_sample) { new_head = 0; } buf->sum -= buf->values[new_head]; buf->values[new_head] = value; buf->sum += value; diff --git a/sw/airborne/arch/lpc21/mcu_periph/dac_arch.c b/sw/airborne/arch/lpc21/mcu_periph/dac_arch.c index 31f2b6969e..d016bfbd87 100644 --- a/sw/airborne/arch/lpc21/mcu_periph/dac_arch.c +++ b/sw/airborne/arch/lpc21/mcu_periph/dac_arch.c @@ -30,6 +30,7 @@ #include "mcu_periph/dac.h" /* turn on DAC pins */ -void dac_init(void) { +void dac_init(void) +{ PINSEL1 |= 2 << 18; } diff --git a/sw/airborne/arch/lpc21/mcu_periph/dac_arch.h b/sw/airborne/arch/lpc21/mcu_periph/dac_arch.h index d808712f34..be25cb93d4 100644 --- a/sw/airborne/arch/lpc21/mcu_periph/dac_arch.h +++ b/sw/airborne/arch/lpc21/mcu_periph/dac_arch.h @@ -33,7 +33,8 @@ #include "std.h" #include "LPC21xx.h" -static inline void DACSet(uint16_t x) { +static inline void DACSet(uint16_t x) +{ DACR = x << 6; } diff --git a/sw/airborne/arch/lpc21/mcu_periph/gpio_arch.h b/sw/airborne/arch/lpc21/mcu_periph/gpio_arch.h index 614ea356d5..5a5ae46b35 100644 --- a/sw/airborne/arch/lpc21/mcu_periph/gpio_arch.h +++ b/sw/airborne/arch/lpc21/mcu_periph/gpio_arch.h @@ -73,11 +73,13 @@ * @param[in] port * @param[in] gpios If multiple pins are to be changed, use logical OR '|' to separate them. */ -static inline void gpio_setup_output(uint32_t port, uint32_t gpios) { - if (port == 0) +static inline void gpio_setup_output(uint32_t port, uint32_t gpios) +{ + if (port == 0) { IO0DIR |= gpios; - else if (port == 1) + } else if (port == 1) { IO1DIR |= gpios; + } } /** @@ -85,11 +87,13 @@ static inline void gpio_setup_output(uint32_t port, uint32_t gpios) { * @param[in] port * @param[in] gpios If multiple pins are to be changed, use logical OR '|' to separate them. */ -static inline void gpio_setup_input(uint32_t port, uint32_t gpios) { - if (port == 0) +static inline void gpio_setup_input(uint32_t port, uint32_t gpios) +{ + if (port == 0) { IO0DIR &= ~gpios; - else if (port == 1) + } else if (port == 1) { IO1DIR &= ~gpios; + } } /** @@ -97,11 +101,13 @@ static inline void gpio_setup_input(uint32_t port, uint32_t gpios) { * @param[in] port * @param[in] gpios If multiple pins are to be changed, use logical OR '|' to separate them. */ -static inline void gpio_set(uint32_t port, uint32_t gpios) { - if (port == 0) +static inline void gpio_set(uint32_t port, uint32_t gpios) +{ + if (port == 0) { IO0SET = gpios; - else if (port == 1) + } else if (port == 1) { IO1SET = gpios; + } } /** @@ -109,11 +115,13 @@ static inline void gpio_set(uint32_t port, uint32_t gpios) { * @param[in] port * @param[in] gpios If multiple pins are to be changed, use logical OR '|' to separate them. */ -static inline void gpio_clear(uint32_t port, uint32_t gpios) { - if (port == 0) +static inline void gpio_clear(uint32_t port, uint32_t gpios) +{ + if (port == 0) { IO0CLR = gpios; - else if (port == 1) + } else if (port == 1) { IO1CLR = gpios; + } } /** @@ -121,15 +129,15 @@ static inline void gpio_clear(uint32_t port, uint32_t gpios) { * @param[in] port * @param[in] gpios If multiple pins are to be changed, use logical OR '|' to separate them. */ -static inline void gpio_toggle(uint32_t port, uint32_t gpios) { +static inline void gpio_toggle(uint32_t port, uint32_t gpios) +{ if (port == 0) { uint32_t set_gpios = IO0PIN; // clear selected gpio pins which are currently set IO0CLR = set_gpios & gpios; // set selected gpio pins which are currently cleared IO0SET = ~set_gpios & gpios; - } - else if (port == 1) { + } else if (port == 1) { uint32_t set_gpios = IO1PIN; // clear selected gpio pins which are currently set IO1CLR = set_gpios & gpios; @@ -143,11 +151,11 @@ static inline void gpio_toggle(uint32_t port, uint32_t gpios) { * @param[in] port GPIO port (0 or 1) * @param[in] gpios GPIO pin(s). If multiple pins are to be changed, use logical OR '|' to separate them. */ -static inline uint32_t gpio_get(uint32_t port, uint32_t gpios) { +static inline uint32_t gpio_get(uint32_t port, uint32_t gpios) +{ if (port == 0) { return IO0PIN & gpios; - } - else if (port == 1) { + } else if (port == 1) { return IO1PIN & gpios; } } diff --git a/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c b/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c index 364ea930de..0f7821ee6f 100644 --- a/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c +++ b/sw/airborne/arch/lpc21/mcu_periph/i2c_arch.c @@ -37,37 +37,42 @@ // I2C Automaton // /////////////////// -__attribute__ ((always_inline)) static inline void I2cSendStart(struct i2c_periph* p) { +__attribute__((always_inline)) static inline void I2cSendStart(struct i2c_periph *p) +{ p->status = I2CStartRequested; ((i2cRegs_t *)(p->reg_addr))->conset = _BV(STA); } -__attribute__ ((always_inline)) static inline void I2cSendAck(void* reg) { +__attribute__((always_inline)) static inline void I2cSendAck(void *reg) +{ ((i2cRegs_t *)reg)->conset = _BV(AA); } -__attribute__ ((always_inline)) static inline void I2cEndOfTransaction(struct i2c_periph* p) { +__attribute__((always_inline)) static inline void I2cEndOfTransaction(struct i2c_periph *p) +{ // handle fifo here p->trans_extract_idx++; - if (p->trans_extract_idx >= I2C_TRANSACTION_QUEUE_LEN) + if (p->trans_extract_idx >= I2C_TRANSACTION_QUEUE_LEN) { p->trans_extract_idx = 0; + } // if no more transaction to process, stop here, else start next transaction if (p->trans_extract_idx == p->trans_insert_idx) { p->status = I2CIdle; - } - else { + } else { I2cSendStart(p); } } -__attribute__ ((always_inline)) static inline void I2cSendStop(struct i2c_periph* p, struct i2c_transaction* t) { +__attribute__((always_inline)) static inline void I2cSendStop(struct i2c_periph *p, struct i2c_transaction *t) +{ ((i2cRegs_t *)(p->reg_addr))->conset = _BV(STO); // transaction finished with success t->status = I2CTransSuccess; I2cEndOfTransaction(p); } -__attribute__ ((always_inline)) static inline void I2cFail(struct i2c_periph* p, struct i2c_transaction* t) { +__attribute__((always_inline)) static inline void I2cFail(struct i2c_periph *p, struct i2c_transaction *t) +{ ((i2cRegs_t *)(p->reg_addr))->conset = _BV(STO); // transaction failed t->status = I2CTransFailed; @@ -75,41 +80,46 @@ __attribute__ ((always_inline)) static inline void I2cFail(struct i2c_periph* p, I2cEndOfTransaction(p); } -__attribute__ ((always_inline)) static inline void I2cSendByte(void* reg, uint8_t b) { +__attribute__((always_inline)) static inline void I2cSendByte(void *reg, uint8_t b) +{ ((i2cRegs_t *)reg)->dat = b; } -__attribute__ ((always_inline)) static inline void I2cReceive(void* reg, bool_t ack) { - if (ack) ((i2cRegs_t *)reg)->conset = _BV(AA); - else ((i2cRegs_t *)reg)->conclr = _BV(AAC); +__attribute__((always_inline)) static inline void I2cReceive(void *reg, bool_t ack) +{ + if (ack) { ((i2cRegs_t *)reg)->conset = _BV(AA); } + else { ((i2cRegs_t *)reg)->conclr = _BV(AAC); } } -__attribute__ ((always_inline)) static inline void I2cClearStart(void* reg) { +__attribute__((always_inline)) static inline void I2cClearStart(void *reg) +{ ((i2cRegs_t *)reg)->conclr = _BV(STAC); } -__attribute__ ((always_inline)) static inline void I2cClearIT(void* reg) { +__attribute__((always_inline)) static inline void I2cClearIT(void *reg) +{ ((i2cRegs_t *)reg)->conclr = _BV(SIC); } -__attribute__ ((always_inline)) static inline void I2cAutomaton(int32_t state, struct i2c_periph* p) { - struct i2c_transaction* trans = p->trans[p->trans_extract_idx]; +__attribute__((always_inline)) static inline void I2cAutomaton(int32_t state, struct i2c_periph *p) +{ + struct i2c_transaction *trans = p->trans[p->trans_extract_idx]; switch (state) { case I2C_START: case I2C_RESTART: // Set R/W flag switch (trans->type) { case I2CTransRx : - SetBit(trans->slave_addr,0); + SetBit(trans->slave_addr, 0); break; case I2CTransTx: case I2CTransTxRx: - ClearBit(trans->slave_addr,0); + ClearBit(trans->slave_addr, 0); break; default: break; } - I2cSendByte(p->reg_addr,trans->slave_addr); + I2cSendByte(p->reg_addr, trans->slave_addr); I2cClearStart(p->reg_addr); p->idx_buf = 0; break; @@ -117,46 +127,45 @@ __attribute__ ((always_inline)) static inline void I2cAutomaton(int32_t state, s if (p->idx_buf < trans->len_r) { trans->buf[p->idx_buf] = ((i2cRegs_t *)(p->reg_addr))->dat; p->idx_buf++; - I2cReceive(p->reg_addr,p->idx_buf < trans->len_r - 1); - } - else { + I2cReceive(p->reg_addr, p->idx_buf < trans->len_r - 1); + } else { /* error , we should have got NACK */ - I2cFail(p,trans); + I2cFail(p, trans); } break; case I2C_MR_DATA_NACK: if (p->idx_buf < trans->len_r) { trans->buf[p->idx_buf] = ((i2cRegs_t *)(p->reg_addr))->dat; } - I2cSendStop(p,trans); + I2cSendStop(p, trans); break; case I2C_MR_SLA_ACK: /* At least one char */ /* Wait and reply with ACK or NACK */ - I2cReceive(p->reg_addr,p->idx_buf < trans->len_r - 1); + I2cReceive(p->reg_addr, p->idx_buf < trans->len_r - 1); break; case I2C_MR_SLA_NACK: case I2C_MT_SLA_NACK: /* Slave is not responding, transaction is failed */ - I2cFail(p,trans); + I2cFail(p, trans); break; case I2C_MT_SLA_ACK: case I2C_MT_DATA_ACK: if (p->idx_buf < trans->len_w) { - I2cSendByte(p->reg_addr,trans->buf[p->idx_buf]); + I2cSendByte(p->reg_addr, trans->buf[p->idx_buf]); p->idx_buf++; } else { if (trans->type == I2CTransTxRx) { - trans->type = I2CTransRx; /* FIXME should not change type */ + trans->type = I2CTransRx; /* FIXME should not change type */ p->idx_buf = 0; trans->slave_addr |= 1; I2cSendStart(p); } else { - I2cSendStop(p,trans); + I2cSendStop(p, trans); } } break; default: - I2cFail(p,trans); + I2cFail(p, trans); /* FIXME log error */ break; } @@ -204,11 +213,12 @@ __attribute__ ((always_inline)) static inline void I2cAutomaton(int32_t state, s void i2c0_ISR(void) __attribute__((naked)); -void i2c0_ISR(void) { +void i2c0_ISR(void) +{ ISR_ENTRY(); uint32_t state = I2C0STAT; - I2cAutomaton(state,&i2c0); + I2cAutomaton(state, &i2c0); I2cClearIT(i2c0.reg_addr); VICVectAddr = 0x00000000; // clear this interrupt from the VIC @@ -219,11 +229,12 @@ uint8_t i2c0_vic_channel; /* SDA0 on P0.3 */ /* SCL0 on P0.2 */ -void i2c0_hw_init ( void ) { +void i2c0_hw_init(void) +{ i2c0.reg_addr = I2C0; i2c0_vic_channel = VIC_I2C0; - i2c0.init_struct = (void*)(&i2c0_vic_channel); + i2c0.init_struct = (void *)(&i2c0_vic_channel); /* set P0.2 and P0.3 to I2C0 */ PINSEL0 |= 1 << 4 | 1 << 6; @@ -287,11 +298,12 @@ void i2c0_hw_init ( void ) { void i2c1_ISR(void) __attribute__((naked)); -void i2c1_ISR(void) { +void i2c1_ISR(void) +{ ISR_ENTRY(); uint32_t state = I2C1STAT; - I2cAutomaton(state,&i2c1); + I2cAutomaton(state, &i2c1); I2cClearIT(i2c1.reg_addr); VICVectAddr = 0x00000000; // clear this interrupt from the VIC @@ -302,11 +314,12 @@ uint8_t i2c1_vic_channel; /* SDA1 on P0.14 */ /* SCL1 on P0.11 */ -void i2c1_hw_init ( void ) { +void i2c1_hw_init(void) +{ i2c1.reg_addr = I2C1; i2c1_vic_channel = VIC_I2C1; - i2c1.init_struct = (void*)(&i2c1_vic_channel); + i2c1.init_struct = (void *)(&i2c1_vic_channel); /* set P0.11 and P0.14 to I2C1 */ PINSEL0 |= 3 << 22 | 3 << 28; @@ -328,15 +341,17 @@ void i2c1_hw_init ( void ) { #endif /* USE_I2C1 */ -bool_t i2c_idle(struct i2c_periph* p) { +bool_t i2c_idle(struct i2c_periph *p) +{ return p->status == I2CIdle; } -bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t) { +bool_t i2c_submit(struct i2c_periph *p, struct i2c_transaction *t) +{ uint8_t idx; idx = p->trans_insert_idx + 1; - if (idx >= I2C_TRANSACTION_QUEUE_LEN) idx = 0; + if (idx >= I2C_TRANSACTION_QUEUE_LEN) { idx = 0; } if (idx == p->trans_extract_idx) { /* queue full */ p->errors->queue_full_cnt++; @@ -353,8 +368,9 @@ bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t) { p->trans[p->trans_insert_idx] = t; p->trans_insert_idx = idx; /* if peripheral is idle, start the transaction */ - if (p->status == I2CIdle) + if (p->status == I2CIdle) { I2cSendStart(p); + } /* else it will be started by the interrupt handler */ /* when the previous transactions completes */ @@ -367,15 +383,17 @@ bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t) { void i2c_event(void) { } -void i2c_setbitrate(struct i2c_periph* p, int bitrate) +void i2c_setbitrate(struct i2c_periph *p, int bitrate) { int period = 15000000 / 2 / bitrate; // Max 400kpbs - if (period < 19) + if (period < 19) { period = 19; + } // Min 5kbps - if (period > 1500) + if (period > 1500) { period = 1500; + } #if (PCLK == 30000000) period *= 2; diff --git a/sw/airborne/arch/lpc21/mcu_periph/pwm_input_arch.c b/sw/airborne/arch/lpc21/mcu_periph/pwm_input_arch.c index cbf9e3c5e5..df81b8641c 100644 --- a/sw/airborne/arch/lpc21/mcu_periph/pwm_input_arch.c +++ b/sw/airborne/arch/lpc21/mcu_periph/pwm_input_arch.c @@ -49,11 +49,10 @@ #define PWM_INPUT2_PINSEL_MASK (0x3 <reg_addr))->cr0, CPOL); } -__attribute__ ((always_inline)) static inline void SpiClearCPOL(struct spi_periph* p) { +__attribute__((always_inline)) static inline void SpiClearCPOL(struct spi_periph *p) +{ ClearBit(((sspRegs_t *)(p->reg_addr))->cr0, CPOL); } -__attribute__ ((always_inline)) static inline void SpiSetCPHA(struct spi_periph* p) { +__attribute__((always_inline)) static inline void SpiSetCPHA(struct spi_periph *p) +{ SetBit(((sspRegs_t *)(p->reg_addr))->cr0, CPHA); } -__attribute__ ((always_inline)) static inline void SpiClearCPHA(struct spi_periph* p) { +__attribute__((always_inline)) static inline void SpiClearCPHA(struct spi_periph *p) +{ ClearBit(((sspRegs_t *)(p->reg_addr))->cr0, CPHA); } ///@} @@ -115,14 +121,15 @@ __attribute__ ((always_inline)) static inline void SpiClearCPHA(struct spi_perip * @param p SPI peripheral to set * @param dss data size */ -__attribute__ ((always_inline)) static inline void SpiSetDataSize(struct spi_periph* p, enum SPIDataSizeSelect dss) { +__attribute__((always_inline)) static inline void SpiSetDataSize(struct spi_periph *p, enum SPIDataSizeSelect dss) +{ switch (dss) { default: - case SPIDss8bit: - ((sspRegs_t *)(p->reg_addr))->cr0 = (((sspRegs_t *)(p->reg_addr))->cr0 & ~(0xF<reg_addr))->cr0 = (((sspRegs_t *)(p->reg_addr))->cr0 & ~(0xF << DSS)) | (DSS_VAL8 << DSS); break; case SPIDss16bit: - ((sspRegs_t *)(p->reg_addr))->cr0 = (((sspRegs_t *)(p->reg_addr))->cr0 & ~(0xF<reg_addr))->cr0 = (((sspRegs_t *)(p->reg_addr))->cr0 & ~(0xF << DSS)) | (DSS_VAL16 << DSS); break; } } @@ -132,52 +139,64 @@ __attribute__ ((always_inline)) static inline void SpiSetDataSize(struct spi_per * Spi control functions. */ ///@{ -__attribute__ ((always_inline)) static inline void SpiEnable(struct spi_periph* p) { +__attribute__((always_inline)) static inline void SpiEnable(struct spi_periph *p) +{ SetBit(((sspRegs_t *)(p->reg_addr))->cr1, SSE); } -__attribute__ ((always_inline)) static inline void SpiDisable(struct spi_periph* p) { +__attribute__((always_inline)) static inline void SpiDisable(struct spi_periph *p) +{ ClearBit(((sspRegs_t *)(p->reg_addr))->cr1, SSE); } -__attribute__ ((always_inline)) static inline void SpiEnableRti(struct spi_periph* p) { +__attribute__((always_inline)) static inline void SpiEnableRti(struct spi_periph *p) +{ SetBit(((sspRegs_t *)(p->reg_addr))->imsc, RTIM); } -__attribute__ ((always_inline)) static inline void SpiDisableRti(struct spi_periph* p) { +__attribute__((always_inline)) static inline void SpiDisableRti(struct spi_periph *p) +{ ClearBit(((sspRegs_t *)(p->reg_addr))->imsc, RTIM); } -__attribute__ ((always_inline)) static inline void SpiClearRti(struct spi_periph* p) { +__attribute__((always_inline)) static inline void SpiClearRti(struct spi_periph *p) +{ SetBit(((sspRegs_t *)(p->reg_addr))->icr, RTIC); } -__attribute__ ((always_inline)) static inline void SpiEnableTxi(struct spi_periph* p) { +__attribute__((always_inline)) static inline void SpiEnableTxi(struct spi_periph *p) +{ SetBit(((sspRegs_t *)(p->reg_addr))->imsc, TXIM); } -__attribute__ ((always_inline)) static inline void SpiDisableTxi(struct spi_periph* p) { +__attribute__((always_inline)) static inline void SpiDisableTxi(struct spi_periph *p) +{ ClearBit(((sspRegs_t *)(p->reg_addr))->imsc, TXIM); } -__attribute__ ((always_inline)) static inline void SpiEnableRxi(struct spi_periph* p) { +__attribute__((always_inline)) static inline void SpiEnableRxi(struct spi_periph *p) +{ SetBit(((sspRegs_t *)(p->reg_addr))->imsc, RXIM); } -__attribute__ ((always_inline)) static inline void SpiDisableRxi(struct spi_periph* p) { +__attribute__((always_inline)) static inline void SpiDisableRxi(struct spi_periph *p) +{ ClearBit(((sspRegs_t *)(p->reg_addr))->imsc, RXIM); } -__attribute__ ((always_inline)) static inline void SpiSend(struct spi_periph* p, uint16_t c) { +__attribute__((always_inline)) static inline void SpiSend(struct spi_periph *p, uint16_t c) +{ ((sspRegs_t *)(p->reg_addr))->dr = c; } -__attribute__ ((always_inline)) static inline void SpiRead(struct spi_periph* p, uint16_t* c) { +__attribute__((always_inline)) static inline void SpiRead(struct spi_periph *p, uint16_t *c) +{ *c = ((sspRegs_t *)(p->reg_addr))->dr; } -__attribute__ ((always_inline)) static inline void SpiTransmit(struct spi_periph* p, struct spi_transaction* t) { +__attribute__((always_inline)) static inline void SpiTransmit(struct spi_periph *p, struct spi_transaction *t) +{ // when all byte are sent, continue until tx_idx reach input_length // needed when input_length is bigger than output_length uint8_t max_idx = Max(t->output_length, t->input_length); @@ -185,14 +204,12 @@ __attribute__ ((always_inline)) static inline void SpiTransmit(struct spi_periph if (p->tx_idx_buf < t->output_length) { if (t->dss == SPIDss8bit) { SpiSend(p, t->output_buf[p->tx_idx_buf]); - } - else if (t->dss == SPIDss16bit) { - uint16_t tmp1 = t->output_buf[2*p->tx_idx_buf]; // LSB - uint16_t tmp2 = t->output_buf[2*p->tx_idx_buf+1]<<8; // MSB + } else if (t->dss == SPIDss16bit) { + uint16_t tmp1 = t->output_buf[2 * p->tx_idx_buf]; // LSB + uint16_t tmp2 = t->output_buf[2 * p->tx_idx_buf + 1] << 8; // MSB SpiSend(p, tmp1 | tmp2); } - } - else { + } else { SpiSend(p, 0); } p->tx_idx_buf++; @@ -202,44 +219,45 @@ __attribute__ ((always_inline)) static inline void SpiTransmit(struct spi_periph } } -__attribute__ ((always_inline)) static inline void SpiReceive(struct spi_periph* p, struct spi_transaction* t) { +__attribute__((always_inline)) static inline void SpiReceive(struct spi_periph *p, struct spi_transaction *t) +{ while (bit_is_set(((sspRegs_t *)(p->reg_addr))->sr, RNE)) { if (p->rx_idx_buf < t->input_length) { uint16_t r; SpiRead(p, &r); if (t->dss == SPIDss8bit) { t->input_buf[p->rx_idx_buf] = (uint8_t)r; - } - else if (t->dss == SPIDss16bit) { - t->input_buf[2*p->rx_idx_buf] = (uint8_t)r; - t->input_buf[2*p->rx_idx_buf+1] = (uint8_t)(r>>8); + } else if (t->dss == SPIDss16bit) { + t->input_buf[2 * p->rx_idx_buf] = (uint8_t)r; + t->input_buf[2 * p->rx_idx_buf + 1] = (uint8_t)(r >> 8); } p->rx_idx_buf++; - } - else { + } else { uint16_t foo; SpiRead(p, &foo); } } } -__attribute__ ((always_inline)) static inline void SpiInitBuf(struct spi_periph* p, struct spi_transaction* t) { +__attribute__((always_inline)) static inline void SpiInitBuf(struct spi_periph *p, struct spi_transaction *t) +{ p->rx_idx_buf = 0; p->tx_idx_buf = 0; - SpiTransmit(p,t); // fill fifo + SpiTransmit(p, t); // fill fifo } -__attribute__ ((always_inline)) static inline void SpiStart(struct spi_periph* p, struct spi_transaction* t) { +__attribute__((always_inline)) static inline void SpiStart(struct spi_periph *p, struct spi_transaction *t) +{ p->status = SPIRunning; t->status = SPITransRunning; // handle spi options (CPOL, CPHA, data size,...) - if (t->cpol == SPICpolIdleHigh) SpiSetCPOL(p); - else SpiClearCPOL(p); + if (t->cpol == SPICpolIdleHigh) { SpiSetCPOL(p); } + else { SpiClearCPOL(p); } - if (t->cpha == SPICphaEdge2) SpiSetCPHA(p); - else SpiClearCPHA(p); + if (t->cpha == SPICphaEdge2) { SpiSetCPHA(p); } + else { SpiClearCPHA(p); } SpiSetDataSize(p, t->dss); @@ -249,18 +267,19 @@ __attribute__ ((always_inline)) static inline void SpiStart(struct spi_periph* p } // callback function before transaction - if (t->before_cb != 0) t->before_cb(t); + if (t->before_cb != 0) { t->before_cb(t); } // start spi transaction SpiEnable(p); - SpiInitBuf(p,t); + SpiInitBuf(p, t); SpiEnableTxi(p); // enable tx fifo half empty interrupt SpiEnableRti(p); // enable rx timeout interrupt } -__attribute__ ((always_inline)) static inline void SpiEndOfTransaction(struct spi_periph* p, struct spi_transaction* t) { +__attribute__((always_inline)) static inline void SpiEndOfTransaction(struct spi_periph *p, struct spi_transaction *t) +{ // callback function after transaction - if (t->after_cb != 0) t->after_cb(t); + if (t->after_cb != 0) { t->after_cb(t); } // handle slave unselect if (t->select == SPISelectUnselect || t->select == SPIUnselect) { @@ -273,20 +292,21 @@ __attribute__ ((always_inline)) static inline void SpiEndOfTransaction(struct sp // handle transaction fifo here p->trans_extract_idx++; - if (p->trans_extract_idx >= SPI_TRANSACTION_QUEUE_LEN) + if (p->trans_extract_idx >= SPI_TRANSACTION_QUEUE_LEN) { p->trans_extract_idx = 0; + } // if no more transaction to process or locked, stop here, else start next transaction if (p->trans_extract_idx == p->trans_insert_idx || p->suspend) { p->status = SPIIdle; - } - else { - SpiStart(p,p->trans[p->trans_extract_idx]); + } else { + SpiStart(p, p->trans[p->trans_extract_idx]); } } -__attribute__ ((always_inline)) static inline void SpiAutomaton(struct spi_periph* p) { - struct spi_transaction* trans = p->trans[p->trans_extract_idx]; +__attribute__((always_inline)) static inline void SpiAutomaton(struct spi_periph *p) +{ + struct spi_transaction *trans = p->trans[p->trans_extract_idx]; /* Tx fifo is half empty */ if (bit_is_set(((sspRegs_t *)(p->reg_addr))->mis, TXMIS)) { @@ -296,8 +316,7 @@ __attribute__ ((always_inline)) static inline void SpiAutomaton(struct spi_perip if (p->tx_idx_buf >= trans->output_length && p->rx_idx_buf == trans->input_length) { if (bit_is_set(((sspRegs_t *)(p->reg_addr))->sr, BSY)) { SpiEnableTxi(p); // FIXME in case Rti is not called - } - else { + } else { SpiDisableRti(p); SpiClearRti(p); /* clear interrupt */ SpiEndOfTransaction(p, trans); @@ -314,22 +333,24 @@ __attribute__ ((always_inline)) static inline void SpiAutomaton(struct spi_perip } } -__attribute__ ((always_inline)) static inline void SpiSlaveStart(struct spi_periph* p, struct spi_transaction* t) { +__attribute__((always_inline)) static inline void SpiSlaveStart(struct spi_periph *p, struct spi_transaction *t) +{ p->status = SPIRunning; t->status = SPITransRunning; // callback function before transaction - if (t->before_cb != 0) t->before_cb(t); + if (t->before_cb != 0) { t->before_cb(t); } // start spi transaction SpiEnable(p); - SpiInitBuf(p,t); + SpiInitBuf(p, t); SpiEnableTxi(p); // enable tx fifo half empty interrupt //SpiEnableRti(p); // enable rx timeout interrupt } -__attribute__ ((always_inline)) static inline void SpiSlaveAutomaton(struct spi_periph* p) { - struct spi_transaction* trans = p->trans[p->trans_extract_idx]; +__attribute__((always_inline)) static inline void SpiSlaveAutomaton(struct spi_periph *p) +{ + struct spi_transaction *trans = p->trans[p->trans_extract_idx]; /* Tx fifo is half empty */ if (bit_is_set(((sspRegs_t *)(p->reg_addr))->mis, TXMIS)) { @@ -345,7 +366,7 @@ __attribute__ ((always_inline)) static inline void SpiSlaveAutomaton(struct spi_ SpiDisableRti(p); // callback function after transaction - if (trans->after_cb != 0) trans->after_cb(trans); + if (trans->after_cb != 0) { trans->after_cb(trans); } SpiDisable(p); // end transaction with success @@ -394,11 +415,12 @@ __attribute__ ((always_inline)) static inline void SpiSlaveAutomaton(struct spi_ uint8_t spi0_vic_slot; -void spi0_arch_init(void) { +void spi0_arch_init(void) +{ spi0.reg_addr = SPI0; spi0_vic_slot = VIC_SPI0; - spi0.init_struct = (void*)(&spi0_vic_slot); + spi0.init_struct = (void *)(&spi0_vic_slot); // TODO set spi0 and interrupt vector } @@ -431,7 +453,8 @@ void spi0_arch_init(void) { void spi1_ISR(void) __attribute__((naked)); -void spi1_ISR(void) { +void spi1_ISR(void) +{ ISR_ENTRY(); SpiAutomaton(&spi1); @@ -442,11 +465,12 @@ void spi1_ISR(void) { uint8_t spi1_vic_slot; -void spi1_arch_init(void) { +void spi1_arch_init(void) +{ spi1.reg_addr = SPI1; spi1_vic_slot = VIC_SPI1; - spi1.init_struct = (void*)(&spi1_vic_slot); + spi1.init_struct = (void *)(&spi1_vic_slot); /* setup pins for SSP (SCK, MISO, MOSI) */ PINSEL1 |= SSP_PINSEL1_SCK | SSP_PINSEL1_MISO | SSP_PINSEL1_MOSI; @@ -467,12 +491,13 @@ void spi1_arch_init(void) { #endif -bool_t spi_submit(struct spi_periph* p, struct spi_transaction* t) { +bool_t spi_submit(struct spi_periph *p, struct spi_transaction *t) +{ //unsigned cpsr; uint8_t idx; idx = p->trans_insert_idx + 1; - if (idx >= SPI_TRANSACTION_QUEUE_LEN) idx = 0; + if (idx >= SPI_TRANSACTION_QUEUE_LEN) { idx = 0; } if (idx == p->trans_extract_idx) { t->status = SPITransFailed; return FALSE; /* queue full */ @@ -490,7 +515,7 @@ bool_t spi_submit(struct spi_periph* p, struct spi_transaction* t) { p->trans_insert_idx = idx; /* if peripheral is idle and not locked, start the transaction */ if (p->status == SPIIdle && !p->suspend) { - SpiStart(p,p->trans[p->trans_extract_idx]); + SpiStart(p, p->trans[p->trans_extract_idx]); } //cpsr = disableIRQ(); // disable global interrupts @@ -501,7 +526,8 @@ bool_t spi_submit(struct spi_periph* p, struct spi_transaction* t) { } -void spi_init_slaves(void) { +void spi_init_slaves(void) +{ #if USE_SPI_SLAVE0 /* setup slave0_select pin * slave0_select is output @@ -525,16 +551,19 @@ void spi_init_slaves(void) { #endif } -void spi_slave_select(uint8_t slave) { +void spi_slave_select(uint8_t slave) +{ SpiSlaveSelect(slave); } -void spi_slave_unselect(uint8_t slave) { +void spi_slave_unselect(uint8_t slave) +{ SpiSlaveUnselect(slave); } -bool_t spi_lock(struct spi_periph* p, uint8_t slave) { - uint8_t* vic = (uint8_t*)(p->init_struct); +bool_t spi_lock(struct spi_periph *p, uint8_t slave) +{ + uint8_t *vic = (uint8_t *)(p->init_struct); VICIntEnClear = VIC_BIT(*vic); if (slave < 254 && p->suspend == 0) { p->suspend = slave + 1; // 0 is reserved for unlock state @@ -545,14 +574,15 @@ bool_t spi_lock(struct spi_periph* p, uint8_t slave) { return FALSE; } -bool_t spi_resume(struct spi_periph* p, uint8_t slave) { - uint8_t* vic = (uint8_t*)(p->init_struct); +bool_t spi_resume(struct spi_periph *p, uint8_t slave) +{ + uint8_t *vic = (uint8_t *)(p->init_struct); VICIntEnClear = VIC_BIT(*vic); if (p->suspend == slave + 1) { // restart fifo p->suspend = 0; if (p->trans_extract_idx != p->trans_insert_idx && p->status == SPIIdle) { - SpiStart(p,p->trans[p->trans_extract_idx]); + SpiStart(p, p->trans[p->trans_extract_idx]); } VICIntEnable = VIC_BIT(*vic); return TRUE; @@ -613,7 +643,8 @@ bool_t spi_resume(struct spi_periph* p, uint8_t slave) { void spi1_slave_ISR(void) __attribute__((naked)); -void spi1_slave_ISR(void) { +void spi1_slave_ISR(void) +{ ISR_ENTRY(); SpiSlaveAutomaton(&spi1); @@ -622,7 +653,8 @@ void spi1_slave_ISR(void) { ISR_EXIT(); } -void spi1_slave_arch_init(void) { +void spi1_slave_arch_init(void) +{ spi1.reg_addr = SPI1; @@ -645,7 +677,8 @@ void spi1_slave_arch_init(void) { #endif /** Register one (and only one) transaction to use spi as slave */ -bool_t spi_slave_register(struct spi_periph* p, struct spi_transaction* t) { +bool_t spi_slave_register(struct spi_periph *p, struct spi_transaction *t) +{ if (p->trans_insert_idx >= 1) { t->status = SPITransFailed; @@ -657,18 +690,19 @@ bool_t spi_slave_register(struct spi_periph* p, struct spi_transaction* t) { p->trans_insert_idx = 1; // handle spi options (CPOL, CPHA, data size,...) - if (t->cpol == SPICpolIdleHigh) SpiSetCPOL(p); - else SpiClearCPOL(p); + if (t->cpol == SPICpolIdleHigh) { SpiSetCPOL(p); } + else { SpiClearCPOL(p); } - if (t->cpha == SPICphaEdge2) SpiSetCPHA(p); - else SpiClearCPHA(p); + if (t->cpha == SPICphaEdge2) { SpiSetCPHA(p); } + else { SpiClearCPHA(p); } SpiSetDataSize(p, t->dss); return TRUE; } -bool_t spi_slave_wait(struct spi_periph* p) { +bool_t spi_slave_wait(struct spi_periph *p) +{ if (p->trans_insert_idx == 0) { // no transaction registered return FALSE; 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 65d8614062..b324c6c09a 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 @@ -94,9 +94,9 @@ static void SSP_ISR(void) __attribute__((naked)); #define SSP_PINSEL1_SSEL (2<<8) -#define SSP_Write(X) SSPDR=(X) -#define SSP_Read() SSPDR -#define SSP_Status() SSPSR +#define SSP_Write(X) SSPDR=(X) +#define SSP_Read() SSPDR +#define SSP_Status() SSPSR /** default initial settings */ #ifndef SPI1_VIC_SLOT @@ -104,7 +104,8 @@ static void SSP_ISR(void) __attribute__((naked)); #endif -void spi_slave_hs_init(void) { +void spi_slave_hs_init(void) +{ /* setup pins for SSP (SCK, MISO, MOSI) */ PINSEL1 |= SSP_PINSEL1_SCK | SSP_PINSEL1_MISO | SSP_PINSEL1_MOSI | SSP_PINSEL1_SSEL; @@ -117,8 +118,8 @@ void spi_slave_hs_init(void) { SSPCPSR = CPSDVSR; /* initialize interrupt vector */ - VICIntSelect &= ~VIC_BIT( VIC_SPI1 ); /* SPI1 selected as IRQ */ - VICIntEnable = VIC_BIT( VIC_SPI1 ); /* enable it */ + VICIntSelect &= ~VIC_BIT(VIC_SPI1); /* SPI1 selected as IRQ */ + VICIntEnable = VIC_BIT(VIC_SPI1); /* enable it */ _VIC_CNTL(SPI1_VIC_SLOT) = VIC_ENABLE | VIC_SPI1; _VIC_ADDR(SPI1_VIC_SLOT) = (uint32_t)SSP_ISR; /* address of the ISR */ @@ -132,30 +133,28 @@ void spi_slave_hs_init(void) { } /* - * SSP Status: + * SSP Status: * - * ROVR Read Overrun - * WCOL Write Collision (send new byte during a transfer in progress - * ABRT SSEL inactive before end of transfer + * ROVR Read Overrun + * WCOL Write Collision (send new byte during a transfer in progress + * ABRT SSEL inactive before end of transfer * * */ -static void SSP_ISR(void) { +static void SSP_ISR(void) +{ ISR_ENTRY(); //LED_TOGGLE(3); // If any TX bytes are pending - if (spi_slave_hs_tx_insert_idx != spi_slave_hs_tx_extract_idx) - { + if (spi_slave_hs_tx_insert_idx != spi_slave_hs_tx_extract_idx) { uint8_t ret = spi_slave_hs_tx_buffer[spi_slave_hs_tx_extract_idx]; - spi_slave_hs_tx_extract_idx = (spi_slave_hs_tx_extract_idx + 1)%SPI_SLAVE_HS_TX_BUFFER_SIZE; + spi_slave_hs_tx_extract_idx = (spi_slave_hs_tx_extract_idx + 1) % SPI_SLAVE_HS_TX_BUFFER_SIZE; SSP_Write(ret); - } - else - { + } else { SSP_Write(0x00); } @@ -165,39 +164,40 @@ static void SSP_ISR(void) { uint16_t temp; // calc next insert index & store character - temp = ( spi_slave_hs_rx_insert_idx + 1) % SPI_SLAVE_HS_RX_BUFFER_SIZE; + temp = (spi_slave_hs_rx_insert_idx + 1) % SPI_SLAVE_HS_RX_BUFFER_SIZE; spi_slave_hs_rx_buffer[ spi_slave_hs_rx_insert_idx] = SSP_Read(); // check for more room in queue - if (temp != spi_slave_hs_rx_extract_idx) - spi_slave_hs_rx_insert_idx = temp; // update insert index + if (temp != spi_slave_hs_rx_extract_idx) { + spi_slave_hs_rx_insert_idx = temp; // update insert index + } // else overrun } // while FIFO not empty //while (SSPSR & RNE); -/* - // loop until not more interrupt sources - while (((iid = U0IIR) & UIIR_NO_INT) == 0) - while (U0LSR & ULSR_THRE) - { - // check if more data to send - if (uart0_tx_insert_idx != uart0_tx_extract_idx) + /* + // loop until not more interrupt sources + while (((iid = U0IIR) & UIIR_NO_INT) == 0) + while (U0LSR & ULSR_THRE) { - U0THR = uart0_tx_buffer[uart0_tx_extract_idx]; - uart0_tx_extract_idx++; - uart0_tx_extract_idx %= UART0_TX_BUFFER_SIZE; + // check if more data to send + 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_BUFFER_SIZE; + } + else + { + // no + uart0_tx_running = 0; // clear running flag + break; + } } - else - { - // no - uart0_tx_running = 0; // clear running flag - break; - } - } -*/ + */ VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */ ISR_EXIT(); } diff --git a/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.h b/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.h index 8a57f5563e..957931742c 100644 --- a/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.h +++ b/sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.h @@ -34,12 +34,12 @@ #include "std.h" -#define SpiEnable() { \ - SetBit(SSPCR1, SSE); \ +#define SpiEnable() { \ + SetBit(SSPCR1, SSE); \ } -#define SpiDisable() { \ - ClearBit(SSPCR1, SSE); \ +#define SpiDisable() { \ + ClearBit(SSPCR1, SSE); \ } @@ -51,10 +51,10 @@ extern uint8_t spi_slave_hs_rx_buffer[SPI_SLAVE_HS_RX_BUFFER_SIZE]; #define SpiSlaveChAvailable() (spi_slave_hs_rx_insert_idx != spi_slave_hs_rx_extract_idx) #define SpiSlaveGetch() ({\ - uint8_t ret = spi_slave_hs_rx_buffer[spi_slave_hs_rx_extract_idx]; \ - spi_slave_hs_rx_extract_idx = (spi_slave_hs_rx_extract_idx + 1)%SPI_SLAVE_HS_RX_BUFFER_SIZE; \ - ret; \ -}) + uint8_t ret = spi_slave_hs_rx_buffer[spi_slave_hs_rx_extract_idx]; \ + spi_slave_hs_rx_extract_idx = (spi_slave_hs_rx_extract_idx + 1)%SPI_SLAVE_HS_RX_BUFFER_SIZE; \ + ret; \ + }) #define SPI_SLAVE_HS_TX_BUFFER_SIZE 64 @@ -62,13 +62,13 @@ extern uint8_t spi_slave_hs_tx_insert_idx, spi_slave_hs_tx_extract_idx; extern uint8_t spi_slave_hs_tx_buffer[SPI_SLAVE_HS_TX_BUFFER_SIZE]; #define SpiSlaveTransmit(data) {\ - uint8_t temp = (spi_slave_hs_tx_insert_idx + 1) % SPI_SLAVE_HS_TX_BUFFER_SIZE; \ - if (temp != spi_slave_hs_tx_extract_idx) /* there is room left */ \ - { \ - spi_slave_hs_tx_buffer[spi_slave_hs_tx_insert_idx] = (uint8_t)data; \ - spi_slave_hs_tx_insert_idx = temp; \ - } \ -} + uint8_t temp = (spi_slave_hs_tx_insert_idx + 1) % SPI_SLAVE_HS_TX_BUFFER_SIZE; \ + if (temp != spi_slave_hs_tx_extract_idx) /* there is room left */ \ + { \ + spi_slave_hs_tx_buffer[spi_slave_hs_tx_insert_idx] = (uint8_t)data; \ + spi_slave_hs_tx_insert_idx = temp; \ + } \ + } diff --git a/sw/airborne/arch/lpc21/mcu_periph/sys_time_arch.c b/sw/airborne/arch/lpc21/mcu_periph/sys_time_arch.c index 36f140aee4..0e0ec59f4c 100644 --- a/sw/airborne/arch/lpc21/mcu_periph/sys_time_arch.c +++ b/sw/airborne/arch/lpc21/mcu_periph/sys_time_arch.c @@ -102,7 +102,8 @@ AMI601_IT) -void sys_time_arch_init( void ) { +void sys_time_arch_init(void) +{ sys_time.cpu_ticks_per_sec = PCLK / T0_PCLK_DIV; /* cpu ticks per desired sys_time timer step */ sys_time.resolution_cpu_ticks = (uint32_t)(sys_time.resolution * sys_time.cpu_ticks_per_sec + 0.5); @@ -127,7 +128,7 @@ void sys_time_arch_init( void ) { * phase, if we miss the first one, the * sys_tick_handler is not called afterward */ - T0MR0 = 4*sys_time.resolution_cpu_ticks; + T0MR0 = 4 * sys_time.resolution_cpu_ticks; /* enable timer 0 */ T0TCR = TCR_ENABLE; @@ -148,7 +149,8 @@ void sys_time_arch_init( void ) { // 97 days at 512hz // 12 hours at 100khz // -static inline void sys_tick_irq_handler(void) { +static inline void sys_tick_irq_handler(void) +{ /* set match register for next interrupt */ T0MR0 += sys_time.resolution_cpu_ticks - 1; @@ -162,7 +164,7 @@ static inline void sys_tick_irq_handler(void) { LED_TOGGLE(SYS_TIME_LED); #endif } - for (unsigned int i=0; i= sys_time.timer[i].end_time) { sys_time.timer[i].end_time += sys_time.timer[i].duration; @@ -174,18 +176,19 @@ static inline void sys_tick_irq_handler(void) { } } -void TIMER0_ISR ( void ) { +void TIMER0_ISR(void) +{ ISR_ENTRY(); while (T0IR & TIMER0_IT_MASK) { - if (T0IR&SYS_TICK_IT) { + if (T0IR & SYS_TICK_IT) { sys_tick_irq_handler(); T0IR = SYS_TICK_IT; } #if defined ACTUATORS && ( defined SERVOS_4017 || defined SERVOS_4015_MAT || defined SERVOS_PPM_MAT) - if (T0IR&ACTUATORS_IT) { + if (T0IR & ACTUATORS_IT) { #ifdef SERVOS_4017 SERVOS_4017_ISR(); #endif @@ -200,44 +203,44 @@ void TIMER0_ISR ( void ) { #endif /* ACTUATORS && (SERVOS_4017 || SERVOS_4015_MAT || SERVOS_PPM_MAT) */ #if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_PPM - if (T0IR&PPM_IT) { + if (T0IR & PPM_IT) { PPM_ISR(); T0IR = PPM_IT; } #endif #ifdef TRIGGER_EXT - if (T0IR&TRIGGER_IT) { + if (T0IR & TRIGGER_IT) { TRIG_ISR(); T0IR = TRIGGER_IT; LED_TOGGLE(3); } #endif #ifdef MB_SCALE - if (T0IR&MB_SCALE_IT) { + if (T0IR & MB_SCALE_IT) { MB_SCALE_ICP_ISR(); T0IR = MB_SCALE_IT; } #endif #ifdef MB_TACHO - if (T0IR&MB_TACHO_IT) { + if (T0IR & MB_TACHO_IT) { MB_TACHO_ISR(); T0IR = MB_TACHO_IT; } #endif #ifdef USE_PWM_INPUT1 - if (T0IR&PWM_INPUT_IT1) { + if (T0IR & PWM_INPUT_IT1) { PWM_INPUT_ISR_1(); T0IR = PWM_INPUT_IT1; } #endif #ifdef USE_PWM_INPUT2 - if (T0IR&PWM_INPUT_IT2) { + if (T0IR & PWM_INPUT_IT2) { PWM_INPUT_ISR_2(); T0IR = PWM_INPUT_IT2; } #endif #ifdef USE_AMI601 - if (T0IR&AMI601_IT) { + if (T0IR & AMI601_IT) { AMI601_ISR(); T0IR = AMI601_IT; } diff --git a/sw/airborne/arch/lpc21/mcu_periph/sys_time_arch.h b/sw/airborne/arch/lpc21/mcu_periph/sys_time_arch.h index 38204d9268..404b097ae0 100644 --- a/sw/airborne/arch/lpc21/mcu_periph/sys_time_arch.h +++ b/sw/airborne/arch/lpc21/mcu_periph/sys_time_arch.h @@ -39,7 +39,7 @@ #include "std.h" -void TIMER0_ISR ( void ) __attribute__((naked)); +void TIMER0_ISR(void) __attribute__((naked)); /* T0 prescaler, set T0_CLK to 15MHz, T0_CLK = PCLK / T0PCLK_DIV */ #if (PCLK == 15000000) @@ -64,7 +64,8 @@ void TIMER0_ISR ( void ) __attribute__((naked)); * WARNING: overflows after ?? * @return microseconds since startup as uint32_t */ -static inline uint32_t get_sys_time_usec(void) { +static inline uint32_t get_sys_time_usec(void) +{ return usec_of_cpu_ticks(T0TC); } @@ -72,7 +73,8 @@ static inline uint32_t get_sys_time_usec(void) { * Get the time in milliseconds since startup. * @return milliseconds since startup as uint32_t */ -static inline uint32_t get_sys_time_msec(void) { +static inline uint32_t get_sys_time_msec(void) +{ return msec_of_cpu_ticks(T0TC); } @@ -82,10 +84,11 @@ static inline uint32_t get_sys_time_msec(void) { #define SysTickTimerStop(_t) { _t = (T0TC - _t); } /** Busy wait, in microseconds */ -static inline void sys_time_usleep(uint32_t us) { +static inline void sys_time_usleep(uint32_t us) +{ uint32_t start = T0TC; uint32_t ticks = cpu_ticks_of_usec(us); - while ((uint32_t)(T0TC-start) < ticks); + while ((uint32_t)(T0TC - start) < ticks); } #endif /* SYS_TIME_ARCH_H */ diff --git a/sw/airborne/arch/lpc21/mcu_periph/uart_arch.c b/sw/airborne/arch/lpc21/mcu_periph/uart_arch.c index 4af82a128b..4abae2266c 100644 --- a/sw/airborne/arch/lpc21/mcu_periph/uart_arch.c +++ b/sw/airborne/arch/lpc21/mcu_periph/uart_arch.c @@ -31,7 +31,8 @@ #include "mcu_periph/uart.h" #include "armVIC.h" -static inline void uart_disable_interrupts(struct uart_periph* p) { +static inline void uart_disable_interrupts(struct uart_periph *p) +{ // disable interrups ((uartRegs_t *)(p->reg_addr))->ier = 0x00; // disable all interrupts ((uartRegs_t *)(p->reg_addr))->iir; // clear interrupt ID @@ -39,12 +40,14 @@ static inline void uart_disable_interrupts(struct uart_periph* p) { ((uartRegs_t *)(p->reg_addr))->lsr; // clear line status register } -static inline void uart_enable_interrupts(struct uart_periph* p) { +static inline void uart_enable_interrupts(struct uart_periph *p) +{ // enable receiver interrupts ((uartRegs_t *)(p->reg_addr))->ier = UIER_ERBFI; } -static inline void uart_set_baudrate(struct uart_periph* p, uint32_t baud) { +static inline void uart_set_baudrate(struct uart_periph *p, uint32_t baud) +{ /* calculate the baudrate */ uint32_t _baud_reg_val = (uint16_t)((PCLK / (((float)baud) * 16.0)) + 0.5); /* select divisor latches */ @@ -62,17 +65,21 @@ static inline void uart_set_baudrate(struct uart_periph* p, uint32_t baud) { ((uartRegs_t *)(p->reg_addr))->fcr = UART_FIFO_8; } -void uart_periph_set_baudrate(struct uart_periph* p, uint32_t baud) { +void uart_periph_set_baudrate(struct uart_periph *p, uint32_t baud) +{ uart_disable_interrupts(p); uart_set_baudrate(p, baud); uart_enable_interrupts(p); } -void uart_periph_set_bits_stop_parity(struct uart_periph* p __attribute__((unused)), uint8_t bits __attribute__((unused)), uint8_t stop __attribute__((unused)), uint8_t __attribute__((unused)) parity) { +void uart_periph_set_bits_stop_parity(struct uart_periph *p __attribute__((unused)), + uint8_t bits __attribute__((unused)), uint8_t stop __attribute__((unused)), uint8_t __attribute__((unused)) parity) +{ // TBD } -void uart_transmit(struct uart_periph* p, uint8_t data ) { +void uart_transmit(struct uart_periph *p, uint8_t data) +{ uint16_t temp; unsigned cpsr; @@ -102,23 +109,20 @@ void uart_transmit(struct uart_periph* p, uint8_t data ) { restoreIRQ(cpsr); // restore global interrupts } -static inline void uart_ISR(struct uart_periph* p) +static inline void uart_ISR(struct uart_periph *p) { uint8_t iid; // loop until not more interrupt sources - while (((iid = ((uartRegs_t *)(p->reg_addr))->iir) & UIIR_NO_INT) == 0) - { + while (((iid = ((uartRegs_t *)(p->reg_addr))->iir) & UIIR_NO_INT) == 0) { // identify & process the highest priority interrupt - switch (iid & UIIR_ID_MASK) - { + switch (iid & UIIR_ID_MASK) { case UIIR_RLS_INT: // Receive Line Status ((uartRegs_t *)(p->reg_addr))->lsr; // read LSR to clear break; case UIIR_CTI_INT: // Character Timeout Indicator case UIIR_RDA_INT: // Receive Data Available - do - { + do { uint16_t temp; // calc next insert index & store character @@ -126,25 +130,21 @@ static inline void uart_ISR(struct uart_periph* p) p->rx_buf[p->rx_insert_idx] = ((uartRegs_t *)(p->reg_addr))->rbr; // check for more room in queue - if (temp != p->rx_extract_idx) - p->rx_insert_idx = temp; // update insert index - } - while (((uartRegs_t *)(p->reg_addr))->lsr & ULSR_RDR); + if (temp != p->rx_extract_idx) { + p->rx_insert_idx = temp; // update insert index + } + } while (((uartRegs_t *)(p->reg_addr))->lsr & ULSR_RDR); break; case UIIR_THRE_INT: // Transmit Holding Register Empty - while (((uartRegs_t *)(p->reg_addr))->lsr & ULSR_THRE) - { + while (((uartRegs_t *)(p->reg_addr))->lsr & ULSR_THRE) { // check if more data to send - if (p->tx_insert_idx != p->tx_extract_idx) - { + if (p->tx_insert_idx != p->tx_extract_idx) { ((uartRegs_t *)(p->reg_addr))->thr = p->tx_buf[p->tx_extract_idx]; p->tx_extract_idx++; p->tx_extract_idx %= UART_TX_BUFFER_SIZE; - } - else - { + } else { // no p->tx_running = 0; // clear running flag break; @@ -169,12 +169,12 @@ static inline void uart_ISR(struct uart_periph* p) /* by default enable UART Tx and Rx */ #ifndef USE_UART0_TX - #ifdef USE_UART0_RX_ONLY - #warning "USE_UART0_RX_ONLY is deprecated, please set USE_UART0_TX=FALSE instead" - #define USE_UART0_TX FALSE - #else - #define USE_UART0_TX TRUE - #endif +#ifdef USE_UART0_RX_ONLY +#warning "USE_UART0_RX_ONLY is deprecated, please set USE_UART0_TX=FALSE instead" +#define USE_UART0_TX FALSE +#else +#define USE_UART0_TX TRUE +#endif #endif #ifndef USE_UART0_RX #define USE_UART0_RX TRUE @@ -182,7 +182,8 @@ static inline void uart_ISR(struct uart_periph* p) void uart0_ISR(void) __attribute__((naked)); -void uart0_ISR(void) { +void uart0_ISR(void) +{ // perform proper ISR entry so thumb-interwork works properly ISR_ENTRY(); @@ -192,7 +193,8 @@ void uart0_ISR(void) { ISR_EXIT(); // recover registers and return } -void uart0_init( void ) { +void uart0_init(void) +{ uart_periph_init(&uart0); uart0.reg_addr = UART0_BASE; @@ -230,12 +232,12 @@ void uart0_init( void ) { /* by default enable UART Tx and Rx */ #ifndef USE_UART1_TX - #ifdef USE_UART1_RX_ONLY - #warning "USE_UART1_RX_ONLY is deprecated, please set USE_UART1_TX=FALSE instead" - #define USE_UART1_TX FALSE - #else - #define USE_UART1_TX TRUE - #endif +#ifdef USE_UART1_RX_ONLY +#warning "USE_UART1_RX_ONLY is deprecated, please set USE_UART1_TX=FALSE instead" +#define USE_UART1_TX FALSE +#else +#define USE_UART1_TX TRUE +#endif #endif #ifndef USE_UART1_RX #define USE_UART1_RX TRUE @@ -243,7 +245,8 @@ void uart0_init( void ) { void uart1_ISR(void) __attribute__((naked)); -void uart1_ISR(void) { +void uart1_ISR(void) +{ // perform proper ISR entry so thumb-interwork works properly ISR_ENTRY(); @@ -253,7 +256,8 @@ void uart1_ISR(void) { ISR_EXIT(); // recover registers and return } -void uart1_init( void ) { +void uart1_init(void) +{ uart_periph_init(&uart1); uart1.reg_addr = UART1_BASE; diff --git a/sw/airborne/arch/lpc21/modules/adcs/max11040_hw.c b/sw/airborne/arch/lpc21/modules/adcs/max11040_hw.c index bf1f95efe6..b75fa59e0e 100644 --- a/sw/airborne/arch/lpc21/modules/adcs/max11040_hw.c +++ b/sw/airborne/arch/lpc21/modules/adcs/max11040_hw.c @@ -46,22 +46,22 @@ static void EXTINT_ISR(void) __attribute__((naked)); #define SPI1_VIC_SLOT 7 #endif -static void SSP_ISR(void) { +static void SSP_ISR(void) +{ int i; ISR_ENTRY(); switch (max11040_status) { - case MAX11040_RESET: - { + case MAX11040_RESET: { /* read dummy control byte reply */ - uint8_t foo __attribute__ ((unused)); + uint8_t foo __attribute__((unused)); foo = SSPDR; foo = SSPDR; /* write configuration register */ SSP_Send(0x60); /* wr conf */ SSP_Send(0x30); /* adc0: en24bit, xtalen, no faultdis */ - for (i=1; i= MAX11040_BUF_SIZE) i=0; + i = max11040_buf_in + 1; + if (i >= MAX11040_BUF_SIZE) { i = 0; } if (i != max11040_buf_out) { max11040_buf_in = i; } else { @@ -234,11 +229,11 @@ static void SSP_ISR(void) { ISR_EXIT(); } -void EXTINT_ISR(void) { +void EXTINT_ISR(void) +{ ISR_ENTRY(); - if (num_irqs++ == 5) - { + if (num_irqs++ == 5) { /* switch SSEL P0.20 to be used as GPIO */ PINSEL1 &= ~(3 << 8); IO0DIR |= 1 << 20; @@ -273,7 +268,8 @@ void EXTINT_ISR(void) { } -void max11040_hw_init( void ) { +void max11040_hw_init(void) +{ int i; /* *** configure SPI *** */ @@ -286,8 +282,8 @@ void max11040_hw_init( void ) { SSPCPSR = 0x02; /* initialize interrupt vector */ - VICIntSelect &= ~VIC_BIT( VIC_SPI1 ); /* SPI1 selected as IRQ */ - VICIntEnable = VIC_BIT( VIC_SPI1 ); /* enable it */ + VICIntSelect &= ~VIC_BIT(VIC_SPI1); /* SPI1 selected as IRQ */ + VICIntEnable = VIC_BIT(VIC_SPI1); /* enable it */ _VIC_CNTL(SPI1_VIC_SLOT) = VIC_ENABLE | VIC_SPI1; _VIC_ADDR(SPI1_VIC_SLOT) = (uint32_t)SSP_ISR; /* address of the ISR */ @@ -300,15 +296,15 @@ void max11040_hw_init( void ) { SetBit(EXTINT, MAXM_DRDY_EINT); /* clear pending EINT */ /* initialize interrupt vector */ - VICIntSelect &= ~VIC_BIT( MAXM_DRDY_VIC_IT ); /* select EINT as IRQ source */ - VICIntEnable = VIC_BIT( MAXM_DRDY_VIC_IT ); /* enable it */ + VICIntSelect &= ~VIC_BIT(MAXM_DRDY_VIC_IT); /* select EINT as IRQ source */ + VICIntEnable = VIC_BIT(MAXM_DRDY_VIC_IT); /* enable it */ _VIC_CNTL(MAX11040_DRDY_VIC_SLOT) = VIC_ENABLE | MAXM_DRDY_VIC_IT; _VIC_ADDR(MAX11040_DRDY_VIC_SLOT) = (uint32_t)EXTINT_ISR; /* address of the ISR */ /* write configuration register */ SSP_Send(0x60); /* wr conf */ - for (i=0; i 2) { \ - micromag_cur_axe = 0; \ - micromag_status = MM_DATA_AVAILABLE; \ - } \ - else \ - micromag_status = MM_IDLE; \ - } \ - break; \ + { \ + 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; \ } \ } @@ -77,7 +77,7 @@ extern volatile uint8_t micromag_cur_axe; SpiEnable(); \ } -#define MmReadRes() { \ +#define MmReadRes() { \ micromag_status = MM_READING_RES; \ MmSelect(); \ /* trigger 2 bytes read */ \ @@ -88,6 +88,6 @@ extern volatile uint8_t micromag_cur_axe; SpiEnableRti(); \ } -extern void micromag_hw_init( void ); +extern void micromag_hw_init(void); #endif /* MICROMAG_HW_H */ diff --git a/sw/airborne/arch/lpc21/modules/sensors/trig_ext_hw.c b/sw/airborne/arch/lpc21/modules/sensors/trig_ext_hw.c index c5a6b2d153..bd51061a9d 100644 --- a/sw/airborne/arch/lpc21/modules/sensors/trig_ext_hw.c +++ b/sw/airborne/arch/lpc21/modules/sensors/trig_ext_hw.c @@ -10,7 +10,8 @@ uint32_t delta_t0; volatile bool_t trig_ext_valid; -void TRIG_ISR() { +void TRIG_ISR() +{ static uint32_t last; uint32_t delta_t0_temp; trigger_t0 = PPM_CR; @@ -22,7 +23,8 @@ void TRIG_ISR() { } } -void trig_ext_init ( void ) { +void trig_ext_init(void) +{ /* select pin for capture */ PPM_PINSEL |= PPM_PINSEL_VAL << PPM_PINSEL_BIT; /* enable capture 0.2 on falling or rising edge + trigger interrupt */ diff --git a/sw/airborne/arch/lpc21/modules/sensors/trig_ext_hw.h b/sw/airborne/arch/lpc21/modules/sensors/trig_ext_hw.h index d2a4441a51..8a03f4935b 100644 --- a/sw/airborne/arch/lpc21/modules/sensors/trig_ext_hw.h +++ b/sw/airborne/arch/lpc21/modules/sensors/trig_ext_hw.h @@ -14,7 +14,7 @@ extern uint32_t delta_t0; extern volatile bool_t trig_ext_valid; void TRIG_ISR(void); -void trig_ext_init( void ); +void trig_ext_init(void); #endif /* TRIG_EXT_HW_H */ diff --git a/sw/airborne/arch/lpc21/peripherals/max1168_arch.c b/sw/airborne/arch/lpc21/peripherals/max1168_arch.c index 2f42ef609d..d696f74017 100644 --- a/sw/airborne/arch/lpc21/peripherals/max1168_arch.c +++ b/sw/airborne/arch/lpc21/peripherals/max1168_arch.c @@ -28,7 +28,8 @@ static void EXTINT0_ISR(void) __attribute__((naked)); -void max1168_arch_init( void ) { +void max1168_arch_init(void) +{ /* connect P0.16 to extint0 (EOC) */ MAX1168_EOC_PINSEL |= MAX1168_EOC_PINSEL_VAL << MAX1168_EOC_PINSEL_BIT; @@ -39,22 +40,23 @@ void max1168_arch_init( void ) { /* clear pending extint0 before enabling interrupts */ SetBit(EXTINT, MAX1168_EOC_EINT); - /* initialize interrupt vector */ - VICIntSelect &= ~VIC_BIT( MAX1168_EOC_VIC_IT ); // EXTINT0 selected as IRQ - VICIntEnable = VIC_BIT( MAX1168_EOC_VIC_IT ); // EXTINT0 interrupt enabled + /* initialize interrupt vector */ + VICIntSelect &= ~VIC_BIT(MAX1168_EOC_VIC_IT); // EXTINT0 selected as IRQ + VICIntEnable = VIC_BIT(MAX1168_EOC_VIC_IT); // EXTINT0 interrupt enabled _VIC_CNTL(MAX1168_EOC_VIC_SLOT) = VIC_ENABLE | MAX1168_EOC_VIC_IT; _VIC_ADDR(MAX1168_EOC_VIC_SLOT) = (uint32_t)EXTINT0_ISR; // address of the ISR } -void EXTINT0_ISR(void) { +void EXTINT0_ISR(void) +{ ISR_ENTRY(); - //ASSERT((max1168_status == MAX1168_SENDING_REQ), DEBUG_MAX_1168, MAX1168_ERR_SPURIOUS_EOC); + //ASSERT((max1168_status == MAX1168_SENDING_REQ), DEBUG_MAX_1168, MAX1168_ERR_SPURIOUS_EOC); max1168_status = MAX1168_GOT_EOC; //SetBit(EXTINT, MAX1168_EOC_EINT); /* clear extint0 */ - EXTINT = (1<= _4017_NB_CHANNELS) { \ - SetBit(IO1SET, SERVO_RESET_PIN); \ - servos_4017_idx = 0; \ - SetBit(IO1CLR, SERVO_RESET_PIN); \ - } \ - \ - /* request clock high on next match */ \ - T0MR1 += servos_values[servos_4017_idx]; \ - /* lower clock pin */ \ - T0EMR &= ~TEMR_EM1; \ - servos_4017_idx++; \ +#define SERVOS_4017_ISR() { \ + if (servos_4017_idx >= _4017_NB_CHANNELS) { \ + SetBit(IO1SET, SERVO_RESET_PIN); \ + servos_4017_idx = 0; \ + SetBit(IO1CLR, SERVO_RESET_PIN); \ + } \ + \ + /* request clock high on next match */ \ + T0MR1 += servos_values[servos_4017_idx]; \ + /* lower clock pin */ \ + T0EMR &= ~TEMR_EM1; \ + servos_4017_idx++; \ } #else /* SERVOS_4017_CLOCK_FALLING */ #define SERVOS_4017_RESET_WIDTH SERVOS_TICS_OF_USEC(1000) #define SERVOS_4017_FIRST_PULSE_WIDTH SERVOS_TICS_OF_USEC(100) -#define SERVOS_4017_ISR() { \ - if (servos_4017_idx == _4017_NB_CHANNELS) { \ - SetBit(IO1SET, SERVO_RESET_PIN); \ - /* Start a long 1ms reset, keep clock low */ \ - T0MR1 += SERVOS_4017_RESET_WIDTH; \ - servos_4017_idx++; \ - T0EMR &= ~TEMR_EM1; \ - } \ - else if (servos_4017_idx > _4017_NB_CHANNELS) { \ - /* Clear the reset*/ \ - SetBit(IO1CLR,SERVO_RESET_PIN); \ - /* assert clock */ \ - T0EMR |= TEMR_EM1; \ - /* Starts a short pulse-like period */ \ - T0MR1 += SERVOS_4017_FIRST_PULSE_WIDTH; \ - servos_4017_idx=0; /** Starts a new sequence next time */ \ - } \ - else { \ - /* request next match */ \ - T0MR1 += servos_values[servos_4017_idx]; \ - /* clock low if not last one, last is done with reset */ \ - if (servos_4017_idx != _4017_NB_CHANNELS-1) { \ - /* raise clock pin */ \ - T0EMR |= TEMR_EM1; \ - } \ - servos_4017_idx++; \ - } \ +#define SERVOS_4017_ISR() { \ + if (servos_4017_idx == _4017_NB_CHANNELS) { \ + SetBit(IO1SET, SERVO_RESET_PIN); \ + /* Start a long 1ms reset, keep clock low */ \ + T0MR1 += SERVOS_4017_RESET_WIDTH; \ + servos_4017_idx++; \ + T0EMR &= ~TEMR_EM1; \ + } \ + else if (servos_4017_idx > _4017_NB_CHANNELS) { \ + /* Clear the reset*/ \ + SetBit(IO1CLR,SERVO_RESET_PIN); \ + /* assert clock */ \ + T0EMR |= TEMR_EM1; \ + /* Starts a short pulse-like period */ \ + T0MR1 += SERVOS_4017_FIRST_PULSE_WIDTH; \ + servos_4017_idx=0; /** Starts a new sequence next time */ \ + } \ + else { \ + /* request next match */ \ + T0MR1 += servos_values[servos_4017_idx]; \ + /* clock low if not last one, last is done with reset */ \ + if (servos_4017_idx != _4017_NB_CHANNELS-1) { \ + /* raise clock pin */ \ + T0EMR |= TEMR_EM1; \ + } \ + servos_4017_idx++; \ + } \ } #endif /* SERVOS_4017_CLOCK_ON_RESET */ diff --git a/sw/airborne/arch/lpc21/subsystems/actuators/servos_ppm_hw.c b/sw/airborne/arch/lpc21/subsystems/actuators/servos_ppm_hw.c index 4b748ec617..14441cf1b4 100644 --- a/sw/airborne/arch/lpc21/subsystems/actuators/servos_ppm_hw.c +++ b/sw/airborne/arch/lpc21/subsystems/actuators/servos_ppm_hw.c @@ -38,7 +38,8 @@ uint32_t servos_delay; #define START_TIMEOUT 0xFFFF; -void actuators_ppm_init ( void ) { +void actuators_ppm_init(void) +{ /* select ppm output pin as MAT0.1 output */ SERVO_CLOCK_PINSEL |= SERVO_CLOCK_PINSEL_VAL << SERVO_CLOCK_PINSEL_BIT; @@ -56,8 +57,9 @@ void actuators_ppm_init ( void ) { /* Set all servos to their midpoints */ /* compulsory for unused servos */ uint8_t i; - for( i=0 ; i < _PPM_NB_CHANNELS ; i++ ) + for (i = 0 ; i < _PPM_NB_CHANNELS ; i++) { servos_values[i] = SERVOS_TICS_OF_USEC(1500); + } servos_delay = SERVO_REFRESH_TICS; } 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 2ca81a3b06..5d05ba97c9 100644 --- a/sw/airborne/arch/lpc21/subsystems/datalink/audio_telemetry_hw.c +++ b/sw/airborne/arch/lpc21/subsystems/datalink/audio_telemetry_hw.c @@ -14,23 +14,26 @@ uint8_t tx_bit_idx; #define NB_STATE 2 #define NB_PHASE 2 -static const uint16_t audio_telemetry_sample[NB_STATE][NB_PHASE][SAMPLES_PER_PERIOD] = - { - {{512, 1023, 512, 1}, - {512 ,1 , 512 , 1023}}, - {{512, 874, 1023, 874}, - {512 ,150 ,1 , 150}} - }; +static const uint16_t audio_telemetry_sample[NB_STATE][NB_PHASE][SAMPLES_PER_PERIOD] = { + { {512, 1023, 512, 1}, + {512 , 1 , 512 , 1023} + }, + { {512, 874, 1023, 874}, + {512 , 150 , 1 , 150} + } +}; static uint8_t audio_telemetry_sample_idx = 0; static uint8_t audio_telemetry_phase = 0; -static inline uint8_t get_next_bit( void ) { +static inline uint8_t get_next_bit(void) +{ uint8_t ret; /* start bit */ - if (tx_bit_idx == 0) + if (tx_bit_idx == 0) { ret = 0; + } /* data bits */ else if (tx_bit_idx < 9) { ret = tx_byte & 0x01; @@ -44,7 +47,7 @@ static inline uint8_t get_next_bit( void ) { if (tx_bit_idx >= 10) { /* if we have nothing left to transmit */ - if( tx_head == tx_tail ) { + if (tx_head == tx_tail) { /* hack to stay with data = 1 */ tx_bit_idx--; } else { @@ -52,14 +55,16 @@ static inline uint8_t get_next_bit( void ) { tx_byte = tx_buf[tx_tail]; tx_bit_idx = 0; tx_tail++; - if( tx_tail >= TX_BUF_SIZE ) + if (tx_tail >= TX_BUF_SIZE) { tx_tail = 0; + } } } return ret; } -void TIMER1_ISR ( void ) { +void TIMER1_ISR(void) +{ ISR_ENTRY(); static uint8_t audio_telemetry_bit; diff --git a/sw/airborne/arch/lpc21/subsystems/datalink/audio_telemetry_hw.h b/sw/airborne/arch/lpc21/subsystems/datalink/audio_telemetry_hw.h index 2b408ae3a8..bbb9a9f01d 100644 --- a/sw/airborne/arch/lpc21/subsystems/datalink/audio_telemetry_hw.h +++ b/sw/airborne/arch/lpc21/subsystems/datalink/audio_telemetry_hw.h @@ -4,7 +4,7 @@ #include "LPC21xx.h" #include BOARD_CONFIG -void TIMER1_ISR ( void ) __attribute__((naked)); +void TIMER1_ISR(void) __attribute__((naked)); /* T1 prescaler, set T1_CLK to 5MHz, T1_CLK = PCLK / T1PCLK_DIV */ @@ -28,15 +28,16 @@ void TIMER1_ISR ( void ) __attribute__((naked)); #define SAMPLES_PER_PERIOD 4 #define SAMPLE_PERIOD (PCLK/4762/SAMPLES_PER_PERIOD/T1_PCLK_DIV) -static inline void audio_telemetry_init ( void ) { - /* turn on DAC pins */ +static inline void audio_telemetry_init(void) +{ + /* turn on DAC pins */ PINSEL1 &= 1 << 19; PINSEL1 |= ~(1 << 18); /* reset & disable timer 1 */ T1TCR = TCR_RESET; /* set the prescale divider */ T1PR = T1_PCLK_DIV - 1; - /* select TIMER1 as IRQ */ + /* select TIMER1 as IRQ */ VICIntSelect &= ~VIC_BIT(VIC_TIMER1); /* enable TIMER1 interrupt */ VICIntEnable = VIC_BIT(VIC_TIMER1); diff --git a/sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.c b/sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.c index 711a94040c..8077d30f99 100644 --- a/sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.c +++ b/sw/airborne/arch/lpc21/subsystems/imu/imu_aspirin_arch.c @@ -30,7 +30,8 @@ void imu_aspirin_arch_init(void) /* * Gyro data ready */ -void exti15_10_irq_handler(void) { +void exti15_10_irq_handler(void) +{ /* clear EXTI */ // if(EXTI_GetITStatus(EXTI_Line14) != RESET) @@ -43,7 +44,8 @@ void exti15_10_irq_handler(void) { /* * Accel data ready */ -void exti2_irq_handler(void) { +void exti2_irq_handler(void) +{ /* clear EXTI */ // if(EXTI_GetITStatus(EXTI_Line2) != RESET) @@ -54,7 +56,8 @@ void exti2_irq_handler(void) { /* * Accel end of DMA transfer */ -void dma1_c4_irq_handler(void) { +void dma1_c4_irq_handler(void) +{ //imu_aspirin.accel_available = TRUE; } diff --git a/sw/airborne/arch/lpc21/subsystems/imu/imu_crista_arch.c b/sw/airborne/arch/lpc21/subsystems/imu/imu_crista_arch.c index 0d22da76db..c992f76104 100644 --- a/sw/airborne/arch/lpc21/subsystems/imu/imu_crista_arch.c +++ b/sw/airborne/arch/lpc21/subsystems/imu/imu_crista_arch.c @@ -55,7 +55,8 @@ static uint8_t channel; #warning "This driver should be updated to use the new SPI peripheral" -void imu_crista_arch_init(void) { +void imu_crista_arch_init(void) +{ channel = 0; /* setup pins for SSP (SCK, MISO, MOSI) */ @@ -74,20 +75,22 @@ void imu_crista_arch_init(void) { /* setup slave select */ /* configure SS pin */ - SetBit( ADS8344_SS_IODIR, ADS8344_SS_PIN); /* pin is output */ + SetBit(ADS8344_SS_IODIR, ADS8344_SS_PIN); /* pin is output */ ADS8344Unselect(); /* pin low */ } -static inline void read_values( void ) { - uint8_t foo __attribute__ ((unused)) = SSPDR; +static inline void read_values(void) +{ + uint8_t foo __attribute__((unused)) = SSPDR; uint8_t msb = SSPDR; uint8_t lsb = SSPDR; uint8_t llsb = SSPDR; ADS8344_values[channel] = (msb << 8 | lsb) << 1 | llsb >> 7; } -static inline void send_request( void ) { +static inline void send_request(void) +{ uint8_t control = 1 << 7 | channel << 4 | SGL_DIF << 2 | POWER_MODE; SSP_Send(control); SSP_Send(0); @@ -95,7 +98,8 @@ static inline void send_request( void ) { SSP_Send(0); } -void ADS8344_start( void ) { +void ADS8344_start(void) +{ ADS8344Select(); SSP_ClearRti(); SSP_EnableRti(); @@ -103,21 +107,21 @@ void ADS8344_start( void ) { send_request(); } -void SPI1_ISR(void) { - ISR_ENTRY(); - read_values(); - channel++; - if (channel > 7-1) { - channel = 0; - ADS8344_available = TRUE; - ADS8344Unselect(); - } - else { - send_request(); - } +void SPI1_ISR(void) +{ + ISR_ENTRY(); + read_values(); + channel++; + if (channel > 7 - 1) { + channel = 0; + ADS8344_available = TRUE; + ADS8344Unselect(); + } else { + send_request(); + } - SSP_ClearRti(); + SSP_ClearRti(); - VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */ - ISR_EXIT(); + VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */ + ISR_EXIT(); } diff --git a/sw/airborne/arch/lpc21/subsystems/imu/imu_crista_arch.h b/sw/airborne/arch/lpc21/subsystems/imu/imu_crista_arch.h index 33c6fe1ae5..478bf4219e 100644 --- a/sw/airborne/arch/lpc21/subsystems/imu/imu_crista_arch.h +++ b/sw/airborne/arch/lpc21/subsystems/imu/imu_crista_arch.h @@ -25,10 +25,10 @@ #include "std.h" -#define ImuCristaArchPeriodic() { \ - ADS8344_start(); \ +#define ImuCristaArchPeriodic() { \ + ADS8344_start(); \ } -extern void ADS8344_start( void ); +extern void ADS8344_start(void); #endif /* IMU_CRISTA_ARCH_H */ diff --git a/sw/airborne/arch/lpc21/subsystems/radio_control/ppm_arch.h b/sw/airborne/arch/lpc21/subsystems/radio_control/ppm_arch.h index 1c5465771a..c239220147 100644 --- a/sw/airborne/arch/lpc21/subsystems/radio_control/ppm_arch.h +++ b/sw/airborne/arch/lpc21/subsystems/radio_control/ppm_arch.h @@ -47,9 +47,9 @@ #define PPM_IT PPM_CRI #define PPM_ISR() { \ - uint32_t now = PPM_CR; \ - ppm_decode_frame(now); \ -} + uint32_t now = PPM_CR; \ + ppm_decode_frame(now); \ + } #ifdef USE_PPM_RSSI_GPIO #define RssiValid() (bit_is_set(PPM_RSSI_IOPIN, PPM_RSSI_PIN) == PPM_RSSI_VALID_LEVEL) diff --git a/sw/airborne/arch/lpc21/subsystems/radio_control/spektrum_arch.c b/sw/airborne/arch/lpc21/subsystems/radio_control/spektrum_arch.c index 40d6ace1d5..d2d89cedc2 100644 --- a/sw/airborne/arch/lpc21/subsystems/radio_control/spektrum_arch.c +++ b/sw/airborne/arch/lpc21/subsystems/radio_control/spektrum_arch.c @@ -24,10 +24,11 @@ bool_t rc_spk_parser_status; uint8_t rc_spk_parser_idx; -uint8_t rc_spk_parser_buf[RADIO_CONTROL_NB_CHANNEL*2]; +uint8_t rc_spk_parser_buf[RADIO_CONTROL_NB_CHANNEL * 2]; const int16_t rc_spk_throw[RADIO_CONTROL_NB_CHANNEL] = RC_SPK_THROWS; -void radio_control_impl_init(void) { +void radio_control_impl_init(void) +{ rc_spk_parser_status = RC_SPK_STA_UNINIT; rc_spk_parser_idx = 0; } diff --git a/sw/airborne/arch/lpc21/subsystems/radio_control/spektrum_arch.h b/sw/airborne/arch/lpc21/subsystems/radio_control/spektrum_arch.h index f63fa369a5..50601186ae 100644 --- a/sw/airborne/arch/lpc21/subsystems/radio_control/spektrum_arch.h +++ b/sw/airborne/arch/lpc21/subsystems/radio_control/spektrum_arch.h @@ -36,7 +36,7 @@ extern bool_t rc_spk_parser_status; extern uint8_t rc_spk_parser_idx; -extern uint8_t rc_spk_parser_buf[RADIO_CONTROL_NB_CHANNEL*2]; +extern uint8_t rc_spk_parser_buf[RADIO_CONTROL_NB_CHANNEL * 2]; #define MAX_SPK 344 @@ -54,44 +54,44 @@ extern const int16_t rc_spk_throw[RADIO_CONTROL_NB_CHANNEL]; while (RcLinkChAvailable()) { \ int8_t c = RcLinkGetCh(); \ switch (rc_spk_parser_status) { \ - case RC_SPK_STA_UNINIT: \ - if (c==RC_SPK_SYNC_1) \ - rc_spk_parser_status = RC_SPK_STA_GOT_SYNC_1; \ - break; \ - case RC_SPK_STA_GOT_SYNC_1: \ - if (c==RC_SPK_SYNC_2) { \ - rc_spk_parser_status = RC_SPK_STA_GOT_SYNC_2; \ - rc_spk_parser_idx = 0; \ - } \ - else \ - rc_spk_parser_status = RC_SPK_STA_UNINIT; \ - break; \ - case RC_SPK_STA_GOT_SYNC_2: \ - rc_spk_parser_buf[rc_spk_parser_idx] = c; \ - rc_spk_parser_idx++; \ - if (rc_spk_parser_idx >= 2*RADIO_CONTROL_NB_CHANNEL) { \ - rc_spk_parser_status = RC_SPK_STA_UNINIT; \ - radio_control.frame_cpt++; \ - radio_control.time_since_last_frame = 0; \ - radio_control.status = RC_OK; \ - uint8_t i; \ - for (i=0;i> 10;*/ \ - const int16_t val = (tmp&0x03FF) - 512; \ - radio_control.values[i] = val; \ - radio_control.values[i] *= rc_spk_throw[i]; \ - if (i==RADIO_CONTROL_THROTTLE) { \ - radio_control.values[i] += MAX_PPRZ; \ - radio_control.values[i] /= 2; \ - } \ - } \ - _received_frame_handler(); \ - } \ - break; \ - default: \ - rc_spk_parser_status = RC_SPK_STA_UNINIT; \ + case RC_SPK_STA_UNINIT: \ + if (c==RC_SPK_SYNC_1) \ + rc_spk_parser_status = RC_SPK_STA_GOT_SYNC_1; \ + break; \ + case RC_SPK_STA_GOT_SYNC_1: \ + if (c==RC_SPK_SYNC_2) { \ + rc_spk_parser_status = RC_SPK_STA_GOT_SYNC_2; \ + rc_spk_parser_idx = 0; \ + } \ + else \ + rc_spk_parser_status = RC_SPK_STA_UNINIT; \ + break; \ + case RC_SPK_STA_GOT_SYNC_2: \ + rc_spk_parser_buf[rc_spk_parser_idx] = c; \ + rc_spk_parser_idx++; \ + if (rc_spk_parser_idx >= 2*RADIO_CONTROL_NB_CHANNEL) { \ + rc_spk_parser_status = RC_SPK_STA_UNINIT; \ + radio_control.frame_cpt++; \ + radio_control.time_since_last_frame = 0; \ + radio_control.status = RC_OK; \ + uint8_t i; \ + for (i=0;i> 10;*/ \ + const int16_t val = (tmp&0x03FF) - 512; \ + radio_control.values[i] = val; \ + radio_control.values[i] *= rc_spk_throw[i]; \ + if (i==RADIO_CONTROL_THROTTLE) { \ + radio_control.values[i] += MAX_PPRZ; \ + radio_control.values[i] /= 2; \ + } \ + } \ + _received_frame_handler(); \ + } \ + break; \ + default: \ + rc_spk_parser_status = RC_SPK_STA_UNINIT; \ } \ } \ } diff --git a/sw/airborne/arch/lpc21/subsystems/settings_arch.c b/sw/airborne/arch/lpc21/subsystems/settings_arch.c index b0a252fa3b..08e57c02dd 100644 --- a/sw/airborne/arch/lpc21/subsystems/settings_arch.c +++ b/sw/airborne/arch/lpc21/subsystems/settings_arch.c @@ -63,36 +63,38 @@ typedef void (*IAP)(uint32_t[], uint32_t[]); typedef struct { - uint32_t addr; - uint32_t total_size; - uint32_t page_nr; - uint32_t page_size; + uint32_t addr; + uint32_t total_size; + uint32_t page_nr; + uint32_t page_size; } FlashInfo; static uint32_t pflash_checksum(uint32_t ptr, uint32_t size); -static int32_t flash_detect(FlashInfo* flash); -static int32_t pflash_erase_page(FlashInfo* flash); -static int32_t pflash_program_array(FlashInfo* flash, +static int32_t flash_detect(FlashInfo *flash); +static int32_t pflash_erase_page(FlashInfo *flash); +static int32_t pflash_program_array(FlashInfo *flash, uint32_t dest, uint32_t src); -static int32_t pflash_program_bytes(FlashInfo* flash, +static int32_t pflash_program_bytes(FlashInfo *flash, uint32_t src, uint32_t size, uint32_t chksum); -static uint32_t pflash_checksum(uint32_t ptr, uint32_t size) { +static uint32_t pflash_checksum(uint32_t ptr, uint32_t size) +{ uint32_t i, sum = 0; /* do it cheap for now */ - for (i=0; ipage_size = 0x1000; flash->page_nr = 26; flash->addr = 0x7C000; @@ -133,7 +133,8 @@ static int32_t flash_detect(FlashInfo* flash) { return 0; } -static int32_t pflash_erase_page(FlashInfo* flash) { +static int32_t pflash_erase_page(FlashInfo *flash) +{ uint32_t command[5], result[3]; IAP iap_entry; @@ -146,7 +147,7 @@ static int32_t pflash_erase_page(FlashInfo* flash) { disableIRQ(); iap_entry(command, result); enableIRQ(); - if (result[0] != 0) return result[0]; + if (result[0] != 0) { return result[0]; } /* erase page/sector */ command[0] = IAP_ERASE_SECTORS; @@ -155,21 +156,22 @@ static int32_t pflash_erase_page(FlashInfo* flash) { disableIRQ(); iap_entry(command, result); enableIRQ(); - if (result[0] != 0) return result[0]; + if (result[0] != 0) { return result[0]; } /* verify erase */ command[0] = IAP_BLANK_CHECK_SECTORS; command[1] = flash->page_nr; command[2] = flash->page_nr; iap_entry(command, result); - if (result[0] != 0) return result[0]; + if (result[0] != 0) { return result[0]; } return 0; } -static int32_t pflash_program_array(FlashInfo* flash, +static int32_t pflash_program_array(FlashInfo *flash, uint32_t dest, - uint32_t src) { + uint32_t src) +{ uint32_t command[5], result[3]; IAP iap_entry; @@ -182,70 +184,73 @@ static int32_t pflash_program_array(FlashInfo* flash, disableIRQ(); iap_entry(command, result); enableIRQ(); - if (result[0] != 0) return result[0]; + if (result[0] != 0) { return result[0]; } /* flash from ram */ command[0] = IAP_COPY_RAM_TO_FLASH; command[1] = dest; command[2] = src; command[3] = BOUND; - command[4] = CCLK/1000; + command[4] = CCLK / 1000; disableIRQ(); iap_entry(command, result); enableIRQ(); - if (result[0] != 0) return result[0]; + if (result[0] != 0) { return result[0]; } return 0; } -static int32_t pflash_program_bytes(FlashInfo* flash, +static int32_t pflash_program_bytes(FlashInfo *flash, uint32_t src, uint32_t size, - uint32_t chksum) { - uint32_t data[BOUND/4], i, j, ret; + uint32_t chksum) +{ + uint32_t data[BOUND / 4], i, j, ret; uint32_t ptr = (uint32_t) &data; /* erase */ - if ((ret = pflash_erase_page(flash))) return ret; + if ((ret = pflash_erase_page(flash))) { return ret; } /* write data in arrays */ - for (i=0; ipage_size - BOUND) { - data[(BOUND-FSIZ)/4] = size; - data[(BOUND-FCHK)/4] = chksum; + data[(BOUND - FSIZ) / 4] = size; + data[(BOUND - FCHK) / 4] = chksum; } - if ((ret = pflash_program_array(flash, flash->addr+i, ptr))) return ret; + if ((ret = pflash_program_array(flash, flash->addr + i, ptr))) { return ret; } } /* last array */ if (i <= flash->page_size - BOUND) { - data[(BOUND-FSIZ)/4] = size; - data[(BOUND-FCHK)/4] = chksum; + data[(BOUND - FSIZ) / 4] = size; + data[(BOUND - FCHK) / 4] = chksum; if ((ret = pflash_program_array(flash, - flash->addr+flash->page_size-BOUND, - ptr))) + flash->addr + flash->page_size - BOUND, + ptr))) { return ret; + } } /* verify data */ - for (i=0; iaddr+i)) != (*(uint8_t*) (src+i))) return -2; + for (i = 0; i < size; i++) { + if ((*(uint8_t *)(flash->addr + i)) != (*(uint8_t *)(src + i))) { return -2; } } - if (*(uint32_t*) (flash->addr+flash->page_size-FSIZ) != size) return -3; - if (*(uint32_t*) (flash->addr+flash->page_size-FCHK) != chksum) return -4; + if (*(uint32_t *)(flash->addr + flash->page_size - FSIZ) != size) { return -3; } + if (*(uint32_t *)(flash->addr + flash->page_size - FCHK) != chksum) { return -4; } return 0; } -int32_t persistent_write(uint32_t ptr, uint32_t size) { +int32_t persistent_write(uint32_t ptr, uint32_t size) +{ FlashInfo flash_info; - if (flash_detect(&flash_info)) return -1; - if ((size > flash_info.page_size-FSIZ) || (size == 0)) return -2; + if (flash_detect(&flash_info)) { return -1; } + if ((size > flash_info.page_size - FSIZ) || (size == 0)) { return -2; } return pflash_program_bytes(&flash_info, ptr, @@ -253,23 +258,25 @@ int32_t persistent_write(uint32_t ptr, uint32_t size) { pflash_checksum(ptr, size)); } -int32_t persistent_read(uint32_t ptr, uint32_t size) { +int32_t persistent_read(uint32_t ptr, uint32_t size) +{ FlashInfo flash; uint32_t i; /* check parameters */ - if (flash_detect(&flash)) return -1; - if ((size > flash.page_size-FSIZ) || (size == 0)) return -2; + if (flash_detect(&flash)) { return -1; } + if ((size > flash.page_size - FSIZ) || (size == 0)) { return -2; } /* check consistency */ - if (size != *(uint32_t*)(flash.addr+flash.page_size-FSIZ)) return -3; + if (size != *(uint32_t *)(flash.addr + flash.page_size - FSIZ)) { return -3; } if (pflash_checksum(flash.addr, size) != - *(uint32_t*)(flash.addr+flash.page_size-FCHK)) + *(uint32_t *)(flash.addr + flash.page_size - FCHK)) { return -4; + } /* copy data */ - for (i=0; i>4; + '8', '9', 'A', 'B', 'C', 'D', 'E', 'F' + }; + unsigned char high = (c & 0xF0) >> 4; unsigned char low = c & 0x0F; uart0_transmit(hex[high]); uart0_transmit(hex[low]); } -void uart0_transmit(unsigned char ch0) { +void uart0_transmit(unsigned char ch0) +{ //when U0LSR_bit.THRE is 0 - U0THR contains valid data. // while (U0LSR_bit.THRE == 0); - while (!(U0LSR & (1<<5))); + while (!(U0LSR & (1 << 5))); U0THR = ch0; } diff --git a/sw/airborne/arch/lpc21/test/uart_tunnel.c b/sw/airborne/arch/lpc21/test/uart_tunnel.c index e70466bea3..cb32afa7e8 100644 --- a/sw/airborne/arch/lpc21/test/uart_tunnel.c +++ b/sw/airborne/arch/lpc21/test/uart_tunnel.c @@ -6,21 +6,24 @@ #define TXD1_PIN 8 #define RXD1_PIN 9 -int main (int argc, char** argv) { +int main(int argc, char **argv) +{ /* TXD0 and TXD1 output */ - IODIR0 |= (1<>8) @@ -84,160 +84,160 @@ static U8 abClassReqData[4]; static const U8 abDescriptors[] = { // device descriptor - 0x12, - DESC_DEVICE, - LE_WORD(0x0200), // bcdUSB - 0x00, // bDeviceClass - 0x00, // bDeviceSubClass - 0x00, // bDeviceProtocol - MAX_PACKET_SIZE0, // bMaxPacketSize - LE_WORD(0x7070), // idVendor - LE_WORD(0x1236), // idProduct - LE_WORD(0x0100), // bcdDevice - 0x01, // iManufacturer - 0x02, // iProduct - 0x03, // iSerialNumber - 0x01, // bNumConfigurations + 0x12, + DESC_DEVICE, + LE_WORD(0x0200), // bcdUSB + 0x00, // bDeviceClass + 0x00, // bDeviceSubClass + 0x00, // bDeviceProtocol + MAX_PACKET_SIZE0, // bMaxPacketSize + LE_WORD(0x7070), // idVendor + LE_WORD(0x1236), // idProduct + LE_WORD(0x0100), // bcdDevice + 0x01, // iManufacturer + 0x02, // iProduct + 0x03, // iSerialNumber + 0x01, // bNumConfigurations // configuration descriptor - 0x09, - DESC_CONFIGURATION, - LE_WORD(32), // wTotalLength - 0x01, // bNumInterfaces - 0x01, // bConfigurationValue - 0x00, // iConfiguration - 0xC0, // bmAttributes - 0x32, // bMaxPower + 0x09, + DESC_CONFIGURATION, + LE_WORD(32), // wTotalLength + 0x01, // bNumInterfaces + 0x01, // bConfigurationValue + 0x00, // iConfiguration + 0xC0, // bmAttributes + 0x32, // bMaxPower // interface - 0x09, - DESC_INTERFACE, - 0x00, // bInterfaceNumber - 0x00, // bAlternateSetting - 0x02, // bNumEndPoints - 0x08, // bInterfaceClass = mass storage - 0x06, // bInterfaceSubClass = transparent SCSI - 0x50, // bInterfaceProtocol = BOT - 0x00, // iInterface + 0x09, + DESC_INTERFACE, + 0x00, // bInterfaceNumber + 0x00, // bAlternateSetting + 0x02, // bNumEndPoints + 0x08, // bInterfaceClass = mass storage + 0x06, // bInterfaceSubClass = transparent SCSI + 0x50, // bInterfaceProtocol = BOT + 0x00, // iInterface // EP - 0x07, - DESC_ENDPOINT, - MSC_BULK_IN_EP, // bEndpointAddress - 0x02, // bmAttributes = bulk - LE_WORD(MAX_PACKET_SIZE),// wMaxPacketSize - 0x00, // bInterval + 0x07, + DESC_ENDPOINT, + MSC_BULK_IN_EP, // bEndpointAddress + 0x02, // bmAttributes = bulk + LE_WORD(MAX_PACKET_SIZE),// wMaxPacketSize + 0x00, // bInterval // EP - 0x07, - DESC_ENDPOINT, - MSC_BULK_OUT_EP, // bEndpointAddress - 0x02, // bmAttributes = bulk - LE_WORD(MAX_PACKET_SIZE),// wMaxPacketSize - 0x00, // bInterval + 0x07, + DESC_ENDPOINT, + MSC_BULK_OUT_EP, // bEndpointAddress + 0x02, // bmAttributes = bulk + LE_WORD(MAX_PACKET_SIZE),// wMaxPacketSize + 0x00, // bInterval // string descriptors - 0x04, - DESC_STRING, - LE_WORD(0x0409), + 0x04, + DESC_STRING, + LE_WORD(0x0409), - 0x0E, - DESC_STRING, - 'L', 0, 'P', 0, 'C', 0, 'U', 0, 'S', 0, 'B', 0, + 0x0E, + DESC_STRING, + 'L', 0, 'P', 0, 'C', 0, 'U', 0, 'S', 0, 'B', 0, - 0x14, - DESC_STRING, - 'S', 0, 'D', 0, '-', 0, 'R', 0, 'e', 0, 'a', 0, 'd', 0, 'e', 0, 'r', 0, + 0x14, + DESC_STRING, + 'S', 0, 'D', 0, '-', 0, 'R', 0, 'e', 0, 'a', 0, 'd', 0, 'e', 0, 'r', 0, - 0x12, - DESC_STRING, - '1', 0, '2', 0, '3', 0, '4', 0, '5', 0, '6', 0, '7', 0, '8', 0, + 0x12, + DESC_STRING, + '1', 0, '2', 0, '3', 0, '4', 0, '5', 0, '6', 0, '7', 0, '8', 0, // terminating zero - 0 + 0 }; #ifdef USE_USB_MSC /************************************************************************* - HandleClassRequest - ================== - Handle mass storage class request + HandleClassRequest + ================== + Handle mass storage class request **************************************************************************/ static BOOL HandleClassRequest(TSetupPacket *pSetup, int *piLen, U8 **ppbData) { - if (pSetup->wIndex != 0) { - return FALSE; - } - if (pSetup->wValue != 0) { - return FALSE; - } + if (pSetup->wIndex != 0) { + return FALSE; + } + if (pSetup->wValue != 0) { + return FALSE; + } - switch (pSetup->bRequest) { + switch (pSetup->bRequest) { - // get max LUN - case 0xFE: - *ppbData[0] = 0; // No LUNs - *piLen = 1; - break; + // get max LUN + case 0xFE: + *ppbData[0] = 0; // No LUNs + *piLen = 1; + break; - // MSC reset - case 0xFF: - if (pSetup->wLength > 0) { - return FALSE; - } - MSCBotReset(); - break; + // MSC reset + case 0xFF: + if (pSetup->wLength > 0) { + return FALSE; + } + MSCBotReset(); + break; - default: - return FALSE; - } - return TRUE; + default: + return FALSE; + } + return TRUE; } /************************************************************************* - main - ==== + main + ==== **************************************************************************/ int main_mass_storage(void) { - unsigned cpsr; + unsigned cpsr; - // disable global interrupts, do it polling - cpsr = disableIRQ(); + // disable global interrupts, do it polling + cpsr = disableIRQ(); - // initialise the SD card - BlockDevInit(); + // initialise the SD card + BlockDevInit(); - // initialise stack - USBInit(); + // initialise stack + USBInit(); - // enable bulk-in interrupts on NAKs - // these are required to get the BOT protocol going again after a STALL - USBHwNakIntEnable(INACK_BI); + // enable bulk-in interrupts on NAKs + // these are required to get the BOT protocol going again after a STALL + USBHwNakIntEnable(INACK_BI); - // register descriptors - USBRegisterDescriptors(abDescriptors); + // register descriptors + USBRegisterDescriptors(abDescriptors); - // register class request handler - USBRegisterRequestHandler(REQTYPE_TYPE_CLASS, HandleClassRequest, abClassReqData); + // register class request handler + USBRegisterRequestHandler(REQTYPE_TYPE_CLASS, HandleClassRequest, abClassReqData); - // register endpoint handlers - USBHwRegisterEPIntHandler(MSC_BULK_IN_EP, MSCBotBulkIn); - USBHwRegisterEPIntHandler(MSC_BULK_OUT_EP, MSCBotBulkOut); + // register endpoint handlers + USBHwRegisterEPIntHandler(MSC_BULK_IN_EP, MSCBotBulkIn); + USBHwRegisterEPIntHandler(MSC_BULK_OUT_EP, MSCBotBulkOut); - // connect to bus - USBHwConnect(TRUE); + // connect to bus + USBHwConnect(TRUE); - // call USB interrupt handler continuously - while (1) { - USBHwISR(); - } + // call USB interrupt handler continuously + while (1) { + USBHwISR(); + } - // possibly restore global interrupts (never happens) - restoreIRQ(cpsr); + // possibly restore global interrupts (never happens) + restoreIRQ(cpsr); - return 0; + return 0; } #endif /* USE_USB_MSC */ diff --git a/sw/airborne/arch/lpc21/usb_msc_hw.h b/sw/airborne/arch/lpc21/usb_msc_hw.h index 76f92d82dc..21c4b28044 100644 --- a/sw/airborne/arch/lpc21/usb_msc_hw.h +++ b/sw/airborne/arch/lpc21/usb_msc_hw.h @@ -1,29 +1,29 @@ /* - LPCUSB, an USB device driver for LPC microcontrollers - Copyright (C) 2006 Bertrik Sikken (bertrik@sikken.nl) - adapted to pprz Martin Mueller (martinmm@pfump.org) + LPCUSB, an USB device driver for LPC microcontrollers + Copyright (C) 2006 Bertrik Sikken (bertrik@sikken.nl) + adapted to pprz Martin Mueller (martinmm@pfump.org) - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: - 1. Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - 2. Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - 3. The name of the author may not be used to endorse or promote products - derived from this software without specific prior written permission. + 1. Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + 2. Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + 3. The name of the author may not be used to endorse or promote products + derived from this software without specific prior written permission. - THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR - IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES - OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. - IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, - INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT - NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, - DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY - THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF - THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR + IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES + OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, + INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #ifndef USB_MSC_HW_H diff --git a/sw/airborne/arch/lpc21/usb_ser_hw.c b/sw/airborne/arch/lpc21/usb_ser_hw.c index 4df537726f..0e6a4373e8 100644 --- a/sw/airborne/arch/lpc21/usb_ser_hw.c +++ b/sw/airborne/arch/lpc21/usb_ser_hw.c @@ -1,47 +1,47 @@ /* - LPCUSB, an USB device driver for LPC microcontrollers - Copyright (C) 2006 Bertrik Sikken (bertrik@sikken.nl) + LPCUSB, an USB device driver for LPC microcontrollers + Copyright (C) 2006 Bertrik Sikken (bertrik@sikken.nl) adapted to pprz Martin Mueller (martinmm@pfump.org) - Redistribution and use in source and binary forms, with or without - modification, are permitted provided that the following conditions are met: + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: - 1. Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - 2. Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - 3. The name of the author may not be used to endorse or promote products - derived from this software without specific prior written permission. + 1. Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + 2. Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + 3. The name of the author may not be used to endorse or promote products + derived from this software without specific prior written permission. - THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR - IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES - OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. - IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, - INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT - NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, - DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY - THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF - THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR + IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES + OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, + INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ /* - Minimal implementation of a USB serial port, using the CDC class. - This example application simply echoes everything it receives right back - to the host. + Minimal implementation of a USB serial port, using the CDC class. + This example application simply echoes everything it receives right back + to the host. - Windows: - Extract the usbser.sys file from .cab file in C:\WINDOWS\Driver Cache\i386 - and store it somewhere (C:\temp is a good place) along with the usbser.inf - file. Then plug in the LPC214x and direct windows to the usbser driver. - Windows then creates an extra COMx port that you can open in a terminal - program, like hyperterminal. + Windows: + Extract the usbser.sys file from .cab file in C:\WINDOWS\Driver Cache\i386 + and store it somewhere (C:\temp is a good place) along with the usbser.inf + file. Then plug in the LPC214x and direct windows to the usbser driver. + Windows then creates an extra COMx port that you can open in a terminal + program, like hyperterminal. - Linux: - The device should be recognised automatically by the cdc_acm driver, - which creates a /dev/ttyACMx device file that acts just like a regular - serial port. + Linux: + The device should be recognised automatically by the cdc_acm driver, + which creates a /dev/ttyACMx device file that acts just like a regular + serial port. */ @@ -78,9 +78,9 @@ #define CS_INTERFACE 0x24 #define CS_ENDPOINT 0x25 -#define SET_LINE_CODING 0x20 -#define GET_LINE_CODING 0x21 -#define SET_CONTROL_LINE_STATE 0x22 +#define SET_LINE_CODING 0x20 +#define GET_LINE_CODING 0x21 +#define SET_CONTROL_LINE_STATE 0x22 #define VCOM_FIFO_SIZE 128 @@ -88,21 +88,21 @@ #define ASSERT(x) typedef struct { - int head; - int tail; - uint8_t *buf; + int head; + int tail; + uint8_t *buf; } fifo_t; // data structure for GET_LINE_CODING / SET_LINE_CODING class requests typedef struct { - uint32_t dwDTERate; - uint8_t bCharFormat; - uint8_t bParityType; - uint8_t bDataBits; + uint32_t dwDTERate; + uint8_t bCharFormat; + uint8_t bParityType; + uint8_t bDataBits; } TLineCoding; int allow_line_coding = 0; - /* this settings are virtual unless you enable line coding */ +/* this settings are virtual unless you enable line coding */ static TLineCoding LineCoding = {115200, 0, 0, 8}; static uint8_t abBulkBuf[64]; static uint8_t abClassReqData[8]; @@ -116,7 +116,7 @@ static fifo_t rxfifo; static bool BulkOut_is_blocked = false; // forward declaration of interrupt handler -static void USBIntHandler(void) __attribute__ ((interrupt("IRQ"))); +static void USBIntHandler(void) __attribute__((interrupt("IRQ"))); static void BulkOut(U8 bEP, U8 bEPStatus); @@ -128,237 +128,234 @@ void fifo_init(fifo_t *fifo, U8 *buf); BOOL fifo_put(fifo_t *fifo, U8 c); BOOL fifo_get(fifo_t *fifo, U8 *pc); int fifo_avail(fifo_t *fifo); -int fifo_free(fifo_t *fifo); +int fifo_free(fifo_t *fifo); static const uint8_t abDescriptors[] = { // device descriptor - 0x12, - DESC_DEVICE, - LE_WORD(0x0101), // bcdUSB - 0x02, // bDeviceClass - 0x00, // bDeviceSubClass - 0x00, // bDeviceProtocol - MAX_PACKET_SIZE0, // bMaxPacketSize - LE_WORD(0x7070), // idVendor - LE_WORD(0x1235), // idProduct - LE_WORD(0x0100), // bcdDevice - 0x01, // iManufacturer - 0x02, // iProduct - 0x03, // iSerialNumber - 0x01, // bNumConfigurations + 0x12, + DESC_DEVICE, + LE_WORD(0x0101), // bcdUSB + 0x02, // bDeviceClass + 0x00, // bDeviceSubClass + 0x00, // bDeviceProtocol + MAX_PACKET_SIZE0, // bMaxPacketSize + LE_WORD(0x7070), // idVendor + LE_WORD(0x1235), // idProduct + LE_WORD(0x0100), // bcdDevice + 0x01, // iManufacturer + 0x02, // iProduct + 0x03, // iSerialNumber + 0x01, // bNumConfigurations // configuration descriptor - 0x09, - DESC_CONFIGURATION, - LE_WORD(67), // wTotalLength - 0x02, // bNumInterfaces - 0x01, // bConfigurationValue - 0x00, // iConfiguration - 0xC0, // bmAttributes - 0x32, // bMaxPower + 0x09, + DESC_CONFIGURATION, + LE_WORD(67), // wTotalLength + 0x02, // bNumInterfaces + 0x01, // bConfigurationValue + 0x00, // iConfiguration + 0xC0, // bmAttributes + 0x32, // bMaxPower // control class interface - 0x09, - DESC_INTERFACE, - 0x00, // bInterfaceNumber - 0x00, // bAlternateSetting - 0x01, // bNumEndPoints - 0x02, // bInterfaceClass - 0x02, // bInterfaceSubClass - 0x01, // bInterfaceProtocol, linux requires value of 1 for the cdc_acm module - 0x00, // iInterface + 0x09, + DESC_INTERFACE, + 0x00, // bInterfaceNumber + 0x00, // bAlternateSetting + 0x01, // bNumEndPoints + 0x02, // bInterfaceClass + 0x02, // bInterfaceSubClass + 0x01, // bInterfaceProtocol, linux requires value of 1 for the cdc_acm module + 0x00, // iInterface // header functional descriptor - 0x05, - CS_INTERFACE, - 0x00, - LE_WORD(0x0110), + 0x05, + CS_INTERFACE, + 0x00, + LE_WORD(0x0110), // call management functional descriptor - 0x05, - CS_INTERFACE, - 0x01, - 0x01, // bmCapabilities = device handles call management - 0x01, // bDataInterface + 0x05, + CS_INTERFACE, + 0x01, + 0x01, // bmCapabilities = device handles call management + 0x01, // bDataInterface // ACM functional descriptor - 0x04, - CS_INTERFACE, - 0x02, - 0x02, // bmCapabilities + 0x04, + CS_INTERFACE, + 0x02, + 0x02, // bmCapabilities // union functional descriptor - 0x05, - CS_INTERFACE, - 0x06, - 0x00, // bMasterInterface - 0x01, // bSlaveInterface0 + 0x05, + CS_INTERFACE, + 0x06, + 0x00, // bMasterInterface + 0x01, // bSlaveInterface0 // notification EP - 0x07, - DESC_ENDPOINT, - INT_IN_EP, // bEndpointAddress - 0x03, // bmAttributes = intr - LE_WORD(8), // wMaxPacketSize - 0xFE, // bInterval + 0x07, + DESC_ENDPOINT, + INT_IN_EP, // bEndpointAddress + 0x03, // bmAttributes = intr + LE_WORD(8), // wMaxPacketSize + 0xFE, // bInterval // data class interface descriptor - 0x09, - DESC_INTERFACE, - 0x01, // bInterfaceNumber - 0x00, // bAlternateSetting - 0x02, // bNumEndPoints - 0x0A, // bInterfaceClass = data - 0x00, // bInterfaceSubClass - 0x00, // bInterfaceProtocol - 0x00, // iInterface + 0x09, + DESC_INTERFACE, + 0x01, // bInterfaceNumber + 0x00, // bAlternateSetting + 0x02, // bNumEndPoints + 0x0A, // bInterfaceClass = data + 0x00, // bInterfaceSubClass + 0x00, // bInterfaceProtocol + 0x00, // iInterface // data EP OUT - 0x07, - DESC_ENDPOINT, - BULK_OUT_EP, // bEndpointAddress - 0x02, // bmAttributes = bulk - LE_WORD(MAX_PACKET_SIZE), // wMaxPacketSize - 0x00, // bInterval + 0x07, + DESC_ENDPOINT, + BULK_OUT_EP, // bEndpointAddress + 0x02, // bmAttributes = bulk + LE_WORD(MAX_PACKET_SIZE), // wMaxPacketSize + 0x00, // bInterval // data EP in - 0x07, - DESC_ENDPOINT, - BULK_IN_EP, // bEndpointAddress - 0x02, // bmAttributes = bulk - LE_WORD(MAX_PACKET_SIZE), // wMaxPacketSize - 0x00, // bInterval + 0x07, + DESC_ENDPOINT, + BULK_IN_EP, // bEndpointAddress + 0x02, // bmAttributes = bulk + LE_WORD(MAX_PACKET_SIZE), // wMaxPacketSize + 0x00, // bInterval - // string descriptors - 0x04, - DESC_STRING, - LE_WORD(0x0409), + // string descriptors + 0x04, + DESC_STRING, + LE_WORD(0x0409), - 0x0E, - DESC_STRING, - 'L', 0, 'P', 0, 'C', 0, 'U', 0, 'S', 0, 'B', 0, + 0x0E, + DESC_STRING, + 'L', 0, 'P', 0, 'C', 0, 'U', 0, 'S', 0, 'B', 0, - 0x14, - DESC_STRING, - 'U', 0, 'S', 0, 'B', 0, 'S', 0, 'e', 0, 'r', 0, 'i', 0, 'a', 0, 'l', 0, + 0x14, + DESC_STRING, + 'U', 0, 'S', 0, 'B', 0, 'S', 0, 'e', 0, 'r', 0, 'i', 0, 'a', 0, 'l', 0, - 0x12, - DESC_STRING, - '1', 0, '2', 0, '3', 0, '4', 0, '5', 0, '6', 0, '7', 0, '8', 0, + 0x12, + DESC_STRING, + '1', 0, '2', 0, '3', 0, '4', 0, '5', 0, '6', 0, '7', 0, '8', 0, // terminating zero - 0 + 0 }; void fifo_init(fifo_t *fifo, U8 *buf) { - fifo->head = 0; - fifo->tail = 0; - fifo->buf = buf; + fifo->head = 0; + fifo->tail = 0; + fifo->buf = buf; } BOOL fifo_put(fifo_t *fifo, U8 c) { - int next; + int next; - // check if FIFO has room - next = (fifo->head + 1) % VCOM_FIFO_SIZE; - if (next == fifo->tail) { - // full - return FALSE; - } + // check if FIFO has room + next = (fifo->head + 1) % VCOM_FIFO_SIZE; + if (next == fifo->tail) { + // full + return FALSE; + } - fifo->buf[fifo->head] = c; - fifo->head = next; + fifo->buf[fifo->head] = c; + fifo->head = next; - return TRUE; + return TRUE; } BOOL fifo_get(fifo_t *fifo, U8 *pc) { - int next; + int next; - // check if FIFO has data - if (fifo->head == fifo->tail) { - return FALSE; - } + // check if FIFO has data + if (fifo->head == fifo->tail) { + return FALSE; + } - next = (fifo->tail + 1) % VCOM_FIFO_SIZE; + next = (fifo->tail + 1) % VCOM_FIFO_SIZE; - *pc = fifo->buf[fifo->tail]; - fifo->tail = next; + *pc = fifo->buf[fifo->tail]; + fifo->tail = next; - return TRUE; + return TRUE; } int fifo_avail(fifo_t *fifo) { - return (VCOM_FIFO_SIZE + fifo->head - fifo->tail) % VCOM_FIFO_SIZE; + return (VCOM_FIFO_SIZE + fifo->head - fifo->tail) % VCOM_FIFO_SIZE; } int fifo_free(fifo_t *fifo) { - return (VCOM_FIFO_SIZE - 1 - fifo_avail(fifo)); + return (VCOM_FIFO_SIZE - 1 - fifo_avail(fifo)); } #ifdef USE_USB_LINE_CODING void set_linecoding(TLineCoding linecoding) { - uint16_t baud; - uint8_t mode; + uint16_t baud; + uint8_t mode; - // set the baudrate - baud = (uint16_t)((PCLK / ((linecoding.dwDTERate) * 16.0)) + 0.5); + // set the baudrate + baud = (uint16_t)((PCLK / ((linecoding.dwDTERate) * 16.0)) + 0.5); - // set the number of characters and other - // user specified operating parameters - switch (linecoding.bCharFormat) - { - case 0: /* 1 stop bit */ - mode = ULCR_STOP_1; - break; - case 1: /* 1.5 stop bit (only with 5 bit character) */ - case 2: /* 2 stop bit */ - mode = ULCR_STOP_2; - break; - default: - mode = ULCR_STOP_1; - break; - } - switch (linecoding.bParityType) - { - case 0: mode += ULCR_PAR_NO; - break; - case 1: mode += ULCR_PAR_ODD; - break; - case 2: mode += ULCR_PAR_EVEN; - break; - case 3: mode += ULCR_PAR_MARK; - break; - case 4: mode += ULCR_PAR_SPACE; - break; - default: mode += ULCR_PAR_NO; - break; - } - switch (linecoding.bDataBits) - { - case 5: mode += ULCR_CHAR_5; - break; - case 6: mode += ULCR_CHAR_6; - break; - case 7: mode += ULCR_CHAR_7; - break; - case 8: mode += ULCR_CHAR_8; - break; - case 16: - default: mode += ULCR_CHAR_8; - break; - } + // set the number of characters and other + // user specified operating parameters + switch (linecoding.bCharFormat) { + case 0: /* 1 stop bit */ + mode = ULCR_STOP_1; + break; + case 1: /* 1.5 stop bit (only with 5 bit character) */ + case 2: /* 2 stop bit */ + mode = ULCR_STOP_2; + break; + default: + mode = ULCR_STOP_1; + break; + } + switch (linecoding.bParityType) { + case 0: mode += ULCR_PAR_NO; + break; + case 1: mode += ULCR_PAR_ODD; + break; + case 2: mode += ULCR_PAR_EVEN; + break; + case 3: mode += ULCR_PAR_MARK; + break; + case 4: mode += ULCR_PAR_SPACE; + break; + default: mode += ULCR_PAR_NO; + break; + } + switch (linecoding.bDataBits) { + case 5: mode += ULCR_CHAR_5; + break; + case 6: mode += ULCR_CHAR_6; + break; + case 7: mode += ULCR_CHAR_7; + break; + case 8: mode += ULCR_CHAR_8; + break; + case 16: + default: mode += ULCR_CHAR_8; + break; + } #if USE_UART0 - U0LCR = ULCR_DLAB_ENABLE; // select divisor latches - U0DLL = (uint8_t)baud; // set for baud low byte - U0DLM = (uint8_t)(baud >> 8); // set for baud high byte - U0LCR = (mode & ~ULCR_DLAB_ENABLE); + U0LCR = ULCR_DLAB_ENABLE; // select divisor latches + U0DLL = (uint8_t)baud; // set for baud low byte + U0DLM = (uint8_t)(baud >> 8); // set for baud high byte + U0LCR = (mode & ~ULCR_DLAB_ENABLE); #endif #if USE_UART1 - U1LCR = ULCR_DLAB_ENABLE; // select divisor latches - U1DLL = (uint8_t)baud; // set for baud low byte - U1DLM = (uint8_t)(baud >> 8); // set for baud high byte - U1LCR = (mode & ~ULCR_DLAB_ENABLE); + U1LCR = ULCR_DLAB_ENABLE; // select divisor latches + U1DLL = (uint8_t)baud; // set for baud low byte + U1DLM = (uint8_t)(baud >> 8); // set for baud high byte + U1LCR = (mode & ~ULCR_DLAB_ENABLE); #endif } #endif @@ -366,25 +363,25 @@ void set_linecoding(TLineCoding linecoding) #ifdef USE_USB_LINE_CODING void VCOM_allow_linecoding(uint8_t mode) { - allow_line_coding = mode; + allow_line_coding = mode; } #endif /** - Writes one character to VCOM port + Writes one character to VCOM port - @param [in] c character to write - @returns character written, or EOF if character could not be written + @param [in] c character to write + @returns character written, or EOF if character could not be written */ int VCOM_putchar(int c) { - return fifo_put(&txfifo, c) ? c : EOF; + return fifo_put(&txfifo, c) ? c : EOF; } /** - Reads one character from VCOM port + Reads one character from VCOM port - @returns character read, or EOF if character could not be read + @returns character read, or EOF if character could not be read */ int VCOM_getchar(void) { @@ -405,163 +402,162 @@ int VCOM_getchar(void) } /** - Checks if buffer free in VCOM buffer + Checks if buffer free in VCOM buffer - @returns TRUE if len bytes are free + @returns TRUE if len bytes are free */ bool_t VCOM_check_free_space(uint8_t len) { - return (fifo_free(&txfifo) >= len ? TRUE : FALSE); + return (fifo_free(&txfifo) >= len ? TRUE : FALSE); } /** - Checks if data available in VCOM buffer + Checks if data available in VCOM buffer - @returns character read, or EOF if character could not be read + @returns character read, or EOF if character could not be read */ int VCOM_check_available(void) { - return (fifo_avail(&rxfifo)); + return (fifo_avail(&rxfifo)); } /** - Local function to handle incoming bulk data + Local function to handle incoming bulk data - @param [in] bEP - @param [in] bEPStatus + @param [in] bEP + @param [in] bEPStatus */ static void BulkOut(U8 bEP, U8 bEPStatus __attribute__((unused))) { - int i, iLen; + int i, iLen; - if (fifo_free(&rxfifo) < MAX_PACKET_SIZE) { - // may not fit into fifo - BulkOut_is_blocked = true; - return; - } + if (fifo_free(&rxfifo) < MAX_PACKET_SIZE) { + // may not fit into fifo + BulkOut_is_blocked = true; + return; + } - // get data from USB into intermediate buffer - iLen = USBHwEPRead(bEP, abBulkBuf, sizeof(abBulkBuf)); - for (i = 0; i < iLen; i++) { - // put into FIFO - if (!fifo_put(&rxfifo, abBulkBuf[i])) { - // overflow... :( - ASSERT(FALSE); - break; - } - } + // get data from USB into intermediate buffer + iLen = USBHwEPRead(bEP, abBulkBuf, sizeof(abBulkBuf)); + for (i = 0; i < iLen; i++) { + // put into FIFO + if (!fifo_put(&rxfifo, abBulkBuf[i])) { + // overflow... :( + ASSERT(FALSE); + break; + } + } } /** - Local function to handle outgoing bulk data + Local function to handle outgoing bulk data - @param [in] bEP - @param [in] bEPStatus + @param [in] bEP + @param [in] bEPStatus */ static void BulkIn(U8 bEP, U8 bEPStatus __attribute__((unused))) { - int i, iLen; + int i, iLen; - if (fifo_avail(&txfifo) == 0) { - // no more data, disable further NAK interrupts until next USB frame - USBHwNakIntEnable(0); - return; - } + if (fifo_avail(&txfifo) == 0) { + // no more data, disable further NAK interrupts until next USB frame + USBHwNakIntEnable(0); + return; + } - // get bytes from transmit FIFO into intermediate buffer - for (i = 0; i < MAX_PACKET_SIZE; i++) { - if (!fifo_get(&txfifo, &abBulkBuf[i])) { - break; - } - } - iLen = i; + // get bytes from transmit FIFO into intermediate buffer + for (i = 0; i < MAX_PACKET_SIZE; i++) { + if (!fifo_get(&txfifo, &abBulkBuf[i])) { + break; + } + } + iLen = i; - // send over USB - if (iLen > 0) { - USBHwEPWrite(bEP, abBulkBuf, iLen); - } + // send over USB + if (iLen > 0) { + USBHwEPWrite(bEP, abBulkBuf, iLen); + } } /** - Local function to handle the USB-CDC class requests + Local function to handle the USB-CDC class requests - @param [in] pSetup - @param [out] piLen - @param [out] ppbData + @param [in] pSetup + @param [out] piLen + @param [out] ppbData */ static BOOL HandleClassRequest(TSetupPacket *pSetup, int *piLen, U8 **ppbData) { - switch (pSetup->bRequest) { + switch (pSetup->bRequest) { - // set line coding - case SET_LINE_CODING: - memcpy((U8 *)&LineCoding, *ppbData, 7); - *piLen = 7; + // set line coding + case SET_LINE_CODING: + memcpy((U8 *)&LineCoding, *ppbData, 7); + *piLen = 7; #ifdef USE_USB_LINE_CODING - if (allow_line_coding) - { - set_linecoding(LineCoding); - } + if (allow_line_coding) { + set_linecoding(LineCoding); + } #endif - break; + break; - // get line coding - case GET_LINE_CODING: - *ppbData = (U8 *)&LineCoding; - *piLen = 7; - break; + // get line coding + case GET_LINE_CODING: + *ppbData = (U8 *)&LineCoding; + *piLen = 7; + break; - // set control line state - case SET_CONTROL_LINE_STATE: - // bit0 = DTR, bit1 = RTS - break; + // set control line state + case SET_CONTROL_LINE_STATE: + // bit0 = DTR, bit1 = RTS + break; - default: - return FALSE; - } - return TRUE; + default: + return FALSE; + } + return TRUE; } /** - Interrupt handler + Interrupt handler - Simply calls the USB ISR, then signals end of interrupt to VIC + Simply calls the USB ISR, then signals end of interrupt to VIC */ static void USBIntHandler(void) { - USBHwISR(); - VICVectAddr = 0x00; // dummy write to VIC to signal end of ISR + USBHwISR(); + VICVectAddr = 0x00; // dummy write to VIC to signal end of ISR } static void USBFrameHandler(U16 wFrame __attribute__((unused))) { - if (fifo_avail(&txfifo) > 0) { - // data available, enable NAK interrupt on bulk in - USBHwNakIntEnable(INACK_BI); - } + if (fifo_avail(&txfifo) > 0) { + // data available, enable NAK interrupt on bulk in + USBHwNakIntEnable(INACK_BI); + } } // Periph with generic device API struct usb_serial_periph usb_serial; // Functions for the generic device API -static int usb_serial_check_free_space(struct usb_serial_periph* p __attribute__((unused)), uint8_t len) +static int usb_serial_check_free_space(struct usb_serial_periph *p __attribute__((unused)), uint8_t len) { return (int)VCOM_check_free_space(len); } -static void usb_serial_transmit(struct usb_serial_periph* p __attribute__((unused)), uint8_t byte) +static void usb_serial_transmit(struct usb_serial_periph *p __attribute__((unused)), uint8_t byte) { VCOM_putchar(byte); } -static void usb_serial_send(struct usb_serial_periph* p __attribute__((unused))) { } +static void usb_serial_send(struct usb_serial_periph *p __attribute__((unused))) { } // Empty for lpc21 void VCOM_event(void) {} @@ -569,44 +565,45 @@ void VCOM_event(void) {} // Empty for lpc21 void VCOM_send_message(void) {} -void VCOM_init(void) { - // initialise stack - USBInit(); +void VCOM_init(void) +{ + // initialise stack + USBInit(); #ifdef USE_USB_LINE_CODING - // set default line coding - set_linecoding(LineCoding); + // set default line coding + set_linecoding(LineCoding); #endif - // register descriptors - USBRegisterDescriptors(abDescriptors); + // register descriptors + USBRegisterDescriptors(abDescriptors); - // register class request handler - USBRegisterRequestHandler(REQTYPE_TYPE_CLASS, HandleClassRequest, abClassReqData); + // register class request handler + USBRegisterRequestHandler(REQTYPE_TYPE_CLASS, HandleClassRequest, abClassReqData); - // register endpoint handlers - USBHwRegisterEPIntHandler(INT_IN_EP, NULL); - USBHwRegisterEPIntHandler(BULK_IN_EP, BulkIn); - USBHwRegisterEPIntHandler(BULK_OUT_EP, BulkOut); + // register endpoint handlers + USBHwRegisterEPIntHandler(INT_IN_EP, NULL); + USBHwRegisterEPIntHandler(BULK_IN_EP, BulkIn); + USBHwRegisterEPIntHandler(BULK_OUT_EP, BulkOut); - // register frame handler - USBHwRegisterFrameHandler(USBFrameHandler); + // register frame handler + USBHwRegisterFrameHandler(USBFrameHandler); - // enable bulk-in interrupts on NAKs - USBHwNakIntEnable(INACK_BI); + // enable bulk-in interrupts on NAKs + USBHwNakIntEnable(INACK_BI); - // initialise fifos - fifo_init(&txfifo, txdata); - fifo_init(&rxfifo, rxdata); + // initialise fifos + fifo_init(&txfifo, txdata); + fifo_init(&rxfifo, rxdata); - // set up USB interrupt - VICIntSelect &= ~VIC_BIT(VIC_USB); // select IRQ for USB - VICIntEnable = VIC_BIT(VIC_USB); + // set up USB interrupt + VICIntSelect &= ~VIC_BIT(VIC_USB); // select IRQ for USB + VICIntEnable = VIC_BIT(VIC_USB); - _VIC_CNTL(USB_VIC_SLOT) = VIC_ENABLE | VIC_USB; - _VIC_ADDR(USB_VIC_SLOT) = (uint32_t)USBIntHandler; + _VIC_CNTL(USB_VIC_SLOT) = VIC_ENABLE | VIC_USB; + _VIC_ADDR(USB_VIC_SLOT) = (uint32_t)USBIntHandler; - // connect to bus - USBHwConnect(TRUE); + // connect to bus + USBHwConnect(TRUE); // Configure generic device usb_serial.device.periph = (void *)(&usb_serial); diff --git a/sw/airborne/arch/sim/baro_MS5534A.h b/sw/airborne/arch/sim/baro_MS5534A.h index 79f473be0c..d4b5444316 100644 --- a/sw/airborne/arch/sim/baro_MS5534A.h +++ b/sw/airborne/arch/sim/baro_MS5534A.h @@ -52,7 +52,7 @@ void baro_MS5534A_reset(void); void baro_MS5534A_send(void); /* Set baro_MS5534A_available when pressure and temp are readable */ -void baro_MS5534A_event_task( void ); +void baro_MS5534A_event_task(void); #endif // USE_BARO_MS5534A diff --git a/sw/airborne/arch/sim/jsbsim_ahrs.c b/sw/airborne/arch/sim/jsbsim_ahrs.c index 40c314bd1d..4be2cdfa08 100644 --- a/sw/airborne/arch/sim/jsbsim_ahrs.c +++ b/sw/airborne/arch/sim/jsbsim_ahrs.c @@ -17,7 +17,8 @@ float sim_r; ///< in radians/s bool_t ahrs_sim_available; // Updates from jsbsim -void provide_attitude_and_rates(float phi, float theta, float psi, float p, float q, float r) { +void provide_attitude_and_rates(float phi, float theta, float psi, float p, float q, float r) +{ sim_phi = phi; sim_theta = theta; sim_psi = psi; diff --git a/sw/airborne/arch/sim/jsbsim_gps.c b/sw/airborne/arch/sim/jsbsim_gps.c index 94ca3e4715..05185b6896 100644 --- a/sw/airborne/arch/sim/jsbsim_gps.c +++ b/sw/airborne/arch/sim/jsbsim_gps.c @@ -16,7 +16,8 @@ #include "subsystems/navigation/common_nav.h" -void sim_use_gps_pos(double lat, double lon, double alt, double course, double gspeed, double climb, double time) { +void sim_use_gps_pos(double lat, double lon, double alt, double course, double gspeed, double climb, double time) +{ gps.fix = 3; // Mode 3D gps.course = course * 1e7; @@ -35,8 +36,8 @@ void sim_use_gps_pos(double lat, double lon, double alt, double course, double g utm_f.zone = nav_utm_zone0; utm_of_lla_f(&utm_f, &lla_f); LLA_BFP_OF_REAL(gps.lla_pos, lla_f); - gps.utm_pos.east = utm_f.east*100; - gps.utm_pos.north = utm_f.north*100; + gps.utm_pos.east = utm_f.east * 100; + gps.utm_pos.north = utm_f.north * 100; gps.utm_pos.zone = nav_utm_zone0; gps_available = TRUE; @@ -44,26 +45,28 @@ void sim_use_gps_pos(double lat, double lon, double alt, double course, double g } /** Space vehicle info simulation */ -void sim_update_sv(void) { +void sim_update_sv(void) +{ - gps.nb_channels=7; + gps.nb_channels = 7; int i; static int time; time++; - for(i = 0; i < gps.nb_channels; i++) { + for (i = 0; i < gps.nb_channels; i++) { gps.svinfos[i].svid = 7 + i; - gps.svinfos[i].elev = (cos(((100*i)+time)/100.) + 1) * 45; - gps.svinfos[i].azim = (time/gps.nb_channels + 50 * i) % 360; - gps.svinfos[i].cno = 40 + sin((time+i*10)/100.) * 10.; - gps.svinfos[i].flags = ((time/10) % (i+1) == 0 ? 0x00 : 0x01); + gps.svinfos[i].elev = (cos(((100 * i) + time) / 100.) + 1) * 45; + gps.svinfos[i].azim = (time / gps.nb_channels + 50 * i) % 360; + gps.svinfos[i].cno = 40 + sin((time + i * 10) / 100.) * 10.; + gps.svinfos[i].flags = ((time / 10) % (i + 1) == 0 ? 0x00 : 0x01); gps.svinfos[i].qi = (int)((time / 1000.) + i) % 8; } - gps.pdop = gps.sacc = gps.pacc = 500+200*sin(time/100.); + gps.pdop = gps.sacc = gps.pacc = 500 + 200 * sin(time / 100.); gps.num_sv = 7; } -void ubxsend_cfg_rst(uint16_t a __attribute__ ((unused)), uint8_t b __attribute__ ((unused))) { +void ubxsend_cfg_rst(uint16_t a __attribute__((unused)), uint8_t b __attribute__((unused))) +{ return; } diff --git a/sw/airborne/arch/sim/jsbsim_hw.c b/sw/airborne/arch/sim/jsbsim_hw.c index 8a38a4fb19..80ced7d4fc 100644 --- a/sw/airborne/arch/sim/jsbsim_hw.c +++ b/sw/airborne/arch/sim/jsbsim_hw.c @@ -26,9 +26,10 @@ uint16_t adc_generic_val2; uint8_t ac_id; -void update_bat(double bat) { - electrical.vsupply = (int) (bat * 10.); +void update_bat(double bat) +{ + electrical.vsupply = (int)(bat * 10.); } -void adc_generic_init( void ) {} -void adc_generic_periodic( void ) {} +void adc_generic_init(void) {} +void adc_generic_periodic(void) {} diff --git a/sw/airborne/arch/sim/jsbsim_hw.h b/sw/airborne/arch/sim/jsbsim_hw.h index 5c71014772..1c7f335702 100644 --- a/sw/airborne/arch/sim/jsbsim_hw.h +++ b/sw/airborne/arch/sim/jsbsim_hw.h @@ -55,11 +55,11 @@ void provide_attitude_and_rates(float phi, float theta, float psi, float p, floa void update_bat(double bat); -void parse_dl_ping(char* argv[]); -void parse_dl_acinfo(char* argv[]); -void parse_dl_setting(char* argv[]); -void parse_dl_get_setting(char* argv[]); -void parse_dl_block(char* argv[]); -void parse_dl_move_wp(char* argv[]); +void parse_dl_ping(char *argv[]); +void parse_dl_acinfo(char *argv[]); +void parse_dl_setting(char *argv[]); +void parse_dl_get_setting(char *argv[]); +void parse_dl_block(char *argv[]); +void parse_dl_move_wp(char *argv[]); #endif diff --git a/sw/airborne/arch/sim/jsbsim_ir.c b/sw/airborne/arch/sim/jsbsim_ir.c index 1259852536..86ec29dfa9 100644 --- a/sw/airborne/arch/sim/jsbsim_ir.c +++ b/sw/airborne/arch/sim/jsbsim_ir.c @@ -15,7 +15,8 @@ #define JSBSIM_IR_PITCH_NEUTRAL 0. #endif -void set_ir(double roll __attribute__ ((unused)), double pitch __attribute__ ((unused))) { +void set_ir(double roll __attribute__((unused)), double pitch __attribute__((unused))) +{ // INFRARED_TELEMETRY : Stupid hack to use with modules #if USE_INFRARED || USE_INFRARED_TELEMETRY double ir_contrast = 150; //FIXME @@ -31,4 +32,5 @@ void set_ir(double roll __attribute__ ((unused)), double pitch __attribute__ ((u void ir_gain_calib(void) {} /** Required by infrared.c:ir_init() */ -void adc_buf_channel(uint8_t adc_channel __attribute__ ((unused)), struct adc_buf* s __attribute__ ((unused)), uint8_t av_nb_sample __attribute__ ((unused))) {} +void adc_buf_channel(uint8_t adc_channel __attribute__((unused)), struct adc_buf *s __attribute__((unused)), + uint8_t av_nb_sample __attribute__((unused))) {} diff --git a/sw/airborne/arch/sim/jsbsim_transport.c b/sw/airborne/arch/sim/jsbsim_transport.c index f963cd815c..5f284b8589 100644 --- a/sw/airborne/arch/sim/jsbsim_transport.c +++ b/sw/airborne/arch/sim/jsbsim_transport.c @@ -8,17 +8,19 @@ #define MOfCm(_x) (((float)(_x))/100.) -void parse_dl_ping(char* argv[] __attribute__ ((unused))) { +void parse_dl_ping(char *argv[] __attribute__((unused))) +{ DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice); } -void parse_dl_acinfo(char* argv[] __attribute__ ((unused))) { +void parse_dl_acinfo(char *argv[] __attribute__((unused))) +{ #ifdef TRAFFIC_INFO uint8_t id = atoi(argv[8]); float ux = MOfCm(atoi(argv[2])); float uy = MOfCm(atoi(argv[3])); float a = MOfCm(atoi(argv[4])); - float c = RadOfDeg(((float)atoi(argv[1]))/ 10.); + float c = RadOfDeg(((float)atoi(argv[1])) / 10.); float s = MOfCm(atoi(argv[6])); float cl = MOfCm(atoi(argv[7])); uint32_t t = atoi(argv[5]); @@ -26,25 +28,29 @@ void parse_dl_acinfo(char* argv[] __attribute__ ((unused))) { #endif } -void parse_dl_setting(char* argv[]) { +void parse_dl_setting(char *argv[]) +{ uint8_t index = atoi(argv[2]); float value = atof(argv[3]); DlSetting(index, value); - DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice,&index, &value); + DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &index, &value); } -void parse_dl_get_setting(char* argv[]) { +void parse_dl_get_setting(char *argv[]) +{ uint8_t index = atoi(argv[2]); float value = settings_get_value(index); - DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice,&index, &value); + DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &index, &value); } -void parse_dl_block(char* argv[]) { +void parse_dl_block(char *argv[]) +{ int block = atoi(argv[1]); nav_goto_block(block); } -void parse_dl_move_wp(char* argv[]) { +void parse_dl_move_wp(char *argv[]) +{ uint8_t wp_id = atoi(argv[1]); float a = MOfCm(atoi(argv[5])); diff --git a/sw/airborne/arch/sim/led_hw.c b/sw/airborne/arch/sim/led_hw.c index 7db5b164d8..3b5195ab48 100644 --- a/sw/airborne/arch/sim/led_hw.c +++ b/sw/airborne/arch/sim/led_hw.c @@ -1,8 +1,9 @@ #include "led_hw.h" -value * leds_closure = 0; +value *leds_closure = 0; -value register_leds_cb(value cb_name) { +value register_leds_cb(value cb_name) +{ leds_closure = caml_named_value(String_val(cb_name)); return Val_unit; } diff --git a/sw/airborne/arch/sim/led_hw.h b/sw/airborne/arch/sim/led_hw.h index f61c3b6a13..8ea1d1874d 100644 --- a/sw/airborne/arch/sim/led_hw.h +++ b/sw/airborne/arch/sim/led_hw.h @@ -6,7 +6,7 @@ #include #include -extern value * leds_closure; +extern value *leds_closure; #define LED_INIT(i) { } #define LED_ON(i) { if (leds_closure) callback2(*leds_closure, Val_int(i), Val_int(1)); } diff --git a/sw/airborne/arch/sim/max1167_hw.c b/sw/airborne/arch/sim/max1167_hw.c index 2eef43a7df..2494b0aad0 100644 --- a/sw/airborne/arch/sim/max1167_hw.c +++ b/sw/airborne/arch/sim/max1167_hw.c @@ -24,11 +24,12 @@ #include "subsystems/imu.h" -void max1167_hw_init( void ) {} +void max1167_hw_init(void) {} -void max1167_read( void ) {} +void max1167_read(void) {} -void max1167_hw_feed_value(VEC* gyro, VEC* accel) { +void max1167_hw_feed_value(VEC *gyro, VEC *accel) +{ max1167_values[0] = gyro->ve[AXIS_P]; max1167_values[1] = gyro->ve[AXIS_Q]; diff --git a/sw/airborne/arch/sim/max1167_hw.h b/sw/airborne/arch/sim/max1167_hw.h index 1fd2bd3d8d..d377a611a2 100644 --- a/sw/airborne/arch/sim/max1167_hw.h +++ b/sw/airborne/arch/sim/max1167_hw.h @@ -30,6 +30,6 @@ #include "std.h" #include -extern void max1167_hw_feed_value(VEC* gyro, VEC* accel); +extern void max1167_hw_feed_value(VEC *gyro, VEC *accel); #endif /* MAX1167_WH */ diff --git a/sw/airborne/arch/sim/mcu_periph/adc_arch.c b/sw/airborne/arch/sim/mcu_periph/adc_arch.c index 21108582a1..8fabfaf369 100644 --- a/sw/airborne/arch/sim/mcu_periph/adc_arch.c +++ b/sw/airborne/arch/sim/mcu_periph/adc_arch.c @@ -26,8 +26,8 @@ #include "mcu_periph/adc.h" -void adc_buf_channel(uint8_t adc_channel __attribute__ ((unused)), - struct adc_buf* s __attribute__ ((unused)), - uint8_t av_nb_sample __attribute__ ((unused))) {} +void adc_buf_channel(uint8_t adc_channel __attribute__((unused)), + struct adc_buf *s __attribute__((unused)), + uint8_t av_nb_sample __attribute__((unused))) {} -void adc_init( void ) {} +void adc_init(void) {} diff --git a/sw/airborne/arch/sim/mcu_periph/i2c_arch.c b/sw/airborne/arch/sim/mcu_periph/i2c_arch.c index a1ad5f51c3..e84743c9aa 100644 --- a/sw/airborne/arch/sim/mcu_periph/i2c_arch.c +++ b/sw/airborne/arch/sim/mcu_periph/i2c_arch.c @@ -27,52 +27,61 @@ #include "mcu_periph/i2c.h" -bool_t i2c_idle(struct i2c_periph *p __attribute__ ((unused))) { return TRUE; } -bool_t i2c_submit(struct i2c_periph* p __attribute__ ((unused)), struct i2c_transaction* t __attribute__ ((unused))) { return TRUE;} +bool_t i2c_idle(struct i2c_periph *p __attribute__((unused))) { return TRUE; } +bool_t i2c_submit(struct i2c_periph *p __attribute__((unused)), struct i2c_transaction *t __attribute__((unused))) { return TRUE;} void i2c_event(void) { } -void i2c2_setbitrate(int bitrate __attribute__ ((unused))) { } +void i2c2_setbitrate(int bitrate __attribute__((unused))) { } #if USE_I2C0 struct i2c_errors i2c0_errors; -void i2c0_hw_init(void) { +void i2c0_hw_init(void) +{ i2c0.errors = &i2c0_errors; ZEROS_ERR_COUNTER(i2c0_errors); } -void i2c0_ev_isr(void) { +void i2c0_ev_isr(void) +{ } -void i2c0_er_isr(void) { +void i2c0_er_isr(void) +{ } #endif #if USE_I2C1 struct i2c_errors i2c1_errors; -void i2c1_hw_init(void) { +void i2c1_hw_init(void) +{ i2c1.errors = &i2c1_errors; ZEROS_ERR_COUNTER(i2c1_errors); } -void i2c1_ev_isr(void) { +void i2c1_ev_isr(void) +{ } -void i2c1_er_isr(void) { +void i2c1_er_isr(void) +{ } #endif #if USE_I2C2 struct i2c_errors i2c2_errors; -void i2c2_hw_init(void) { +void i2c2_hw_init(void) +{ i2c2.errors = &i2c2_errors; ZEROS_ERR_COUNTER(i2c2_errors); } -void i2c2_ev_isr(void) { +void i2c2_ev_isr(void) +{ } -void i2c2_er_isr(void) { +void i2c2_er_isr(void) +{ } #endif diff --git a/sw/airborne/arch/sim/mcu_periph/spi_arch.c b/sw/airborne/arch/sim/mcu_periph/spi_arch.c index 95e8b1fa56..bd5b72659f 100644 --- a/sw/airborne/arch/sim/mcu_periph/spi_arch.c +++ b/sw/airborne/arch/sim/mcu_periph/spi_arch.c @@ -27,15 +27,15 @@ #include "mcu_periph/spi.h" -bool_t spi_submit(struct spi_periph* p __attribute__ ((unused)), struct spi_transaction* t __attribute__ ((unused))) { return TRUE;} +bool_t spi_submit(struct spi_periph *p __attribute__((unused)), struct spi_transaction *t __attribute__((unused))) { return TRUE;} void spi_init_slaves(void) {} -void spi_slave_select(uint8_t slave __attribute__ ((unused))) {} +void spi_slave_select(uint8_t slave __attribute__((unused))) {} -void spi_slave_unselect(uint8_t slave __attribute__ ((unused))) {} +void spi_slave_unselect(uint8_t slave __attribute__((unused))) {} -bool_t spi_lock(struct spi_periph* p __attribute__ ((unused)), uint8_t slave __attribute__ ((unused))) { return TRUE; } +bool_t spi_lock(struct spi_periph *p __attribute__((unused)), uint8_t slave __attribute__((unused))) { return TRUE; } -bool_t spi_resume(struct spi_periph* p __attribute__ ((unused)), uint8_t slave __attribute__ ((unused))) { return TRUE; } +bool_t spi_resume(struct spi_periph *p __attribute__((unused)), uint8_t slave __attribute__((unused))) { return TRUE; } diff --git a/sw/airborne/arch/sim/mcu_periph/sys_time_arch.c b/sw/airborne/arch/sim/mcu_periph/sys_time_arch.c index b261ab76b6..b5b69dd82a 100644 --- a/sw/airborne/arch/sim/mcu_periph/sys_time_arch.c +++ b/sw/airborne/arch/sim/mcu_periph/sys_time_arch.c @@ -28,20 +28,22 @@ #include "mcu_periph/sys_time.h" -void sys_time_arch_init( void ) { +void sys_time_arch_init(void) +{ // simulate 1us cpu ticks sys_time.cpu_ticks_per_sec = 1e6; sys_time.resolution_cpu_ticks = (uint32_t)(sys_time.resolution * sys_time.cpu_ticks_per_sec + 0.5); } -void sys_tick_handler( void ) { +void sys_tick_handler(void) +{ sys_time.nb_tick++; sys_time.nb_sec_rem += sys_time.resolution_cpu_ticks; if (sys_time.nb_sec_rem >= sys_time.cpu_ticks_per_sec) { sys_time.nb_sec_rem -= sys_time.cpu_ticks_per_sec; sys_time.nb_sec++; } - for (unsigned int i=0; i= sys_time.timer[i].end_time) { sys_time.timer[i].end_time += sys_time.timer[i].duration; diff --git a/sw/airborne/arch/sim/mcu_periph/sys_time_arch.h b/sw/airborne/arch/sim/mcu_periph/sys_time_arch.h index e5bc6927a0..72f57323fb 100644 --- a/sw/airborne/arch/sim/mcu_periph/sys_time_arch.h +++ b/sw/airborne/arch/sim/mcu_periph/sys_time_arch.h @@ -36,21 +36,23 @@ extern void sys_tick_handler(void); * Get the time in microseconds since startup. * @return microseconds since startup as uint32_t */ -static inline uint32_t get_sys_time_usec(void) { +static inline uint32_t get_sys_time_usec(void) +{ return sys_time.nb_sec * 1000000 + - usec_of_cpu_ticks(sys_time.nb_sec_rem); + usec_of_cpu_ticks(sys_time.nb_sec_rem); } /** * Get the time in milliseconds since startup. * @return milliseconds since startup as uint32_t */ -static inline uint32_t get_sys_time_msec(void) { +static inline uint32_t get_sys_time_msec(void) +{ return sys_time.nb_sec * 1000 + - msec_of_cpu_ticks(sys_time.nb_sec_rem); + msec_of_cpu_ticks(sys_time.nb_sec_rem); } -static inline void sys_time_usleep(uint32_t us __attribute__ ((unused))) {} +static inline void sys_time_usleep(uint32_t us __attribute__((unused))) {} #endif /* SYS_TIME_ARCH_H */ diff --git a/sw/airborne/arch/sim/mcu_periph/uart_arch.h b/sw/airborne/arch/sim/mcu_periph/uart_arch.h index 098c358373..6ed4ac0180 100644 --- a/sw/airborne/arch/sim/mcu_periph/uart_arch.h +++ b/sw/airborne/arch/sim/mcu_periph/uart_arch.h @@ -40,7 +40,8 @@ extern uint8_t stdinout_rx_extract_idx; #define UART_SPEED(_def) {} -static inline bool StdInOutChAvailable(void) { +static inline bool StdInOutChAvailable(void) +{ struct timeval tv; fd_set fds; tv.tv_sec = 0; @@ -52,7 +53,7 @@ static inline bool StdInOutChAvailable(void) { char tmp_buf[STDINOUT_BUFFER_SIZE]; uint8_t n = read(FD_STDIN, tmp_buf, STDINOUT_BUFFER_SIZE); unsigned int i; - for(i = 0; i < n; i++) { + for (i = 0; i < n; i++) { stdinout_buffer[stdinout_rx_insert_idx] = tmp_buf[i]; stdinout_rx_insert_idx++; /* Auto overflow */ } @@ -62,6 +63,6 @@ static inline bool StdInOutChAvailable(void) { #define StdInOutTransmit(_char) putchar(_char) #define StdInOutGetch() ({ \ - assert(stdinout_rx_insert_idx != stdinout_rx_extract_idx); \ - stdinout_buffer[stdinout_rx_extract_idx++]; \ -}) + assert(stdinout_rx_insert_idx != stdinout_rx_extract_idx); \ + stdinout_buffer[stdinout_rx_extract_idx++]; \ + }) diff --git a/sw/airborne/arch/sim/modules/core/booz_pwm_arch.c b/sw/airborne/arch/sim/modules/core/booz_pwm_arch.c index 31c9347514..5396aedd2b 100644 --- a/sw/airborne/arch/sim/modules/core/booz_pwm_arch.c +++ b/sw/airborne/arch/sim/modules/core/booz_pwm_arch.c @@ -22,6 +22,6 @@ #include "modules/core/booz_pwm_arch.h" -void booz_pwm_init_arch( void ) {} +void booz_pwm_init_arch(void) {} diff --git a/sw/airborne/arch/sim/modules/core/trigger_ext_hw.c b/sw/airborne/arch/sim/modules/core/trigger_ext_hw.c index 16a807f5e2..0763981327 100644 --- a/sw/airborne/arch/sim/modules/core/trigger_ext_hw.c +++ b/sw/airborne/arch/sim/modules/core/trigger_ext_hw.c @@ -24,7 +24,8 @@ #include "core/trigger_ext_hw.h" #include "std.h" -void trigger_ext_init ( void ) { +void trigger_ext_init(void) +{ trigger_ext_valid = FALSE; } diff --git a/sw/airborne/arch/sim/modules/enose/sim_enose.c b/sw/airborne/arch/sim/modules/enose/sim_enose.c index c3e7802128..89fd32dc32 100644 --- a/sw/airborne/arch/sim/modules/enose/sim_enose.c +++ b/sw/airborne/arch/sim/modules/enose/sim_enose.c @@ -8,18 +8,22 @@ uint16_t enose_PID_val; uint16_t nominal_val[ENOSE_NB_SENSOR] = {1500, 1500, 4000}; uint16_t min_val[ENOSE_NB_SENSOR] = {1100, 1200, 2500}; -void enose_init( void ) { +void enose_init(void) +{ int i; - for(i = 0; i < ENOSE_NB_SENSOR; i++) + for (i = 0; i < ENOSE_NB_SENSOR; i++) { enose_val[i] = nominal_val[i]; + } } void enose_set_heat(uint8_t no_sensor, uint8_t value) { } -void enose_periodic( void ) { +void enose_periodic(void) +{ int i; - for(i = 0; i < ENOSE_NB_SENSOR; i++) { - if (enose_val[i] < min_val[i]) + for (i = 0; i < ENOSE_NB_SENSOR; i++) { + if (enose_val[i] < min_val[i]) { enose_val[i] = min_val[i]; - int d = nominal_val[i]- enose_val[i]; + } + int d = nominal_val[i] - enose_val[i]; enose_val[i] += d / 10.; } } diff --git a/sw/airborne/arch/sim/modules/ins/ins_arduimu.c b/sw/airborne/arch/sim/modules/ins/ins_arduimu.c index 4ff9503db8..5d46073008 100644 --- a/sw/airborne/arch/sim/modules/ins/ins_arduimu.c +++ b/sw/airborne/arch/sim/modules/ins/ins_arduimu.c @@ -22,8 +22,9 @@ float throttle_slew; extern float sim_phi; extern float sim_theta; -void ArduIMU_init( void ) {} -void ArduIMU_periodic( void ) { +void ArduIMU_init(void) {} +void ArduIMU_periodic(void) +{ // Feed directly the estimator struct FloatEulers att = { sim_phi - ins_roll_neutral, @@ -32,6 +33,6 @@ void ArduIMU_periodic( void ) { }; stateSetNedToBodyEulers_f(&att); } -void ArduIMU_periodicGPS( void ) {} -void IMU_Daten_verarbeiten( void ) {} +void ArduIMU_periodicGPS(void) {} +void IMU_Daten_verarbeiten(void) {} diff --git a/sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c b/sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c index 6b130d49f2..010fd189f1 100644 --- a/sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c +++ b/sw/airborne/arch/sim/modules/ins/ins_arduimu_basic.c @@ -24,8 +24,9 @@ extern float sim_p; extern float sim_q; extern float sim_r; -void ArduIMU_init( void ) {} -void ArduIMU_periodic( void ) { +void ArduIMU_init(void) {} +void ArduIMU_periodic(void) +{ // Feed directly the estimator struct FloatEulers att = { sim_phi - ins_roll_neutral, @@ -36,5 +37,5 @@ void ArduIMU_periodic( void ) { struct FloatRates rates = { sim_p, sim_q, sim_r }; stateSetBodyRates_f(&rates); } -void ArduIMU_periodicGPS( void ) {} -void ArduIMU_event( void ) {} +void ArduIMU_periodicGPS(void) {} +void ArduIMU_event(void) {} diff --git a/sw/airborne/arch/sim/peripherals/hmc5843_arch.c b/sw/airborne/arch/sim/peripherals/hmc5843_arch.c index 7c3b6c5b95..95423c33cb 100644 --- a/sw/airborne/arch/sim/peripherals/hmc5843_arch.c +++ b/sw/airborne/arch/sim/peripherals/hmc5843_arch.c @@ -21,6 +21,6 @@ #include "peripherals/hmc5843.h" -void hmc5843_arch_init( void ) {} +void hmc5843_arch_init(void) {} -void hmc5843_read( void ) {} +void hmc5843_read(void) {} diff --git a/sw/airborne/arch/sim/peripherals/max1168_arch.c b/sw/airborne/arch/sim/peripherals/max1168_arch.c index a4328fadb4..549e3ae6b8 100644 --- a/sw/airborne/arch/sim/peripherals/max1168_arch.c +++ b/sw/airborne/arch/sim/peripherals/max1168_arch.c @@ -21,6 +21,6 @@ #include "peripherals/max1168.h" -void max1168_arch_init( void ) {} +void max1168_arch_init(void) {} -void max1168_read( void ) {} +void max1168_read(void) {} diff --git a/sw/airborne/arch/sim/sim_adc_generic.c b/sw/airborne/arch/sim/sim_adc_generic.c index 4965ed15de..5e77335e35 100644 --- a/sw/airborne/arch/sim/sim_adc_generic.c +++ b/sw/airborne/arch/sim/sim_adc_generic.c @@ -4,13 +4,16 @@ uint16_t adc_generic_val1; uint16_t adc_generic_val2; -void adc_generic_init( void ) { +void adc_generic_init(void) +{ } -void adc_generic_periodic( void ) { +void adc_generic_periodic(void) +{ } -value update_adc1(value adc1) { +value update_adc1(value adc1) +{ adc_generic_val1 = Int_val(adc1); return Val_unit; } diff --git a/sw/airborne/arch/sim/sim_ahrs.c b/sw/airborne/arch/sim/sim_ahrs.c index 181be5a11c..559c25688e 100644 --- a/sw/airborne/arch/sim/sim_ahrs.c +++ b/sw/airborne/arch/sim/sim_ahrs.c @@ -19,17 +19,19 @@ bool_t ahrs_sim_available; // Updates from Ocaml sim -value provide_attitude(value phi, value theta, value psi) { +value provide_attitude(value phi, value theta, value psi) +{ sim_phi = Double_val(phi); sim_theta = Double_val(theta); - sim_psi = - Double_val(psi) + M_PI/2.; + sim_psi = - Double_val(psi) + M_PI / 2.; ahrs_sim_available = TRUE; return Val_unit; } -value provide_rates(value p, value q, value r) { +value provide_rates(value p, value q, value r) +{ sim_p = Double_val(p); sim_q = Double_val(q); sim_r = Double_val(r); diff --git a/sw/airborne/arch/sim/sim_ap.c b/sw/airborne/arch/sim/sim_ap.c index 51502e5617..d9ea5e32b7 100644 --- a/sw/airborne/arch/sim/sim_ap.c +++ b/sw/airborne/arch/sim/sim_ap.c @@ -56,12 +56,14 @@ uint8_t ac_id; #endif /** needs to be called at SYS_TIME_FREQUENCY */ -value sim_sys_time_task(value unit) { +value sim_sys_time_task(value unit) +{ sys_tick_handler(); return unit; } -value sim_periodic_task(value unit) { +value sim_periodic_task(value unit) +{ sensors_task(); attitude_loop(); reporting_task(); @@ -73,25 +75,29 @@ value sim_periodic_task(value unit) { return unit; } -value sim_monitor_task(value unit) { +value sim_monitor_task(value unit) +{ monitor_task(); return unit; } -value sim_nav_task(value unit) { +value sim_nav_task(value unit) +{ navigation_task(); return unit; } -float ftimeofday(void) { +float ftimeofday(void) +{ struct timeval t; struct timezone z; gettimeofday(&t, &z); - return (t.tv_sec + t.tv_usec/1e6); + return (t.tv_sec + t.tv_usec / 1e6); } -value sim_init(value unit) { +value sim_init(value unit) +{ init_fbw(); init_ap(); #ifdef SIM_UART @@ -106,53 +112,60 @@ value sim_init(value unit) { if (stat(link_pipe_name, &st)) { if (mkfifo(link_pipe_name, 0644) == -1) { perror("make pipe"); - exit (10); + exit(10); } } - if ( !(pipe_stream = fopen(link_pipe_name, "w")) ) { + if (!(pipe_stream = fopen(link_pipe_name, "w"))) { perror("open pipe"); - exit (10); + exit(10); } #endif return unit; } -value update_bat(value bat) { +value update_bat(value bat) +{ electrical.vsupply = Int_val(bat); return Val_unit; } -value update_dl_status(value dl_enabled) { +value update_dl_status(value dl_enabled) +{ ivy_tp.ivy_dl_enabled = Int_val(dl_enabled); return Val_unit; } -value get_commands(value val_commands) { +value get_commands(value val_commands) +{ int i; - for(i=0; i < COMMANDS_NB; i++) + for (i = 0; i < COMMANDS_NB; i++) { Store_field(val_commands, i, Val_int(commands[i])); + } return Val_int(commands[COMMAND_THROTTLE]); } -value set_datalink_message(value s) { +value set_datalink_message(value s) +{ int n = string_length(s); char *ss = String_val(s); assert(n <= MSG_SIZE); int i; - for(i = 0; i < n; i++) + for (i = 0; i < n; i++) { dl_buffer[i] = ss[i]; + } dl_parse_msg(); return Val_unit; } /** Required by electrical */ -void adc_buf_channel(void* a __attribute__ ((unused)), - void* b __attribute__ ((unused)), - void* c __attribute__ ((unused))) { +void adc_buf_channel(void *a __attribute__((unused)), + void *b __attribute__((unused)), + void *c __attribute__((unused))) +{ } diff --git a/sw/airborne/arch/sim/sim_gps.c b/sw/airborne/arch/sim/sim_gps.c index 75ded9ea8b..e3f46e6a07 100644 --- a/sw/airborne/arch/sim/sim_gps.c +++ b/sw/airborne/arch/sim/sim_gps.c @@ -17,7 +17,9 @@ #include -value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, value cl, value t, value m, value lat, value lon) { +value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, value cl, value t, value m, value lat, + value lon) +{ gps.fix = (Bool_val(m) ? 3 : 0); gps.course = Double_val(c) * 1e7; gps.hmsl = Double_val(a) * 1000.; @@ -39,8 +41,8 @@ value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, valu utm_f.zone = nav_utm_zone0; utm_of_lla_f(&utm_f, &lla_f); LLA_BFP_OF_REAL(gps.lla_pos, lla_f); - gps.utm_pos.east = utm_f.east*100; - gps.utm_pos.north = utm_f.north*100; + gps.utm_pos.east = utm_f.east * 100; + gps.utm_pos.north = utm_f.north * 100; gps.utm_pos.zone = nav_utm_zone0; x = y = z; /* Just to get rid of the "unused arg" warning */ y = x; /* Just to get rid of the "unused arg" warning */ @@ -54,19 +56,19 @@ value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, valu /** Space vehicle info simulation */ - gps.nb_channels=7; + gps.nb_channels = 7; int i; static int time; time++; - for(i = 0; i < gps.nb_channels; i++) { + for (i = 0; i < gps.nb_channels; i++) { gps.svinfos[i].svid = 7 + i; - gps.svinfos[i].elev = (cos(((100*i)+time)/100.) + 1) * 45; - gps.svinfos[i].azim = (time/gps.nb_channels + 50 * i) % 360; - gps.svinfos[i].cno = 40 + sin((time+i*10)/100.) * 10.; - gps.svinfos[i].flags = ((time/10) % (i+1) == 0 ? 0x00 : 0x01); + gps.svinfos[i].elev = (cos(((100 * i) + time) / 100.) + 1) * 45; + gps.svinfos[i].azim = (time / gps.nb_channels + 50 * i) % 360; + gps.svinfos[i].cno = 40 + sin((time + i * 10) / 100.) * 10.; + gps.svinfos[i].flags = ((time / 10) % (i + 1) == 0 ? 0x00 : 0x01); gps.svinfos[i].qi = (int)((time / 1000.) + i) % 8; } - gps.pdop = gps.sacc = gps.pacc = 500+200*sin(time/100.); + gps.pdop = gps.sacc = gps.pacc = 500 + 200 * sin(time / 100.); gps.num_sv = 7; //gps_verbose_downlink = !launch; @@ -78,12 +80,14 @@ value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, valu } /* Second binding required because number of args > 5 */ -value sim_use_gps_pos_bytecode(value *a, int argn) { +value sim_use_gps_pos_bytecode(value *a, int argn) +{ assert(argn == 11); - return sim_use_gps_pos(a[0],a[1],a[2],a[3],a[4],a[5],a[6],a[7], a[8],a[9], a[10]); + return sim_use_gps_pos(a[0], a[1], a[2], a[3], a[4], a[5], a[6], a[7], a[8], a[9], a[10]); } -void ubxsend_cfg_rst(uint16_t a __attribute__ ((unused)), uint8_t b __attribute__ ((unused))) { +void ubxsend_cfg_rst(uint16_t a __attribute__((unused)), uint8_t b __attribute__((unused))) +{ return; } diff --git a/sw/airborne/arch/sim/sim_ir.c b/sw/airborne/arch/sim/sim_ir.c index 59684001db..83ccec2c2b 100644 --- a/sw/airborne/arch/sim/sim_ir.c +++ b/sw/airborne/arch/sim/sim_ir.c @@ -13,15 +13,17 @@ float sim_air_speed; -void ir_gain_calib(void) { +void ir_gain_calib(void) +{ } value set_ir_and_airspeed( - value roll __attribute__ ((unused)), - value front __attribute__ ((unused)), - value top __attribute__ ((unused)), - value air_speed - ) { + value roll __attribute__((unused)), + value front __attribute__((unused)), + value top __attribute__((unused)), + value air_speed +) +{ // INFRARED_TELEMETRY : Stupid hack to use with modules #if USE_INFRARED || USE_INFRARED_TELEMETRY infrared.roll = Int_val(roll); diff --git a/sw/airborne/arch/sim/sim_uart.c b/sw/airborne/arch/sim/sim_uart.c index a834111310..01e0eed92e 100644 --- a/sw/airborne/arch/sim/sim_uart.c +++ b/sw/airborne/arch/sim/sim_uart.c @@ -1,3 +1,3 @@ #include "sim_uart.h" -FILE* pipe_stream; +FILE *pipe_stream; diff --git a/sw/airborne/arch/sim/sim_uart.h b/sw/airborne/arch/sim/sim_uart.h index 9f137541e3..ed6d23ce16 100644 --- a/sw/airborne/arch/sim/sim_uart.h +++ b/sw/airborne/arch/sim/sim_uart.h @@ -4,7 +4,7 @@ #include -extern FILE* pipe_stream; +extern FILE *pipe_stream; #define SimUartCheckFreeSpace(_) TRUE diff --git a/sw/airborne/arch/sim/subsystems/actuators/actuators_dualpwm_arch.c b/sw/airborne/arch/sim/subsystems/actuators/actuators_dualpwm_arch.c index 9fcfc6c883..e1b282f906 100644 --- a/sw/airborne/arch/sim/subsystems/actuators/actuators_dualpwm_arch.c +++ b/sw/airborne/arch/sim/subsystems/actuators/actuators_dualpwm_arch.c @@ -25,7 +25,8 @@ #include "subsystems/actuators/actuators_dualpwm_arch.h" -void actuators_dualpwm_arch_init(void) { +void actuators_dualpwm_arch_init(void) +{ } diff --git a/sw/airborne/arch/sim/subsystems/actuators/actuators_pwm_arch.c b/sw/airborne/arch/sim/subsystems/actuators/actuators_pwm_arch.c index ad6b754f91..1894360893 100644 --- a/sw/airborne/arch/sim/subsystems/actuators/actuators_pwm_arch.c +++ b/sw/airborne/arch/sim/subsystems/actuators/actuators_pwm_arch.c @@ -25,7 +25,8 @@ #include "subsystems/actuators/actuators_pwm_arch.h" -void actuators_pwm_arch_init(void) { +void actuators_pwm_arch_init(void) +{ } diff --git a/sw/airborne/arch/sim/subsystems/datalink/superbitrf.c b/sw/airborne/arch/sim/subsystems/datalink/superbitrf.c index c518d2f51d..178a33b6c8 100644 --- a/sw/airborne/arch/sim/subsystems/datalink/superbitrf.c +++ b/sw/airborne/arch/sim/subsystems/datalink/superbitrf.c @@ -31,10 +31,12 @@ /* The superbitRF structure */ struct SuperbitRF superbitrf; -void superbitrf_set_mfg_id(uint32_t id) { +void superbitrf_set_mfg_id(uint32_t id) +{ superbitrf.bind_mfg_id32 = id; } -void superbitrf_set_protocol(uint8_t protocol) { +void superbitrf_set_protocol(uint8_t protocol) +{ superbitrf.protocol = protocol; } diff --git a/sw/airborne/arch/sim/subsystems/radio_control/rc_datalink.c b/sw/airborne/arch/sim/subsystems/radio_control/rc_datalink.c index 75468d3504..e0d621ca02 100644 --- a/sw/airborne/arch/sim/subsystems/radio_control/rc_datalink.c +++ b/sw/airborne/arch/sim/subsystems/radio_control/rc_datalink.c @@ -24,10 +24,12 @@ #include #include -value update_rc_channel(value c __attribute__ ((unused)), value v __attribute__ ((unused))) { +value update_rc_channel(value c __attribute__((unused)), value v __attribute__((unused))) +{ return Val_unit; } -value send_ppm(value unit) { +value send_ppm(value unit) +{ return unit; } diff --git a/sw/airborne/arch/sim/subsystems/radio_control/spektrum_arch.c b/sw/airborne/arch/sim/subsystems/radio_control/spektrum_arch.c index 047985b919..c879609293 100644 --- a/sw/airborne/arch/sim/subsystems/radio_control/spektrum_arch.c +++ b/sw/airborne/arch/sim/subsystems/radio_control/spektrum_arch.c @@ -45,7 +45,8 @@ void radio_control_impl_init(void) { spektrum_available = FALSE; } -void RadioControlEventImp(void (*frame_handler)(void) ) { +void RadioControlEventImp(void (*frame_handler)(void)) +{ if (spektrum_available) { radio_control.frame_cpt++; radio_control.time_since_last_frame = 0; @@ -57,7 +58,8 @@ void RadioControlEventImp(void (*frame_handler)(void) ) { #if USE_NPS #ifdef RADIO_CONTROL -void radio_control_feed(void) { +void radio_control_feed(void) +{ radio_control.values[RADIO_ROLL] = nps_radio_control.roll * MAX_PPRZ; radio_control.values[RADIO_PITCH] = nps_radio_control.pitch * MAX_PPRZ; radio_control.values[RADIO_YAW] = nps_radio_control.yaw * MAX_PPRZ; @@ -71,26 +73,28 @@ void radio_control_feed(void) {} #elif !USE_JSBSIM // not NPS and not JSBSIM -> simple ocaml sim #ifdef RADIO_CONTROL -value update_rc_channel(value c, value v) { +value update_rc_channel(value c, value v) +{ // OCaml sim sends ppm values read from radio xml //assume "ppm" value range from 1000 to 2000 for now.. like in fake spektrum.xml if (Int_val(c) == 0) { // throttle channel has neutral at 1000 radio_control.values[Int_val(c)] = (Double_val(v) - 1000.0) / 1000 * MAX_PPRZ; - } - else { + } else { // all other channels at 1500 radio_control.values[Int_val(c)] = (Double_val(v) - 1500.0) / 500 * MAX_PPRZ; } return Val_unit; } -value send_ppm(value unit) { +value send_ppm(value unit) +{ spektrum_available = TRUE; return unit; } #else // RADIO_CONTROL -value update_rc_channel(value c __attribute__ ((unused)), value v __attribute__ ((unused))) { +value update_rc_channel(value c __attribute__((unused)), value v __attribute__((unused))) +{ return Val_unit; } value send_ppm(value unit) {return unit;} diff --git a/sw/airborne/arch/sim/subsystems/settings_arch.c b/sw/airborne/arch/sim/subsystems/settings_arch.c index ea69ba9f12..179c8fe5df 100644 --- a/sw/airborne/arch/sim/subsystems/settings_arch.c +++ b/sw/airborne/arch/sim/subsystems/settings_arch.c @@ -28,10 +28,12 @@ #include "subsystems/settings.h" -int32_t persistent_write(uint32_t ptr UNUSED, uint32_t size UNUSED) { +int32_t persistent_write(uint32_t ptr UNUSED, uint32_t size UNUSED) +{ return -1; } -int32_t persistent_read(uint32_t ptr UNUSED, uint32_t size UNUSED) { +int32_t persistent_read(uint32_t ptr UNUSED, uint32_t size UNUSED) +{ return -1; } diff --git a/sw/airborne/arch/stm32/link_mcu_hw.h b/sw/airborne/arch/stm32/link_mcu_hw.h index 2a997b361c..122032b0a0 100644 --- a/sw/airborne/arch/stm32/link_mcu_hw.h +++ b/sw/airborne/arch/stm32/link_mcu_hw.h @@ -32,7 +32,8 @@ #define CrcLow(x) ((x)&0xff) #define CrcHigh(x) ((x)>>8) -static inline uint16_t CrcUpdate(uint16_t crc, uint8_t data) { +static inline uint16_t CrcUpdate(uint16_t crc, uint8_t data) +{ uint8_t a = ((uint8_t)CrcHigh(crc)) + data; uint8_t b = ((uint8_t)CrcLow(crc)) + a; crc = b | a << 8; diff --git a/sw/airborne/arch/stm32/mcu_arch.c b/sw/airborne/arch/stm32/mcu_arch.c index d628c5731c..b64fad1d2a 100644 --- a/sw/airborne/arch/stm32/mcu_arch.c +++ b/sw/airborne/arch/stm32/mcu_arch.c @@ -39,9 +39,10 @@ #include "std.h" -void mcu_arch_init(void) { +void mcu_arch_init(void) +{ #if LUFTBOOT -PRINT_CONFIG_MSG("We are running luftboot, the interrupt vector is being relocated.") + PRINT_CONFIG_MSG("We are running luftboot, the interrupt vector is being relocated.") #if defined STM32F4 SCB_VTOR = 0x00004000; #else @@ -50,28 +51,28 @@ PRINT_CONFIG_MSG("We are running luftboot, the interrupt vector is being relocat #endif #if EXT_CLK == 8000000 #if defined(STM32F1) -PRINT_CONFIG_MSG("Using 8MHz external clock to PLL it to 72MHz.") + PRINT_CONFIG_MSG("Using 8MHz external clock to PLL it to 72MHz.") rcc_clock_setup_in_hse_8mhz_out_72mhz(); #elif defined(STM32F4) -PRINT_CONFIG_MSG("Using 8MHz external clock to PLL it to 168MHz.") + PRINT_CONFIG_MSG("Using 8MHz external clock to PLL it to 168MHz.") rcc_clock_setup_hse_3v3(&hse_8mhz_3v3[CLOCK_3V3_168MHZ]); #endif #elif EXT_CLK == 12000000 #if defined(STM32F1) -PRINT_CONFIG_MSG("Using 12MHz external clock to PLL it to 72MHz.") + PRINT_CONFIG_MSG("Using 12MHz external clock to PLL it to 72MHz.") rcc_clock_setup_in_hse_12mhz_out_72mhz(); #elif defined(STM32F4) -PRINT_CONFIG_MSG("Using 12MHz external clock to PLL it to 168MHz.") + PRINT_CONFIG_MSG("Using 12MHz external clock to PLL it to 168MHz.") rcc_clock_setup_hse_3v3(&hse_12mhz_3v3[CLOCK_3V3_168MHZ]); #endif #elif EXT_CLK == 16000000 #if defined(STM32F4) -PRINT_CONFIG_MSG("Using 16MHz external clock to PLL it to 168MHz.") + PRINT_CONFIG_MSG("Using 16MHz external clock to PLL it to 168MHz.") rcc_clock_setup_hse_3v3(&hse_16mhz_3v3[CLOCK_3V3_168MHZ]); #endif #elif EXT_CLK == 25000000 #if defined(STM32F4) -PRINT_CONFIG_MSG("Using 25MHz external clock to PLL it to 168MHz.") + PRINT_CONFIG_MSG("Using 25MHz external clock to PLL it to 168MHz.") rcc_clock_setup_hse_3v3(&hse_25mhz_3v3[CLOCK_3V3_168MHZ]); #endif #else @@ -89,11 +90,11 @@ PRINT_CONFIG_MSG("Using 25MHz external clock to PLL it to 168MHz.") } #if defined(STM32F1) -#define RCC_CFGR_PPRE2_SHIFT 11 -#define RCC_CFGR_PPRE2 (7 << RCC_CFGR_PPRE2_SHIFT) +#define RCC_CFGR_PPRE2_SHIFT 11 +#define RCC_CFGR_PPRE2 (7 << RCC_CFGR_PPRE2_SHIFT) -#define RCC_CFGR_PPRE1_SHIFT 8 -#define RCC_CFGR_PPRE1 (7 << RCC_CFGR_PPRE1_SHIFT) +#define RCC_CFGR_PPRE1_SHIFT 8 +#define RCC_CFGR_PPRE1 (7 << RCC_CFGR_PPRE1_SHIFT) static inline uint32_t rcc_get_ppre1(void) { @@ -107,12 +108,12 @@ static inline uint32_t rcc_get_ppre2(void) #elif defined(STM32F4) static inline uint32_t rcc_get_ppre1(void) { - return RCC_CFGR &((1 << 10) | (1 << 11) | (1 << 12)); + return RCC_CFGR & ((1 << 10) | (1 << 11) | (1 << 12)); } static inline uint32_t rcc_get_ppre2(void) { - return RCC_CFGR &((1 << 13) | (1 << 14) | (1 << 15)); + return RCC_CFGR & ((1 << 13) | (1 << 14) | (1 << 15)); } #endif @@ -126,7 +127,7 @@ static inline uint32_t rcc_get_ppre2(void) uint32_t timer_get_frequency(uint32_t timer_peripheral) { switch (timer_peripheral) { - // Timers on APB1 + // Timers on APB1 case TIM1: case TIM8: #ifdef TIM9 @@ -140,11 +141,13 @@ uint32_t timer_get_frequency(uint32_t timer_peripheral) #endif if (!rcc_get_ppre2()) // no APB2 prescaler + { return rcc_ppre2_frequency; - else + } else { return rcc_ppre2_frequency * 2; + } - // timers on APB2 + // timers on APB2 case TIM2: case TIM3: case TIM4: @@ -162,9 +165,11 @@ uint32_t timer_get_frequency(uint32_t timer_peripheral) #endif if (!rcc_get_ppre1()) // no APB2 prescaler + { return rcc_ppre1_frequency; - else + } else { return rcc_ppre1_frequency * 2; + } default: // other timers currently not supported break; diff --git a/sw/airborne/arch/stm32/mcu_arch.h b/sw/airborne/arch/stm32/mcu_arch.h index 37fffacc69..af80e0dc22 100644 --- a/sw/airborne/arch/stm32/mcu_arch.h +++ b/sw/airborne/arch/stm32/mcu_arch.h @@ -40,10 +40,10 @@ extern void mcu_arch_init(void); */ #define MyByteSwap16(in, out) { \ asm volatile ( \ - "rev16 %0, %1\n\t" \ - : "=r" (out) \ - : "r"(in) \ - ); \ + "rev16 %0, %1\n\t" \ + : "=r" (out) \ + : "r"(in) \ + ); \ } #define mcu_int_enable() {} diff --git a/sw/airborne/arch/stm32/mcu_periph/adc_arch.c b/sw/airborne/arch/stm32/mcu_periph/adc_arch.c index e753280a3a..7613f4d841 100644 --- a/sw/airborne/arch/stm32/mcu_periph/adc_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/adc_arch.c @@ -168,13 +168,13 @@ PRINT_CONFIG_MSG("Analog to Digital Coverter 3 active") /*** STATIC FUNCTION PROTOTYPES ***/ /***************************************/ -static inline void adc_init_single(uint32_t adc, uint8_t nb_channels, uint8_t* channel_map); +static inline void adc_init_single(uint32_t adc, uint8_t nb_channels, uint8_t *channel_map); -static inline void adc_push_sample(struct adc_buf * buf, +static inline void adc_push_sample(struct adc_buf *buf, uint16_t sample); -static inline void adc_init_rcc( void ); -static inline void adc_init_irq( void ); +static inline void adc_init_rcc(void); +static inline void adc_init_irq(void); /********************************/ @@ -195,15 +195,15 @@ static uint8_t nb_adc3_channels = 0; #if USE_AD1 /// List of buffers, one for each active channel. -static struct adc_buf * adc1_buffers[4]; +static struct adc_buf *adc1_buffers[4]; #endif #if USE_AD2 /// List of buffers, one for each active channel. -static struct adc_buf * adc2_buffers[4]; +static struct adc_buf *adc2_buffers[4]; #endif #if USE_AD3 /// List of buffers, one for each active channel. -static struct adc_buf * adc3_buffers[4]; +static struct adc_buf *adc3_buffers[4]; #endif #if USE_ADC_WATCHDOG @@ -220,9 +220,10 @@ static struct { /*** PUBLIC FUNCTION DEFINITIONS ***/ /***************************************/ -void adc_init( void ) { +void adc_init(void) +{ - uint8_t x=0; + uint8_t x = 0; // ADC channel mapping uint8_t adc_channel_map[4]; @@ -361,27 +362,25 @@ void adc_init( void ) { #if USE_ADC_WATCHDOG adc_watchdog.cb = NULL; - adc_watchdog.timeStamp=0; + adc_watchdog.timeStamp = 0; #endif } -void adc_buf_channel(uint8_t adc_channel, struct adc_buf * s, uint8_t av_nb_sample) +void adc_buf_channel(uint8_t adc_channel, struct adc_buf *s, uint8_t av_nb_sample) { if (adc_channel < nb_adc1_channels) { #if USE_AD1 adc1_buffers[adc_channel] = s; #endif - } - else if (adc_channel < (nb_adc1_channels+nb_adc2_channels)) { + } else if (adc_channel < (nb_adc1_channels + nb_adc2_channels)) { #if USE_AD2 - adc2_buffers[adc_channel-nb_adc1_channels] = s; + adc2_buffers[adc_channel - nb_adc1_channels] = s; #endif - } - else if (adc_channel < (nb_adc1_channels+nb_adc2_channels+nb_adc3_channels)) { + } else if (adc_channel < (nb_adc1_channels + nb_adc2_channels + nb_adc3_channels)) { #if USE_AD3 - adc3_buffers[adc_channel-(nb_adc1_channels+nb_adc2_channels)] = s; + adc3_buffers[adc_channel - (nb_adc1_channels + nb_adc2_channels)] = s; #endif } @@ -390,7 +389,8 @@ void adc_buf_channel(uint8_t adc_channel, struct adc_buf * s, uint8_t av_nb_samp } #if USE_ADC_WATCHDOG -void register_adc_watchdog(uint32_t adc, uint8_t chan, uint16_t low, uint16_t high, adc_watchdog_callback cb) { +void register_adc_watchdog(uint32_t adc, uint8_t chan, uint16_t low, uint16_t high, adc_watchdog_callback cb) +{ adc_watchdog.adc = adc; adc_watchdog.cb = cb; @@ -419,7 +419,7 @@ void register_adc_watchdog(uint32_t adc, uint8_t chan, uint16_t low, uint16_t hi #endif /** Configure and enable RCC for peripherals (ADC1, ADC2, Timer) */ -static inline void adc_init_rcc( void ) +static inline void adc_init_rcc(void) { #if USE_AD1 || USE_AD2 || USE_AD3 /* Timer peripheral clock enable. */ @@ -456,7 +456,7 @@ static inline void adc_init_rcc( void ) } /** Configure and enable ADC interrupt */ -static inline void adc_init_irq( void ) +static inline void adc_init_irq(void) { #if defined(STM32F1) nvic_set_priority(NVIC_ADC1_2_IRQ, NVIC_ADC_IRQ_PRIO); @@ -468,7 +468,7 @@ static inline void adc_init_irq( void ) } -static inline void adc_init_single(uint32_t adc, uint8_t nb_channels, uint8_t* channel_map) +static inline void adc_init_single(uint32_t adc, uint8_t nb_channels, uint8_t *channel_map) { // Paranoia, must be down for 2+ ADC clock cycles before calibration adc_off(adc); @@ -559,7 +559,8 @@ static inline void adc_init_single(uint32_t adc, uint8_t nb_channels, uint8_t* c } // adc_init_single -static inline void adc_push_sample(struct adc_buf * buf, uint16_t value) { +static inline void adc_push_sample(struct adc_buf *buf, uint16_t value) +{ uint8_t new_head = buf->head + 1; if (new_head >= buf->av_nb_sample) { @@ -583,7 +584,7 @@ void adc_isr(void) { uint8_t channel = 0; uint16_t value = 0; - struct adc_buf * buf; + struct adc_buf *buf; #if USE_ADC_WATCHDOG /* @@ -593,8 +594,9 @@ void adc_isr(void) */ const uint32_t timeStampDiff = get_sys_time_msec() - adc_watchdog.timeStamp; const bool_t shouldAccumulateValue = timeStampDiff > 20; - if (shouldAccumulateValue) + if (shouldAccumulateValue) { adc_watchdog.timeStamp = get_sys_time_msec(); + } if (adc_watchdog.cb != NULL) { if (adc_awd(adc_watchdog.adc)) { @@ -606,58 +608,58 @@ void adc_isr(void) #if USE_AD1 // Clear Injected End Of Conversion - if (adc_eoc_injected(ADC1)){ + if (adc_eoc_injected(ADC1)) { ADC_SR(ADC1) &= ~ADC_SR_JEOC; #if USE_ADC_WATCHDOG if (shouldAccumulateValue) { #endif - for (channel = 0; channel < nb_adc1_channels; channel++) { - buf = adc1_buffers[channel]; - if (buf) { - value = adc_read_injected(ADC1, channel+1); - adc_push_sample(buf, value); + for (channel = 0; channel < nb_adc1_channels; channel++) { + buf = adc1_buffers[channel]; + if (buf) { + value = adc_read_injected(ADC1, channel + 1); + adc_push_sample(buf, value); + } } - } #if USE_ADC_WATCHDOG - } + } #endif } #endif // USE_AD1 #if USE_AD2 - if (adc_eoc_injected(ADC2)){ + if (adc_eoc_injected(ADC2)) { ADC_SR(ADC2) &= ~ADC_SR_JEOC; #if USE_ADC_WATCHDOG if (shouldAccumulateValue) { #endif - for (channel = 0; channel < nb_adc2_channels; channel++) { - buf = adc2_buffers[channel]; - if (buf) { - value = adc_read_injected(ADC2, channel+1); - adc_push_sample(buf, value); + for (channel = 0; channel < nb_adc2_channels; channel++) { + buf = adc2_buffers[channel]; + if (buf) { + value = adc_read_injected(ADC2, channel + 1); + adc_push_sample(buf, value); + } } - } #if USE_ADC_WATCHDOG - } + } #endif } #endif // USE_AD2 #if USE_AD3 - if (adc_eoc_injected(ADC3)){ + if (adc_eoc_injected(ADC3)) { ADC_SR(ADC3) &= ~ADC_SR_JEOC; #if USE_ADC_WATCHDOG if (shouldAccumulateValue) { #endif - for (channel = 0; channel < nb_adc3_channels; channel++) { - buf = adc3_buffers[channel]; - if (buf) { - value = adc_read_injected(ADC3, channel+1); - adc_push_sample(buf, value); + for (channel = 0; channel < nb_adc3_channels; channel++) { + buf = adc3_buffers[channel]; + if (buf) { + value = adc_read_injected(ADC3, channel + 1); + adc_push_sample(buf, value); + } } - } #if USE_ADC_WATCHDOG - } + } #endif } #endif // USE_AD3 diff --git a/sw/airborne/arch/stm32/mcu_periph/can_arch.c b/sw/airborne/arch/stm32/mcu_periph/can_arch.c index e3b0e81e9f..e14cb13cf3 100644 --- a/sw/airborne/arch/stm32/mcu_periph/can_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/can_arch.c @@ -119,8 +119,7 @@ void can_hw_init(void) CAN_BTR_TS2_7TQ, 2, /* BRP+1: Baud rate prescaler */ false, /* loopback mode */ - false)) /* silent mode */ - { + false)) { /* silent mode */ /* TODO we need something somewhere where we can leave a note * that CAN was unable to initialize. Just like any other * driver should... @@ -153,7 +152,7 @@ int can_hw_transmit(uint32_t id, const uint8_t *buf, uint8_t len) return -2; } - if(len > 8){ + if (len > 8) { return -1; } diff --git a/sw/airborne/arch/stm32/mcu_periph/debug_led.h b/sw/airborne/arch/stm32/mcu_periph/debug_led.h index 62f060a64d..73dc3c6ef0 100644 --- a/sw/airborne/arch/stm32/mcu_periph/debug_led.h +++ b/sw/airborne/arch/stm32/mcu_periph/debug_led.h @@ -2,28 +2,28 @@ static inline void LED1_ON(void) { - GPIO_WriteBit(GPIOB, GPIO_Pin_6 , Bit_SET ); + GPIO_WriteBit(GPIOB, GPIO_Pin_6 , Bit_SET); } static inline void LED1_OFF(void) { - GPIO_WriteBit(GPIOB, GPIO_Pin_6 , !Bit_SET ); + GPIO_WriteBit(GPIOB, GPIO_Pin_6 , !Bit_SET); } static inline void LED2_ON(void) { - GPIO_WriteBit(GPIOB, GPIO_Pin_7 , Bit_SET ); + GPIO_WriteBit(GPIOB, GPIO_Pin_7 , Bit_SET); } static inline void LED2_OFF(void) { - GPIO_WriteBit(GPIOB, GPIO_Pin_7 , !Bit_SET ); + GPIO_WriteBit(GPIOB, GPIO_Pin_7 , !Bit_SET); } static inline void LED_INIT(void) { GPIO_InitTypeDef GPIO_InitStructure; - RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOB, ENABLE); + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7 | GPIO_Pin_6; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; @@ -37,8 +37,7 @@ static inline void LED_INIT(void) static inline void LED_ERROR(uint8_t base, uint8_t nr) { LED2_ON(); - for (int i=0;i<(base+nr);i++) - { + for (int i = 0; i < (base + nr); i++) { LED1_ON(); LED1_OFF(); } @@ -55,38 +54,43 @@ static inline void LED_SHOW_ACTIVE_BITS(I2C_TypeDef *regs) LED1_ON(); // 1 Start - if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_SB, SR1 ) ) + if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_SB, SR1)) { LED2_ON(); - else + } else { LED2_OFF(); + } LED2_OFF(); // 2 Addr - if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_ADDR, SR1 ) ) + if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_ADDR, SR1)) { LED2_ON(); - else + } else { LED2_OFF(); + } LED2_OFF(); // 3 BTF - if (BIT_X_IS_SET_IN_REG( I2C_SR1_BIT_BTF, SR1 ) ) + if (BIT_X_IS_SET_IN_REG(I2C_SR1_BIT_BTF, SR1)) { LED2_ON(); - else + } else { LED2_OFF(); + } LED2_OFF(); // 4 ERROR - if (( SR1 & I2C_SR1_BITS_ERR ) != 0x0000) + if ((SR1 & I2C_SR1_BITS_ERR) != 0x0000) { LED2_ON(); - else + } else { LED2_OFF(); + } LED2_OFF(); // Anything? - if (( SR1 + SR2) != 0x0000) + if ((SR1 + SR2) != 0x0000) { LED2_ON(); - else + } else { LED2_OFF(); + } LED2_OFF(); LED1_OFF(); @@ -95,38 +99,43 @@ static inline void LED_SHOW_ACTIVE_BITS(I2C_TypeDef *regs) LED1_ON(); // 1 Start - if (BIT_X_IS_SET_IN_REG( I2C_CR1_BIT_START, CR1 ) ) + if (BIT_X_IS_SET_IN_REG(I2C_CR1_BIT_START, CR1)) { LED2_ON(); - else + } else { LED2_OFF(); + } LED2_OFF(); // 2 Stop - if (BIT_X_IS_SET_IN_REG( I2C_CR1_BIT_STOP, CR1 ) ) + if (BIT_X_IS_SET_IN_REG(I2C_CR1_BIT_STOP, CR1)) { LED2_ON(); - else + } else { LED2_OFF(); + } LED2_OFF(); // 3 Busy - if (BIT_X_IS_SET_IN_REG( I2C_SR2_BIT_BUSY, SR2 ) ) + if (BIT_X_IS_SET_IN_REG(I2C_SR2_BIT_BUSY, SR2)) { LED2_ON(); - else + } else { LED2_OFF(); + } LED2_OFF(); // 4 Tra - if (BIT_X_IS_SET_IN_REG( I2C_SR2_BIT_TRA, SR2 ) ) + if (BIT_X_IS_SET_IN_REG(I2C_SR2_BIT_TRA, SR2)) { LED2_ON(); - else + } else { LED2_OFF(); + } LED2_OFF(); // 5 Master - if (BIT_X_IS_SET_IN_REG( I2C_SR2_BIT_MSL, SR2 ) ) + if (BIT_X_IS_SET_IN_REG(I2C_SR2_BIT_MSL, SR2)) { LED2_ON(); - else + } else { LED2_OFF(); + } LED2_OFF(); LED1_OFF(); @@ -137,24 +146,27 @@ static inline void LED_SHOW_ACTIVE_BITS(I2C_TypeDef *regs) LED1_ON(); // 1 Anything CR? - if (( CR1) != 0x0000) + if ((CR1) != 0x0000) { LED2_ON(); - else + } else { LED2_OFF(); + } LED2_OFF(); // 2 PE - if (BIT_X_IS_SET_IN_REG( I2C_CR1_BIT_PE, CR1 ) ) + if (BIT_X_IS_SET_IN_REG(I2C_CR1_BIT_PE, CR1)) { LED2_ON(); - else + } else { LED2_OFF(); + } LED2_OFF(); // 3 SWRESET - if (BIT_X_IS_SET_IN_REG( I2C_CR1_BIT_SWRST, CR1 ) ) + if (BIT_X_IS_SET_IN_REG(I2C_CR1_BIT_SWRST, CR1)) { LED2_ON(); - else + } else { LED2_OFF(); + } LED2_OFF(); LED1_OFF(); diff --git a/sw/airborne/arch/stm32/mcu_periph/gpio_arch.c b/sw/airborne/arch/stm32/mcu_periph/gpio_arch.c index 9f9b16f63b..73ec1be88b 100644 --- a/sw/airborne/arch/stm32/mcu_periph/gpio_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/gpio_arch.c @@ -31,7 +31,8 @@ #include #include -void gpio_enable_clock(uint32_t port) { +void gpio_enable_clock(uint32_t port) +{ switch (port) { case GPIOA: rcc_periph_clock_enable(RCC_GPIOA); @@ -76,75 +77,88 @@ void gpio_enable_clock(uint32_t port) { } #ifdef STM32F1 -void gpio_setup_output(uint32_t port, uint16_t gpios) { +void gpio_setup_output(uint32_t port, uint16_t gpios) +{ gpio_enable_clock(port); gpio_set_mode(port, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_PUSHPULL, gpios); } -void gpio_setup_input(uint32_t port, uint16_t gpios) { +void gpio_setup_input(uint32_t port, uint16_t gpios) +{ gpio_enable_clock(port); gpio_set_mode(port, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, gpios); } -void gpio_setup_input_pullup(uint32_t port, uint16_t gpios) { +void gpio_setup_input_pullup(uint32_t port, uint16_t gpios) +{ gpio_enable_clock(port); gpio_set(port, gpios); gpio_set_mode(port, GPIO_MODE_INPUT, GPIO_CNF_INPUT_PULL_UPDOWN, gpios); } -void gpio_setup_input_pulldown(uint32_t port, uint16_t gpios) { +void gpio_setup_input_pulldown(uint32_t port, uint16_t gpios) +{ gpio_enable_clock(port); gpio_clear(port, gpios); gpio_set_mode(port, GPIO_MODE_INPUT, GPIO_CNF_INPUT_PULL_UPDOWN, gpios); } -void gpio_setup_pin_af(uint32_t port, uint16_t pin, uint32_t af, bool_t is_output) { +void gpio_setup_pin_af(uint32_t port, uint16_t pin, uint32_t af, bool_t is_output) +{ gpio_enable_clock(port); /* remap alternate function if needed */ if (af) { rcc_periph_clock_enable(RCC_AFIO); AFIO_MAPR |= af; } - if (is_output) + if (is_output) { gpio_set_mode(port, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, pin); - else + } else { gpio_set_mode(port, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, pin); + } } -void gpio_setup_pin_analog(uint32_t port, uint16_t pin) { +void gpio_setup_pin_analog(uint32_t port, uint16_t pin) +{ gpio_enable_clock(port); gpio_set_mode(port, GPIO_MODE_INPUT, GPIO_CNF_INPUT_ANALOG, pin); } #elif defined STM32F4 -void gpio_setup_output(uint32_t port, uint16_t gpios) { +void gpio_setup_output(uint32_t port, uint16_t gpios) +{ gpio_enable_clock(port); gpio_mode_setup(port, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, gpios); } -void gpio_setup_input(uint32_t port, uint16_t gpios) { +void gpio_setup_input(uint32_t port, uint16_t gpios) +{ gpio_enable_clock(port); gpio_mode_setup(port, GPIO_MODE_INPUT, GPIO_PUPD_NONE, gpios); } -void gpio_setup_input_pullup(uint32_t port, uint16_t gpios) { +void gpio_setup_input_pullup(uint32_t port, uint16_t gpios) +{ gpio_enable_clock(port); gpio_mode_setup(port, GPIO_MODE_INPUT, GPIO_PUPD_PULLUP, gpios); } -void gpio_setup_input_pulldown(uint32_t port, uint16_t gpios) { +void gpio_setup_input_pulldown(uint32_t port, uint16_t gpios) +{ gpio_enable_clock(port); gpio_mode_setup(port, GPIO_MODE_INPUT, GPIO_PUPD_PULLDOWN, gpios); } -void gpio_setup_pin_af(uint32_t port, uint16_t pin, uint8_t af, bool_t is_output __attribute__ ((unused))) { +void gpio_setup_pin_af(uint32_t port, uint16_t pin, uint8_t af, bool_t is_output __attribute__((unused))) +{ gpio_enable_clock(port); gpio_mode_setup(port, GPIO_MODE_AF, GPIO_PUPD_NONE, pin); gpio_set_af(port, af, pin); } -void gpio_setup_pin_analog(uint32_t port, uint16_t pin) { +void gpio_setup_pin_analog(uint32_t port, uint16_t pin) +{ gpio_enable_clock(port); gpio_mode_setup(port, GPIO_MODE_ANALOG, GPIO_PUPD_NONE, pin); } diff --git a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c index cec1349723..d7a5176ac8 100644 --- a/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/i2c_arch.c @@ -58,17 +58,17 @@ // Bit Control -#define BIT_X_IS_SET_IN_REG(X,REG) (((REG) & (X)) == (X)) +#define BIT_X_IS_SET_IN_REG(X,REG) (((REG) & (X)) == (X)) // disable and enable irq functions are not implemented in libopencm3 defining them here // XXX: consider moving this definitions into libopencm3 -static inline void __disable_irq(void) { asm volatile ("cpsid i"); } -static inline void __enable_irq(void) { asm volatile ("cpsie i"); } +static inline void __disable_irq(void) { asm volatile("cpsid i"); } +static inline void __enable_irq(void) { asm volatile("cpsie i"); } // Critical Zones -#define __I2C_REG_CRITICAL_ZONE_START __disable_irq(); -#define __I2C_REG_CRITICAL_ZONE_STOP __enable_irq(); +#define __I2C_REG_CRITICAL_ZONE_START __disable_irq(); +#define __I2C_REG_CRITICAL_ZONE_STOP __enable_irq(); #ifndef NVIC_I2C_IRQ_PRIO @@ -83,7 +83,8 @@ static inline void __enable_irq(void) { asm volatile ("cpsie i"); } #if USE_I2C1 || USE_I2C2 || USE_I2C3 #if defined(STM32F1) -static void i2c_setup_gpio(uint32_t i2c) { +static void i2c_setup_gpio(uint32_t i2c) +{ switch (i2c) { #if USE_I2C1 case I2C1: @@ -108,7 +109,8 @@ static void i2c_setup_gpio(uint32_t i2c) { #elif defined(STM32F4) -static void i2c_setup_gpio(uint32_t i2c) { +static void i2c_setup_gpio(uint32_t i2c) +{ switch (i2c) { #if USE_I2C1 case I2C1: @@ -208,8 +210,8 @@ static inline void PPRZ_I2C_SEND_START(struct i2c_periph *periph) /////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////// // -// SUBTRANSACTION SEQUENCES -// -We arrive here every time a ISR is called with no error +// SUBTRANSACTION SEQUENCES +// -We arrive here every time a ISR is called with no error enum STMI2CSubTransactionStatus { STMI2C_SubTra_Busy, @@ -220,13 +222,13 @@ enum STMI2CSubTransactionStatus { // Doc ID 13902 Rev 11 p 710/1072 // Transfer Sequence Diagram for Master Transmitter -static inline enum STMI2CSubTransactionStatus stmi2c_send(uint32_t i2c, struct i2c_periph *periph, struct i2c_transaction *trans) +static inline enum STMI2CSubTransactionStatus stmi2c_send(uint32_t i2c, struct i2c_periph *periph, + struct i2c_transaction *trans) { uint16_t SR1 = I2C_SR1(i2c); // Start Condition Was Just Generated - if (BIT_X_IS_SET_IN_REG( I2C_SR1_SB, SR1 ) ) - { + if (BIT_X_IS_SET_IN_REG(I2C_SR1_SB, SR1)) { // Disable buffer interrupt i2c_disable_interrupt(i2c, I2C_CR2_ITBUFEN); // Send Slave address and wait for ADDR interrupt @@ -235,10 +237,9 @@ static inline enum STMI2CSubTransactionStatus stmi2c_send(uint32_t i2c, struct i periph->status = I2CAddrWrSent; } // Address Was Sent - else if (BIT_X_IS_SET_IN_REG(I2C_SR1_ADDR, SR1) ) - { + else if (BIT_X_IS_SET_IN_REG(I2C_SR1_ADDR, SR1)) { // Now read SR2 to clear the ADDR status Bit - uint16_t SR2 __attribute__ ((unused)) = I2C_SR2(i2c); + uint16_t SR2 __attribute__((unused)) = I2C_SR2(i2c); // Maybe check we are transmitting (did not loose arbitration for instance) // if (! BIT_X_IS_SET_IN_REG(I2C_SR2_TRA, SR2)) { } @@ -246,53 +247,45 @@ static inline enum STMI2CSubTransactionStatus stmi2c_send(uint32_t i2c, struct i // Send First max 2 bytes i2c_send_data(i2c, trans->buf[0]); - if (trans->len_w > 1) - { + if (trans->len_w > 1) { i2c_send_data(i2c, trans->buf[1]); periph->idx_buf = 2; - } - else - { + } else { periph->idx_buf = 1; } // Enable buffer-space available interrupt // only if there is more to send: wait for TXE, no more to send: wait for BTF - if ( periph->idx_buf < trans->len_w) + if (periph->idx_buf < trans->len_w) { i2c_enable_interrupt(i2c, I2C_CR2_ITBUFEN); + } // Document the current Status periph->status = I2CSendingByte; } // The buffer is not full anymore AND we were not waiting for BTF - else if ((BIT_X_IS_SET_IN_REG(I2C_SR1_TxE, SR1) ) && (BIT_X_IS_SET_IN_REG(I2C_CR2_ITBUFEN, I2C_CR2(i2c))) ) - { + else if ((BIT_X_IS_SET_IN_REG(I2C_SR1_TxE, SR1)) && (BIT_X_IS_SET_IN_REG(I2C_CR2_ITBUFEN, I2C_CR2(i2c)))) { // Send the next byte i2c_send_data(i2c, trans->buf[periph->idx_buf]); periph->idx_buf++; // All bytes Sent? Then wait for BTF instead - if ( periph->idx_buf >= trans->len_w) - { + if (periph->idx_buf >= trans->len_w) { // Not interested anymore to know the buffer has space left i2c_disable_interrupt(i2c, I2C_CR2_ITBUFEN); // Next interrupt will be BTF (or error) } } // BTF: means last byte was sent - else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BTF, SR1) ) - { - if (trans->type == I2CTransTx) - { + else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BTF, SR1)) { + if (trans->type == I2CTransTx) { // Tell the driver we are ready trans->status = I2CTransSuccess; } // Otherwise we still need to do the receiving part return STMI2C_SubTra_Ready; - } - else // Event Logic Error - { + } else { // Event Logic Error return STMI2C_SubTra_Error; } @@ -301,13 +294,13 @@ static inline enum STMI2CSubTransactionStatus stmi2c_send(uint32_t i2c, struct i // Doc ID 13902 Rev 11 p 714/1072 // Transfer Sequence Diagram for Master Receiver for N=1 -static inline enum STMI2CSubTransactionStatus stmi2c_read1(uint32_t i2c, struct i2c_periph *periph, struct i2c_transaction *trans) +static inline enum STMI2CSubTransactionStatus stmi2c_read1(uint32_t i2c, struct i2c_periph *periph, + struct i2c_transaction *trans) { uint16_t SR1 = I2C_SR1(i2c); // Start Condition Was Just Generated - if (BIT_X_IS_SET_IN_REG( I2C_SR1_SB, SR1 ) ) - { + if (BIT_X_IS_SET_IN_REG(I2C_SR1_SB, SR1)) { i2c_disable_interrupt(i2c, I2C_CR2_ITBUFEN); i2c_send_data(i2c, trans->slave_addr | 0x01); @@ -315,8 +308,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read1(uint32_t i2c, struct periph->status = I2CAddrRdSent; } // Address Was Sent - else if (BIT_X_IS_SET_IN_REG(I2C_SR1_ADDR, SR1) ) - { + else if (BIT_X_IS_SET_IN_REG(I2C_SR1_ADDR, SR1)) { // First Clear the ACK bit: after the next byte we do not want new bytes i2c_nack_current(i2c); i2c_disable_ack(i2c); @@ -325,7 +317,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read1(uint32_t i2c, struct __I2C_REG_CRITICAL_ZONE_START; // Only after setting ACK, read SR2 to clear the ADDR (next byte will start arriving) - uint16_t SR2 __attribute__ ((unused)) = I2C_SR2(i2c); + uint16_t SR2 __attribute__((unused)) = I2C_SR2(i2c); // Schedule a Stop PPRZ_I2C_SEND_STOP(i2c); @@ -340,8 +332,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read1(uint32_t i2c, struct periph->status = I2CReadingLastByte; } // As soon as there is 1 byte ready to read, we have our byte - else if (BIT_X_IS_SET_IN_REG(I2C_SR1_RxNE, SR1) ) - { + else if (BIT_X_IS_SET_IN_REG(I2C_SR1_RxNE, SR1)) { i2c_disable_interrupt(i2c, I2C_CR2_ITBUFEN); trans->buf[0] = I2C_DR(i2c); @@ -353,9 +344,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read1(uint32_t i2c, struct periph->status = I2CStopRequested; return STMI2C_SubTra_Ready_StopRequested; - } - else // Event Logic Error - { + } else { // Event Logic Error return STMI2C_SubTra_Error; } @@ -364,13 +353,13 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read1(uint32_t i2c, struct // Doc ID 13902 Rev 11 p 713/1072 // Transfer Sequence Diagram for Master Receiver for N=2 -static inline enum STMI2CSubTransactionStatus stmi2c_read2(uint32_t i2c, struct i2c_periph *periph, struct i2c_transaction *trans) +static inline enum STMI2CSubTransactionStatus stmi2c_read2(uint32_t i2c, struct i2c_periph *periph, + struct i2c_transaction *trans) { uint16_t SR1 = I2C_SR1(i2c); // Start Condition Was Just Generated - if (BIT_X_IS_SET_IN_REG( I2C_SR1_SB, SR1 ) ) - { + if (BIT_X_IS_SET_IN_REG(I2C_SR1_SB, SR1)) { // according to the datasheet: instantly shedule a NAK on the second received byte: i2c_disable_interrupt(i2c, I2C_CR2_ITBUFEN); i2c_enable_ack(i2c); @@ -381,8 +370,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read2(uint32_t i2c, struct periph->status = I2CAddrRdSent; } // Address Was Sent - else if (BIT_X_IS_SET_IN_REG(I2C_SR1_ADDR, SR1) ) - { + else if (BIT_X_IS_SET_IN_REG(I2C_SR1_ADDR, SR1)) { // --- make absolutely sure this command is not delayed too much after the previous: // --- the NAK bits must be set before the first byte arrived: allow other interrupts here __I2C_REG_CRITICAL_ZONE_START; @@ -390,7 +378,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read2(uint32_t i2c, struct // if transfer of DR was finished already then we will get too many bytes // BEFORE clearing ACK, read SR2 to clear the ADDR (next byte will start arriving) // clearing ACK after the byte transfer has already started will NACK the next (2nd) - uint16_t SR2 __attribute__ ((unused)) = I2C_SR2(i2c); + uint16_t SR2 __attribute__((unused)) = I2C_SR2(i2c); // NOT First Clear the ACK bit but only AFTER clearing ADDR i2c_disable_ack(i2c); @@ -407,8 +395,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read2(uint32_t i2c, struct periph->status = I2CReadingByte; } // Receive buffer if full, master is halted: BTF - else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BTF, SR1) ) - { + else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BTF, SR1)) { // Stop condition MUST be set BEFORE reading the DR // otherwise since there is new buffer space a new byte will be read PPRZ_I2C_SEND_STOP(i2c); @@ -423,9 +410,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read2(uint32_t i2c, struct trans->status = I2CTransSuccess; return STMI2C_SubTra_Ready_StopRequested; - } - else // Event Logic Error - { + } else { // Event Logic Error return STMI2C_SubTra_Error; } @@ -434,13 +419,13 @@ static inline enum STMI2CSubTransactionStatus stmi2c_read2(uint32_t i2c, struct // Doc ID 13902 Rev 11 p 712/1072 // Transfer Sequence Diagram for Master Receiver for N>2 -static inline enum STMI2CSubTransactionStatus stmi2c_readmany(uint32_t i2c, struct i2c_periph *periph, struct i2c_transaction *trans) +static inline enum STMI2CSubTransactionStatus stmi2c_readmany(uint32_t i2c, struct i2c_periph *periph, + struct i2c_transaction *trans) { uint16_t SR1 = I2C_SR1(i2c); // Start Condition Was Just Generated - if (BIT_X_IS_SET_IN_REG( I2C_SR1_SB, SR1 ) ) - { + if (BIT_X_IS_SET_IN_REG(I2C_SR1_SB, SR1)) { i2c_disable_interrupt(i2c, I2C_CR2_ITBUFEN); // The first data byte will be acked in read many so the slave knows it should send more i2c_nack_current(i2c); @@ -452,38 +437,33 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(uint32_t i2c, stru periph->status = I2CAddrRdSent; } // Address Was Sent - else if (BIT_X_IS_SET_IN_REG(I2C_SR1_ADDR, SR1) ) - { + else if (BIT_X_IS_SET_IN_REG(I2C_SR1_ADDR, SR1)) { periph->idx_buf = 0; // Enable RXNE: receive an interrupt any time a byte is available // only enable if MORE than 3 bytes need to be read - if (periph->idx_buf < (trans->len_r - 3)) - { + if (periph->idx_buf < (trans->len_r - 3)) { i2c_enable_interrupt(i2c, I2C_CR2_ITBUFEN); } // ACK is still on to get more DATA // Read SR2 to clear the ADDR (next byte will start arriving) - uint16_t SR2 __attribute__ ((unused)) = I2C_SR2(i2c); + uint16_t SR2 __attribute__((unused)) = I2C_SR2(i2c); // Document the current Status periph->status = I2CReadingByte; } // one or more bytes are available AND we were interested in Buffer interrupts - else if ( (BIT_X_IS_SET_IN_REG(I2C_SR1_RxNE, SR1) ) && (BIT_X_IS_SET_IN_REG(I2C_CR2_ITBUFEN, I2C_CR2(i2c))) ) - { + else if ((BIT_X_IS_SET_IN_REG(I2C_SR1_RxNE, SR1)) && (BIT_X_IS_SET_IN_REG(I2C_CR2_ITBUFEN, I2C_CR2(i2c)))) { // read byte until 3 bytes remain to be read (e.g. len_r = 6, -> idx=3 means idx 3,4,5 = 3 remain to be read - if (periph->idx_buf < (trans->len_r - 3)) - { + if (periph->idx_buf < (trans->len_r - 3)) { trans->buf[periph->idx_buf] = I2C_DR(i2c); periph->idx_buf ++; } // from : 3bytes -> last byte: do nothing // // finally: this was the last byte - else if (periph->idx_buf >= (trans->len_r - 1)) - { + else if (periph->idx_buf >= (trans->len_r - 1)) { i2c_disable_interrupt(i2c, I2C_CR2_ITBUFEN); // Last Value @@ -497,12 +477,9 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(uint32_t i2c, stru } // Check for end of transaction: start waiting for BTF instead of RXNE - if (periph->idx_buf < (trans->len_r - 3)) - { + if (periph->idx_buf < (trans->len_r - 3)) { i2c_enable_interrupt(i2c, I2C_CR2_ITBUFEN); - } - else // idx >= len-3: there are 3 bytes to be read - { + } else { // idx >= len-3: there are 3 bytes to be read // We want to halt I2C to have sufficient time to clear ACK, so: // Stop listening to RXNE as it will be triggered infinitely since we did not empty the buffer // on the next (second in buffer) received byte BTF will be set (buffer full and I2C halted) @@ -510,8 +487,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(uint32_t i2c, stru } } // Buffer is full while this was not a RXNE interrupt - else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BTF, SR1) ) - { + else if (BIT_X_IS_SET_IN_REG(I2C_SR1_BTF, SR1)) { // Now the shift register and data register contain data(n-2) and data(n-1) // And I2C is halted so we have time @@ -543,9 +519,7 @@ static inline enum STMI2CSubTransactionStatus stmi2c_readmany(uint32_t i2c, stru // Ask for an interrupt to read the last byte (which is normally still busy now) // The last byte will be received with RXNE i2c_enable_interrupt(i2c, I2C_CR2_ITBUFEN); - } - else // Event Logic Error - { + } else { // Event Logic Error return STMI2C_SubTra_Error; } @@ -625,7 +599,7 @@ static inline void stmi2c_clear_pending_interrupts(uint32_t i2c) // Certainly do not wait for buffer interrupts: // ------------------------------------------- - i2c_disable_interrupt(i2c, I2C_CR2_ITBUFEN); // Disable TXE, RXNE + i2c_disable_interrupt(i2c, I2C_CR2_ITBUFEN); // Disable TXE, RXNE // Error interrupts are handled separately: // --------------------------------------- @@ -634,22 +608,19 @@ static inline void stmi2c_clear_pending_interrupts(uint32_t i2c) // -------------------------------- // Start Condition Was Generated - if (BIT_X_IS_SET_IN_REG( I2C_SR1_SB, SR1 ) ) - { + if (BIT_X_IS_SET_IN_REG(I2C_SR1_SB, SR1)) { // SB: cleared by software when reading SR1 and writing to DR i2c_send_data(i2c, 0x00); } // Address Was Sent - if (BIT_X_IS_SET_IN_REG(I2C_SR1_ADDR, SR1) ) - { + if (BIT_X_IS_SET_IN_REG(I2C_SR1_ADDR, SR1)) { // ADDR: Cleared by software when reading SR1 and then SR2 - uint16_t SR2 __attribute__ ((unused)) = I2C_SR2(i2c); + uint16_t SR2 __attribute__((unused)) = I2C_SR2(i2c); } // Byte Transfer Finished - if (BIT_X_IS_SET_IN_REG(I2C_SR1_BTF, SR1) ) - { + if (BIT_X_IS_SET_IN_REG(I2C_SR1_BTF, SR1)) { // SB: cleared by software when reading SR1 and reading/writing to DR - uint8_t dummy __attribute__ ((unused)) = i2c_get_data(i2c); + uint8_t dummy __attribute__((unused)) = i2c_get_data(i2c); i2c_send_data(i2c, 0x00); } @@ -670,11 +641,11 @@ static inline void i2c_irq(struct i2c_periph *periph) We are always interested in all IT_EV_FEV: all are required. - 1) SB // Start Condition Success in Master mode - 2) ADDR // Address sent received Acknowledge - [ADDR10] // -- 10bit address stuff: not used - [STOPF] // -- only for slaves: master has no stop interrupt: not used - 3) BTF // I2C has stopped working (it is waiting for new data, all buffers are tx_empty/rx_full) + 1) SB // Start Condition Success in Master mode + 2) ADDR // Address sent received Acknowledge + [ADDR10] // -- 10bit address stuff: not used + [STOPF] // -- only for slaves: master has no stop interrupt: not used + 3) BTF // I2C has stopped working (it is waiting for new data, all buffers are tx_empty/rx_full) // Beware: using the buffered I2C has some interesting properties: - in master receive mode: BTF only occurs after the 2nd received byte: after the first byte is received it is @@ -740,8 +711,7 @@ static inline void i2c_irq(struct i2c_periph *periph) ///////////////////////////// // Check if we were ready ... - if (periph->trans_extract_idx == periph->trans_insert_idx) - { + if (periph->trans_extract_idx == periph->trans_insert_idx) { // Nothing Left To Do #ifdef I2C_DEBUG_LED @@ -780,12 +750,11 @@ static inline void i2c_irq(struct i2c_periph *periph) // get the I2C transaction we were working on ... enum STMI2CSubTransactionStatus ret = 0; - struct i2c_transaction* trans = periph->trans[periph->trans_extract_idx]; + struct i2c_transaction *trans = periph->trans[periph->trans_extract_idx]; /////////////////////////// // If there was an error: - if (( I2C_SR1(i2c) & I2C_SR1_ERR_MASK ) != 0x0000) - { + if ((I2C_SR1(i2c) & I2C_SR1_ERR_MASK) != 0x0000) { #ifdef I2C_DEBUG_LED LED1_ON(); @@ -820,50 +789,43 @@ static inline void i2c_irq(struct i2c_periph *periph) /////////////////////////// // Normal Event: - else - { + else { /////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////// // - // SUB-TRANSACTION HANDLER + // SUB-TRANSACTION HANDLER - if (trans->type == I2CTransRx) // TxRx are converted to Rx after the Tx Part - { - switch (trans->len_r) - { - case 1: - ret = stmi2c_read1(i2c,periph,trans); - break; - case 2: - ret = stmi2c_read2(i2c,periph,trans); - break; - default: - ret = stmi2c_readmany(i2c,periph,trans); - break; + if (trans->type == I2CTransRx) { // TxRx are converted to Rx after the Tx Part + switch (trans->len_r) { + case 1: + ret = stmi2c_read1(i2c, periph, trans); + break; + case 2: + ret = stmi2c_read2(i2c, periph, trans); + break; + default: + ret = stmi2c_readmany(i2c, periph, trans); + break; } - } - else // TxRx or Tx - { - ret = stmi2c_send(i2c,periph,trans); + } else { // TxRx or Tx + ret = stmi2c_send(i2c, periph, trans); } } ///////////////////////////////// // Sub-transaction has finished - if (ret != STMI2C_SubTra_Busy) - { + if (ret != STMI2C_SubTra_Busy) { // Ready or SubTraError // -ready: with or without stop already asked // In case of unexpected event condition during subtransaction handling: - if (ret == STMI2C_SubTra_Error) - { + if (ret == STMI2C_SubTra_Error) { // Tell everyone about the subtransaction error: // this is the previously called SPURRIOUS INTERRUPT periph->status = I2CFailed; - trans->type = I2CTransRx; // Avoid possible restart - trans->status = I2CTransFailed; // Notify Ready + trans->type = I2CTransRx; // Avoid possible restart + trans->status = I2CTransFailed; // Notify Ready periph->errors->unexpected_event_cnt++; // Error @@ -881,8 +843,7 @@ static inline void i2c_irq(struct i2c_periph *periph) } // RxTx -> Restart and do Rx part - if (trans->type == I2CTransTxRx) - { + if (trans->type == I2CTransTxRx) { trans->type = I2CTransRx; periph->status = I2CStartRequested; i2c_send_start(i2c); @@ -891,11 +852,9 @@ static inline void i2c_irq(struct i2c_periph *periph) i2c_send_data(i2c, 0x00); } // If a restart is not needed: Rx part or Tx-only - else - { + else { // Ready, no stop condition set yet - if (ret == STMI2C_SubTra_Ready) - { + if (ret == STMI2C_SubTra_Ready) { // Program a stop PPRZ_I2C_SEND_STOP(i2c); @@ -906,16 +865,16 @@ static inline void i2c_irq(struct i2c_periph *periph) // Jump to the next transaction periph->trans_extract_idx++; - if (periph->trans_extract_idx >= I2C_TRANSACTION_QUEUE_LEN) + if (periph->trans_extract_idx >= I2C_TRANSACTION_QUEUE_LEN) { periph->trans_extract_idx = 0; + } // Tell everyone we are ready periph->status = I2CIdle; // if we have no more transaction to process, stop here - if (periph->trans_extract_idx == periph->trans_insert_idx) - { + if (periph->trans_extract_idx == periph->trans_insert_idx) { periph->watchdog = -1; // stop watchdog #ifdef I2C_DEBUG_LED @@ -928,8 +887,7 @@ static inline void i2c_irq(struct i2c_periph *periph) #endif } // if not, start next transaction - else - { + else { // Restart transaction doing the Rx part now // --- moved to idle function PPRZ_I2C_SEND_START(periph); @@ -966,7 +924,8 @@ PRINT_CONFIG_VAR(I2C1_CLOCK_SPEED) struct i2c_errors i2c1_errors; -void i2c1_hw_init(void) { +void i2c1_hw_init(void) +{ i2c1.reg_addr = (void *)I2C1; i2c1.init_struct = NULL; @@ -989,7 +948,7 @@ void i2c1_hw_init(void) { nvic_enable_irq(NVIC_I2C1_EV_IRQ); /* Configure and enable I2C1 err interrupt ----------------------------------*/ - nvic_set_priority(NVIC_I2C1_ER_IRQ, NVIC_I2C1_IRQ_PRIO+1); + nvic_set_priority(NVIC_I2C1_ER_IRQ, NVIC_I2C1_IRQ_PRIO + 1); nvic_enable_irq(NVIC_I2C1_ER_IRQ); /* Enable peripheral clocks -------------------------------------------------*/ @@ -1012,7 +971,8 @@ void i2c1_hw_init(void) { #endif } -void i2c1_ev_isr(void) { +void i2c1_ev_isr(void) +{ uint32_t i2c = (uint32_t) i2c1.reg_addr; i2c_disable_interrupt(i2c, I2C_CR2_ITERREN); i2c1.watchdog = 0; // restart watchdog @@ -1020,7 +980,8 @@ void i2c1_ev_isr(void) { i2c_enable_interrupt(i2c, I2C_CR2_ITERREN); } -void i2c1_er_isr(void) { +void i2c1_er_isr(void) +{ uint32_t i2c = (uint32_t) i2c1.reg_addr; i2c_disable_interrupt(i2c, I2C_CR2_ITEVTEN); i2c1.watchdog = 0; // restart watchdog @@ -1040,7 +1001,8 @@ PRINT_CONFIG_VAR(I2C2_CLOCK_SPEED) struct i2c_errors i2c2_errors; -void i2c2_hw_init(void) { +void i2c2_hw_init(void) +{ i2c2.reg_addr = (void *)I2C2; i2c2.init_struct = NULL; @@ -1058,7 +1020,7 @@ void i2c2_hw_init(void) { nvic_enable_irq(NVIC_I2C2_EV_IRQ); /* Configure and enable I2C2 err interrupt ----------------------------------*/ - nvic_set_priority(NVIC_I2C2_ER_IRQ, NVIC_I2C2_IRQ_PRIO+1); + nvic_set_priority(NVIC_I2C2_ER_IRQ, NVIC_I2C2_IRQ_PRIO + 1); nvic_enable_irq(NVIC_I2C2_ER_IRQ); /* Enable peripheral clocks -------------------------------------------------*/ @@ -1081,7 +1043,8 @@ void i2c2_hw_init(void) { i2c_setbitrate(&i2c2, I2C2_CLOCK_SPEED); } -void i2c2_ev_isr(void) { +void i2c2_ev_isr(void) +{ uint32_t i2c = (uint32_t) i2c2.reg_addr; i2c_disable_interrupt(i2c, I2C_CR2_ITERREN); i2c2.watchdog = 0; // restart watchdog @@ -1089,7 +1052,8 @@ void i2c2_ev_isr(void) { i2c_enable_interrupt(i2c, I2C_CR2_ITERREN); } -void i2c2_er_isr(void) { +void i2c2_er_isr(void) +{ uint32_t i2c = (uint32_t) i2c2.reg_addr; i2c_disable_interrupt(i2c, I2C_CR2_ITEVTEN); i2c2.watchdog = 0; // restart watchdog @@ -1110,7 +1074,8 @@ PRINT_CONFIG_VAR(I2C3_CLOCK_SPEED) struct i2c_errors i2c3_errors; -void i2c3_hw_init(void) { +void i2c3_hw_init(void) +{ i2c3.reg_addr = (void *)I2C3; i2c3.init_struct = NULL; @@ -1128,7 +1093,7 @@ void i2c3_hw_init(void) { nvic_enable_irq(NVIC_I2C3_EV_IRQ); /* Configure and enable I2C3 err interrupt ----------------------------------*/ - nvic_set_priority(NVIC_I2C3_ER_IRQ, NVIC_I2C3_IRQ_PRIO+1); + nvic_set_priority(NVIC_I2C3_ER_IRQ, NVIC_I2C3_IRQ_PRIO + 1); nvic_enable_irq(NVIC_I2C3_ER_IRQ); /* Enable peripheral clocks -------------------------------------------------*/ @@ -1151,7 +1116,8 @@ void i2c3_hw_init(void) { i2c_setbitrate(&i2c3, I2C3_CLOCK_SPEED); } -void i2c3_ev_isr(void) { +void i2c3_ev_isr(void) +{ uint32_t i2c = (uint32_t) i2c3.reg_addr; i2c_disable_interrupt(i2c, I2C_CR2_ITERREN); i2c3.watchdog = 0; // restart watchdog @@ -1159,7 +1125,8 @@ void i2c3_ev_isr(void) { i2c_enable_interrupt(i2c, I2C_CR2_ITERREN); } -void i2c3_er_isr(void) { +void i2c3_er_isr(void) +{ uint32_t i2c = (uint32_t) i2c3.reg_addr; i2c_disable_interrupt(i2c, I2C_CR2_ITEVTEN); I2C_CR2(i2c) &= ~I2C_CR2_ITEVTEN; @@ -1178,8 +1145,7 @@ void i2c3_er_isr(void) { void i2c_setbitrate(struct i2c_periph *periph, int bitrate) { // If NOT Busy - if (i2c_idle(periph)) - { + if (i2c_idle(periph)) { volatile int devider; volatile int risetime; @@ -1202,31 +1168,36 @@ void i2c_setbitrate(struct i2c_periph *periph, int bitrate) // 3) Configure rise time register ******************************************************/ - if (bitrate < 3000) + if (bitrate < 3000) { bitrate = 3000; + } // rcc_ppre1_frequency is normally configured to max: 36MHz on F1 and 42MHz on F4 // in fast mode: 2counts low 1 count high -> / 3: // in standard mode: 1 count low, 1 count high -> /2: - devider = (rcc_ppre1_frequency/2000) / (bitrate/1000); + devider = (rcc_ppre1_frequency / 2000) / (bitrate / 1000); // never allow faster than 600kbps - if (devider < 20) + if (devider < 20) { devider = 20; + } // no overflow either - if (devider >=4095) + if (devider >= 4095) { devider = 4095; + } // risetime can be up to 1/6th of the period - risetime = 1000000 / (bitrate/1000) / 6 / 28; + risetime = 1000000 / (bitrate / 1000) / 6 / 28; - if (risetime < 10) + if (risetime < 10) { risetime = 10; + } // more will overflow the register: for more you should lower the FREQ - if (risetime >=31) + if (risetime >= 31) { risetime = 31; + } // we do not expect an interrupt as the interface should have been idle, but just in case... __disable_irq(); // this code is in user space: @@ -1271,40 +1242,49 @@ void i2c_setbitrate(struct i2c_periph *periph, int bitrate) } #if USE_I2C1 || USE_I2C2 || USE_I2C3 -static inline void i2c_scl_set(uint32_t i2c) { +static inline void i2c_scl_set(uint32_t i2c) +{ #if USE_I2C1 - if (i2c == I2C1) + if (i2c == I2C1) { gpio_set(I2C1_GPIO_PORT, I2C1_GPIO_SCL); + } #endif #if USE_I2C2 - if (i2c == I2C2) + if (i2c == I2C2) { gpio_set(I2C2_GPIO_PORT, I2C2_GPIO_SCL); + } #endif #if USE_I2C3 - if (i2c == I2C3) + if (i2c == I2C3) { gpio_set(I2C3_GPIO_PORT_SCL, I2C3_GPIO_SCL); + } #endif } -static inline void i2c_scl_clear(uint32_t i2c) { +static inline void i2c_scl_clear(uint32_t i2c) +{ #if USE_I2C1 - if (i2c == I2C1) + if (i2c == I2C1) { gpio_clear(I2C1_GPIO_PORT, I2C1_GPIO_SCL); + } #endif #if USE_I2C2 - if (i2c == I2C2) + if (i2c == I2C2) { gpio_clear(I2C2_GPIO_PORT, I2C2_GPIO_SCL); + } #endif #if USE_I2C3 - if (i2c == I2C3) + if (i2c == I2C3) { gpio_clear(I2C3_GPIO_PORT_SCL, I2C3_GPIO_SCL); + } #endif } #define WD_DELAY 20 // number of ticks with 2ms - 40ms delay before resetting the bus #define WD_RECOVERY_TICKS 10 // number of generated SCL clocking pulses -static void i2c_wd_check(struct i2c_periph *periph) { +static void i2c_wd_check(struct i2c_periph *periph) +{ uint32_t i2c = (uint32_t) periph->reg_addr; if (periph->watchdog > WD_DELAY) { @@ -1330,19 +1310,18 @@ static void i2c_wd_check(struct i2c_periph *periph) { #if USE_I2C3 if (i2c == I2C3) { gpio_setup_output(I2C3_GPIO_PORT_SCL, I2C3_GPIO_SCL); - gpio_setup_input(I2C3_GPIO_PORT_SDA,I2C3_GPIO_SDA); + gpio_setup_input(I2C3_GPIO_PORT_SDA, I2C3_GPIO_SDA); } #endif i2c_scl_clear(i2c); - } - else if (periph->watchdog < WD_DELAY + WD_RECOVERY_TICKS) { - if ((periph->watchdog - WD_DELAY) % 2) + } else if (periph->watchdog < WD_DELAY + WD_RECOVERY_TICKS) { + if ((periph->watchdog - WD_DELAY) % 2) { i2c_scl_clear(i2c); - else + } else { i2c_scl_set(i2c); - } - else { + } + } else { i2c_scl_set(i2c); /* setup gpios for normal i2c operation again */ @@ -1363,14 +1342,16 @@ static void i2c_wd_check(struct i2c_periph *periph) { return; } } - if (periph->watchdog >= 0) + if (periph->watchdog >= 0) { periph->watchdog++; + } } #endif // USE_I2Cx #include "mcu_periph/sys_time.h" -void i2c_event(void) { +void i2c_event(void) +{ static uint32_t i2c_wd_timer; if (SysTimeTimer(i2c_wd_timer) > 2000) { // 2ms (500Hz) periodic watchdog check @@ -1391,11 +1372,12 @@ void i2c_event(void) { ///////////////////////////////////////////////////////// // Implement Interface Functions -bool_t i2c_submit(struct i2c_periph* periph, struct i2c_transaction* t) { +bool_t i2c_submit(struct i2c_periph *periph, struct i2c_transaction *t) +{ uint8_t temp; temp = periph->trans_insert_idx + 1; - if (temp >= I2C_TRANSACTION_QUEUE_LEN) temp = 0; + if (temp >= I2C_TRANSACTION_QUEUE_LEN) { temp = 0; } if (temp == periph->trans_extract_idx) { // queue full periph->errors->queue_full_cnt++; @@ -1412,17 +1394,14 @@ bool_t i2c_submit(struct i2c_periph* periph, struct i2c_transaction* t) { /* if peripheral is idle, start the transaction */ // if (PPRZ_I2C_IS_IDLE(p)) - if (periph->status == I2CIdle) - { + if (periph->status == I2CIdle) { //if (i2c_idle(periph)) { #ifdef I2C_DEBUG_LED #if USE_I2C1 - if (periph == &i2c1) - { + if (periph == &i2c1) { - } - else + } else #endif #endif { @@ -1440,7 +1419,7 @@ bool_t i2c_submit(struct i2c_periph* periph, struct i2c_transaction* t) { return TRUE; } -bool_t i2c_idle(struct i2c_periph* periph) +bool_t i2c_idle(struct i2c_periph *periph) { // This is actually a difficult function: // -simply reading the status flags can clear bits and corrupt the transaction @@ -1449,16 +1428,16 @@ bool_t i2c_idle(struct i2c_periph* periph) #ifdef I2C_DEBUG_LED #if USE_I2C1 - if (periph == &i2c1) - { + if (periph == &i2c1) { return TRUE; } #endif #endif // First we check if the software thinks it is ready - if (periph->status == I2CIdle) - return ! (BIT_X_IS_SET_IN_REG( I2C_SR2_BUSY, I2C_SR2(i2c) ) ); - else + if (periph->status == I2CIdle) { + return !(BIT_X_IS_SET_IN_REG(I2C_SR2_BUSY, I2C_SR2(i2c))); + } else { return FALSE; + } } diff --git a/sw/airborne/arch/stm32/mcu_periph/pwm_input_arch.c b/sw/airborne/arch/stm32/mcu_periph/pwm_input_arch.c index a36a3fe912..c13320892f 100644 --- a/sw/airborne/arch/stm32/mcu_periph/pwm_input_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/pwm_input_arch.c @@ -48,16 +48,17 @@ #define PWM_INPUT_IRQ_PRIO 2 #endif -static inline void pwm_input_set_timer(uint32_t tim) { +static inline void pwm_input_set_timer(uint32_t tim) +{ timer_reset(tim); timer_set_mode(tim, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP); timer_set_period(tim, 0xFFFF); uint32_t timer_clk = timer_get_frequency(tim); - timer_set_prescaler(tim, (timer_clk / (PWM_INPUT_TICKS_PER_USEC*ONE_MHZ_CLK)) - 1); + timer_set_prescaler(tim, (timer_clk / (PWM_INPUT_TICKS_PER_USEC * ONE_MHZ_CLK)) - 1); timer_enable_counter(tim); } -void pwm_input_init ( void ) +void pwm_input_init(void) { int i; // initialize the arrays to 0 @@ -167,23 +168,24 @@ void pwm_input_init ( void ) #if USE_PWM_INPUT_TIM1 #if defined(STM32F1) -void tim1_up_isr(void) { +void tim1_up_isr(void) +{ #elif defined(STM32F4) void tim1_up_tim10_isr(void) { #endif - if((TIM1_SR & TIM_SR_UIF) != 0) { + if ((TIM1_SR & TIM_SR_UIF) != 0) { timer_clear_flag(TIM1, TIM_SR_UIF); // FIXME clear overflow interrupt but what else ? } } void tim1_cc_isr(void) { - if((TIM1_SR & TIM1_CC_IF_PERIOD) != 0) { + if ((TIM1_SR & TIM1_CC_IF_PERIOD) != 0) { timer_clear_flag(TIM1, TIM1_CC_IF_PERIOD); pwm_input_period_tics[TIM1_PWM_INPUT_IDX] = TIM1_CCR_PERIOD; pwm_input_period_valid[TIM1_PWM_INPUT_IDX] = TRUE; } - if((TIM1_SR & TIM1_CC_IF_DUTY) != 0) { + if ((TIM1_SR & TIM1_CC_IF_DUTY) != 0) { timer_clear_flag(TIM1, TIM1_CC_IF_DUTY); pwm_input_duty_tics[TIM1_PWM_INPUT_IDX] = TIM1_CCR_DUTY; pwm_input_duty_valid[TIM1_PWM_INPUT_IDX] = TRUE; @@ -195,17 +197,17 @@ void tim1_cc_isr(void) { #if USE_PWM_INPUT_TIM2 void tim2_isr(void) { - if((TIM2_SR & TIM2_CC_IF_PERIOD) != 0) { + if ((TIM2_SR & TIM2_CC_IF_PERIOD) != 0) { timer_clear_flag(TIM2, TIM2_CC_IF_PERIOD); pwm_input_period_tics[TIM2_PWM_INPUT_IDX] = TIM2_CCR_PERIOD; pwm_input_period_valid[TIM2_PWM_INPUT_IDX] = TRUE; } - if((TIM2_SR & TIM2_CC_IF_DUTY) != 0) { + if ((TIM2_SR & TIM2_CC_IF_DUTY) != 0) { timer_clear_flag(TIM2, TIM2_CC_IF_DUTY); pwm_input_duty_tics[TIM2_PWM_INPUT_IDX] = TIM2_CCR_DUTY; pwm_input_duty_valid[TIM2_PWM_INPUT_IDX] = TRUE; } - if((TIM2_SR & TIM_SR_UIF) != 0) { + if ((TIM2_SR & TIM_SR_UIF) != 0) { timer_clear_flag(TIM2, TIM_SR_UIF); // FIXME clear overflow interrupt but what else ? } @@ -216,17 +218,17 @@ void tim2_isr(void) { #if USE_PWM_INPUT_TIM3 void tim3_isr(void) { - if((TIM3_SR & TIM3_CC_IF_PERIOD) != 0) { + if ((TIM3_SR & TIM3_CC_IF_PERIOD) != 0) { timer_clear_flag(TIM3, TIM3_CC_IF_PERIOD); pwm_input_period_tics[TIM3_PWM_INPUT_IDX] = TIM3_CCR_PERIOD; pwm_input_period_valid[TIM3_PWM_INPUT_IDX] = TRUE; } - if((TIM3_SR & TIM3_CC_IF_DUTY) != 0) { + if ((TIM3_SR & TIM3_CC_IF_DUTY) != 0) { timer_clear_flag(TIM3, TIM3_CC_IF_DUTY); pwm_input_duty_tics[TIM3_PWM_INPUT_IDX] = TIM3_CCR_DUTY; pwm_input_duty_valid[TIM3_PWM_INPUT_IDX] = TRUE; } - if((TIM3_SR & TIM_SR_UIF) != 0) { + if ((TIM3_SR & TIM_SR_UIF) != 0) { timer_clear_flag(TIM3, TIM_SR_UIF); // FIXME clear overflow interrupt but what else ? } diff --git a/sw/airborne/arch/stm32/mcu_periph/spi_arch.c b/sw/airborne/arch/stm32/mcu_periph/spi_arch.c index 8d3e0fed8c..2d8a5d0ec1 100644 --- a/sw/airborne/arch/stm32/mcu_periph/spi_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/spi_arch.c @@ -118,12 +118,12 @@ static struct spi_periph_dma spi2_dma; static struct spi_periph_dma spi3_dma; #endif -static void spi_start_dma_transaction(struct spi_periph* periph, struct spi_transaction* _trans); -static void spi_next_transaction(struct spi_periph* periph); +static void spi_start_dma_transaction(struct spi_periph *periph, struct spi_transaction *_trans); +static void spi_next_transaction(struct spi_periph *periph); static void spi_configure_dma(uint32_t dma, uint32_t rcc_dma, uint8_t chan, uint32_t periph_addr, uint32_t buf_addr, uint16_t len, enum SPIDataSizeSelect dss, bool_t increment); -static void process_rx_dma_interrupt(struct spi_periph* periph); -static void process_tx_dma_interrupt(struct spi_periph* periph); +static void process_rx_dma_interrupt(struct spi_periph *periph); +static void process_tx_dma_interrupt(struct spi_periph *periph); static void spi_arch_int_enable(struct spi_periph *spi); static void spi_arch_int_disable(struct spi_periph *spi); @@ -134,8 +134,9 @@ static void spi_arch_int_disable(struct spi_periph *spi); * *****************************************************************************/ -static inline void SpiSlaveUnselect(uint8_t slave) { - switch(slave) { +static inline void SpiSlaveUnselect(uint8_t slave) +{ + switch (slave) { #if USE_SPI_SLAVE0 case 0: gpio_set(SPI_SELECT_SLAVE0_PORT, SPI_SELECT_SLAVE0_PIN); @@ -171,8 +172,9 @@ static inline void SpiSlaveUnselect(uint8_t slave) { } } -static inline void SpiSlaveSelect(uint8_t slave) { - switch(slave) { +static inline void SpiSlaveSelect(uint8_t slave) +{ + switch (slave) { #if USE_SPI_SLAVE0 case 0: gpio_clear(SPI_SELECT_SLAVE0_PORT, SPI_SELECT_SLAVE0_PIN); @@ -208,15 +210,18 @@ static inline void SpiSlaveSelect(uint8_t slave) { } } -void spi_slave_select(uint8_t slave) { +void spi_slave_select(uint8_t slave) +{ SpiSlaveSelect(slave); } -void spi_slave_unselect(uint8_t slave) { +void spi_slave_unselect(uint8_t slave) +{ SpiSlaveUnselect(slave); } -void spi_init_slaves(void) { +void spi_init_slaves(void) +{ #if USE_SPI_SLAVE0 gpio_setup_output(SPI_SELECT_SLAVE0_PORT, SPI_SELECT_SLAVE0_PIN); @@ -255,11 +260,11 @@ void spi_init_slaves(void) { * Implementation of the generic SPI functions * *****************************************************************************/ -bool_t spi_submit(struct spi_periph* p, struct spi_transaction* t) +bool_t spi_submit(struct spi_periph *p, struct spi_transaction *t) { uint8_t idx; idx = p->trans_insert_idx + 1; - if (idx >= SPI_TRANSACTION_QUEUE_LEN) idx = 0; + if (idx >= SPI_TRANSACTION_QUEUE_LEN) { idx = 0; } if ((idx == p->trans_extract_idx) || ((t->input_length == 0) && (t->output_length == 0))) { t->status = SPITransFailed; return FALSE; /* queue full or input_length and output_length both 0 */ @@ -285,7 +290,8 @@ bool_t spi_submit(struct spi_periph* p, struct spi_transaction* t) return TRUE; } -bool_t spi_lock(struct spi_periph* p, uint8_t slave) { +bool_t spi_lock(struct spi_periph *p, uint8_t slave) +{ spi_arch_int_disable(p); if (slave < 254 && p->suspend == 0) { p->suspend = slave + 1; // 0 is reserved for unlock state @@ -296,8 +302,9 @@ bool_t spi_lock(struct spi_periph* p, uint8_t slave) { return FALSE; } -bool_t spi_resume(struct spi_periph* p, uint8_t slave) { - spi_arch_int_disable( p ); +bool_t spi_resume(struct spi_periph *p, uint8_t slave) +{ + spi_arch_int_disable(p); if (p->suspend == slave + 1) { // restart fifo p->suspend = 0; @@ -317,7 +324,8 @@ bool_t spi_resume(struct spi_periph* p, uint8_t slave) { * Transaction configuration helper functions * *****************************************************************************/ -static void set_default_comm_config(struct locm3_spi_comm* c) { +static void set_default_comm_config(struct locm3_spi_comm *c) +{ c->br = SPI_CR1_BAUDRATE_FPCLK_DIV_64; c->cpol = SPI_CR1_CPOL_CLK_TO_1_WHEN_IDLE; c->cpha = SPI_CR1_CPHA_CLK_TRANSITION_2; @@ -325,12 +333,14 @@ static void set_default_comm_config(struct locm3_spi_comm* c) { c->lsbfirst = SPI_CR1_MSBFIRST; } -static inline uint8_t get_transaction_signature(struct spi_transaction* t) { +static inline uint8_t get_transaction_signature(struct spi_transaction *t) +{ return ((t->dss << 6) | (t->cdiv << 3) | (t->bitorder << 2) | (t->cpha << 1) | (t->cpol)); } -static uint8_t get_comm_signature(struct locm3_spi_comm* c) { +static uint8_t get_comm_signature(struct locm3_spi_comm *c) +{ uint8_t sig = 0; if (c->cpol == SPI_CR1_CPOL_CLK_TO_0_WHEN_IDLE) { sig |= SPICpolIdleLow; @@ -386,7 +396,8 @@ static uint8_t get_comm_signature(struct locm3_spi_comm* c) { } /** Update SPI communication conf from generic paparazzi SPI transaction */ -static void set_comm_from_transaction(struct locm3_spi_comm* c, struct spi_transaction* t) { +static void set_comm_from_transaction(struct locm3_spi_comm *c, struct spi_transaction *t) +{ if (t->dss == SPIDss8bit) { c->dff = SPI_CR1_DFF_8BIT; } else { @@ -476,50 +487,56 @@ static void spi_configure_dma(uint32_t dma, uint32_t rcc_dma, uint8_t chan, uint } #endif - if (increment) + if (increment) { dma_enable_memory_increment_mode(dma, chan); - else + } else { dma_disable_memory_increment_mode(dma, chan); + } } /// Enable DMA channel interrupts -static void spi_arch_int_enable(struct spi_periph *spi) { +static void spi_arch_int_enable(struct spi_periph *spi) +{ /// @todo fix priority levels if necessary // enable receive interrupt - nvic_set_priority( ((struct spi_periph_dma *)spi->init_struct)->rx_nvic_irq, NVIC_SPI_IRQ_PRIO); - nvic_enable_irq( ((struct spi_periph_dma *)spi->init_struct)->rx_nvic_irq ); + nvic_set_priority(((struct spi_periph_dma *)spi->init_struct)->rx_nvic_irq, NVIC_SPI_IRQ_PRIO); + nvic_enable_irq(((struct spi_periph_dma *)spi->init_struct)->rx_nvic_irq); // enable transmit interrupt - nvic_set_priority( ((struct spi_periph_dma *)spi->init_struct)->tx_nvic_irq, NVIC_SPI_IRQ_PRIO); - nvic_enable_irq( ((struct spi_periph_dma *)spi->init_struct)->tx_nvic_irq ); + nvic_set_priority(((struct spi_periph_dma *)spi->init_struct)->tx_nvic_irq, NVIC_SPI_IRQ_PRIO); + nvic_enable_irq(((struct spi_periph_dma *)spi->init_struct)->tx_nvic_irq); } /// Disable DMA channel interrupts -static void spi_arch_int_disable(struct spi_periph *spi) { - nvic_disable_irq( ((struct spi_periph_dma *)spi->init_struct)->rx_nvic_irq ); - nvic_disable_irq( ((struct spi_periph_dma *)spi->init_struct)->tx_nvic_irq ); +static void spi_arch_int_disable(struct spi_periph *spi) +{ + nvic_disable_irq(((struct spi_periph_dma *)spi->init_struct)->rx_nvic_irq); + nvic_disable_irq(((struct spi_periph_dma *)spi->init_struct)->tx_nvic_irq); } /// start next transaction if there is one in the queue -static void spi_next_transaction(struct spi_periph* periph) { +static void spi_next_transaction(struct spi_periph *periph) +{ /* Increment the transaction to handle */ periph->trans_extract_idx++; /* wrap read index of circular buffer */ - if (periph->trans_extract_idx >= SPI_TRANSACTION_QUEUE_LEN) + if (periph->trans_extract_idx >= SPI_TRANSACTION_QUEUE_LEN) { periph->trans_extract_idx = 0; + } /* Check if there is another pending SPI transaction */ - if ((periph->trans_extract_idx == periph->trans_insert_idx) || periph->suspend) + if ((periph->trans_extract_idx == periph->trans_insert_idx) || periph->suspend) { periph->status = SPIIdle; - else + } else { spi_start_dma_transaction(periph, periph->trans[periph->trans_extract_idx]); + } } /** * Start a new transaction with DMA. */ -static void spi_start_dma_transaction(struct spi_periph* periph, struct spi_transaction* trans) +static void spi_start_dma_transaction(struct spi_periph *periph, struct spi_transaction *trans) { struct spi_periph_dma *dma; uint8_t sig = 0x00; @@ -578,7 +595,7 @@ static void spi_start_dma_transaction(struct spi_periph* periph, struct spi_tran if (trans->input_length == 0) { /* run the dummy rx dma for the complete transaction length */ spi_configure_dma(dma->dma, dma->rcc_dma, dma->rx_chan, (uint32_t)dma->spidr, - (uint32_t)&(dma->rx_dummy_buf), trans->output_length, trans->dss, FALSE); + (uint32_t) & (dma->rx_dummy_buf), trans->output_length, trans->dss, FALSE); } else { /* run the real rx dma for input_length */ spi_configure_dma(dma->dma, dma->rcc_dma, dma->rx_chan, (uint32_t)dma->spidr, @@ -611,7 +628,7 @@ static void spi_start_dma_transaction(struct spi_periph* periph, struct spi_tran */ if (trans->output_length == 0) { spi_configure_dma(dma->dma, dma->rcc_dma, dma->tx_chan, (uint32_t)dma->spidr, - (uint32_t)&(dma->tx_dummy_buf), trans->input_length, trans->dss, FALSE); + (uint32_t) & (dma->tx_dummy_buf), trans->input_length, trans->dss, FALSE); } else { spi_configure_dma(dma->dma, dma->rcc_dma, dma->tx_chan, (uint32_t)dma->spidr, (uint32_t)trans->output_buf, trans->output_length, trans->dss, TRUE); @@ -655,7 +672,8 @@ static void spi_start_dma_transaction(struct spi_periph* periph, struct spi_tran * *****************************************************************************/ #if USE_SPI1 -void spi1_arch_init(void) { +void spi1_arch_init(void) +{ // set dma options spi1_dma.spidr = (uint32_t)&SPI1_DR; @@ -749,7 +767,8 @@ void spi1_arch_init(void) { #endif #if USE_SPI2 -void spi2_arch_init(void) { +void spi2_arch_init(void) +{ // set dma options spi2_dma.spidr = (uint32_t)&SPI2_DR; @@ -839,7 +858,8 @@ void spi2_arch_init(void) { #endif #if USE_SPI3 -void spi3_arch_init(void) { +void spi3_arch_init(void) +{ // set the default configuration spi3_dma.spidr = (uint32_t)&SPI3_DR; @@ -950,8 +970,7 @@ void dma1_channel2_isr(void) DMA1_IFCR |= DMA_IFCR_CTCIF2; } #elif defined STM32F4 -void dma2_stream0_isr(void) -{ +void dma2_stream0_isr(void) { if ((DMA2_LISR & DMA_LISR_TCIF0) != 0) { // clear int pending bit DMA2_LIFCR |= DMA_LIFCR_CTCIF0; @@ -962,15 +981,13 @@ void dma2_stream0_isr(void) /// transmit transferred over DMA #ifdef STM32F1 -void dma1_channel3_isr(void) -{ +void dma1_channel3_isr(void) { if ((DMA1_ISR & DMA_ISR_TCIF3) != 0) { // clear int pending bit DMA1_IFCR |= DMA_IFCR_CTCIF3; } #elif defined STM32F4 -void dma2_stream5_isr(void) -{ +void dma2_stream5_isr(void) { if ((DMA2_HISR & DMA_HISR_TCIF5) != 0) { // clear int pending bit DMA2_HIFCR |= DMA_HIFCR_CTCIF5; @@ -984,15 +1001,13 @@ void dma2_stream5_isr(void) #ifdef USE_SPI2 /// receive transferred over DMA #ifdef STM32F1 -void dma1_channel4_isr(void) -{ +void dma1_channel4_isr(void) { if ((DMA1_ISR & DMA_ISR_TCIF4) != 0) { // clear int pending bit DMA1_IFCR |= DMA_IFCR_CTCIF4; } #elif defined STM32F4 -void dma1_stream3_isr(void) -{ +void dma1_stream3_isr(void) { if ((DMA1_LISR & DMA_LISR_TCIF3) != 0) { // clear int pending bit DMA1_LIFCR |= DMA_LIFCR_CTCIF3; @@ -1003,15 +1018,13 @@ void dma1_stream3_isr(void) /// transmit transferred over DMA #ifdef STM32F1 -void dma1_channel5_isr(void) -{ +void dma1_channel5_isr(void) { if ((DMA1_ISR & DMA_ISR_TCIF5) != 0) { // clear int pending bit DMA1_IFCR |= DMA_IFCR_CTCIF5; } #elif defined STM32F4 -void dma1_stream4_isr(void) -{ +void dma1_stream4_isr(void) { if ((DMA1_HISR & DMA_HISR_TCIF4) != 0) { // clear int pending bit DMA1_HIFCR |= DMA_HIFCR_CTCIF4; @@ -1025,15 +1038,13 @@ void dma1_stream4_isr(void) #if USE_SPI3 /// receive transferred over DMA #ifdef STM32F1 -void dma2_channel1_isr(void) -{ +void dma2_channel1_isr(void) { if ((DMA2_ISR & DMA_ISR_TCIF1) != 0) { // clear int pending bit DMA2_IFCR |= DMA_IFCR_CTCIF1; } #elif defined STM32F4 -void dma1_stream0_isr(void) -{ +void dma1_stream0_isr(void) { if ((DMA1_LISR & DMA_LISR_TCIF0) != 0) { // clear int pending bit DMA1_LIFCR |= DMA_LIFCR_CTCIF0; @@ -1044,15 +1055,13 @@ void dma1_stream0_isr(void) /// transmit transferred over DMA #ifdef STM32F1 -void dma2_channel2_isr(void) -{ +void dma2_channel2_isr(void) { if ((DMA2_ISR & DMA_ISR_TCIF2) != 0) { // clear int pending bit DMA2_IFCR |= DMA_IFCR_CTCIF2; } #elif defined STM32F4 -void dma1_stream5_isr(void) -{ +void dma1_stream5_isr(void) { if ((DMA1_HISR & DMA_HISR_TCIF5) != 0) { // clear int pending bit DMA1_HIFCR |= DMA_HIFCR_CTCIF5; @@ -1064,7 +1073,7 @@ void dma1_stream5_isr(void) #endif /// Processing done after rx completes. -void process_rx_dma_interrupt(struct spi_periph *periph) { +void process_rx_dma_interrupt(struct spi_periph * periph) { struct spi_periph_dma *dma = periph->init_struct; struct spi_transaction *trans = periph->trans[periph->trans_extract_idx]; @@ -1096,7 +1105,7 @@ void process_rx_dma_interrupt(struct spi_periph *periph) { uint16_t len_remaining = trans->output_length - trans->input_length; spi_configure_dma(dma->dma, dma->rcc_dma, dma->rx_chan, (uint32_t)dma->spidr, - (uint32_t)&(dma->rx_dummy_buf), len_remaining, trans->dss, FALSE); + (uint32_t) & (dma->rx_dummy_buf), len_remaining, trans->dss, FALSE); #ifdef STM32F1 dma_set_read_from_peripheral(dma->dma, dma->rx_chan); dma_set_priority(dma->dma, dma->rx_chan, DMA_CCR_PL_HIGH); @@ -1116,8 +1125,7 @@ void process_rx_dma_interrupt(struct spi_periph *periph) { #endif /* Enable SPI transfers via DMA */ spi_enable_rx_dma((uint32_t)periph->reg_addr); - } - else { + } else { /* * Since the receive DMA is always run until the very end * and this interrupt is triggered after the last data word was read, @@ -1140,7 +1148,7 @@ void process_rx_dma_interrupt(struct spi_periph *periph) { } /// Processing done after tx completes -void process_tx_dma_interrupt(struct spi_periph *periph) { +void process_tx_dma_interrupt(struct spi_periph * periph) { struct spi_periph_dma *dma = periph->init_struct; struct spi_transaction *trans = periph->trans[periph->trans_extract_idx]; @@ -1171,7 +1179,7 @@ void process_tx_dma_interrupt(struct spi_periph *periph) { uint16_t len_remaining = trans->input_length - trans->output_length; spi_configure_dma(dma->dma, dma->rcc_dma, dma->tx_chan, (uint32_t)dma->spidr, - (uint32_t)&(dma->tx_dummy_buf), len_remaining, trans->dss, FALSE); + (uint32_t) & (dma->tx_dummy_buf), len_remaining, trans->dss, FALSE); #ifdef STM32F1 dma_set_read_from_memory(dma->dma, dma->tx_chan); dma_set_priority(dma->dma, dma->tx_chan, DMA_CCR_PL_MEDIUM); @@ -1206,7 +1214,7 @@ void process_tx_dma_interrupt(struct spi_periph *periph) { */ #ifdef SPI_SLAVE -static void process_slave_rx_dma_interrupt(struct spi_periph* periph); +static void process_slave_rx_dma_interrupt(struct spi_periph * periph); // SPI arch slave init @@ -1262,8 +1270,8 @@ void spi1_slave_arch_init(void) { GPIO_SPI1_MISO); gpio_set_mode(GPIO_BANK_SPI1_NSS, GPIO_MODE_INPUT, - GPIO_CNF_INPUT_FLOAT, - GPIO_SPI1_NSS); + GPIO_CNF_INPUT_FLOAT, + GPIO_SPI1_NSS); // reset SPI spi_reset(SPI1); @@ -1292,8 +1300,7 @@ void spi1_slave_arch_init(void) { } /// receive transferred over DMA -void dma1_channel2_isr(void) -{ +void dma1_channel2_isr(void) { if ((DMA1_ISR & DMA_ISR_TCIF2) != 0) { // clear int pending bit DMA1_IFCR |= DMA_IFCR_CTCIF2; @@ -1305,8 +1312,7 @@ void dma1_channel2_isr(void) #endif /* USE_SPI1_SLAVE */ -static void spi_slave_set_config(struct spi_periph* periph, struct spi_transaction* trans) -{ +static void spi_slave_set_config(struct spi_periph * periph, struct spi_transaction * trans) { struct spi_periph_dma *dma; dma = periph->init_struct; @@ -1328,8 +1334,7 @@ static void spi_slave_set_config(struct spi_periph* periph, struct spi_transacti //static void spi_start_slave_dma_transaction(struct spi_periph* periph, struct spi_transaction* trans) -bool_t spi_slave_register(struct spi_periph* periph, struct spi_transaction* trans) -{ +bool_t spi_slave_register(struct spi_periph * periph, struct spi_transaction * trans) { spi_slave_set_config(periph, trans); struct spi_periph_dma *dma; @@ -1377,7 +1382,7 @@ bool_t spi_slave_register(struct spi_periph* periph, struct spi_transaction* tra return TRUE; } -void process_slave_rx_dma_interrupt(struct spi_periph *periph) { +void process_slave_rx_dma_interrupt(struct spi_periph * periph) { struct spi_periph_dma *dma = periph->init_struct; struct spi_transaction *trans = periph->trans[periph->trans_extract_idx]; diff --git a/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.c b/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.c index 88dee2cf88..99dd072708 100644 --- a/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.c @@ -44,7 +44,8 @@ void sys_tick_handler(void); /** Initialize SysTick. * Generate SysTick interrupt every sys_time.resolution_cpu_ticks */ -void sys_time_arch_init( void ) { +void sys_time_arch_init(void) +{ /* run cortex systick timer with 72MHz (FIXME only 72 or does it work with 168MHz???) */ #if USE_OCM3_SYSTICK_INIT systick_set_clocksource(STK_CSR_CLKSOURCE_AHB); @@ -58,7 +59,7 @@ void sys_time_arch_init( void ) { /* The timer interrupt is activated on the transition from 1 to 0, * therefore it activates every n+1 clock ticks. */ - systick_set_reload(sys_time.resolution_cpu_ticks-1); + systick_set_reload(sys_time.resolution_cpu_ticks - 1); systick_interrupt_enable(); systick_counter_enable(); @@ -71,7 +72,8 @@ void sys_time_arch_init( void ) { // 97 days at 512hz // 12 hours at 100khz // -void sys_tick_handler(void) { +void sys_tick_handler(void) +{ sys_time.nb_tick++; sys_time.nb_sec_rem += sys_time.resolution_cpu_ticks; @@ -83,7 +85,7 @@ void sys_tick_handler(void) { LED_TOGGLE(SYS_TIME_LED); #endif } - for (unsigned int i=0; i= sys_time.timer[i].end_time) { sys_time.timer[i].end_time += sys_time.timer[i].duration; diff --git a/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.h b/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.h index a8bc6396c2..7c6b39bd00 100644 --- a/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.h +++ b/sw/airborne/arch/stm32/mcu_periph/sys_time_arch.h @@ -48,13 +48,14 @@ * WARNING: overflows after 70min! * @return microseconds since startup as uint32_t */ -static inline uint32_t get_sys_time_usec(void) { +static inline uint32_t get_sys_time_usec(void) +{ #ifdef RTOS_IS_CHIBIOS return (chibios_chTimeNow() * (1000000 / CH_FREQUENCY)); #else return sys_time.nb_sec * 1000000 + - usec_of_cpu_ticks(sys_time.nb_sec_rem) + - usec_of_cpu_ticks(systick_get_reload() - systick_get_value()); + usec_of_cpu_ticks(sys_time.nb_sec_rem) + + usec_of_cpu_ticks(systick_get_reload() - systick_get_value()); #endif } @@ -62,13 +63,14 @@ static inline uint32_t get_sys_time_usec(void) { * Get the time in milliseconds since startup. * @return milliseconds since startup as uint32_t */ -static inline uint32_t get_sys_time_msec(void) { +static inline uint32_t get_sys_time_msec(void) +{ #ifdef RTOS_IS_CHIBIOS return (chibios_chTimeNow() * (1000 / CH_FREQUENCY)); #else return sys_time.nb_sec * 1000 + - msec_of_cpu_ticks(sys_time.nb_sec_rem) + - msec_of_cpu_ticks(systick_get_reload() - systick_get_value()); + msec_of_cpu_ticks(sys_time.nb_sec_rem) + + msec_of_cpu_ticks(systick_get_reload() - systick_get_value()); #endif } @@ -78,9 +80,10 @@ static inline uint32_t get_sys_time_msec(void) { * max value is limited by the max number of cycle * i.e 2^32 * usec_of_cpu_ticks(systick_get_reload()) */ -static inline void sys_time_usleep(uint32_t us) { +static inline void sys_time_usleep(uint32_t us) +{ #ifdef RTOS_IS_CHIBIOS - chibios_chThdSleepMicroseconds (us); + chibios_chThdSleepMicroseconds(us); #else // start time uint32_t start = systick_get_value(); @@ -94,8 +97,7 @@ static inline void sys_time_usleep(uint32_t us) { uint32_t end; if (rem < start) { end = start - rem; - } - else { + } else { // one more count flag is required n++; end = systick_get_reload() - rem + start; diff --git a/sw/airborne/arch/stm32/mcu_periph/uart_arch.c b/sw/airborne/arch/stm32/mcu_periph/uart_arch.c index 31a2dde006..ac6ecbbe14 100644 --- a/sw/airborne/arch/stm32/mcu_periph/uart_arch.c +++ b/sw/airborne/arch/stm32/mcu_periph/uart_arch.c @@ -39,7 +39,8 @@ #include BOARD_CONFIG -void uart_periph_set_baudrate(struct uart_periph* p, uint32_t baud) { +void uart_periph_set_baudrate(struct uart_periph *p, uint32_t baud) +{ /* Configure USART baudrate */ usart_set_baudrate((uint32_t)p->reg_addr, baud); @@ -58,57 +59,63 @@ void uart_periph_set_baudrate(struct uart_periph* p, uint32_t baud) { } -void uart_periph_set_bits_stop_parity(struct uart_periph* p, uint8_t bits, uint8_t stop, uint8_t parity) { +void uart_periph_set_bits_stop_parity(struct uart_periph *p, uint8_t bits, uint8_t stop, uint8_t parity) +{ /* Configure USART parity and data bits */ if (parity == UPARITY_EVEN) { usart_set_parity((uint32_t)p->reg_addr, USART_PARITY_EVEN); - if (bits == UBITS_7) + if (bits == UBITS_7) { usart_set_databits((uint32_t)p->reg_addr, 8); - else // 8 data bits by default + } else { // 8 data bits by default usart_set_databits((uint32_t)p->reg_addr, 9); - } - else if (parity == UPARITY_ODD) { + } + } else if (parity == UPARITY_ODD) { usart_set_parity((uint32_t)p->reg_addr, USART_PARITY_ODD); - if (bits == UBITS_7) + if (bits == UBITS_7) { usart_set_databits((uint32_t)p->reg_addr, 8); - else // 8 data bits by default + } else { // 8 data bits by default usart_set_databits((uint32_t)p->reg_addr, 9); - } - else { // 8 data bist, NO_PARITY by default + } + } else { // 8 data bist, NO_PARITY by default usart_set_parity((uint32_t)p->reg_addr, USART_PARITY_NONE); usart_set_databits((uint32_t)p->reg_addr, 8); // is 7bits without parity possible ? } /* Configure USART stop bits */ - if (stop == USTOP_2) + if (stop == USTOP_2) { usart_set_stopbits((uint32_t)p->reg_addr, USART_STOPBITS_2); - else // 1 stop bit by default + } else { // 1 stop bit by default usart_set_stopbits((uint32_t)p->reg_addr, USART_STOPBITS_1); + } } -void uart_periph_set_mode(struct uart_periph* p, bool_t tx_enabled, bool_t rx_enabled, bool_t hw_flow_control) { +void uart_periph_set_mode(struct uart_periph *p, bool_t tx_enabled, bool_t rx_enabled, bool_t hw_flow_control) +{ uint32_t mode = 0; - if (tx_enabled) + if (tx_enabled) { mode |= USART_MODE_TX; - if (rx_enabled) + } + if (rx_enabled) { mode |= USART_MODE_RX; + } /* set mode to Tx, Rx or TxRx */ usart_set_mode((uint32_t)p->reg_addr, mode); if (hw_flow_control) { usart_set_flow_control((uint32_t)p->reg_addr, USART_FLOWCONTROL_RTS_CTS); - } - else { + } else { usart_set_flow_control((uint32_t)p->reg_addr, USART_FLOWCONTROL_NONE); } } -void uart_transmit(struct uart_periph* p, uint8_t data ) { +void uart_transmit(struct uart_periph *p, uint8_t data) +{ uint16_t temp = (p->tx_insert_idx + 1) % UART_TX_BUFFER_SIZE; - if (temp == p->tx_extract_idx) - return; // no room + if (temp == p->tx_extract_idx) { + return; // no room + } USART_CR1((uint32_t)p->reg_addr) &= ~USART_CR1_TXEIE; // Disable TX interrupt @@ -116,8 +123,7 @@ void uart_transmit(struct uart_periph* p, uint8_t data ) { if (p->tx_running) { // yes, add to queue p->tx_buf[p->tx_insert_idx] = data; p->tx_insert_idx = temp; - } - else { // no, set running flag and write to output register + } else { // no, set running flag and write to output register p->tx_running = TRUE; usart_send((uint32_t)p->reg_addr, data); } @@ -126,17 +132,17 @@ void uart_transmit(struct uart_periph* p, uint8_t data ) { } -static inline void usart_isr(struct uart_periph* p) { +static inline void usart_isr(struct uart_periph *p) +{ if (((USART_CR1((uint32_t)p->reg_addr) & USART_CR1_TXEIE) != 0) && ((USART_SR((uint32_t)p->reg_addr) & USART_SR_TXE) != 0)) { // check if more data to send if (p->tx_insert_idx != p->tx_extract_idx) { - usart_send((uint32_t)p->reg_addr,p->tx_buf[p->tx_extract_idx]); + usart_send((uint32_t)p->reg_addr, p->tx_buf[p->tx_extract_idx]); p->tx_extract_idx++; p->tx_extract_idx %= UART_TX_BUFFER_SIZE; - } - else { + } else { p->tx_running = FALSE; // clear running flag USART_CR1((uint32_t)p->reg_addr) &= ~USART_CR1_TXEIE; // Disable TX interrupt } @@ -150,10 +156,10 @@ static inline void usart_isr(struct uart_periph* p) { uint16_t temp = (p->rx_insert_idx + 1) % UART_RX_BUFFER_SIZE;; p->rx_buf[p->rx_insert_idx] = usart_recv((uint32_t)p->reg_addr); // check for more room in queue - if (temp != p->rx_extract_idx) - p->rx_insert_idx = temp; // update insert index - } - else { + if (temp != p->rx_extract_idx) { + p->rx_insert_idx = temp; // update insert index + } + } else { /* ORE, NE or FE error - read USART_DR reg and log the error */ if (((USART_CR1((uint32_t)p->reg_addr) & USART_CR1_RXNEIE) != 0) && ((USART_SR((uint32_t)p->reg_addr) & USART_SR_ORE) != 0)) { @@ -173,7 +179,8 @@ static inline void usart_isr(struct uart_periph* p) { } } -static inline void usart_enable_irq(uint8_t IRQn) { +static inline void usart_enable_irq(uint8_t IRQn) +{ /* Note: * In libstm32 times the priority of this interrupt was set to * preemption priority 2 and sub priority 1 @@ -209,7 +216,8 @@ static inline void usart_enable_irq(uint8_t IRQn) { #define UART1_PARITY UPARITY_NO #endif -void uart1_init( void ) { +void uart1_init(void) +{ uart_periph_init(&uart1); uart1.reg_addr = (void *)USART1; @@ -273,7 +281,8 @@ void usart1_isr(void) { usart_isr(&uart1); } #define UART2_PARITY UPARITY_NO #endif -void uart2_init( void ) { +void uart2_init(void) +{ uart_periph_init(&uart2); uart2.reg_addr = (void *)USART2; @@ -337,7 +346,8 @@ void usart2_isr(void) { usart_isr(&uart2); } #define UART3_PARITY UPARITY_NO #endif -void uart3_init( void ) { +void uart3_init(void) +{ uart_periph_init(&uart3); uart3.reg_addr = (void *)USART3; @@ -397,7 +407,8 @@ void usart3_isr(void) { usart_isr(&uart3); } #define UART4_PARITY UPARITY_NO #endif -void uart4_init( void ) { +void uart4_init(void) +{ uart_periph_init(&uart4); uart4.reg_addr = (void *)UART4; @@ -448,7 +459,8 @@ void uart4_isr(void) { usart_isr(&uart4); } #define UART5_PARITY UPARITY_NO #endif -void uart5_init( void ) { +void uart5_init(void) +{ uart_periph_init(&uart5); uart5.reg_addr = (void *)UART5; @@ -503,7 +515,8 @@ void uart5_isr(void) { usart_isr(&uart5); } #define UART6_PARITY UPARITY_NO #endif -void uart6_init( void ) { +void uart6_init(void) +{ uart_periph_init(&uart6); uart6.reg_addr = (void *)USART6; diff --git a/sw/airborne/arch/stm32/my_debug_servo.h b/sw/airborne/arch/stm32/my_debug_servo.h index 3c8bbc7615..19f76db83c 100644 --- a/sw/airborne/arch/stm32/my_debug_servo.h +++ b/sw/airborne/arch/stm32/my_debug_servo.h @@ -41,7 +41,7 @@ DEBUG_S1_OFF(); \ DEBUG_S2_OFF(); \ DEBUG_S3_OFF(); \ - } + } #define DEBUG_SERVO2_INIT() { \ /* S4: PC9 */ \ diff --git a/sw/airborne/arch/stm32/peripherals/hmc5843_arch.c b/sw/airborne/arch/stm32/peripherals/hmc5843_arch.c index 9cdf9087ec..3332e476de 100644 --- a/sw/airborne/arch/stm32/peripherals/hmc5843_arch.c +++ b/sw/airborne/arch/stm32/peripherals/hmc5843_arch.c @@ -32,12 +32,13 @@ #error "HMC5843 arch currently only implemented for STM32F1" #endif -void hmc5843_arch_init( void ) { +void hmc5843_arch_init(void) +{ /* configure external interrupt exti5 on PB5( mag int ) */ 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); @@ -51,10 +52,11 @@ void hmc5843_arch_init( void ) { void hmc5843_arch_reset(void) { - i2c2_er_irq_handler(); + i2c2_er_irq_handler(); } -void exti9_5_isr(void) { +void exti9_5_isr(void) +{ exti_reset_request(EXTI5); } diff --git a/sw/airborne/arch/stm32/peripherals/max1168_arch.c b/sw/airborne/arch/stm32/peripherals/max1168_arch.c index 15abda93ff..d77019ce0c 100644 --- a/sw/airborne/arch/stm32/peripherals/max1168_arch.c +++ b/sw/airborne/arch/stm32/peripherals/max1168_arch.c @@ -29,7 +29,8 @@ #error "HMC5843 arch currently only implemented for STM32F1" #endif -void max1168_arch_init( void ) { +void max1168_arch_init(void) +{ /* configure external interrupt exti2 on PD2( data ready ) v1.0*/ /* PB2( data ready ) v1.1*/ @@ -47,7 +48,8 @@ void max1168_arch_init( void ) { } -void exti2_isr(void) { +void exti2_isr(void) +{ /* clear EXTI */ exti_reset_request(EXTI2); diff --git a/sw/airborne/arch/stm32/peripherals/ms2100_arch.c b/sw/airborne/arch/stm32/peripherals/ms2100_arch.c index 51d31cd7ab..16f1293009 100644 --- a/sw/airborne/arch/stm32/peripherals/ms2100_arch.c +++ b/sw/airborne/arch/stm32/peripherals/ms2100_arch.c @@ -37,7 +37,8 @@ #error "MS2100 arch currently only implemented for STM32F1" #endif -void ms2100_arch_init( void ) { +void ms2100_arch_init(void) +{ /* set mag reset as output (reset on PC13) ----*/ rcc_periph_clock_enable(RCC_GPIOC); @@ -59,7 +60,8 @@ void ms2100_arch_init( void ) { nvic_enable_irq(NVIC_EXTI9_5_IRQ); } -void ms2100_reset_cb( struct spi_transaction * t __attribute__ ((unused)) ) { +void ms2100_reset_cb(struct spi_transaction *t __attribute__((unused))) +{ // set RESET pin high for at least 100 nsec // busy wait should not harm Ms2100Set(); @@ -67,15 +69,17 @@ void ms2100_reset_cb( struct spi_transaction * t __attribute__ ((unused)) ) { // FIXME, make nanosleep funcion uint32_t dt_ticks = cpu_ticks_of_nsec(110); int32_t end_cpu_ticks = systick_get_value() - dt_ticks; - if (end_cpu_ticks < 0) + if (end_cpu_ticks < 0) { end_cpu_ticks += systick_get_reload(); + } while (systick_get_value() > (uint32_t)end_cpu_ticks) ; Ms2100Reset(); } -void exti9_5_isr(void) { +void exti9_5_isr(void) +{ ms2100.status = MS2100_GOT_EOC; exti_reset_request(EXTI5); } diff --git a/sw/airborne/arch/stm32/peripherals/ms2100_arch.h b/sw/airborne/arch/stm32/peripherals/ms2100_arch.h index d34bd32d0d..0037b3c97b 100644 --- a/sw/airborne/arch/stm32/peripherals/ms2100_arch.h +++ b/sw/airborne/arch/stm32/peripherals/ms2100_arch.h @@ -35,7 +35,8 @@ * Here Reset indicates the Ms2100 is in normal state, i.e. * the reset line is driven low (i.e. the GPIO is "reset") */ -static inline void Ms2100Reset(void) { +static inline void Ms2100Reset(void) +{ GPIOC_BRR = GPIO13; } @@ -43,7 +44,8 @@ static inline void Ms2100Reset(void) { * Here Set indicates the Ms2100 is in reset state, i.e. * the reset line is driven high (i.e. the GPIO is "set") */ -static inline void Ms2100Set(void) { +static inline void Ms2100Set(void) +{ GPIOC_BSRR = GPIO13; } @@ -52,6 +54,6 @@ static inline void Ms2100Set(void) { /** Reset callback. * called before spi transaction and after slave select */ -extern void ms2100_reset_cb( struct spi_transaction * t ); +extern void ms2100_reset_cb(struct spi_transaction *t); #endif /* MS2100_ARCH_H */ diff --git a/sw/airborne/arch/stm32/peripherals/sc18is600_arch.c b/sw/airborne/arch/stm32/peripherals/sc18is600_arch.c index 6c86112391..f19c07fd73 100644 --- a/sw/airborne/arch/stm32/peripherals/sc18is600_arch.c +++ b/sw/airborne/arch/stm32/peripherals/sc18is600_arch.c @@ -22,7 +22,8 @@ extern void dma1_c4_irq_handler(void); static inline void sc18is600_setup_SPI_DMA(uint8_t _len); -void sc18is600_arch_init(void) { +void sc18is600_arch_init(void) +{ /* set slave select as output and assert it ( on PB12) */ Sc18Is600Unselect(); @@ -97,11 +98,12 @@ void sc18is600_arch_init(void) { } -static inline void sc18is600_setup_SPI_DMA(uint8_t _len) { +static inline void sc18is600_setup_SPI_DMA(uint8_t _len) +{ /* SPI2_Rx_DMA_Channel configuration ------------------------------------*/ DMA_DeInit(DMA1_Channel4); DMA_InitTypeDef DMA_initStructure_4 = { - .DMA_PeripheralBaseAddr = (uint32_t)(SPI2_BASE+0x0C), + .DMA_PeripheralBaseAddr = (uint32_t)(SPI2_BASE + 0x0C), .DMA_MemoryBaseAddr = (uint32_t)sc18is600.priv_rx_buf, .DMA_DIR = DMA_DIR_PeripheralSRC, .DMA_BufferSize = _len, @@ -117,7 +119,7 @@ static inline void sc18is600_setup_SPI_DMA(uint8_t _len) { /* SPI2_Tx_DMA_Channel configuration ------------------------------------*/ DMA_DeInit(DMA1_Channel5); DMA_InitTypeDef DMA_initStructure_5 = { - .DMA_PeripheralBaseAddr = (uint32_t)(SPI2_BASE+0x0C), + .DMA_PeripheralBaseAddr = (uint32_t)(SPI2_BASE + 0x0C), .DMA_MemoryBaseAddr = (uint32_t)sc18is600.priv_tx_buf, .DMA_DIR = DMA_DIR_PeripheralDST, .DMA_BufferSize = _len, @@ -146,7 +148,8 @@ static inline void sc18is600_setup_SPI_DMA(uint8_t _len) { } -void sc18is600_transmit(uint8_t addr, uint8_t len) { +void sc18is600_transmit(uint8_t addr, uint8_t len) +{ sc18is600.transaction = Sc18Is600Transmit; sc18is600.status = Sc18Is600SendingRequest; @@ -154,15 +157,17 @@ void sc18is600_transmit(uint8_t addr, uint8_t len) { sc18is600.priv_tx_buf[1] = len; sc18is600.priv_tx_buf[2] = addr; Sc18Is600Select(); - sc18is600_setup_SPI_DMA(len+3); + sc18is600_setup_SPI_DMA(len + 3); } -void sc18is600_receive(uint8_t addr, uint8_t len) { +void sc18is600_receive(uint8_t addr, uint8_t len) +{ } -void sc18is600_tranceive(uint8_t addr, uint8_t len_tx, uint8_t len_rx) { +void sc18is600_tranceive(uint8_t addr, uint8_t len_tx, uint8_t len_rx) +{ sc18is600.transaction = Sc18Is600Transcieve; sc18is600.status = Sc18Is600SendingRequest; sc18is600.rx_len = len_rx; @@ -170,12 +175,13 @@ void sc18is600_tranceive(uint8_t addr, uint8_t len_tx, uint8_t len_rx) { sc18is600.priv_tx_buf[1] = len_tx; sc18is600.priv_tx_buf[2] = len_rx; sc18is600.priv_tx_buf[3] = addr; - sc18is600.priv_tx_buf[4+len_tx] = addr; + sc18is600.priv_tx_buf[4 + len_tx] = addr; Sc18Is600Select(); - sc18is600_setup_SPI_DMA(len_tx+5); + sc18is600_setup_SPI_DMA(len_tx + 5); } -void sc18is600_write_to_register(uint8_t addr, uint8_t value) { +void sc18is600_write_to_register(uint8_t addr, uint8_t value) +{ sc18is600.transaction = Sc18Is600WriteRegister; sc18is600.status = Sc18Is600SendingRequest; sc18is600.priv_tx_buf[0] = Sc18Is600_Cmd_Write_To_Reg; // write to register @@ -186,7 +192,8 @@ void sc18is600_write_to_register(uint8_t addr, uint8_t value) { } -void sc18is600_read_from_register(uint8_t addr) { +void sc18is600_read_from_register(uint8_t addr) +{ sc18is600.transaction = Sc18Is600ReadRegister; sc18is600.status = Sc18Is600SendingRequest; sc18is600.priv_tx_buf[0] = Sc18Is600_Cmd_Read_From_Reg; // read from register @@ -196,88 +203,88 @@ void sc18is600_read_from_register(uint8_t addr) { sc18is600_setup_SPI_DMA(3); } -#define ReadI2CStatReg() { \ +#define ReadI2CStatReg() { \ sc18is600.priv_tx_buf[0] = Sc18Is600_Cmd_Read_From_Reg; \ - sc18is600.priv_tx_buf[1] = Sc18Is600_I2CStat; \ - sc18is600.priv_tx_buf[2] = 0; \ - Sc18Is600Select(); \ - sc18is600_setup_SPI_DMA(3); \ + sc18is600.priv_tx_buf[1] = Sc18Is600_I2CStat; \ + sc18is600.priv_tx_buf[2] = 0; \ + Sc18Is600Select(); \ + sc18is600_setup_SPI_DMA(3); \ } -void exti2_irq_handler(void) { +void exti2_irq_handler(void) +{ /* clear EXTI */ - if(EXTI_GetITStatus(EXTI_Line2) != RESET) + if (EXTI_GetITStatus(EXTI_Line2) != RESET) { EXTI_ClearITPendingBit(EXTI_Line2); + } switch (sc18is600.transaction) { - case Sc18Is600Receive: - case Sc18Is600Transmit: - case Sc18Is600Transcieve: - if (sc18is600.status == Sc18Is600WaitingForI2C) { - sc18is600.status = Sc18Is600ReadingI2CStat; - ReadI2CStatReg(); - } - break; - case Sc18Is600ReadRegister: - case Sc18Is600WriteRegister: - // should not happen - break; - default: - break; + case Sc18Is600Receive: + case Sc18Is600Transmit: + case Sc18Is600Transcieve: + if (sc18is600.status == Sc18Is600WaitingForI2C) { + sc18is600.status = Sc18Is600ReadingI2CStat; + ReadI2CStatReg(); + } + break; + case Sc18Is600ReadRegister: + case Sc18Is600WriteRegister: + // should not happen + break; + default: + break; } } -void dma1_c4_irq_handler(void) { +void dma1_c4_irq_handler(void) +{ - DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, DISABLE); - /* Disable SPI_2 Rx and TX request */ - SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Rx, DISABLE); - SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Tx, DISABLE); - /* Disable DMA1 Channel4 and 5 */ - DMA_Cmd(DMA1_Channel4, DISABLE); - DMA_Cmd(DMA1_Channel5, DISABLE); + DMA_ITConfig(DMA1_Channel4, DMA_IT_TC, DISABLE); + /* Disable SPI_2 Rx and TX request */ + SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Rx, DISABLE); + SPI_I2S_DMACmd(SPI2, SPI_I2S_DMAReq_Tx, DISABLE); + /* Disable DMA1 Channel4 and 5 */ + DMA_Cmd(DMA1_Channel4, DISABLE); + DMA_Cmd(DMA1_Channel5, DISABLE); - switch (sc18is600.transaction) { + switch (sc18is600.transaction) { case Sc18Is600ReadRegister: case Sc18Is600WriteRegister: sc18is600.status = Sc18Is600TransactionComplete; Sc18Is600Unselect(); break; case Sc18Is600Transmit: - if (sc18is600.status==Sc18Is600SendingRequest) { - sc18is600.status = Sc18Is600WaitingForI2C; - Sc18Is600Unselect(); - } - else if (sc18is600.status == Sc18Is600ReadingI2CStat) { - sc18is600.i2c_status = sc18is600.priv_rx_buf[2]; - sc18is600.status = Sc18Is600TransactionComplete; - Sc18Is600Unselect(); + if (sc18is600.status == Sc18Is600SendingRequest) { + sc18is600.status = Sc18Is600WaitingForI2C; + Sc18Is600Unselect(); + } else if (sc18is600.status == Sc18Is600ReadingI2CStat) { + sc18is600.i2c_status = sc18is600.priv_rx_buf[2]; + sc18is600.status = Sc18Is600TransactionComplete; + Sc18Is600Unselect(); } break; case Sc18Is600Receive: case Sc18Is600Transcieve: - if (sc18is600.status==Sc18Is600SendingRequest) { - sc18is600.status = Sc18Is600WaitingForI2C; - Sc18Is600Unselect(); - } - else if (sc18is600.status == Sc18Is600ReadingI2CStat) { - sc18is600.status = Sc18Is600ReadingBuffer; - sc18is600.priv_tx_buf[0] = Sc18Is600_Cmd_Read_Buffer; - // debug - for (int i=1; iBRR = GPIO_Pin_12 extern void dma1_c4_irq_handler(void); -static void ADS8344_read_channel( void ); +static void ADS8344_read_channel(void); -void imu_crista_arch_init(void) { +void imu_crista_arch_init(void) +{ channel = 0; /* Enable SPI2 Periph clock -------------------------------------------------*/ @@ -87,7 +88,8 @@ void imu_crista_arch_init(void) { } -void ADS8344_start( void ) { +void ADS8344_start(void) +{ ADS8344Select(); channel = 0; @@ -95,7 +97,8 @@ void ADS8344_start( void ) { } -static void ADS8344_read_channel( void ) { +static void ADS8344_read_channel(void) +{ // control byte buf_out[0] = 1 << 7 | channel << 4 | SGL_DIF << 2 | POWER_MODE; @@ -104,7 +107,7 @@ static void ADS8344_read_channel( void ) { /* SPI2_Rx_DMA_Channel configuration ------------------------------------*/ DMA_DeInit(DMA1_Channel4); DMA_InitTypeDef DMA_initStructure_4 = { - .DMA_PeripheralBaseAddr = (uint32_t)(SPI2_BASE+0x0C), + .DMA_PeripheralBaseAddr = (uint32_t)(SPI2_BASE + 0x0C), .DMA_MemoryBaseAddr = (uint32_t)buf_in, .DMA_DIR = DMA_DIR_PeripheralSRC, .DMA_BufferSize = 4, @@ -121,7 +124,7 @@ static void ADS8344_read_channel( void ) { /* SPI2_Tx_DMA_Channel configuration ------------------------------------*/ DMA_DeInit(DMA1_Channel5); DMA_InitTypeDef DMA_initStructure_5 = { - .DMA_PeripheralBaseAddr = (uint32_t)(SPI2_BASE+0x0C), + .DMA_PeripheralBaseAddr = (uint32_t)(SPI2_BASE + 0x0C), .DMA_MemoryBaseAddr = (uint32_t)buf_out, .DMA_DIR = DMA_DIR_PeripheralDST, .DMA_BufferSize = 4, @@ -151,7 +154,8 @@ static void ADS8344_read_channel( void ) { } -void dma1_c4_irq_handler(void) { +void dma1_c4_irq_handler(void) +{ ADS8344_values[channel] = (buf_in[1] << 8 | buf_in[2]) << 1 | buf_in[3] >> 7; channel++; @@ -165,8 +169,7 @@ void dma1_c4_irq_handler(void) { /* Disable DMA1 Channel4 and 5 */ DMA_Cmd(DMA1_Channel4, DISABLE); DMA_Cmd(DMA1_Channel5, DISABLE); - } - else { + } else { ADS8344_read_channel(); } } diff --git a/sw/airborne/arch/stm32/subsystems/imu/imu_crista_arch.h b/sw/airborne/arch/stm32/subsystems/imu/imu_crista_arch.h index 58496ac1d6..d698aceee3 100644 --- a/sw/airborne/arch/stm32/subsystems/imu/imu_crista_arch.h +++ b/sw/airborne/arch/stm32/subsystems/imu/imu_crista_arch.h @@ -23,11 +23,11 @@ #define IMU_CRISTA_ARCH_H -#define ImuCristaArchPeriodic() { \ - ADS8344_start(); \ +#define ImuCristaArchPeriodic() { \ + ADS8344_start(); \ } -extern void ADS8344_start( void ); +extern void ADS8344_start(void); #endif /* IMU_CRISTA_ARCH_H */ diff --git a/sw/airborne/arch/stm32/subsystems/imu/imu_krooz_sd_arch.c b/sw/airborne/arch/stm32/subsystems/imu/imu_krooz_sd_arch.c index a9ae2b91f5..24659b0a3c 100644 --- a/sw/airborne/arch/stm32/subsystems/imu/imu_krooz_sd_arch.c +++ b/sw/airborne/arch/stm32/subsystems/imu/imu_krooz_sd_arch.c @@ -7,7 +7,8 @@ #include "subsystems/imu/imu_krooz_sd_arch.h" -void imu_krooz_sd_arch_init(void) { +void imu_krooz_sd_arch_init(void) +{ rcc_periph_clock_enable(RCC_SYSCFG); rcc_periph_clock_enable(RCC_GPIOB); rcc_periph_clock_enable(RCC_GPIOB); @@ -24,13 +25,14 @@ void imu_krooz_sd_arch_init(void) { nvic_set_priority(NVIC_EXTI9_5_IRQ, 0x0F); } -void exti9_5_isr(void) { +void exti9_5_isr(void) +{ /* clear EXTI */ - if(EXTI_PR & EXTI6) { + if (EXTI_PR & EXTI6) { exti_reset_request(EXTI6); imu_krooz.hmc_eoc = TRUE; } - if(EXTI_PR & EXTI5) { + if (EXTI_PR & EXTI5) { exti_reset_request(EXTI5); imu_krooz.mpu_eoc = TRUE; } diff --git a/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c b/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c index 57ea6cfed6..f8c54736b2 100644 --- a/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c +++ b/sw/airborne/arch/stm32/subsystems/radio_control/spektrum_arch.c @@ -85,32 +85,32 @@ #define SecondaryUart(_x) _SecondaryUart(RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT, _x) struct SpektrumStateStruct { - uint8_t ReSync; - uint8_t SpektrumTimer; - uint8_t Sync; - uint8_t ChannelCnt; - uint8_t FrameCnt; - uint8_t HighByte; - uint8_t SecondFrame; - uint16_t LostFrameCnt; - uint8_t RcAvailable; - int16_t values[SPEKTRUM_CHANNELS_PER_FRAME*MAX_SPEKTRUM_FRAMES]; + uint8_t ReSync; + uint8_t SpektrumTimer; + uint8_t Sync; + uint8_t ChannelCnt; + uint8_t FrameCnt; + uint8_t HighByte; + uint8_t SecondFrame; + uint16_t LostFrameCnt; + uint8_t RcAvailable; + int16_t values[SPEKTRUM_CHANNELS_PER_FRAME *MAX_SPEKTRUM_FRAMES]; }; typedef struct SpektrumStateStruct SpektrumStateType; -SpektrumStateType PrimarySpektrumState = {1,0,0,0,0,0,0,0,0,{0}}; +SpektrumStateType PrimarySpektrumState = {1, 0, 0, 0, 0, 0, 0, 0, 0, {0}}; PRINT_CONFIG_VAR(RADIO_CONTROL_SPEKTRUM_PRIMARY_PORT) #ifdef RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT PRINT_CONFIG_MSG("Using secondary spektrum receiver.") PRINT_CONFIG_VAR(RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT) -SpektrumStateType SecondarySpektrumState = {1,0,0,0,0,0,0,0,0,{0}}; +SpektrumStateType SecondarySpektrumState = {1, 0, 0, 0, 0, 0, 0, 0, 0, {0}}; #else PRINT_CONFIG_MSG("NOT using secondary spektrum receiver.") #endif -int16_t SpektrumBuf[SPEKTRUM_CHANNELS_PER_FRAME*MAX_SPEKTRUM_FRAMES]; +int16_t SpektrumBuf[SPEKTRUM_CHANNELS_PER_FRAME *MAX_SPEKTRUM_FRAMES]; /* the order of the channels on a spektrum is always as follows : * * Throttle 0 @@ -153,12 +153,13 @@ void tim6_irq_handler(void); #define RC_SET_POLARITY gpio_clear #endif - /***************************************************************************** - * - * Initialise the timer an uarts used by the Spektrum receiver subsystem - * - *****************************************************************************/ -void radio_control_impl_init(void) { +/***************************************************************************** +* +* Initialise the timer an uarts used by the Spektrum receiver subsystem +* +*****************************************************************************/ +void radio_control_impl_init(void) +{ PrimarySpektrumState.ReSync = 1; @@ -277,20 +278,21 @@ void radio_control_impl_init(void) { * 100% transmitter-travel *****************************************************************************/ - /***************************************************************************** - * - * Spektrum Parser captures frame data by using time between frames to sync on - * - *****************************************************************************/ +/***************************************************************************** +* +* Spektrum Parser captures frame data by using time between frames to sync on +* +*****************************************************************************/ -static inline void SpektrumParser(uint8_t _c, SpektrumStateType* spektrum_state, bool_t secondary_receiver) { +static inline void SpektrumParser(uint8_t _c, SpektrumStateType *spektrum_state, bool_t secondary_receiver) +{ uint16_t ChannelData; uint8_t TimedOut; static uint8_t TmpEncType = 0; /* 0 = 10bit, 1 = 11 bit */ static uint8_t TmpExpFrames = 0; /* # of frames for channel data */ - TimedOut = (!spektrum_state->SpektrumTimer) ? 1 : 0; + TimedOut = (!spektrum_state->SpektrumTimer) ? 1 : 0; /* If we have just started the resync process or */ /* if we have recieved a character before our */ @@ -311,18 +313,19 @@ static inline void SpektrumParser(uint8_t _c, SpektrumStateType* spektrum_state, /* more than 7ms after the last received byte. */ /* It represents the number of lost frames so far.*/ if (spektrum_state->Sync == 0) { - spektrum_state->LostFrameCnt = _c; - if(secondary_receiver) /* secondary receiver */ - spektrum_state->LostFrameCnt = spektrum_state->LostFrameCnt << 8; - spektrum_state->Sync = 1; - spektrum_state->SpektrumTimer = MAX_BYTE_SPACE; - return; + spektrum_state->LostFrameCnt = _c; + if (secondary_receiver) { /* secondary receiver */ + spektrum_state->LostFrameCnt = spektrum_state->LostFrameCnt << 8; + } + spektrum_state->Sync = 1; + spektrum_state->SpektrumTimer = MAX_BYTE_SPACE; + return; } /* all other bytes should be recieved within */ /* MAX_BYTE_SPACE time of the last byte received */ /* otherwise something went wrong resynchronise */ - if(TimedOut) { + if (TimedOut) { spektrum_state->ReSync = 1; /* next frame not expected sooner than 7ms */ spektrum_state->SpektrumTimer = MIN_FRAME_SPACE; @@ -331,16 +334,16 @@ static inline void SpektrumParser(uint8_t _c, SpektrumStateType* spektrum_state, /* second character determines resolution and frame rate for main */ /* receiver or low byte of LostFrameCount for secondary receiver */ - if(spektrum_state->Sync == 1) { - if(secondary_receiver) { - spektrum_state->LostFrameCnt +=_c; + if (spektrum_state->Sync == 1) { + if (secondary_receiver) { + spektrum_state->LostFrameCnt += _c; TmpExpFrames = ExpectedFrames; } else { /** @todo collect more data. I suspect that there is a low res */ /* protocol that is still 10 bit but without using the full range. */ - TmpEncType =(_c & 0x10)>>4; /* 0 = 10bit, 1 = 11 bit */ + TmpEncType = (_c & 0x10) >> 4; /* 0 = 10bit, 1 = 11 bit */ TmpExpFrames = _c & 0x03; /* 1 = 1 frame contains all channels */ - /* 2 = 2 channel data in 2 frames */ + /* 2 = 2 channel data in 2 frames */ } spektrum_state->Sync = 2; spektrum_state->SpektrumTimer = MAX_BYTE_SPACE; @@ -350,7 +353,7 @@ static inline void SpektrumParser(uint8_t _c, SpektrumStateType* spektrum_state, /* high byte of channel data if this is the first byte */ /* of channel data and the most significant bit is set */ /* then this is the second frame of channel data. */ - if(spektrum_state->Sync == 2) { + if (spektrum_state->Sync == 2) { spektrum_state->HighByte = _c; if (spektrum_state->ChannelCnt == 0) { spektrum_state->SecondFrame = (spektrum_state->HighByte & 0x80) ? 1 : 0; @@ -361,28 +364,27 @@ static inline void SpektrumParser(uint8_t _c, SpektrumStateType* spektrum_state, } /* low byte of channel data */ - if(spektrum_state->Sync == 3) { + if (spektrum_state->Sync == 3) { spektrum_state->Sync = 2; spektrum_state->SpektrumTimer = MAX_BYTE_SPACE; /* we overwrite the buffer now so rc data is not available now */ spektrum_state->RcAvailable = 0; ChannelData = ((uint16_t)spektrum_state->HighByte << 8) | _c; spektrum_state->values[spektrum_state->ChannelCnt - + (spektrum_state->SecondFrame * 7)] = ChannelData; + + (spektrum_state->SecondFrame * 7)] = ChannelData; spektrum_state->ChannelCnt ++; } /* If we have a whole frame */ - if(spektrum_state->ChannelCnt >= SPEKTRUM_CHANNELS_PER_FRAME) { + if (spektrum_state->ChannelCnt >= SPEKTRUM_CHANNELS_PER_FRAME) { /* how many frames did we expect ? */ ++spektrum_state->FrameCnt; - if (spektrum_state->FrameCnt == TmpExpFrames) - { + if (spektrum_state->FrameCnt == TmpExpFrames) { /* set the rc_available_flag */ spektrum_state->RcAvailable = 1; spektrum_state->FrameCnt = 0; } - if(!secondary_receiver) { /* main receiver */ + if (!secondary_receiver) { /* main receiver */ EncodingType = TmpEncType; /* only update on a good */ ExpectedFrames = TmpExpFrames; /* main receiver frame */ } @@ -400,7 +402,8 @@ static inline void SpektrumParser(uint8_t _c, SpektrumStateType* spektrum_state, * *****************************************************************************/ -void RadioControlEventImp(void (*frame_handler)(void)) { +void RadioControlEventImp(void (*frame_handler)(void)) +{ uint8_t ChannelCnt; uint8_t ChannelNum; uint16_t ChannelData; @@ -427,40 +430,40 @@ void RadioControlEventImp(void (*frame_handler)(void)) { #else /* if we have one receiver and it has new data */ - if(PrimarySpektrumState.RcAvailable) { + if (PrimarySpektrumState.RcAvailable) { PrimarySpektrumState.RcAvailable = 0; #endif ChannelCnt = 0; /* for every piece of channel data we have received */ - for(int i = 0; (i < SPEKTRUM_CHANNELS_PER_FRAME * ExpectedFrames); i++) { + for (int i = 0; (i < SPEKTRUM_CHANNELS_PER_FRAME * ExpectedFrames); i++) { #ifndef RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT ChannelData = PrimarySpektrumState.values[i]; #else ChannelData = (!BestReceiver) ? PrimarySpektrumState.values[i] : - SecondarySpektrumState.values[i]; + SecondarySpektrumState.values[i]; #endif /* find out the channel number and its value by */ /* using the EncodingType which is only received */ /* from the main receiver */ - switch(EncodingType) { - case(0) : /* 10 bit */ + switch (EncodingType) { + case (0) : /* 10 bit */ ChannelNum = (ChannelData >> 10) & 0x0f; /* don't bother decoding unused channels */ if (ChannelNum < SPEKTRUM_NB_CHANNEL) { - SpektrumBuf[ChannelNum] = ChannelData & 0x3ff; - SpektrumBuf[ChannelNum] -= 0x200; - SpektrumBuf[ChannelNum] *= MAX_PPRZ/0x156; - ChannelCnt++; + SpektrumBuf[ChannelNum] = ChannelData & 0x3ff; + SpektrumBuf[ChannelNum] -= 0x200; + SpektrumBuf[ChannelNum] *= MAX_PPRZ / 0x156; + ChannelCnt++; } break; - case(1) : /* 11 bit */ + case (1) : /* 11 bit */ ChannelNum = (ChannelData >> 11) & 0x0f; /* don't bother decoding unused channels */ if (ChannelNum < SPEKTRUM_NB_CHANNEL) { SpektrumBuf[ChannelNum] = ChannelData & 0x7ff; SpektrumBuf[ChannelNum] -= 0x400; - SpektrumBuf[ChannelNum] *= MAX_PPRZ/0x2AC; + SpektrumBuf[ChannelNum] *= MAX_PPRZ / 0x2AC; ChannelCnt++; } break; @@ -468,8 +471,9 @@ void RadioControlEventImp(void (*frame_handler)(void)) { default : ChannelNum = 0x0F; break; /* never going to get here */ } /* store the value of the highest valid channel */ - if ((ChannelNum != 0x0F) && (ChannelNum > MaxChannelNum)) + if ((ChannelNum != 0x0F) && (ChannelNum > MaxChannelNum)) { MaxChannelNum = ChannelNum; + } } @@ -483,7 +487,7 @@ void RadioControlEventImp(void (*frame_handler)(void)) { */ for (int i = 0; i < Min(RADIO_CONTROL_NB_CHANNEL, (MaxChannelNum + 1)); i++) { radio_control.values[i] = SpektrumBuf[i]; - if (i == RADIO_THROTTLE ) { + if (i == RADIO_THROTTLE) { radio_control.values[i] += MAX_PPRZ; radio_control.values[i] /= 2; } @@ -501,7 +505,8 @@ void RadioControlEventImp(void (*frame_handler)(void)) { * timebase for SpektrumParser * *****************************************************************************/ -void SpektrumTimerInit( void ) { +void SpektrumTimerInit(void) +{ /* enable TIM6 clock */ rcc_periph_clock_enable(RCC_TIM6); @@ -509,7 +514,7 @@ void SpektrumTimerInit( void ) { /* TIM6 configuration, always counts up */ timer_set_mode(TIM6, TIM_CR1_CKD_CK_INT, 0, 0); /* 100 microseconds ie 0.1 millisecond */ - timer_set_period(TIM6, TIM_TICS_FOR_100us-1); + timer_set_period(TIM6, TIM_TICS_FOR_100us - 1); uint32_t tim6_clk = timer_get_frequency(TIM6); /* timer ticks with 1us */ timer_set_prescaler(TIM6, ((tim6_clk / ONE_MHZ) - 1)); @@ -538,18 +543,21 @@ void SpektrumTimerInit( void ) { * *****************************************************************************/ #ifdef STM32F1 -void tim6_isr( void ) { +void tim6_isr(void) +{ #elif defined STM32F4 -void tim6_dac_isr( void ) { +void tim6_dac_isr(void) { #endif timer_clear_flag(TIM6, TIM_SR_UIF); - if (PrimarySpektrumState.SpektrumTimer) + if (PrimarySpektrumState.SpektrumTimer) { --PrimarySpektrumState.SpektrumTimer; + } #ifdef RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT - if (SecondarySpektrumState.SpektrumTimer) + if (SecondarySpektrumState.SpektrumTimer) { --SecondarySpektrumState.SpektrumTimer; + } #endif } @@ -591,7 +599,7 @@ void SpektrumUartInit(void) { rcc_periph_clock_enable(SecondaryUart(_RCC)); /* Enable USART interrupts */ - nvic_set_priority(SecondaryUart(_IRQ), NVIC_PRIMARY_UART_PRIO+1); + nvic_set_priority(SecondaryUart(_IRQ), NVIC_PRIMARY_UART_PRIO + 1); nvic_enable_irq(SecondaryUart(_IRQ)); /* Init GPIOS */; @@ -702,8 +710,9 @@ void radio_control_spektrum_try_bind(void) { /* exit if the BIND_PIN is low, it needs to be pulled high at startup to initiate bind */ - if (gpio_get(SPEKTRUM_BIND_PIN_PORT, SPEKTRUM_BIND_PIN) == 0) + if (gpio_get(SPEKTRUM_BIND_PIN_PORT, SPEKTRUM_BIND_PIN) == 0) { return; + } #else /* Init GPIO for the bind pin, we enable the pullup resistor in case we have * a floating pin that does not have a hardware pullup resistor as it is the @@ -713,8 +722,9 @@ void radio_control_spektrum_try_bind(void) { /* exit if the BIND_PIN is high, it needs to be pulled low at startup to initiate bind */ - if (gpio_get(SPEKTRUM_BIND_PIN_PORT, SPEKTRUM_BIND_PIN) != 0) + if (gpio_get(SPEKTRUM_BIND_PIN_PORT, SPEKTRUM_BIND_PIN) != 0) { return; + } #endif /* Master receiver Rx push-pull */ @@ -735,8 +745,7 @@ void radio_control_spektrum_try_bind(void) { power up is. This works for the moment but will need revisiting */ sys_time_usleep(61000); - for (int i = 0; i < MASTER_RECEIVER_PULSES ; i++) - { + for (int i = 0; i < MASTER_RECEIVER_PULSES ; i++) { gpio_clear(SPEKTRUM_PRIMARY_BIND_CONF_PORT, SPEKTRUM_PRIMARY_BIND_CONF_PIN); sys_time_usleep(118); gpio_set(SPEKTRUM_PRIMARY_BIND_CONF_PORT, SPEKTRUM_PRIMARY_BIND_CONF_PIN); @@ -744,8 +753,7 @@ void radio_control_spektrum_try_bind(void) { } #ifdef RADIO_CONTROL_SPEKTRUM_SECONDARY_PORT - for (int i = 0; i < SLAVE_RECEIVER_PULSES; i++) - { + for (int i = 0; i < SLAVE_RECEIVER_PULSES; i++) { gpio_clear(SPEKTRUM_SECONDARY_BIND_CONF_PORT, SPEKTRUM_SECONDARY_BIND_CONF_PIN); sys_time_usleep(120); gpio_set(SPEKTRUM_SECONDARY_BIND_CONF_PORT, SPEKTRUM_SECONDARY_BIND_CONF_PIN); diff --git a/sw/airborne/arch/stm32/subsystems/settings_arch.c b/sw/airborne/arch/stm32/subsystems/settings_arch.c index eb4022132c..fb9a8ce2e2 100644 --- a/sw/airborne/arch/stm32/subsystems/settings_arch.c +++ b/sw/airborne/arch/stm32/subsystems/settings_arch.c @@ -39,19 +39,19 @@ #include struct FlashInfo { - uint32_t addr; - uint32_t total_size; - uint32_t page_nr; - uint32_t page_size; + uint32_t addr; + uint32_t total_size; + uint32_t page_nr; + uint32_t page_size; }; static uint32_t pflash_checksum(uint32_t ptr, uint32_t size); -static int32_t flash_detect(struct FlashInfo* flash); -static int32_t pflash_program_bytes(struct FlashInfo* flash, - uint32_t src, - uint32_t size, - uint32_t chksum); +static int32_t flash_detect(struct FlashInfo *flash); +static int32_t pflash_program_bytes(struct FlashInfo *flash, + uint32_t src, + uint32_t size, + uint32_t chksum); #if defined(STM32F1) #define FLASH_SIZE_ MMIO16(0x1FFFF7E0) @@ -64,7 +64,8 @@ static int32_t pflash_program_bytes(struct FlashInfo* flash, #define FCHK 4 -static uint32_t pflash_checksum(uint32_t ptr, uint32_t size) { +static uint32_t pflash_checksum(uint32_t ptr, uint32_t size) +{ uint32_t i; /* reset crc */ @@ -72,32 +73,32 @@ static uint32_t pflash_checksum(uint32_t ptr, uint32_t size) { if (ptr % 4) { /* calc in 8bit chunks */ - for (i=0; i<(size & ~3); i+=4) { - CRC_DR = (*(uint8_t*) (ptr+i)) | - (*(uint8_t*) (ptr+i+1)) << 8 | - (*(uint8_t*) (ptr+i+2)) << 16 | - (*(uint8_t*) (ptr+i+3)) << 24; + for (i = 0; i < (size & ~3); i += 4) { + CRC_DR = (*(uint8_t *)(ptr + i)) | + (*(uint8_t *)(ptr + i + 1)) << 8 | + (*(uint8_t *)(ptr + i + 2)) << 16 | + (*(uint8_t *)(ptr + i + 3)) << 24; } } else { /* calc in 32bit */ - for (i=0; i<(size & ~3); i+=4) { - CRC_DR = *(uint32_t*) (ptr+i); + for (i = 0; i < (size & ~3); i += 4) { + CRC_DR = *(uint32_t *)(ptr + i); } } /* remaining bytes */ switch (size % 4) { case 1: - CRC_DR = *(uint8_t*) (ptr+i); + CRC_DR = *(uint8_t *)(ptr + i); break; case 2: - CRC_DR = (*(uint8_t*) (ptr+i)) | - (*(uint8_t*) (ptr+i+1)) << 8; + CRC_DR = (*(uint8_t *)(ptr + i)) | + (*(uint8_t *)(ptr + i + 1)) << 8; break; case 3: - CRC_DR = (*(uint8_t*) (ptr+i)) | - (*(uint8_t*) (ptr+i+1)) << 8 | - (*(uint8_t*) (ptr+i+2)) << 16; + CRC_DR = (*(uint8_t *)(ptr + i)) | + (*(uint8_t *)(ptr + i + 1)) << 8 | + (*(uint8_t *)(ptr + i + 2)) << 16; break; default: break; @@ -106,7 +107,8 @@ static uint32_t pflash_checksum(uint32_t ptr, uint32_t size) { return CRC_DR; } -static int32_t flash_detect(struct FlashInfo* flash) { +static int32_t flash_detect(struct FlashInfo *flash) +{ flash->total_size = FLASH_SIZE_ * 0x400; @@ -115,23 +117,21 @@ static int32_t flash_detect(struct FlashInfo* flash) { device ID is only readable when freshly loaded through JTAG?! */ switch (flash->total_size) { - /* low density */ + /* low density */ case 0x00004000: /* 16 kBytes */ case 0x00008000: /* 32 kBytes */ - /* medium density, e.g. STM32F103RBT6 (Olimex STM32-H103) */ + /* medium density, e.g. STM32F103RBT6 (Olimex STM32-H103) */ case 0x00010000: /* 64 kBytes */ - case 0x00020000: /* 128 kBytes */ - { + case 0x00020000: { /* 128 kBytes */ flash->page_size = 0x400; break; } /* high density, e.g. STM32F103RE (Joby Lisa/M, Lisa/L) */ case 0x00040000: /* 256 kBytes */ case 0x00080000: /* 512 kBytes */ - /* XL density */ + /* XL density */ case 0x000C0000: /* 768 kBytes */ - case 0x00100000: /* 1 MByte */ - { + case 0x00100000: { /* 1 MByte */ flash->page_size = 0x800; break; } @@ -145,21 +145,19 @@ static int32_t flash_detect(struct FlashInfo* flash) { device_id = DBGMCU_IDCODE & DBGMCU_IDCODE_DEV_ID_MASK; switch (device_id) { - /* low density */ + /* low density */ case 0x412: - /* medium density, e.g. STM32F103RB (Olimex STM32-H103) */ - case 0x410: - { + /* medium density, e.g. STM32F103RB (Olimex STM32-H103) */ + case 0x410: { flash->page_size = 0x400; break; } /* high density, e.g. STM32F103RE (Joby Lisa/L) */ case 0x414: - /* XL density */ + /* XL density */ case 0x430: - /* connectivity line */ - case 0x418: - { + /* connectivity line */ + case 0x418: { flash->page_size = 0x800; break; } @@ -190,10 +188,11 @@ static int32_t flash_detect(struct FlashInfo* flash) { // $1 = {addr = 134739968, total_size = 524288, page_nr = 255, page_size = 2048} // 0x807F800 0x80000 #if defined(STM32F1) -static int32_t pflash_program_bytes(struct FlashInfo* flash, - uint32_t src, - uint32_t size, - uint32_t chksum) { +static int32_t pflash_program_bytes(struct FlashInfo *flash, + uint32_t src, + uint32_t size, + uint32_t chksum) +{ uint32_t i; /* erase */ @@ -202,55 +201,57 @@ static int32_t pflash_program_bytes(struct FlashInfo* flash, flash_lock(); /* verify erase */ - for (i=0; ipage_size; i+=4) { - if ((*(uint32_t*) (flash->addr + i)) != 0xFFFFFFFF) return -1; + for (i = 0; i < flash->page_size; i += 4) { + if ((*(uint32_t *)(flash->addr + i)) != 0xFFFFFFFF) { return -1; } } flash_unlock(); /* write full 16 bit words */ - for (i=0; i<(size & ~1); i+=2) { - flash_program_half_word(flash->addr+i, - (uint16_t)(*(uint8_t*)(src+i) | (*(uint8_t*)(src+i+1)) << 8)); + for (i = 0; i < (size & ~1); i += 2) { + flash_program_half_word(flash->addr + i, + (uint16_t)(*(uint8_t *)(src + i) | (*(uint8_t *)(src + i + 1)) << 8)); } /* fill bytes with a zero */ if (size & 1) { - flash_program_half_word(flash->addr+i, (uint16_t)(*(uint8_t*)(src+i))); + flash_program_half_word(flash->addr + i, (uint16_t)(*(uint8_t *)(src + i))); } /* write size */ - flash_program_half_word(flash->addr+flash->page_size-FSIZ, + flash_program_half_word(flash->addr + flash->page_size - FSIZ, (uint16_t)(size & 0xFFFF)); - flash_program_half_word(flash->addr+flash->page_size-FSIZ+2, + flash_program_half_word(flash->addr + flash->page_size - FSIZ + 2, (uint16_t)((size >> 16) & 0xFFFF)); /* write checksum */ - flash_program_half_word(flash->addr+flash->page_size-FCHK, + flash_program_half_word(flash->addr + flash->page_size - FCHK, (uint16_t)(chksum & 0xFFFF)); - flash_program_half_word(flash->addr+flash->page_size-FCHK+2, + flash_program_half_word(flash->addr + flash->page_size - FCHK + 2, (uint16_t)((chksum >> 16) & 0xFFFF)); flash_lock(); /* verify data */ - for (i=0; iaddr+i)) != (*(uint8_t*) (src+i))) return -2; + for (i = 0; i < size; i++) { + if ((*(uint8_t *)(flash->addr + i)) != (*(uint8_t *)(src + i))) { return -2; } } - if (*(uint32_t*) (flash->addr+flash->page_size-FSIZ) != size) return -3; - if (*(uint32_t*) (flash->addr+flash->page_size-FCHK) != chksum) return -4; + if (*(uint32_t *)(flash->addr + flash->page_size - FSIZ) != size) { return -3; } + if (*(uint32_t *)(flash->addr + flash->page_size - FCHK) != chksum) { return -4; } return 0; } #elif defined(STM32F4) -static int32_t pflash_program_bytes(struct FlashInfo* flash __attribute__((unused)), +static int32_t pflash_program_bytes(struct FlashInfo *flash __attribute__((unused)), uint32_t src __attribute__((unused)), uint32_t size __attribute__((unused)), - uint32_t chksum __attribute__((unused))) { + uint32_t chksum __attribute__((unused))) +{ return -1; } #endif -int32_t persistent_write(uint32_t ptr, uint32_t size) { +int32_t persistent_write(uint32_t ptr, uint32_t size) +{ struct FlashInfo flash_info; - if (flash_detect(&flash_info)) return -1; - if ((size > flash_info.page_size-FSIZ) || (size == 0)) return -2; + if (flash_detect(&flash_info)) { return -1; } + if ((size > flash_info.page_size - FSIZ) || (size == 0)) { return -2; } return pflash_program_bytes(&flash_info, ptr, @@ -258,23 +259,25 @@ int32_t persistent_write(uint32_t ptr, uint32_t size) { pflash_checksum(ptr, size)); } -int32_t persistent_read(uint32_t ptr, uint32_t size) { +int32_t persistent_read(uint32_t ptr, uint32_t size) +{ struct FlashInfo flash; uint32_t i; /* check parameters */ - if (flash_detect(&flash)) return -1; - if ((size > flash.page_size-FSIZ) || (size == 0)) return -2; + if (flash_detect(&flash)) { return -1; } + if ((size > flash.page_size - FSIZ) || (size == 0)) { return -2; } /* check consistency */ - if (size != *(uint32_t*)(flash.addr+flash.page_size-FSIZ)) return -3; + if (size != *(uint32_t *)(flash.addr + flash.page_size - FSIZ)) { return -3; } if (pflash_checksum(flash.addr, size) != - *(uint32_t*)(flash.addr+flash.page_size-FCHK)) + *(uint32_t *)(flash.addr + flash.page_size - FCHK)) { return -4; + } /* copy data */ - for (i=0; i> 8) & 0x00FF) ) -#define MyByteSwap32(n) \ - ( ((((uint32_t) n) << 24) & 0xFF000000) | \ - ((((uint32_t) n) << 8) & 0x00FF0000) | \ - ((((uint32_t) n) >> 8) & 0x0000FF00) | \ +#define MyByteSwap32(n) \ + ( ((((uint32_t) n) << 24) & 0xFF000000) | \ + ((((uint32_t) n) << 8) & 0x00FF0000) | \ + ((((uint32_t) n) >> 8) & 0x0000FF00) | \ ((((uint32_t) n) >> 24) & 0x000000FF) ) #define MyByteSwap32_1(in, out) \ - asm volatile ( \ - "rev %0, %1\n\t" \ - : "=r" (out) \ - : "r"(in) \ -); + asm volatile ( \ + "rev %0, %1\n\t" \ + : "=r" (out) \ + : "r"(in) \ + ); #define MyByteSwap16_1(in, out) \ - asm volatile ( \ - "rev16 %0, %1\n\t" \ - : "=r" (out) \ - : "r"(in) \ -); + asm volatile ( \ + "rev16 %0, %1\n\t" \ + : "=r" (out) \ + : "r"(in) \ + ); -int main(void) { +int main(void) +{ // uint32_t foo = 0; // uint32_t bar = __builtin_bswap32(*(uint32_t*)&foo); @@ -43,14 +44,14 @@ int main(void) { uint16_t foo = 12345; uint16_t bar = MyByteSwap16(foo); - MyByteSwap16_1(foo,bar); + MyByteSwap16_1(foo, bar); bar = __REV16(foo); uint32_t a = 23456; uint32_t b = MyByteSwap32(a); - MyByteSwap32_1(a,b); + MyByteSwap32_1(a, b); return 0; } diff --git a/sw/airborne/arch/stm32/uart_tunnel.c b/sw/airborne/arch/stm32/uart_tunnel.c index e0151b3b56..7c08f142bf 100644 --- a/sw/airborne/arch/stm32/uart_tunnel.c +++ b/sw/airborne/arch/stm32/uart_tunnel.c @@ -41,18 +41,20 @@ #define B_TX_PIN GPIO2 #define B_TX_PORT B_PORT -static inline void main_periodic( void ); -static inline void main_event( void ); +static inline void main_periodic(void); +static inline void main_event(void); void Delay(volatile uint32_t nCount); -void Delay(volatile uint32_t nCount) { - for(; nCount != 0; nCount--); +void Delay(volatile uint32_t nCount) +{ + for (; nCount != 0; nCount--); } -int main(void) { +int main(void) +{ mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); /* init RCC */ rcc_periph_clock_enable(RCC_GPIOA); @@ -60,23 +62,24 @@ 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); /* */ while (1) { - if (sys_time_check_and_ack_timer(0)) + if (sys_time_check_and_ack_timer(0)) { main_periodic(); + } main_event(); } @@ -85,42 +88,46 @@ int main(void) { -static inline void main_periodic( void ) { +static inline void main_periodic(void) +{ LED_PERIODIC(); } -static inline void main_event( void ) { +static inline void main_event(void) +{ // Delay(2000); static uint8_t foo = 0; foo++; #if 0 - if (!(foo%2)) + if (!(foo % 2)) { gpio_set(B_TX_PORT, B_TX_PIN); - else + } else { gpio_clear(B_TX_PORT, B_TX_PIN); + } #endif #if 0 - if (!(foo%2)) + if (!(foo % 2)) { gpio_clear(A_TX_PORT, A_TX_PIN); - else + } else { gpio_set(A_TX_PORT, A_TX_PIN); + } #endif #if 1 /* passthrough B_RX to A_TX */ - if (GPIO_IDR(B_RX_PORT) & B_RX_PIN) + if (GPIO_IDR(B_RX_PORT) & B_RX_PIN) { gpio_set(A_TX_PORT, A_TX_PIN); - else + } else { gpio_clear(A_TX_PORT, A_TX_PIN); + } #endif /* passthrough A_RX to B_TX */ if (gpio_get(A_RX_PORT, A_RX_PIN)) { gpio_set(B_TX_PORT, B_TX_PIN); LED_ON(2); - } - else { + } else { gpio_clear(B_TX_PORT, B_TX_PIN); LED_OFF(2); } diff --git a/sw/airborne/arch/stm32/usb_ser_hw.c b/sw/airborne/arch/stm32/usb_ser_hw.c index cefe51d70a..bfbfae9911 100644 --- a/sw/airborne/arch/stm32/usb_ser_hw.c +++ b/sw/airborne/arch/stm32/usb_ser_hw.c @@ -475,7 +475,7 @@ void VCOM_send_message(void) if (!fifo_get(&txfifo, &buf[i])) { break; } - } + } usbd_ep_write_packet(my_usbd_dev, 0x82, buf, i); } } diff --git a/sw/airborne/boards/apogee/baro_board.c b/sw/airborne/boards/apogee/baro_board.c index 1e4895277a..84c3df4e42 100644 --- a/sw/airborne/boards/apogee/baro_board.c +++ b/sw/airborne/boards/apogee/baro_board.c @@ -1,24 +1,24 @@ - /* - * Copyright (C) 2013 Gautier Hattenberger (ENAC) - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ +/* +* Copyright (C) 2013 Gautier Hattenberger (ENAC) +* +* This file is part of paparazzi. +* +* paparazzi is free software; you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation; either version 2, or (at your option) +* any later version. +* +* paparazzi is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with paparazzi; see the file COPYING. If not, write to +* the Free Software Foundation, 59 Temple Place - Suite 330, +* Boston, MA 02111-1307, USA. +* +*/ /** * @file boards/apogee/baro_board.c @@ -65,7 +65,8 @@ uint16_t startup_cnt; struct Mpl3115 apogee_baro; -void baro_init( void ) { +void baro_init(void) +{ mpl3115_init(&apogee_baro, &i2c1, MPL3115_I2C_ADDR); #ifdef BARO_LED LED_OFF(BARO_LED); @@ -73,7 +74,8 @@ void baro_init( void ) { startup_cnt = BARO_STARTUP_COUNTER; } -void baro_periodic( void ) { +void baro_periodic(void) +{ // Baro is slave of the MPU, only start reading it after MPU is configured if (imu_apogee.mpu.config.initialized) { @@ -94,10 +96,11 @@ void baro_periodic( void ) { } } -void apogee_baro_event(void) { +void apogee_baro_event(void) +{ mpl3115_event(&apogee_baro); if (apogee_baro.data_available && startup_cnt == 0) { - float pressure = ((float)apogee_baro.pressure/(1<<2)); + float pressure = ((float)apogee_baro.pressure / (1 << 2)); AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); float temp = apogee_baro.temperature / 16.0f; AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, &temp); diff --git a/sw/airborne/boards/apogee/imu_apogee.c b/sw/airborne/boards/apogee/imu_apogee.c index 47f004fc29..a50293e683 100644 --- a/sw/airborne/boards/apogee/imu_apogee.c +++ b/sw/airborne/boards/apogee/imu_apogee.c @@ -59,9 +59,10 @@ PRINT_CONFIG_VAR(APOGEE_ACCEL_RANGE) struct ImuApogee imu_apogee; // baro config will be done later in bypass mode -bool_t configure_baro_slave(Mpu60x0ConfigSet mpu_set, void* mpu); +bool_t configure_baro_slave(Mpu60x0ConfigSet mpu_set, void *mpu); -bool_t configure_baro_slave(Mpu60x0ConfigSet mpu_set __attribute__ ((unused)), void* mpu __attribute__ ((unused))) { +bool_t configure_baro_slave(Mpu60x0ConfigSet mpu_set __attribute__((unused)), void *mpu __attribute__((unused))) +{ return TRUE; } @@ -84,7 +85,7 @@ void imu_impl_init(void) imu_apogee.acc_valid = FALSE; } -void imu_periodic( void ) +void imu_periodic(void) { // Start reading the latest gyroscope data mpu60x0_i2c_periodic(&imu_apogee.mpu); @@ -92,20 +93,24 @@ void imu_periodic( void ) //RunOnceEvery(10,imu_apogee_downlink_raw()); } -void imu_apogee_downlink_raw( void ) +void imu_apogee_downlink_raw(void) { - DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice,&imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r); - DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice,&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z); + DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, + &imu.gyro_unscaled.r); + DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, &imu.accel_unscaled.x, &imu.accel_unscaled.y, + &imu.accel_unscaled.z); } -void imu_apogee_event( void ) +void imu_apogee_event(void) { // If the itg3200 I2C transaction has succeeded: convert the data mpu60x0_i2c_event(&imu_apogee.mpu); if (imu_apogee.mpu.data_available) { - RATES_ASSIGN(imu.gyro_unscaled, imu_apogee.mpu.data_rates.rates.p, -imu_apogee.mpu.data_rates.rates.q, -imu_apogee.mpu.data_rates.rates.r); - VECT3_ASSIGN(imu.accel_unscaled, imu_apogee.mpu.data_accel.vect.x, -imu_apogee.mpu.data_accel.vect.y, -imu_apogee.mpu.data_accel.vect.z); + RATES_ASSIGN(imu.gyro_unscaled, imu_apogee.mpu.data_rates.rates.p, -imu_apogee.mpu.data_rates.rates.q, + -imu_apogee.mpu.data_rates.rates.r); + VECT3_ASSIGN(imu.accel_unscaled, imu_apogee.mpu.data_accel.vect.x, -imu_apogee.mpu.data_accel.vect.y, + -imu_apogee.mpu.data_accel.vect.z); imu_apogee.mpu.data_available = FALSE; imu_apogee.gyr_valid = TRUE; imu_apogee.acc_valid = TRUE; diff --git a/sw/airborne/boards/apogee/imu_apogee.h b/sw/airborne/boards/apogee/imu_apogee.h index 0c49a81e34..8dfb1ea6f4 100644 --- a/sw/airborne/boards/apogee/imu_apogee.h +++ b/sw/airborne/boards/apogee/imu_apogee.h @@ -109,11 +109,13 @@ extern void imu_periodic(void); */ /* Own Extra Functions */ -extern void imu_apogee_event( void ); -extern void imu_apogee_downlink_raw( void ); +extern void imu_apogee_event(void); +extern void imu_apogee_downlink_raw(void); -static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void) __attribute__((unused))) { +static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), + void (* _mag_handler)(void) __attribute__((unused))) +{ imu_apogee_event(); if (imu_apogee.gyr_valid) { imu_apogee.gyr_valid = FALSE; diff --git a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c index b93ee319a9..58731fdb49 100644 --- a/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c +++ b/sw/airborne/boards/ardrone/actuators_ardrone2_raw.c @@ -55,25 +55,25 @@ */ int actuator_ardrone2_raw_fd; /**< File descriptor for the port */ -#define ARDRONE_GPIO_PORT 0x32524 +#define ARDRONE_GPIO_PORT 0x32524 -#define ARDRONE_GPIO_PIN_MOTOR1 171 -#define ARDRONE_GPIO_PIN_MOTOR2 172 -#define ARDRONE_GPIO_PIN_MOTOR3 173 -#define ARDRONE_GPIO_PIN_MOTOR4 174 +#define ARDRONE_GPIO_PIN_MOTOR1 171 +#define ARDRONE_GPIO_PIN_MOTOR2 172 +#define ARDRONE_GPIO_PIN_MOTOR3 173 +#define ARDRONE_GPIO_PIN_MOTOR4 174 -#define ARDRONE_GPIO_PIN_IRQ_FLIPFLOP 175 -#define ARDRONE_GPIO_PIN_IRQ_INPUT 176 +#define ARDRONE_GPIO_PIN_IRQ_FLIPFLOP 175 +#define ARDRONE_GPIO_PIN_IRQ_INPUT 176 uint32_t led_hw_values; static inline void actuators_ardrone_reset_flipflop(void) { - gpio_setup_output(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); - gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); + gpio_setup_output(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); + gpio_clear(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); int32_t stop = sys_time.nb_sec + 2; while (sys_time.nb_sec < stop); - gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); + gpio_set(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); } @@ -84,8 +84,7 @@ void actuators_ardrone_init(void) //open mot port actuator_ardrone2_raw_fd = open("/dev/ttyO0", O_RDWR | O_NOCTTY | O_NDELAY); - if (actuator_ardrone2_raw_fd == -1) - { + if (actuator_ardrone2_raw_fd == -1) { perror("open_port: Unable to open /dev/ttyO0 - "); return; } @@ -102,64 +101,63 @@ void actuators_ardrone_init(void) options.c_cflag |= (CLOCAL | CREAD); //Enable the receiver and set local mode options.c_iflag = 0; //clear input options - options.c_lflag=0; //clear local options + options.c_lflag = 0; //clear local options options.c_oflag &= ~OPOST; //clear output options (raw output) //Set the new options for the port tcsetattr(actuator_ardrone2_raw_fd, TCSANOW, &options); //reset IRQ flipflop - on error 106 read 1, this code resets 106 to 0 - gpio_setup_input(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_INPUT); + gpio_setup_input(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_IRQ_INPUT); actuators_ardrone_reset_flipflop(); //all select lines active - gpio_setup_output(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR1); - gpio_setup_output(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR2); - gpio_setup_output(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR3); - gpio_setup_output(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR4); - gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR1); - gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR2); - gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR3); - gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR4); + gpio_setup_output(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_MOTOR1); + gpio_setup_output(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_MOTOR2); + gpio_setup_output(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_MOTOR3); + gpio_setup_output(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_MOTOR4); + gpio_set(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_MOTOR1); + gpio_set(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_MOTOR2); + gpio_set(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_MOTOR3); + gpio_set(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_MOTOR4); //configure motors uint8_t reply[256]; - for(int m=0;m<4;m++) { - gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR1 + m); - actuators_ardrone_cmd(0xe0,reply,2); - if(reply[0]!=0xe0 || reply[1]!=0x00) - { - printf("motor%d cmd=0x%02x reply=0x%02x\n",m+1,(int)reply[0],(int)reply[1]); + for (int m = 0; m < 4; m++) { + gpio_clear(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_MOTOR1 + m); + actuators_ardrone_cmd(0xe0, reply, 2); + if (reply[0] != 0xe0 || reply[1] != 0x00) { + printf("motor%d cmd=0x%02x reply=0x%02x\n", m + 1, (int)reply[0], (int)reply[1]); } - actuators_ardrone_cmd(m+1,reply,1); - gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR1 + m); + actuators_ardrone_cmd(m + 1, reply, 1); + gpio_set(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_MOTOR1 + m); } //all select lines active - gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR1); - gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR2); - gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR3); - gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_MOTOR4); + gpio_clear(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_MOTOR1); + gpio_clear(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_MOTOR2); + gpio_clear(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_MOTOR3); + gpio_clear(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_MOTOR4); //start multicast - actuators_ardrone_cmd(0xa0,reply,1); - actuators_ardrone_cmd(0xa0,reply,1); - actuators_ardrone_cmd(0xa0,reply,1); - actuators_ardrone_cmd(0xa0,reply,1); - actuators_ardrone_cmd(0xa0,reply,1); + actuators_ardrone_cmd(0xa0, reply, 1); + actuators_ardrone_cmd(0xa0, reply, 1); + actuators_ardrone_cmd(0xa0, reply, 1); + actuators_ardrone_cmd(0xa0, reply, 1); + actuators_ardrone_cmd(0xa0, reply, 1); //reset IRQ flipflop - on error 176 reads 1, this code resets 176 to 0 - gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); - gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); + gpio_clear(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); + gpio_set(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); // Left Red, Right Green - actuators_ardrone_set_leds(MOT_LEDRED,MOT_LEDGREEN, MOT_LEDGREEN, MOT_LEDRED); + actuators_ardrone_set_leds(MOT_LEDRED, MOT_LEDGREEN, MOT_LEDGREEN, MOT_LEDRED); } -int actuators_ardrone_cmd(uint8_t cmd, uint8_t *reply, int replylen) { - if (full_write(actuator_ardrone2_raw_fd, &cmd, 1) < 0) - { +int actuators_ardrone_cmd(uint8_t cmd, uint8_t *reply, int replylen) +{ + if (full_write(actuator_ardrone2_raw_fd, &cmd, 1) < 0) { perror("actuators_ardrone_cmd: write failed"); return -1; } @@ -175,36 +173,27 @@ void actuators_ardrone_motor_status(void) // Reset Flipflop sequence static bool_t reset_flipflop_counter = 0; - if (reset_flipflop_counter > 0) - { + if (reset_flipflop_counter > 0) { reset_flipflop_counter--; - if (reset_flipflop_counter == 10) - { + if (reset_flipflop_counter == 10) { // Reset flipflop - gpio_setup_output(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); - gpio_clear(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); - } - else if (reset_flipflop_counter == 1) - { + gpio_setup_output(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); + gpio_clear(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); + } else if (reset_flipflop_counter == 1) { // Listen to IRQ again - gpio_set(ARDRONE_GPIO_PORT,ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); + gpio_set(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_IRQ_FLIPFLOP); } return; } // If a motor IRQ line is set - if (gpio_get(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_IRQ_INPUT) == 1) - { - if (autopilot_motors_on) - { - if (last_motor_on) - { + if (gpio_get(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_IRQ_INPUT) == 1) { + if (autopilot_motors_on) { + if (last_motor_on) { // Tell paparazzi that one motor has stalled autopilot_set_motors_on(FALSE); - } - else - { + } else { // Toggle Flipflop reset so motors can be re-enabled reset_flipflop_counter = 20; } @@ -221,17 +210,18 @@ 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) - { + 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) ); + 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) { - actuators_ardrone_set_pwm(actuators_pwm_values[0], actuators_pwm_values[1], actuators_pwm_values[2], actuators_pwm_values[3]); - RunOnceEvery(100,actuators_ardrone_motor_status()); + actuators_ardrone_set_pwm(actuators_pwm_values[0], actuators_pwm_values[1], actuators_pwm_values[2], + actuators_pwm_values[3]); + RunOnceEvery(100, actuators_ardrone_motor_status()); } /** @@ -241,13 +231,13 @@ void actuators_ardrone_commit(void) void actuators_ardrone_set_pwm(uint16_t pwm0, uint16_t pwm1, uint16_t pwm2, uint16_t pwm3) { uint8_t cmd[5]; - cmd[0] = 0x20 | ((pwm0&0x1ff)>>4); - cmd[1] = ((pwm0&0x1ff)<<4) | ((pwm1&0x1ff)>>5); - cmd[2] = ((pwm1&0x1ff)<<3) | ((pwm2&0x1ff)>>6); - cmd[3] = ((pwm2&0x1ff)<<2) | ((pwm3&0x1ff)>>7); - cmd[4] = ((pwm3&0x1ff)<<1); + cmd[0] = 0x20 | ((pwm0 & 0x1ff) >> 4); + cmd[1] = ((pwm0 & 0x1ff) << 4) | ((pwm1 & 0x1ff) >> 5); + cmd[2] = ((pwm1 & 0x1ff) << 3) | ((pwm2 & 0x1ff) >> 6); + cmd[3] = ((pwm2 & 0x1ff) << 2) | ((pwm3 & 0x1ff) >> 7); + cmd[4] = ((pwm3 & 0x1ff) << 1); full_write(actuator_ardrone2_raw_fd, cmd, 5); - RunOnceEvery(20,actuators_ardrone_led_run()); + RunOnceEvery(20, actuators_ardrone_led_run()); } /** @@ -255,10 +245,10 @@ void actuators_ardrone_set_pwm(uint16_t pwm0, uint16_t pwm1, uint16_t pwm2, uint * cmd = 011rrrr0 000gggg0 (this is ardrone1 format, we need ardrone2 format) * * - * led0 = RearLeft - * led1 = RearRight - * led2 = FrontRight - * led3 = FrontLeft + * led0 = RearLeft + * led1 = RearRight + * led2 = FrontRight + * led3 = FrontLeft */ void actuators_ardrone_set_leds(uint8_t led0, uint8_t led1, uint8_t led2, uint8_t led3) @@ -272,8 +262,8 @@ void actuators_ardrone_set_leds(uint8_t led0, uint8_t led1, uint8_t led2, uint8_ //printf("LEDS: %d %d %d %d \n", led0, led1, led2, led3); - cmd[0]=0x60 | ((led0&1)<<4) | ((led1&1)<<3) | ((led2&1)<<2) | ((led3&1) <<1); - cmd[1]=((led0&2)<<3) | ((led1&2)<<2) | ((led2&2)<<1) | ((led3&2)<<0); + cmd[0] = 0x60 | ((led0 & 1) << 4) | ((led1 & 1) << 3) | ((led2 & 1) << 2) | ((led3 & 1) << 1); + cmd[1] = ((led0 & 2) << 3) | ((led1 & 2) << 2) | ((led2 & 2) << 1) | ((led3 & 2) << 0); full_write(actuator_ardrone2_raw_fd, cmd, 2); } diff --git a/sw/airborne/boards/ardrone/actuators_at.c b/sw/airborne/boards/ardrone/actuators_at.c index 380a919a53..060b8f8c92 100644 --- a/sw/airborne/boards/ardrone/actuators_at.c +++ b/sw/airborne/boards/ardrone/actuators_at.c @@ -29,34 +29,44 @@ #include "generated/airframe.h" #include "boards/ardrone/at_com.h" -void actuators_init(void) { +void actuators_init(void) +{ init_at_com(); } -void actuators_set(pprz_t commands[]) { +void actuators_set(pprz_t commands[]) +{ //Calculate the thrus, roll, pitch and yaw from the PPRZ commands - float thrust = ((float)(commands[COMMAND_THRUST]-MAX_PPRZ/2) / (float)MAX_PPRZ)*2.0f; + float thrust = ((float)(commands[COMMAND_THRUST] - MAX_PPRZ / 2) / (float)MAX_PPRZ) * 2.0f; float roll = ((float)commands[COMMAND_ROLL] / (float)MAX_PPRZ); float pitch = ((float)commands[COMMAND_PITCH] / (float)MAX_PPRZ); float yaw = ((float)commands[COMMAND_YAW] / (float)MAX_PPRZ); //Starting engine - if(thrust > 0 && (ahrs_impl.control_state == CTRL_DEFAULT || ahrs_impl.control_state == CTRL_INIT || ahrs_impl.control_state == CTRL_LANDED)) + if (thrust > 0 && (ahrs_impl.control_state == CTRL_DEFAULT || ahrs_impl.control_state == CTRL_INIT + || ahrs_impl.control_state == CTRL_LANDED)) { at_com_send_ref(REF_TAKEOFF); + } //Check emergency or stop engine - if((ahrs_impl.state & ARDRONE_EMERGENCY_MASK) != 0) + if ((ahrs_impl.state & ARDRONE_EMERGENCY_MASK) != 0) { at_com_send_ref(REF_EMERGENCY); - else if(thrust < -0.9 && !(ahrs_impl.control_state == CTRL_DEFAULT || ahrs_impl.control_state == CTRL_INIT || ahrs_impl.control_state == CTRL_LANDED)) + } else if (thrust < -0.9 && !(ahrs_impl.control_state == CTRL_DEFAULT || ahrs_impl.control_state == CTRL_INIT + || ahrs_impl.control_state == CTRL_LANDED)) { at_com_send_ref(0); + } //Calibration - if((ahrs_impl.state & ARDRONE_MAGNETO_NEEDS_CALIB) != 0 && (ahrs_impl.control_state == CTRL_FLYING || ahrs_impl.control_state == CTRL_HOVERING)) + if ((ahrs_impl.state & ARDRONE_MAGNETO_NEEDS_CALIB) != 0 && (ahrs_impl.control_state == CTRL_FLYING + || ahrs_impl.control_state == CTRL_HOVERING)) { at_com_send_calib(0); + } //Moving - if((ahrs_impl.state & ARDRONE_MAGNETO_NEEDS_CALIB) == 0 && (ahrs_impl.control_state == CTRL_FLYING || ahrs_impl.control_state == CTRL_HOVERING)) + if ((ahrs_impl.state & ARDRONE_MAGNETO_NEEDS_CALIB) == 0 && (ahrs_impl.control_state == CTRL_FLYING + || ahrs_impl.control_state == CTRL_HOVERING)) { at_com_send_pcmd(1, thrust, roll, pitch, yaw); + } //Keep alive (FIXME) at_com_send_config("general:navdata_demo", "FALSE"); diff --git a/sw/airborne/boards/ardrone/at_com.c b/sw/airborne/boards/ardrone/at_com.c index 75112aae5f..188a216ba9 100644 --- a/sw/airborne/boards/ardrone/at_com.c +++ b/sw/airborne/boards/ardrone/at_com.c @@ -41,21 +41,23 @@ int at_socket = -1, //AT socket connection navdata_socket = -1; //Navdata socket connection struct sockaddr_in pc_addr, //Own pc address - drone_at, //Drone AT address - drone_nav, //Drone nav address - from; //From address + drone_at, //Drone AT address + drone_nav, //Drone nav address + from; //From address bool_t at_com_ready = FALSE; //Status of the at communication char sessionId[9]; //THe config session ID -void at_com_send(char* command); +void at_com_send(char *command); void init_at_config(void); //Init the at_com -void init_at_com(void) { +void init_at_com(void) +{ //Check if already initialized - if (at_com_ready) + if (at_com_ready) { return; + } //Create the at and navdata socket if ((at_socket = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { @@ -88,7 +90,7 @@ void init_at_com(void) { //Set unicast mode on int one = 1; sendto(navdata_socket, &one, 4, 0, (struct sockaddr *) &drone_nav, - sizeof(drone_nav)); + sizeof(drone_nav)); //Init at config init_at_config(); @@ -98,7 +100,8 @@ void init_at_com(void) { } //Init the at config -void init_at_config(void) { +void init_at_config(void) +{ //Generate a session id uint32_t binaryId = (uint32_t) rand(); binaryId = (0 != binaryId) ? binaryId : 1u; @@ -120,54 +123,59 @@ void init_at_config(void) { #ifndef ARDRONE_FLIGHT_INDOOR at_com_send_config("control:outdoor", "TRUE"); #else - at_com_send_config("control:outdoor","FALSE"); + at_com_send_config("control:outdoor", "FALSE"); #endif #ifndef ARDRONE_WITHOUT_SHELL at_com_send_config("control:flight_without_shell", "FALSE"); #else - at_com_send_config("control:flight_without_shell","TRUE"); + at_com_send_config("control:flight_without_shell", "TRUE"); #endif #ifdef ARDRONE_OWNER_MAC - at_com_send_config("network:owner_mac",ARDRONE_OWNER_MAC); + at_com_send_config("network:owner_mac", ARDRONE_OWNER_MAC); #endif } //Recieve a navdata packet -int at_com_recieve_navdata(unsigned char* buffer) { +int at_com_recieve_navdata(unsigned char *buffer) +{ int l = sizeof(from); 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; } //Send an AT command -void at_com_send(char* command) { - sendto(at_socket, command, strlen(command), 0, (struct sockaddr*) &drone_at, - sizeof(drone_at)); +void at_com_send(char *command) +{ + sendto(at_socket, command, strlen(command), 0, (struct sockaddr *) &drone_at, + sizeof(drone_at)); } //Send a Config -void at_com_send_config(char* key, char* value) { +void at_com_send_config(char *key, char *value) +{ char command[256]; sprintf(command, "AT*CONFIG_IDS=%d,\"%s\",\"2BF07F58\",\"9D7BFD45\"\r", - packet_seq++, sessionId); + packet_seq++, sessionId); at_com_send(command); sprintf(command, "AT*CONFIG=%d,\"%s\",\"%s\"\r", packet_seq++, key, value); at_com_send(command); } //Send a Flat trim -void at_com_send_ftrim(void) { +void at_com_send_ftrim(void) +{ char command[256]; sprintf(command, "AT*FTRIM=%d\r", packet_seq++); at_com_send(command); } //Send a Ref -void at_com_send_ref(int bits) { +void at_com_send_ref(int bits) +{ char command[256]; sprintf(command, "AT*REF=%d,%d\r", packet_seq++, bits | REF_DEFAULT); at_com_send(command); @@ -175,7 +183,8 @@ void at_com_send_ref(int bits) { //Send a Pcmd void at_com_send_pcmd(int mode, float thrust, float roll, float pitch, - float yaw) { + float yaw) +{ int f_thrust, f_roll, f_pitch, f_yaw; char command[256]; @@ -186,12 +195,13 @@ void at_com_send_pcmd(int mode, float thrust, float roll, float pitch, memcpy(&f_yaw, &yaw, sizeof yaw); sprintf(command, "AT*PCMD=%d,%d,%d,%d,%d,%d\r", packet_seq++, mode, f_roll, - f_pitch, f_thrust, f_yaw); + f_pitch, f_thrust, f_yaw); at_com_send(command); } //Send a Calib -void at_com_send_calib(int device) { +void at_com_send_calib(int device) +{ char command[256]; sprintf(command, "AT*CALIB=%d,%d\r", packet_seq++, device); at_com_send(command); diff --git a/sw/airborne/boards/ardrone/at_com.h b/sw/airborne/boards/ardrone/at_com.h index 79084315ef..f5196399e1 100644 --- a/sw/airborne/boards/ardrone/at_com.h +++ b/sw/airborne/boards/ardrone/at_com.h @@ -94,7 +94,7 @@ typedef struct _navdata_option_t { uint16_t tag; uint16_t size; uint8_t data[1]; -} __attribute__ ((packed)) navdata_option_t; +} __attribute__((packed)) navdata_option_t; //Main navdata packet typedef struct _navdata_t { @@ -104,14 +104,14 @@ typedef struct _navdata_t { uint32_t vision_defined; navdata_option_t options[1]; -} __attribute__ ((packed)) navdata_t; +} __attribute__((packed)) navdata_t; //Navdata checksum packet typedef struct _navdata_cks_t { uint16_t tag; uint16_t size; uint32_t cks; -} __attribute__ ((packed)) navdata_cks_t; +} __attribute__((packed)) navdata_cks_t; //Navdata demo option typedef struct _navdata_demo_t { @@ -135,7 +135,7 @@ typedef struct _navdata_demo_t { // Camera parameters compute by drone struct FloatMat33 drone_camera_rot; /*!< Deprecated ! Don't use ! */ struct FloatVect3 drone_camera_trans; /*!< Deprecated ! Don't use ! */ -} __attribute__ ((packed)) navdata_demo_t; +} __attribute__((packed)) navdata_demo_t; //Navdata physical measures option typedef struct _navdata_phys_measures_t { @@ -149,7 +149,7 @@ typedef struct _navdata_phys_measures_t { uint32_t alim3V3; // 3.3volt alim [LSB] uint32_t vrefEpson; // ref volt Epson gyro [LSB] uint32_t vrefIDG; // ref volt IDG gyro [LSB] -} __attribute__ ((packed)) navdata_phys_measures_t; +} __attribute__((packed)) navdata_phys_measures_t; //Navdata gps packet typedef double float64_t; //TODO: Fix this nicely, but this is only used here @@ -176,10 +176,10 @@ typedef struct _navdata_gps_t { float32_t degree; /*!< Degree */ float32_t degree_mag; /*!< Degree of the magnetic */ uint8_t unk_2[16]; - struct{ + struct { uint8_t sat; uint8_t cn0; - }channels[12]; + } channels[12]; int32_t gps_plugged; /*!< When the gps is plugged */ uint8_t unk_3[108]; float64_t gps_time; /*!< The gps time of week */ @@ -198,12 +198,12 @@ typedef struct _navdata_gps_t { uint8_t unk_5[72]; float32_t temprature; float32_t pressure; -} __attribute__ ((packed)) navdata_gps_t; +} __attribute__((packed)) navdata_gps_t; //External functions extern void init_at_com(void); -extern int at_com_recieve_navdata(unsigned char* buffer); -extern void at_com_send_config(char* key, char* value); +extern int at_com_recieve_navdata(unsigned char *buffer); +extern void at_com_send_config(char *key, char *value); extern void at_com_send_ftrim(void); extern void at_com_send_ref(int bits); extern void at_com_send_pcmd(int mode, float thrust, float roll, float pitch, float yaw); diff --git a/sw/airborne/boards/ardrone/baro_board.c b/sw/airborne/boards/ardrone/baro_board.c index 52d2395741..60a745e5f2 100644 --- a/sw/airborne/boards/ardrone/baro_board.c +++ b/sw/airborne/boards/ardrone/baro_board.c @@ -41,7 +41,8 @@ struct MedianFilterInt baro_median; #define BMP180_OSS 0 // Parrot ARDrone uses no oversampling -void baro_init(void) { +void baro_init(void) +{ #if USE_BARO_MEDIAN_FILTER init_median_filter(&baro_median); #endif @@ -60,11 +61,11 @@ static inline int32_t baro_apply_calibration(int32_t raw) int32_t x1 = (((int32_t)baro_calibration.b2) * (b6 * b6 >> 12)) >> 11; int32_t x2 = ((int32_t)baro_calibration.ac2) * b6 >> 11; int32_t x3 = x1 + x2; - int32_t b3 = (((((int32_t)baro_calibration.ac1) * 4 + x3) << BMP180_OSS) + 2)/4; + int32_t b3 = (((((int32_t)baro_calibration.ac1) * 4 + x3) << BMP180_OSS) + 2) / 4; x1 = ((int32_t)baro_calibration.ac3) * b6 >> 13; x2 = (((int32_t)baro_calibration.b1) * (b6 * b6 >> 12)) >> 16; x3 = ((x1 + x2) + 2) >> 2; - uint32_t b4 = (((int32_t)baro_calibration.ac4) * (uint32_t) (x3 + 32768L)) >> 15; + uint32_t b4 = (((int32_t)baro_calibration.ac4) * (uint32_t)(x3 + 32768L)) >> 15; uint32_t b7 = (raw - b3) * (50000L >> BMP180_OSS); int32_t p = b7 < 0x80000000L ? (b7 * 2) / b4 : (b7 / b4) * 2; x1 = (p >> 8) * (p >> 8); diff --git a/sw/airborne/boards/ardrone/electrical_raw.c b/sw/airborne/boards/ardrone/electrical_raw.c index 6a9d2488a3..bbc5ece085 100644 --- a/sw/airborne/boards/ardrone/electrical_raw.c +++ b/sw/airborne/boards/ardrone/electrical_raw.c @@ -142,5 +142,6 @@ void electrical_periodic(void) /* electrical.current y = ( b^n - (b* x/a)^n )^1/n * a=1, n = electrical_priv.nonlin_factor */ - electrical.current = b - pow((pow(b, electrical_priv.nonlin_factor) - pow((b * x), electrical_priv.nonlin_factor)), (1. / electrical_priv.nonlin_factor)); + electrical.current = b - pow((pow(b, electrical_priv.nonlin_factor) - pow((b * x), electrical_priv.nonlin_factor)), + (1. / electrical_priv.nonlin_factor)); } diff --git a/sw/airborne/boards/ardrone/gpio_ardrone.c b/sw/airborne/boards/ardrone/gpio_ardrone.c index 848c6909a1..efcff4b390 100644 --- a/sw/airborne/boards/ardrone/gpio_ardrone.c +++ b/sw/airborne/boards/ardrone/gpio_ardrone.c @@ -55,11 +55,12 @@ struct gpio_direction { void gpio_set(uint32_t port, uint16_t pin) { - if (port != 0x32524) return; // protect ardrone board from unauthorized use + if (port != 0x32524) { return; } // protect ardrone board from unauthorized use struct gpio_data data; // Open the device if not open - if (gpiofp == 0) - gpiofp = open("/dev/gpio",O_RDWR); + if (gpiofp == 0) { + gpiofp = open("/dev/gpio", O_RDWR); + } // Read the GPIO value data.pin = pin; @@ -70,11 +71,12 @@ void gpio_set(uint32_t port, uint16_t pin) void gpio_clear(uint32_t port, uint16_t pin) { - if (port != 0x32524) return; // protect ardrone board from unauthorized use + if (port != 0x32524) { return; } // protect ardrone board from unauthorized use struct gpio_data data; // Open the device if not open - if (gpiofp == 0) - gpiofp = open("/dev/gpio",O_RDWR); + if (gpiofp == 0) { + gpiofp = open("/dev/gpio", O_RDWR); + } // Read the GPIO value data.pin = pin; @@ -85,11 +87,12 @@ void gpio_clear(uint32_t port, uint16_t pin) void gpio_setup_input(uint32_t port, uint16_t pin) { - if (port != 0x32524) return; // protect ardrone board from unauthorized use + if (port != 0x32524) { return; } // protect ardrone board from unauthorized use struct gpio_direction dir; // Open the device if not open - if (gpiofp == 0) - gpiofp = open("/dev/gpio",O_RDWR); + if (gpiofp == 0) { + gpiofp = open("/dev/gpio", O_RDWR); + } // Read the GPIO value dir.pin = pin; @@ -101,7 +104,7 @@ 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 + if (port != 0x32524) return; // protect ardrone board from unauthorized use struct gpio_direction dir; // Open the device if not open if (gpiofp == 0) @@ -118,11 +121,12 @@ void gpio_setup_output(uint32_t port, uint16_t pin) uint16_t gpio_get(uint32_t port, uint16_t pin) { - if (port != 0x32524) return 0; // protect ardrone board from unauthorized use + if (port != 0x32524) { return 0; } // protect ardrone board from unauthorized use struct gpio_data data; // Open the device if not open - if (gpiofp == 0) - gpiofp = open("/dev/gpio",O_RDWR); + if (gpiofp == 0) { + gpiofp = open("/dev/gpio", O_RDWR); + } // Read the GPIO value data.pin = pin; diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c index b337f37510..be5e5be248 100644 --- a/sw/airborne/boards/ardrone/navdata.c +++ b/sw/airborne/boards/ardrone/navdata.c @@ -366,7 +366,8 @@ static void baro_update_logic(void) static uint16_t lasttempval = 0; static int32_t lastpressval_nospike = 0; static uint16_t lasttempval_nospike = 0; - static uint8_t temp_or_press_was_updated_last = 0; // 0 = press, so we now wait for temp, 1 = temp so we now wait for press + static uint8_t temp_or_press_was_updated_last = + 0; // 0 = press, so we now wait for temp, 1 = temp so we now wait for press static int sync_errors = 0; static int spike_detected = 0; @@ -406,12 +407,14 @@ static void baro_update_logic(void) } // Detected a pressure switch - if (lastpressval != 0 && lasttempval != 0 && ABS(lastpressval - navdata.pressure) > ABS(lasttempval - navdata.pressure)) { + if (lastpressval != 0 && lasttempval != 0 + && ABS(lastpressval - navdata.pressure) > ABS(lasttempval - navdata.pressure)) { navdata_baro_available = FALSE; } // Detected a temprature switch - if (lastpressval != 0 && lasttempval != 0 && ABS(lasttempval - navdata.temperature_pressure) > ABS(lastpressval - navdata.temperature_pressure)) { + if (lastpressval != 0 && lasttempval != 0 + && ABS(lasttempval - navdata.temperature_pressure) > ABS(lastpressval - navdata.temperature_pressure)) { navdata_baro_available = FALSE; } @@ -458,7 +461,8 @@ static void baro_update_logic(void) */ // if press and temp are same and temp has jump: neglect the next frame - if (navdata.temperature_pressure == navdata.pressure) { // && (abs((int32_t)navdata.temperature_pressure - (int32_t)lasttempval) > 40)) + if (navdata.temperature_pressure == + navdata.pressure) { // && (abs((int32_t)navdata.temperature_pressure - (int32_t)lasttempval) > 40)) // dont use next 3 packets spike_detected = 3; } @@ -508,7 +512,8 @@ void navdata_update() // When checksum is incorrect if (navdata.chksum != checksum) { - printf("Checksum error [calculated: %d] [packet: %d] [diff: %d]\n", checksum , navdata.chksum, checksum - navdata.chksum); + printf("Checksum error [calculated: %d] [packet: %d] [diff: %d]\n", checksum , navdata.chksum, + checksum - navdata.chksum); nav_port.checksum_errors++; } @@ -521,7 +526,7 @@ void navdata_update() //printf("%d\r",navdata.nu_trame); // When checksum is correct - if(navdata.chksum == checksum) { + if (navdata.chksum == checksum) { // Invert byte order so that TELEMETRY works better uint8_t tmp; uint8_t *p = (uint8_t *) & (navdata.pressure); @@ -575,7 +580,8 @@ static void navdata_cropbuffer(int cropsize) { if (nav_port.bytesRead - cropsize < 0) { // TODO think about why the amount of bytes read minus the cropsize gets below zero - printf("BytesRead(=%d) - Cropsize(=%d) may not be below zero. port->buffer=%p\n", nav_port.bytesRead, cropsize, nav_port.buffer); + printf("BytesRead(=%d) - Cropsize(=%d) may not be below zero. port->buffer=%p\n", nav_port.bytesRead, cropsize, + nav_port.buffer); return; } diff --git a/sw/airborne/boards/ardrone/navdata.h b/sw/airborne/boards/ardrone/navdata.h index f075c77c39..2613bc70eb 100644 --- a/sw/airborne/boards/ardrone/navdata.h +++ b/sw/airborne/boards/ardrone/navdata.h @@ -33,8 +33,7 @@ #include #include -typedef struct -{ +typedef struct { uint16_t taille; uint16_t nu_trame; @@ -75,10 +74,9 @@ typedef struct uint16_t chksum; -} __attribute__ ((packed)) measures_t; +} __attribute__((packed)) measures_t; -struct bmp180_baro_calibration -{ +struct bmp180_baro_calibration { int16_t ac1; int16_t ac2; int16_t ac3; @@ -97,20 +95,20 @@ struct bmp180_baro_calibration #define NAVDATA_BUFFER_SIZE 80 typedef struct { - uint8_t isInitialized; - uint16_t bytesRead; - uint32_t totalBytesRead; - uint32_t packetsRead; - uint32_t checksum_errors; - uint32_t lost_imu_frames; - uint16_t last_packet_number; - uint8_t buffer[NAVDATA_BUFFER_SIZE]; + uint8_t isInitialized; + uint16_t bytesRead; + uint32_t totalBytesRead; + uint32_t packetsRead; + uint32_t checksum_errors; + uint32_t lost_imu_frames; + uint16_t last_packet_number; + uint8_t buffer[NAVDATA_BUFFER_SIZE]; } navdata_port; extern measures_t navdata; extern navdata_port nav_port; struct bmp180_baro_calibration baro_calibration; -navdata_port* port; +navdata_port *port; uint16_t navdata_cks; uint8_t navdata_imu_available; uint8_t navdata_baro_available; diff --git a/sw/airborne/boards/baro_board_ms5611_i2c.c b/sw/airborne/boards/baro_board_ms5611_i2c.c index d69b496079..da5d5a9119 100644 --- a/sw/airborne/boards/baro_board_ms5611_i2c.c +++ b/sw/airborne/boards/baro_board_ms5611_i2c.c @@ -60,7 +60,8 @@ struct Ms5611_I2c bb_ms5611; -void baro_init(void) { +void baro_init(void) +{ ms5611_i2c_init(&bb_ms5611, &BB_MS5611_I2C_DEV, BB_MS5611_SLAVE_ADDR); #ifdef BARO_LED @@ -68,7 +69,8 @@ void baro_init(void) { #endif } -void baro_periodic(void) { +void baro_periodic(void) +{ if (sys_time.nb_sec > 1) { /* call the convenience periodic that initializes the sensor and starts reading*/ @@ -76,20 +78,21 @@ void baro_periodic(void) { #if DEBUG if (bb_ms5611.initialized) - RunOnceEvery((50*30), DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice, - &bb_ms5611.data.c[0], - &bb_ms5611.data.c[1], - &bb_ms5611.data.c[2], - &bb_ms5611.data.c[3], - &bb_ms5611.data.c[4], - &bb_ms5611.data.c[5], - &bb_ms5611.data.c[6], - &bb_ms5611.data.c[7])); + RunOnceEvery((50 * 30), DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice, + &bb_ms5611.data.c[0], + &bb_ms5611.data.c[1], + &bb_ms5611.data.c[2], + &bb_ms5611.data.c[3], + &bb_ms5611.data.c[4], + &bb_ms5611.data.c[5], + &bb_ms5611.data.c[6], + &bb_ms5611.data.c[7])); #endif } } -void baro_event(void) { +void baro_event(void) +{ if (sys_time.nb_sec > 1) { ms5611_i2c_event(&bb_ms5611); @@ -101,7 +104,7 @@ void baro_event(void) { bb_ms5611.data_available = FALSE; #ifdef BARO_LED - RunOnceEvery(10,LED_TOGGLE(BARO_LED)); + RunOnceEvery(10, LED_TOGGLE(BARO_LED)); #endif #if DEBUG diff --git a/sw/airborne/boards/baro_board_ms5611_spi.c b/sw/airborne/boards/baro_board_ms5611_spi.c index 9190943ff1..7100efa2ba 100644 --- a/sw/airborne/boards/baro_board_ms5611_spi.c +++ b/sw/airborne/boards/baro_board_ms5611_spi.c @@ -49,7 +49,8 @@ struct Ms5611_Spi bb_ms5611; -void baro_init(void) { +void baro_init(void) +{ ms5611_spi_init(&bb_ms5611, &BB_MS5611_SPI_DEV, BB_MS5611_SLAVE_IDX); #ifdef BARO_LED @@ -57,7 +58,8 @@ void baro_init(void) { #endif } -void baro_periodic(void) { +void baro_periodic(void) +{ if (sys_time.nb_sec > 1) { /* call the convenience periodic that initializes the sensor and starts reading*/ @@ -65,20 +67,21 @@ void baro_periodic(void) { #if DEBUG if (bb_ms5611.initialized) - RunOnceEvery((50*30), DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice, - &bb_ms5611.data.c[0], - &bb_ms5611.data.c[1], - &bb_ms5611.data.c[2], - &bb_ms5611.data.c[3], - &bb_ms5611.data.c[4], - &bb_ms5611.data.c[5], - &bb_ms5611.data.c[6], - &bb_ms5611.data.c[7])); + RunOnceEvery((50 * 30), DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice, + &bb_ms5611.data.c[0], + &bb_ms5611.data.c[1], + &bb_ms5611.data.c[2], + &bb_ms5611.data.c[3], + &bb_ms5611.data.c[4], + &bb_ms5611.data.c[5], + &bb_ms5611.data.c[6], + &bb_ms5611.data.c[7])); #endif } } -void baro_event(void) { +void baro_event(void) +{ if (sys_time.nb_sec > 1) { ms5611_spi_event(&bb_ms5611); @@ -90,7 +93,7 @@ void baro_event(void) { bb_ms5611.data_available = FALSE; #ifdef BARO_LED - RunOnceEvery(10,LED_TOGGLE(BARO_LED)); + RunOnceEvery(10, LED_TOGGLE(BARO_LED)); #endif #if DEBUG diff --git a/sw/airborne/boards/booz/baro_board.c b/sw/airborne/boards/booz/baro_board.c index e7d955ecdb..8e5c3528f9 100644 --- a/sw/airborne/boards/booz/baro_board.c +++ b/sw/airborne/boards/booz/baro_board.c @@ -62,7 +62,8 @@ struct BaroBoard baro_board; -void baro_init( void ) { +void baro_init(void) +{ adc_buf_channel(ADC_CHANNEL_BARO, &baro_board.buf, DEFAULT_AV_NB_SAMPLE); @@ -76,32 +77,33 @@ void baro_init( void ) { #endif } -void baro_periodic(void) { +void baro_periodic(void) +{ - baro_board.absolute = baro_board.buf.sum/baro_board.buf.av_nb_sample; - baro_board.value_filtered = (3*baro_board.value_filtered + baro_board.absolute)/4; + baro_board.absolute = baro_board.buf.sum / baro_board.buf.av_nb_sample; + baro_board.value_filtered = (3 * baro_board.value_filtered + baro_board.absolute) / 4; if (baro_board.status == BB_UNINITIALIZED) { RunOnceEvery(10, { baro_board_calibrate();}); - } - else { - float pressure = 101325.0 - BOOZ_BARO_SENS*(BOOZ_ANALOG_BARO_THRESHOLD - baro_board.absolute); + } else { + float pressure = 101325.0 - BOOZ_BARO_SENS * (BOOZ_ANALOG_BARO_THRESHOLD - baro_board.absolute); AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); } } /* decrement offset until adc reading is over a threshold */ -void baro_board_calibrate(void) { +void baro_board_calibrate(void) +{ if (baro_board.value_filtered < BOOZ_ANALOG_BARO_THRESHOLD && baro_board.offset >= 1) { - if (baro_board.value_filtered == 0 && baro_board.offset > 15) + if (baro_board.value_filtered == 0 && baro_board.offset > 15) { baro_board.offset -= 15; - else + } else { baro_board.offset--; + } DACSet(baro_board.offset); #ifdef BARO_LED LED_TOGGLE(BARO_LED); #endif - } - else { + } else { baro_board.status = BB_RUNNING; #ifdef BARO_LED LED_ON(BARO_LED); diff --git a/sw/airborne/boards/booz/baro_board.h b/sw/airborne/boards/booz/baro_board.h index f47abe700b..2434a711ad 100644 --- a/sw/airborne/boards/booz/baro_board.h +++ b/sw/airborne/boards/booz/baro_board.h @@ -1,24 +1,24 @@ - /* - * Copyright (C) 2010-2013 Antoine Drouin, Gautier Hattenberger - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ +/* +* Copyright (C) 2010-2013 Antoine Drouin, Gautier Hattenberger +* +* This file is part of paparazzi. +* +* paparazzi is free software; you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation; either version 2, or (at your option) +* any later version. +* +* paparazzi is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with paparazzi; see the file COPYING. If not, write to +* the Free Software Foundation, 59 Temple Place - Suite 330, +* Boston, MA 02111-1307, USA. +* +*/ /** * @file boards/booz/baro_board.h @@ -52,7 +52,8 @@ extern struct BaroBoard baro_board; extern void baro_board_calibrate(void); #define BaroEvent() {} -static inline void baro_board_SetOffset(uint16_t _o) { +static inline void baro_board_SetOffset(uint16_t _o) +{ baro_board.offset = _o; DACSet(_o); } diff --git a/sw/airborne/boards/hbmini/baro_board.c b/sw/airborne/boards/hbmini/baro_board.c index c2ce71e87b..1bf92a5450 100644 --- a/sw/airborne/boards/hbmini/baro_board.c +++ b/sw/airborne/boards/hbmini/baro_board.c @@ -1,24 +1,24 @@ - /* - * Copyright (C) 2010 Christoph Niemann - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ +/* +* Copyright (C) 2010 Christoph Niemann +* +* This file is part of paparazzi. +* +* paparazzi is free software; you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation; either version 2, or (at your option) +* any later version. +* +* paparazzi is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with paparazzi; see the file COPYING. If not, write to +* the Free Software Foundation, 59 Temple Place - Suite 330, +* Boston, MA 02111-1307, USA. +* +*/ /** @file boards/hbmini/baro_board.c * Baro board interface for Bosch BMP085 on HBmini I2C1 with EOC check. @@ -34,23 +34,25 @@ struct Bmp085 baro_bmp085; -void baro_init( void ) { +void baro_init(void) +{ bmp085_init(&baro_bmp085, &i2c1, BMP085_SLAVE_ADDR); #ifdef BARO_LED LED_OFF(BARO_LED); #endif } -void baro_periodic( void ) { +void baro_periodic(void) +{ if (baro_bmp085.initialized) { bmp085_periodic(&baro_bmp085); - } - else { + } else { bmp085_read_eeprom_calib(&baro_bmp085); } } -void bmp_baro_event(void) { +void bmp_baro_event(void) +{ bmp085_event(&baro_bmp085); if (baro_bmp085.data_available) { @@ -58,7 +60,7 @@ void bmp_baro_event(void) { AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); baro_bmp085.data_available = FALSE; #ifdef BARO_LED - RunOnceEvery(10,LED_TOGGLE(BARO_LED)); + RunOnceEvery(10, LED_TOGGLE(BARO_LED)); #endif } } diff --git a/sw/airborne/boards/hbmini/imu_hbmini.c b/sw/airborne/boards/hbmini/imu_hbmini.c index 71c348fa63..6b52ade639 100644 --- a/sw/airborne/boards/hbmini/imu_hbmini.c +++ b/sw/airborne/boards/hbmini/imu_hbmini.c @@ -74,7 +74,7 @@ void imu_impl_init(void) } -void imu_periodic( void ) +void imu_periodic(void) { Max1168Periodic(); @@ -86,14 +86,16 @@ void imu_periodic( void ) } -void imu_hbmini_downlink_raw( void ) +void imu_hbmini_downlink_raw(void) { - DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice,&imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r); - DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice,&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z); - DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice,&imu.mag_unscaled.x,&imu.mag_unscaled.y,&imu.mag_unscaled.z); + DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, + &imu.gyro_unscaled.r); + DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, &imu.accel_unscaled.x, &imu.accel_unscaled.y, + &imu.accel_unscaled.z); + DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z); } -void imu_hbmini_event( void ) +void imu_hbmini_event(void) { max1168_event(); diff --git a/sw/airborne/boards/hbmini/imu_hbmini.h b/sw/airborne/boards/hbmini/imu_hbmini.h index 547d18a97b..661b3dcbd4 100644 --- a/sw/airborne/boards/hbmini/imu_hbmini.h +++ b/sw/airborne/boards/hbmini/imu_hbmini.h @@ -72,10 +72,11 @@ struct ImuHbmini { extern struct ImuHbmini imu_hbmini; /* Own Extra Functions */ -extern void imu_hbmini_event( void ); -extern void imu_hbmini_downlink_raw( void ); +extern void imu_hbmini_event(void); +extern void imu_hbmini_downlink_raw(void); -static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) { +static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) +{ imu_hbmini_event(); if (imu_hbmini.gyr_valid) { imu_hbmini.gyr_valid = FALSE; diff --git a/sw/airborne/boards/krooz/imu_krooz.c b/sw/airborne/boards/krooz/imu_krooz.c index 5a335a1eb8..492f348bcc 100644 --- a/sw/airborne/boards/krooz/imu_krooz.c +++ b/sw/airborne/boards/krooz/imu_krooz.c @@ -64,7 +64,7 @@ struct MedianFilter3Int median_accel; #endif struct MedianFilter3Int median_mag; -void imu_impl_init( void ) +void imu_impl_init(void) { ///////////////////////////////////////////////////////////////////// // MPU-60X0 @@ -98,18 +98,22 @@ void imu_impl_init( void ) imu_krooz_sd_arch_init(); } -void imu_periodic( void ) +void imu_periodic(void) { // Start reading the latest gyroscope data - if (!imu_krooz.mpu.config.initialized) + if (!imu_krooz.mpu.config.initialized) { mpu60x0_i2c_start_configure(&imu_krooz.mpu); + } - if (!imu_krooz.hmc.initialized) + if (!imu_krooz.hmc.initialized) { hmc58xx_start_configure(&imu_krooz.hmc); + } if (imu_krooz.meas_nb) { - RATES_ASSIGN(imu.gyro_unscaled, -imu_krooz.rates_sum.q / imu_krooz.meas_nb, imu_krooz.rates_sum.p / imu_krooz.meas_nb, imu_krooz.rates_sum.r / imu_krooz.meas_nb); - VECT3_ASSIGN(imu.accel_unscaled, -imu_krooz.accel_sum.y / imu_krooz.meas_nb, imu_krooz.accel_sum.x / imu_krooz.meas_nb, imu_krooz.accel_sum.z / imu_krooz.meas_nb); + RATES_ASSIGN(imu.gyro_unscaled, -imu_krooz.rates_sum.q / imu_krooz.meas_nb, imu_krooz.rates_sum.p / imu_krooz.meas_nb, + imu_krooz.rates_sum.r / imu_krooz.meas_nb); + VECT3_ASSIGN(imu.accel_unscaled, -imu_krooz.accel_sum.y / imu_krooz.meas_nb, imu_krooz.accel_sum.x / imu_krooz.meas_nb, + imu_krooz.accel_sum.z / imu_krooz.meas_nb); #if IMU_KROOZ_USE_ACCEL_MEDIAN_FILTER UpdateMedianFilterVect3Int(median_accel, imu.accel_unscaled); @@ -130,14 +134,16 @@ void imu_periodic( void ) //RunOnceEvery(10,imu_krooz_downlink_raw()); } -void imu_krooz_downlink_raw( void ) +void imu_krooz_downlink_raw(void) { - DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice,&imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r); - DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice,&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z); + DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, + &imu.gyro_unscaled.r); + DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, &imu.accel_unscaled.x, &imu.accel_unscaled.y, + &imu.accel_unscaled.z); } -void imu_krooz_event( void ) +void imu_krooz_event(void) { if (imu_krooz.mpu_eoc) { mpu60x0_i2c_read(&imu_krooz.mpu); diff --git a/sw/airborne/boards/krooz/imu_krooz.h b/sw/airborne/boards/krooz/imu_krooz.h index 3584891fc1..06cab8e5fe 100644 --- a/sw/airborne/boards/krooz/imu_krooz.h +++ b/sw/airborne/boards/krooz/imu_krooz.h @@ -128,10 +128,12 @@ extern void imu_periodic(void); */ /* Own Extra Functions */ -extern void imu_krooz_event( void ); -extern void imu_krooz_downlink_raw( void ); +extern void imu_krooz_event(void); +extern void imu_krooz_downlink_raw(void); -static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void) __attribute__((unused))) { +static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), + void (* _mag_handler)(void) __attribute__((unused))) +{ imu_krooz_event(); if (imu_krooz.gyr_valid) { imu_krooz.gyr_valid = FALSE; diff --git a/sw/airborne/boards/krooz/imu_krooz_memsic.c b/sw/airborne/boards/krooz/imu_krooz_memsic.c index bf82495e05..53715e7cbe 100644 --- a/sw/airborne/boards/krooz/imu_krooz_memsic.c +++ b/sw/airborne/boards/krooz/imu_krooz_memsic.c @@ -65,7 +65,7 @@ static uint32_t ad7689_event_timer; static uint8_t axis_cnt; static uint8_t axis_nb; -void imu_impl_init( void ) +void imu_impl_init(void) { ///////////////////////////////////////////////////////////////////// // MPU-60X0 @@ -104,9 +104,9 @@ void imu_impl_init( void ) imu_krooz.ad7689_trans.bitorder = SPIMSBFirst; imu_krooz.ad7689_trans.cdiv = SPIDiv16; imu_krooz.ad7689_trans.output_length = sizeof(imu_krooz.ad7689_spi_tx_buffer); - imu_krooz.ad7689_trans.output_buf = (uint8_t*) imu_krooz.ad7689_spi_tx_buffer; + imu_krooz.ad7689_trans.output_buf = (uint8_t *) imu_krooz.ad7689_spi_tx_buffer; imu_krooz.ad7689_trans.input_length = sizeof(imu_krooz.ad7689_spi_rx_buffer); - imu_krooz.ad7689_trans.input_buf = (uint8_t*) imu_krooz.ad7689_spi_rx_buffer; + imu_krooz.ad7689_trans.input_buf = (uint8_t *) imu_krooz.ad7689_spi_rx_buffer; imu_krooz.ad7689_trans.before_cb = NULL; imu_krooz.ad7689_trans.after_cb = NULL; axis_cnt = 0; @@ -115,19 +115,21 @@ void imu_impl_init( void ) imu_krooz_sd_arch_init(); } -void imu_periodic( void ) +void imu_periodic(void) { // Start reading the latest gyroscope data - if (!imu_krooz.mpu.config.initialized) + if (!imu_krooz.mpu.config.initialized) { mpu60x0_i2c_start_configure(&imu_krooz.mpu); + } - if (!imu_krooz.hmc.initialized) + if (!imu_krooz.hmc.initialized) { hmc58xx_start_configure(&imu_krooz.hmc); + } if (imu_krooz.meas_nb) { RATES_ASSIGN(imu.gyro_unscaled, -imu_krooz.rates_sum.q / imu_krooz.meas_nb, - imu_krooz.rates_sum.p / imu_krooz.meas_nb, - imu_krooz.rates_sum.r / imu_krooz.meas_nb); + imu_krooz.rates_sum.p / imu_krooz.meas_nb, + imu_krooz.rates_sum.r / imu_krooz.meas_nb); RATES_ASSIGN(imu_krooz.rates_sum, 0, 0, 0); imu_krooz.meas_nb = 0; @@ -152,10 +154,10 @@ void imu_periodic( void ) imu_krooz.acc_valid = TRUE; } - RunOnceEvery(128,{axis_nb = 5;}); + RunOnceEvery(128, {axis_nb = 5;}); } -void imu_krooz_event( void ) +void imu_krooz_event(void) { if (imu_krooz.mpu_eoc) { mpu60x0_i2c_read(&imu_krooz.mpu); @@ -172,10 +174,11 @@ void imu_krooz_event( void ) if (SysTimeTimer(ad7689_event_timer) > 215) { SysTimeTimerStart(ad7689_event_timer); - if (axis_cnt < axis_nb) + if (axis_cnt < axis_nb) { axis_cnt++; - else + } else { axis_cnt = 0; + } imu_krooz.ad7689_trans.output_buf[0] = axis_cnt <= 2 ? 0xF0 | (axis_cnt << 1) : (axis_cnt >= 4 ? 0xF0 | ((axis_cnt - 3) << 1) : 0xB0); imu_krooz.ad7689_trans.output_buf[1] = 0x44; @@ -184,7 +187,7 @@ void imu_krooz_event( void ) if (imu_krooz.ad7689_trans.status == SPITransSuccess) { imu_krooz.ad7689_trans.status = SPITransDone; uint16_t buf = (imu_krooz.ad7689_trans.input_buf[0] << 8) | imu_krooz.ad7689_trans.input_buf[1]; - switch(axis_cnt) { + switch (axis_cnt) { case 0: case 3: imu_krooz.accel_sum.x += (int32_t)buf; diff --git a/sw/airborne/boards/krooz/imu_krooz_memsic.h b/sw/airborne/boards/krooz/imu_krooz_memsic.h index 9b7b6c7ddc..c53b657388 100644 --- a/sw/airborne/boards/krooz/imu_krooz_memsic.h +++ b/sw/airborne/boards/krooz/imu_krooz_memsic.h @@ -134,10 +134,12 @@ extern void imu_periodic(void); */ /* Own Extra Functions */ -extern void imu_krooz_event( void ); -extern void imu_krooz_downlink_raw( void ); +extern void imu_krooz_event(void); +extern void imu_krooz_downlink_raw(void); -static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void) __attribute__((unused))) { +static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), + void (* _mag_handler)(void) __attribute__((unused))) +{ imu_krooz_event(); if (imu_krooz.gyr_valid) { imu_krooz.gyr_valid = FALSE; diff --git a/sw/airborne/boards/lia_1.1.h b/sw/airborne/boards/lia_1.1.h index a5464d1217..280b65bd0d 100644 --- a/sw/airborne/boards/lia_1.1.h +++ b/sw/airborne/boards/lia_1.1.h @@ -32,9 +32,9 @@ #define LED_2_GPIO_ON gpio_clear #define LED_2_GPIO_OFF gpio_set #define LED_2_AFIO_REMAP { \ - rcc_periph_clock_enable(RCC_AFIO); \ - AFIO_MAPR |= AFIO_MAPR_SWJ_CFG_FULL_SWJ_NO_JNTRST; \ -} + rcc_periph_clock_enable(RCC_AFIO); \ + AFIO_MAPR |= AFIO_MAPR_SWJ_CFG_FULL_SWJ_NO_JNTRST; \ + } /* green, shared with ADC12 (ADC_6 on connector ANALOG2) */ #ifndef USE_LED_3 diff --git a/sw/airborne/boards/lisa_l/baro_board.c b/sw/airborne/boards/lisa_l/baro_board.c index a7a6a7834b..54cf3c5dbe 100644 --- a/sw/airborne/boards/lisa_l/baro_board.c +++ b/sw/airborne/boards/lisa_l/baro_board.c @@ -71,7 +71,8 @@ static inline void baro_board_read_from_current_register(uint8_t baro_addr); #define LISA_L_DIFF_SENS 1.0 #endif -void baro_init(void) { +void baro_init(void) +{ #ifdef BARO_LED LED_OFF(BARO_LED); #endif @@ -80,135 +81,142 @@ void baro_init(void) { } -void baro_periodic(void) { +void baro_periodic(void) +{ // check i2c_done - if (!i2c_idle(&i2c2)) return; + if (!i2c_idle(&i2c2)) { return; } switch (baro_board.status) { - case LBS_UNINITIALIZED: - baro_board_send_reset(); - baro_board.status = LBS_RESETED; - break; - case LBS_RESETED: - baro_board_send_config_abs(); - baro_board.status = LBS_INITIALIZING_ABS; - break; - case LBS_INITIALIZING_ABS: - baro_board_set_current_register(BARO_ABS_ADDR, 0x00); - baro_board.status = LBS_INITIALIZING_ABS_1; - break; - case LBS_INITIALIZING_ABS_1: - baro_board_send_config_diff(); - baro_board.status = LBS_INITIALIZING_DIFF; - break; - case LBS_INITIALIZING_DIFF: - baro_board_set_current_register(BARO_DIFF_ADDR, 0x00); - baro_board.status = LBS_INITIALIZING_DIFF_1; - // baro_board.status = LBS_UNINITIALIZED; - break; - case LBS_INITIALIZING_DIFF_1: - baro_board.running = TRUE; - case LBS_READ_DIFF: - baro_board_read_from_current_register(BARO_ABS_ADDR); - baro_board.status = LBS_READING_ABS; - break; - case LBS_READ_ABS: - baro_board_read_from_current_register(BARO_DIFF_ADDR); - baro_board.status = LBS_READING_DIFF; - break; - default: - break; + case LBS_UNINITIALIZED: + baro_board_send_reset(); + baro_board.status = LBS_RESETED; + break; + case LBS_RESETED: + baro_board_send_config_abs(); + baro_board.status = LBS_INITIALIZING_ABS; + break; + case LBS_INITIALIZING_ABS: + baro_board_set_current_register(BARO_ABS_ADDR, 0x00); + baro_board.status = LBS_INITIALIZING_ABS_1; + break; + case LBS_INITIALIZING_ABS_1: + baro_board_send_config_diff(); + baro_board.status = LBS_INITIALIZING_DIFF; + break; + case LBS_INITIALIZING_DIFF: + baro_board_set_current_register(BARO_DIFF_ADDR, 0x00); + baro_board.status = LBS_INITIALIZING_DIFF_1; + // baro_board.status = LBS_UNINITIALIZED; + break; + case LBS_INITIALIZING_DIFF_1: + baro_board.running = TRUE; + case LBS_READ_DIFF: + baro_board_read_from_current_register(BARO_ABS_ADDR); + baro_board.status = LBS_READING_ABS; + break; + case LBS_READ_ABS: + baro_board_read_from_current_register(BARO_DIFF_ADDR); + baro_board.status = LBS_READING_DIFF; + break; + default: + break; } #ifdef BARO_LED if (baro_board.running == TRUE) { LED_ON(BARO_LED); - } - else { + } else { LED_TOGGLE(BARO_LED); } #endif } -void lisa_l_baro_event(void) { +void lisa_l_baro_event(void) +{ if (baro_board.status == LBS_READING_ABS && baro_trans.status != I2CTransPending) { baro_board.status = LBS_READ_ABS; if (baro_trans.status == I2CTransSuccess) { - int16_t tmp = baro_trans.buf[0]<<8 | baro_trans.buf[1]; - float pressure = LISA_L_BARO_SENS*(float)tmp; + int16_t tmp = baro_trans.buf[0] << 8 | baro_trans.buf[1]; + float pressure = LISA_L_BARO_SENS * (float)tmp; AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); } - } - else if (baro_board.status == LBS_READING_DIFF && - baro_trans.status != I2CTransPending) { + } else if (baro_board.status == LBS_READING_DIFF && + baro_trans.status != I2CTransPending) { baro_board.status = LBS_READ_DIFF; if (baro_trans.status == I2CTransSuccess) { - int16_t tmp = baro_trans.buf[0]<<8 | baro_trans.buf[1]; - float diff = LISA_L_DIFF_SENS*(float)tmp; + int16_t tmp = baro_trans.buf[0] << 8 | baro_trans.buf[1]; + float diff = LISA_L_DIFF_SENS * (float)tmp; AbiSendMsgBARO_DIFF(BARO_BOARD_SENDER_ID, &diff); } } } -static inline void baro_board_send_config_abs(void) { +static inline void baro_board_send_config_abs(void) +{ #ifndef BARO_LOW_GAIN -INFO("Using High LisaL Baro Gain: Do not use below 1000hPa") + INFO("Using High LisaL Baro Gain: Do not use below 1000hPa") baro_board_write_to_register(BARO_ABS_ADDR, 0x01, 0x86, 0x83); #else -INFO("Using Low LisaL Baro Gain, capable of measuring below 1000hPa or more") + INFO("Using Low LisaL Baro Gain, capable of measuring below 1000hPa or more") //config register should be 0x84 in low countries, or 0x86 in normal countries baro_board_write_to_register(BARO_ABS_ADDR, 0x01, 0x84, 0x83); #endif } -static inline void baro_board_send_config_diff(void) { +static inline void baro_board_send_config_diff(void) +{ baro_board_write_to_register(BARO_DIFF_ADDR, 0x01, 0x84, 0x83); } -static inline void baro_board_send_reset(void) { +static inline void baro_board_send_reset(void) +{ baro_trans.type = I2CTransTx; baro_trans.slave_addr = 0x00; baro_trans.len_w = 1; baro_trans.buf[0] = 0x06; - i2c_submit(&i2c2,&baro_trans); + i2c_submit(&i2c2, &baro_trans); } -static inline void baro_board_write_to_register(uint8_t baro_addr, uint8_t reg_addr, uint8_t val_msb, uint8_t val_lsb) { +static inline void baro_board_write_to_register(uint8_t baro_addr, uint8_t reg_addr, uint8_t val_msb, uint8_t val_lsb) +{ baro_trans.type = I2CTransTx; baro_trans.slave_addr = baro_addr; baro_trans.len_w = 3; baro_trans.buf[0] = reg_addr; baro_trans.buf[1] = val_msb; baro_trans.buf[2] = val_lsb; - i2c_submit(&i2c2,&baro_trans); + i2c_submit(&i2c2, &baro_trans); } -static inline void baro_board_read_from_register(uint8_t baro_addr, uint8_t reg_addr) { +static inline void baro_board_read_from_register(uint8_t baro_addr, uint8_t reg_addr) +{ baro_trans.type = I2CTransTxRx; baro_trans.slave_addr = baro_addr; baro_trans.len_w = 1; baro_trans.len_r = 2; baro_trans.buf[0] = reg_addr; - i2c_submit(&i2c2,&baro_trans); + i2c_submit(&i2c2, &baro_trans); // i2c2.buf[0] = reg_addr; // i2c2_transceive(baro_addr, 1, 2, &baro_board.i2c_done); } -static inline void baro_board_set_current_register(uint8_t baro_addr, uint8_t reg_addr) { +static inline void baro_board_set_current_register(uint8_t baro_addr, uint8_t reg_addr) +{ baro_trans.type = I2CTransTx; baro_trans.slave_addr = baro_addr; baro_trans.len_w = 1; baro_trans.buf[0] = reg_addr; - i2c_submit(&i2c2,&baro_trans); + i2c_submit(&i2c2, &baro_trans); // i2c2.buf[0] = reg_addr; // i2c2_transmit(baro_addr, 1, &baro_board.i2c_done); } -static inline void baro_board_read_from_current_register(uint8_t baro_addr) { +static inline void baro_board_read_from_current_register(uint8_t baro_addr) +{ baro_trans.type = I2CTransRx; baro_trans.slave_addr = baro_addr; baro_trans.len_r = 2; - i2c_submit(&i2c2,&baro_trans); + i2c_submit(&i2c2, &baro_trans); // i2c2_receive(baro_addr, 2, &baro_board.i2c_done); } diff --git a/sw/airborne/boards/lisa_m/baro_board.c b/sw/airborne/boards/lisa_m/baro_board.c index 6e070c3ae3..9ffbca3673 100644 --- a/sw/airborne/boards/lisa_m/baro_board.c +++ b/sw/airborne/boards/lisa_m/baro_board.c @@ -41,7 +41,8 @@ static bool_t baro_eoc(void) return gpio_get(GPIOB, GPIO0); } -void baro_init(void) { +void baro_init(void) +{ bmp085_init(&baro_bmp085, &i2c2, BMP085_SLAVE_ADDR); /* setup eoc check function */ @@ -49,7 +50,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); @@ -57,11 +58,11 @@ void baro_init(void) { } -void baro_periodic(void) { +void baro_periodic(void) +{ if (baro_bmp085.initialized) { bmp085_periodic(&baro_bmp085); - } - else { + } else { bmp085_read_eeprom_calib(&baro_bmp085); } } @@ -79,7 +80,7 @@ void baro_event(void) AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, &temp); baro_bmp085.data_available = FALSE; #ifdef BARO_LED - RunOnceEvery(10,LED_TOGGLE(BARO_LED)); + RunOnceEvery(10, LED_TOGGLE(BARO_LED)); #endif } } diff --git a/sw/airborne/boards/lisa_m_1.0.h b/sw/airborne/boards/lisa_m_1.0.h index ece0bbe559..39512cf49d 100644 --- a/sw/airborne/boards/lisa_m_1.0.h +++ b/sw/airborne/boards/lisa_m_1.0.h @@ -19,9 +19,9 @@ #define LED_1_GPIO_ON gpio_clear #define LED_1_GPIO_OFF gpio_set #define LED_1_AFIO_REMAP { \ - rcc_periph_clock_enable(RCC_AFIO); \ - AFIO_MAPR |= AFIO_MAPR_SWJ_CFG_FULL_SWJ_NO_JNTRST; \ -} + rcc_periph_clock_enable(RCC_AFIO); \ + AFIO_MAPR |= AFIO_MAPR_SWJ_CFG_FULL_SWJ_NO_JNTRST; \ + } /* blue */ #ifndef USE_LED_2 diff --git a/sw/airborne/boards/lisa_m_2.0.h b/sw/airborne/boards/lisa_m_2.0.h index c921f7af8a..1a2ae03085 100644 --- a/sw/airborne/boards/lisa_m_2.0.h +++ b/sw/airborne/boards/lisa_m_2.0.h @@ -32,9 +32,9 @@ #define LED_2_GPIO_ON gpio_clear #define LED_2_GPIO_OFF gpio_set #define LED_2_AFIO_REMAP { \ - rcc_periph_clock_enable(RCC_AFIO); \ - AFIO_MAPR |= AFIO_MAPR_SWJ_CFG_FULL_SWJ_NO_JNTRST; \ -} + rcc_periph_clock_enable(RCC_AFIO); \ + AFIO_MAPR |= AFIO_MAPR_SWJ_CFG_FULL_SWJ_NO_JNTRST; \ + } /* green, shared with ADC12 (ADC_6 on connector ANALOG2) */ #ifndef USE_LED_3 diff --git a/sw/airborne/boards/lisa_m_2.1.h b/sw/airborne/boards/lisa_m_2.1.h index 9cb8cd6996..2e8f3c378b 100644 --- a/sw/airborne/boards/lisa_m_2.1.h +++ b/sw/airborne/boards/lisa_m_2.1.h @@ -32,9 +32,9 @@ #define LED_2_GPIO_ON gpio_clear #define LED_2_GPIO_OFF gpio_set #define LED_2_AFIO_REMAP { \ - rcc_periph_clock_enable(RCC_AFIO); \ - AFIO_MAPR |= AFIO_MAPR_SWJ_CFG_FULL_SWJ_NO_JNTRST; \ -} + rcc_periph_clock_enable(RCC_AFIO); \ + AFIO_MAPR |= AFIO_MAPR_SWJ_CFG_FULL_SWJ_NO_JNTRST; \ + } /* green, shared with ADC12 (ADC_6 on connector ANALOG2) */ #ifndef USE_LED_3 diff --git a/sw/airborne/boards/lisa_m_common.h b/sw/airborne/boards/lisa_m_common.h index d2a890ef2f..a35e07af5d 100644 --- a/sw/airborne/boards/lisa_m_common.h +++ b/sw/airborne/boards/lisa_m_common.h @@ -227,28 +227,28 @@ #define USE_PWM4 1 #if DUAL_PWM_ON - #define DUAL_PWM_USE_TIM5 1 +#define DUAL_PWM_USE_TIM5 1 - #define USE_DUAL_PWM5 1 - #define USE_DUAL_PWM6 1 +#define USE_DUAL_PWM5 1 +#define USE_DUAL_PWM6 1 #else - #define USE_PWM5 1 - #define USE_PWM6 1 +#define USE_PWM5 1 +#define USE_PWM6 1 #endif #if USE_SERVOS_7AND8 - #if USE_I2C1 - #error "You cannot USE_SERVOS_7AND8 and USE_I2C1 at the same time" - #else - #define ACTUATORS_PWM_NB 8 - #define USE_PWM7 1 - #define USE_PWM8 1 - #define PWM_USE_TIM4 1 - #endif +#if USE_I2C1 +#error "You cannot USE_SERVOS_7AND8 and USE_I2C1 at the same time" #else - #define ACTUATORS_PWM_NB 6 +#define ACTUATORS_PWM_NB 8 +#define USE_PWM7 1 +#define USE_PWM8 1 +#define PWM_USE_TIM4 1 +#endif +#else +#define ACTUATORS_PWM_NB 6 #endif // Servo numbering on LisaM silkscreen/docs starts with 1 @@ -303,51 +303,51 @@ #endif #if USE_PWM5 - #define PWM_SERVO_5 4 - #define PWM_SERVO_5_TIMER TIM5 - #define PWM_SERVO_5_GPIO GPIOA - #define PWM_SERVO_5_PIN GPIO0 - #define PWM_SERVO_5_AF 0 - #define PWM_SERVO_5_OC TIM_OC1 - #define PWM_SERVO_5_OC_BIT (1<<0) +#define PWM_SERVO_5 4 +#define PWM_SERVO_5_TIMER TIM5 +#define PWM_SERVO_5_GPIO GPIOA +#define PWM_SERVO_5_PIN GPIO0 +#define PWM_SERVO_5_AF 0 +#define PWM_SERVO_5_OC TIM_OC1 +#define PWM_SERVO_5_OC_BIT (1<<0) #elif USE_DUAL_PWM5 - #define DUAL_PWM_SERVO_5 4 +#define DUAL_PWM_SERVO_5 4 - #define FIRST_DUAL_PWM_SERVO DUAL_PWM_SERVO_5 +#define FIRST_DUAL_PWM_SERVO DUAL_PWM_SERVO_5 - #define DUAL_PWM_SERVO_5_TIMER TIM5 - #define DUAL_PWM_SERVO_5_RCC RCC_GPIOA - #define DUAL_PWM_SERVO_5_GPIO GPIOA - #define DUAL_PWM_SERVO_5_PIN GPIO0 - #define DUAL_PWM_SERVO_5_AF 0 - #define DUAL_PWM_SERVO_5_OC TIM_OC1 - #define PWM_SERVO_5_OC_BIT (1<<0) +#define DUAL_PWM_SERVO_5_TIMER TIM5 +#define DUAL_PWM_SERVO_5_RCC RCC_GPIOA +#define DUAL_PWM_SERVO_5_GPIO GPIOA +#define DUAL_PWM_SERVO_5_PIN GPIO0 +#define DUAL_PWM_SERVO_5_AF 0 +#define DUAL_PWM_SERVO_5_OC TIM_OC1 +#define PWM_SERVO_5_OC_BIT (1<<0) #else - #define PWM_SERVO_5_OC_BIT 0 +#define PWM_SERVO_5_OC_BIT 0 #endif #if USE_PWM6 - #define PWM_SERVO_6 5 - #define PWM_SERVO_6_TIMER TIM5 - #define PWM_SERVO_6_GPIO GPIOA - #define PWM_SERVO_6_PIN GPIO1 - #define PWM_SERVO_6_AF 0 - #define PWM_SERVO_6_OC TIM_OC2 - #define PWM_SERVO_6_OC_BIT (1<<1) +#define PWM_SERVO_6 5 +#define PWM_SERVO_6_TIMER TIM5 +#define PWM_SERVO_6_GPIO GPIOA +#define PWM_SERVO_6_PIN GPIO1 +#define PWM_SERVO_6_AF 0 +#define PWM_SERVO_6_OC TIM_OC2 +#define PWM_SERVO_6_OC_BIT (1<<1) #elif USE_DUAL_PWM6 - #define DUAL_PWM_SERVO_6 5 +#define DUAL_PWM_SERVO_6 5 - #define SECOND_DUAL_PWM_SERVO DUAL_PWM_SERVO_6 +#define SECOND_DUAL_PWM_SERVO DUAL_PWM_SERVO_6 - #define DUAL_PWM_SERVO_6_TIMER TIM5 - #define DUAL_PWM_SERVO_6_RCC RCC_GPIOA - #define DUAL_PWM_SERVO_6_GPIO GPIOA - #define DUAL_PWM_SERVO_6_PIN GPIO1 - #define DUAL_PWM_SERVO_6_AF 0 - #define DUAL_PWM_SERVO_6_OC TIM_OC2 - #define PWM_SERVO_6_OC_BIT (1<<1) +#define DUAL_PWM_SERVO_6_TIMER TIM5 +#define DUAL_PWM_SERVO_6_RCC RCC_GPIOA +#define DUAL_PWM_SERVO_6_GPIO GPIOA +#define DUAL_PWM_SERVO_6_PIN GPIO1 +#define DUAL_PWM_SERVO_6_AF 0 +#define DUAL_PWM_SERVO_6_OC TIM_OC2 +#define PWM_SERVO_6_OC_BIT (1<<1) #else - #define PWM_SERVO_6_OC_BIT 0 +#define PWM_SERVO_6_OC_BIT 0 #endif diff --git a/sw/airborne/boards/lisa_mx_common.h b/sw/airborne/boards/lisa_mx_common.h index 4b9fc92d2b..d867cc1812 100644 --- a/sw/airborne/boards/lisa_mx_common.h +++ b/sw/airborne/boards/lisa_mx_common.h @@ -364,16 +364,16 @@ #define USE_PWM6 1 #if USE_SERVOS_7AND8 - #if USE_I2C1 - #error "You cannot USE_SERVOS_7AND8 and USE_I2C1 at the same time" - #else - #define ACTUATORS_PWM_NB 8 - #define USE_PWM7 1 - #define USE_PWM8 1 - #define PWM_USE_TIM4 1 - #endif +#if USE_I2C1 +#error "You cannot USE_SERVOS_7AND8 and USE_I2C1 at the same time" #else - #define ACTUATORS_PWM_NB 6 +#define ACTUATORS_PWM_NB 8 +#define USE_PWM7 1 +#define USE_PWM8 1 +#define PWM_USE_TIM4 1 +#endif +#else +#define ACTUATORS_PWM_NB 6 #endif // Servo numbering on LisaM silkscreen/docs starts with 1 diff --git a/sw/airborne/boards/lisa_s_1.0.h b/sw/airborne/boards/lisa_s_1.0.h index 9e5ef2d3f2..aed531a452 100644 --- a/sw/airborne/boards/lisa_s_1.0.h +++ b/sw/airborne/boards/lisa_s_1.0.h @@ -279,16 +279,16 @@ #if USE_SERVOS_1AND2 - #if USE_I2C1 - #error "You cannot USE_SERVOS_1AND2 and USE_I2C1 at the same time" - #else - #define ACTUATORS_PWM_NB 6 - #define USE_PWM1 1 - #define USE_PWM2 1 - #define PWM_USE_TIM4 1 - #endif +#if USE_I2C1 +#error "You cannot USE_SERVOS_1AND2 and USE_I2C1 at the same time" #else - #define ACTUATORS_PWM_NB 4 +#define ACTUATORS_PWM_NB 6 +#define USE_PWM1 1 +#define USE_PWM2 1 +#define PWM_USE_TIM4 1 +#endif +#else +#define ACTUATORS_PWM_NB 4 #endif #define USE_PWM3 1 @@ -297,13 +297,13 @@ //TODO : test that part //TODO : merge the USE_SERVOS_1AND2 and DUAL_PWM_ON #if DUAL_PWM_ON - #define DUAL_PWM_USE_TIM5 1 +#define DUAL_PWM_USE_TIM5 1 - #define USE_DUAL_PWM5 1 - #define USE_DUAL_PWM6 1 +#define USE_DUAL_PWM5 1 +#define USE_DUAL_PWM6 1 #else - #define USE_PWM5 1 - #define USE_PWM6 1 +#define USE_PWM5 1 +#define USE_PWM6 1 #endif // Servo numbering on LisaM silkscreen/docs starts with 1 @@ -361,51 +361,51 @@ #endif #if USE_PWM5 - #define PWM_SERVO_5 2 - #define PWM_SERVO_5_TIMER TIM5 - #define PWM_SERVO_5_GPIO GPIOA - #define PWM_SERVO_5_PIN GPIO0 - #define PWM_SERVO_5_AF 0 - #define PWM_SERVO_5_OC TIM_OC1 - #define PWM_SERVO_5_OC_BIT (1<<0) +#define PWM_SERVO_5 2 +#define PWM_SERVO_5_TIMER TIM5 +#define PWM_SERVO_5_GPIO GPIOA +#define PWM_SERVO_5_PIN GPIO0 +#define PWM_SERVO_5_AF 0 +#define PWM_SERVO_5_OC TIM_OC1 +#define PWM_SERVO_5_OC_BIT (1<<0) #elif USE_DUAL_PWM5 - #define DUAL_PWM_SERVO_5 2 +#define DUAL_PWM_SERVO_5 2 - #define FIRST_DUAL_PWM_SERVO DUAL_PWM_SERVO_5 +#define FIRST_DUAL_PWM_SERVO DUAL_PWM_SERVO_5 - #define DUAL_PWM_SERVO_5_TIMER TIM5 - #define DUAL_PWM_SERVO_5_RCC RCC_GPIOA - #define DUAL_PWM_SERVO_5_GPIO GPIOA - #define DUAL_PWM_SERVO_5_PIN GPIO0 - #define DUAL_PWM_SERVO_5_AF 0 - #define DUAL_PWM_SERVO_5_OC TIM_OC1 - #define PWM_SERVO_5_OC_BIT (1<<0) +#define DUAL_PWM_SERVO_5_TIMER TIM5 +#define DUAL_PWM_SERVO_5_RCC RCC_GPIOA +#define DUAL_PWM_SERVO_5_GPIO GPIOA +#define DUAL_PWM_SERVO_5_PIN GPIO0 +#define DUAL_PWM_SERVO_5_AF 0 +#define DUAL_PWM_SERVO_5_OC TIM_OC1 +#define PWM_SERVO_5_OC_BIT (1<<0) #else - #define PWM_SERVO_5_OC_BIT 0 +#define PWM_SERVO_5_OC_BIT 0 #endif #if USE_PWM6 - #define PWM_SERVO_6 3 - #define PWM_SERVO_6_TIMER TIM5 - #define PWM_SERVO_6_GPIO GPIOA - #define PWM_SERVO_6_PIN GPIO1 - #define PWM_SERVO_6_AF 0 - #define PWM_SERVO_6_OC TIM_OC2 - #define PWM_SERVO_6_OC_BIT (1<<1) +#define PWM_SERVO_6 3 +#define PWM_SERVO_6_TIMER TIM5 +#define PWM_SERVO_6_GPIO GPIOA +#define PWM_SERVO_6_PIN GPIO1 +#define PWM_SERVO_6_AF 0 +#define PWM_SERVO_6_OC TIM_OC2 +#define PWM_SERVO_6_OC_BIT (1<<1) #elif USE_DUAL_PWM6 - #define DUAL_PWM_SERVO_6 3 +#define DUAL_PWM_SERVO_6 3 - #define SECOND_DUAL_PWM_SERVO DUAL_PWM_SERVO_6 +#define SECOND_DUAL_PWM_SERVO DUAL_PWM_SERVO_6 - #define DUAL_PWM_SERVO_6_TIMER TIM5 - #define DUAL_PWM_SERVO_6_RCC RCC_GPIOA - #define DUAL_PWM_SERVO_6_GPIO GPIOA - #define DUAL_PWM_SERVO_6_PIN GPIO1 - #define DUAL_PWM_SERVO_6_AF 0 - #define DUAL_PWM_SERVO_6_OC TIM_OC2 - #define PWM_SERVO_6_OC_BIT (1<<1) +#define DUAL_PWM_SERVO_6_TIMER TIM5 +#define DUAL_PWM_SERVO_6_RCC RCC_GPIOA +#define DUAL_PWM_SERVO_6_GPIO GPIOA +#define DUAL_PWM_SERVO_6_PIN GPIO1 +#define DUAL_PWM_SERVO_6_AF 0 +#define DUAL_PWM_SERVO_6_OC TIM_OC2 +#define PWM_SERVO_6_OC_BIT (1<<1) #else - #define PWM_SERVO_6_OC_BIT 0 +#define PWM_SERVO_6_OC_BIT 0 #endif diff --git a/sw/airborne/boards/navgo/baro_board.c b/sw/airborne/boards/navgo/baro_board.c index fd99a5c6ca..95e04e2310 100644 --- a/sw/airborne/boards/navgo/baro_board.c +++ b/sw/airborne/boards/navgo/baro_board.c @@ -1,24 +1,24 @@ - /* - * Copyright (C) 2010 ENAC - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ +/* +* Copyright (C) 2010 ENAC +* +* This file is part of paparazzi. +* +* paparazzi is free software; you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation; either version 2, or (at your option) +* any later version. +* +* paparazzi is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with paparazzi; see the file COPYING. If not, write to +* the Free Software Foundation, 59 Temple Place - Suite 330, +* Boston, MA 02111-1307, USA. +* +*/ /* driver for the analog Barometer Mpxa6115 using ADC ads1114 (16 bits I2C 860SpS max) from Texas instruments @@ -54,7 +54,8 @@ #define BARO_STARTUP_COUNTER 200 uint16_t startup_cnt; -void baro_init( void ) { +void baro_init(void) +{ mcp355x_init(); #ifdef BARO_LED LED_OFF(BARO_LED); @@ -62,7 +63,8 @@ void baro_init( void ) { startup_cnt = BARO_STARTUP_COUNTER; } -void baro_periodic( void ) { +void baro_periodic(void) +{ // Run some loops to get correct readings from the adc if (startup_cnt > 0) { --startup_cnt; @@ -74,15 +76,16 @@ void baro_periodic( void ) { #endif } // Read the ADC (at 50/4 Hz, conversion time is 68 ms) - RunOnceEvery(4,mcp355x_read()); + RunOnceEvery(4, mcp355x_read()); } -void navgo_baro_event(void) { +void navgo_baro_event(void) +{ mcp355x_event(); if (mcp355x_data_available) { if (startup_cnt == 0) { // Send data when init phase is done - float pressure = NAVGO_BARO_SENS*(mcp355x_data+NAVGO_BARO_OFFSET); + float pressure = NAVGO_BARO_SENS * (mcp355x_data + NAVGO_BARO_OFFSET); AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); } mcp355x_data_available = FALSE; diff --git a/sw/airborne/boards/navgo/imu_navgo.c b/sw/airborne/boards/navgo/imu_navgo.c index 1d5fc8734e..761ed8d0db 100644 --- a/sw/airborne/boards/navgo/imu_navgo.c +++ b/sw/airborne/boards/navgo/imu_navgo.c @@ -93,7 +93,7 @@ void imu_impl_init(void) imu_navgo.mag_valid = FALSE; } -void imu_periodic( void ) +void imu_periodic(void) { // Start reading the latest gyroscope data itg3200_periodic(&imu_navgo.itg); @@ -102,7 +102,7 @@ void imu_periodic( void ) // Periodicity is automatically adapted // 3200 is the maximum output freq corresponding to the parameter 0xF // A factor 2 is applied to reduice the delay without overloading the i2c - RunOnceEvery((PERIODIC_FREQUENCY/(2*3200>>(0xf-NAVGO_ACCEL_RATE))), adxl345_i2c_periodic(&imu_navgo.adxl)); + RunOnceEvery((PERIODIC_FREQUENCY / (2 * 3200 >> (0xf - NAVGO_ACCEL_RATE))), adxl345_i2c_periodic(&imu_navgo.adxl)); // Read HMC58XX at 100Hz (main loop for rotorcraft: 512Hz) RunOnceEvery(5, hmc58xx_periodic(&imu_navgo.hmc)); @@ -111,14 +111,16 @@ void imu_periodic( void ) } -void imu_navgo_downlink_raw( void ) +void imu_navgo_downlink_raw(void) { - DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice,&imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r); - DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice,&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z); - DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice,&imu.mag_unscaled.x,&imu.mag_unscaled.y,&imu.mag_unscaled.z); + DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, + &imu.gyro_unscaled.r); + DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, &imu.accel_unscaled.x, &imu.accel_unscaled.y, + &imu.accel_unscaled.z); + DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z); } -void imu_navgo_event( void ) +void imu_navgo_event(void) { // If the itg3200 I2C transaction has succeeded: convert the data diff --git a/sw/airborne/boards/navgo/imu_navgo.h b/sw/airborne/boards/navgo/imu_navgo.h index 54fdb198c6..e464ca6ce5 100644 --- a/sw/airborne/boards/navgo/imu_navgo.h +++ b/sw/airborne/boards/navgo/imu_navgo.h @@ -71,11 +71,12 @@ struct ImuNavgo { extern struct ImuNavgo imu_navgo; /* Own Extra Functions */ -extern void imu_navgo_event( void ); -extern void imu_navgo_downlink_raw( void ); +extern void imu_navgo_event(void); +extern void imu_navgo_downlink_raw(void); -static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) { +static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) +{ imu_navgo_event(); if (imu_navgo.gyr_valid) { imu_navgo.gyr_valid = FALSE; diff --git a/sw/airborne/boards/navstik/baro_board.c b/sw/airborne/boards/navstik/baro_board.c index 967cf96fed..8327543b43 100644 --- a/sw/airborne/boards/navstik/baro_board.c +++ b/sw/airborne/boards/navstik/baro_board.c @@ -36,7 +36,8 @@ struct Bmp085 baro_bmp085; -void baro_init(void) { +void baro_init(void) +{ bmp085_init(&baro_bmp085, &i2c3, BMP085_SLAVE_ADDR); #ifdef BARO_LED @@ -44,16 +45,17 @@ void baro_init(void) { #endif } -void baro_periodic(void) { +void baro_periodic(void) +{ if (baro_bmp085.initialized) { bmp085_periodic(&baro_bmp085); - } - else { + } else { bmp085_read_eeprom_calib(&baro_bmp085); } } -void baro_event(void) { +void baro_event(void) +{ bmp085_event(&baro_bmp085); if (baro_bmp085.data_available) { @@ -63,7 +65,7 @@ void baro_event(void) { AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, &temp); baro_bmp085.data_available = FALSE; #ifdef BARO_LED - RunOnceEvery(10,LED_TOGGLE(BARO_LED)); + RunOnceEvery(10, LED_TOGGLE(BARO_LED)); #endif } } diff --git a/sw/airborne/boards/sdlog_1.0.h b/sw/airborne/boards/sdlog_1.0.h index 0cd319b776..ddd79e7b43 100644 --- a/sw/airborne/boards/sdlog_1.0.h +++ b/sw/airborne/boards/sdlog_1.0.h @@ -190,9 +190,9 @@ #define CARD_DETECT_PIN 20 -#define LED_GREEN 3 -#define LED_YELLOW 2 -#define LED_RED 1 +#define LED_GREEN 3 +#define LED_YELLOW 2 +#define LED_RED 1 #endif /* CONFIG_SD_LOGGER_PPRZ_H */ diff --git a/sw/airborne/boards/stm32f4_discovery.h b/sw/airborne/boards/stm32f4_discovery.h index 5935a0eacf..7914e1f9d6 100644 --- a/sw/airborne/boards/stm32f4_discovery.h +++ b/sw/airborne/boards/stm32f4_discovery.h @@ -275,7 +275,7 @@ /************************************** ADC *************************************************/ /***************************************************************************************************/ -#define USE_AD_TIM2 1 +#define USE_AD_TIM2 1 // For experimenting only #define USE_ADC_1 1 diff --git a/sw/airborne/boards/umarim/baro_board.c b/sw/airborne/boards/umarim/baro_board.c index c02542ed14..63ffb90361 100644 --- a/sw/airborne/boards/umarim/baro_board.c +++ b/sw/airborne/boards/umarim/baro_board.c @@ -1,24 +1,24 @@ - /* - * Copyright (C) 2010-2013 Gautier Hattenberger - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ +/* +* Copyright (C) 2010-2013 Gautier Hattenberger +* +* This file is part of paparazzi. +* +* paparazzi is free software; you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation; either version 2, or (at your option) +* any later version. +* +* paparazzi is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with paparazzi; see the file COPYING. If not, write to +* the Free Software Foundation, 59 Temple Place - Suite 330, +* Boston, MA 02111-1307, USA. +* +*/ /* driver for the analog Barometer Mpxa6115 using ADC ads1114 (16 bits I2C 860SpS max) from Texas instruments @@ -46,7 +46,8 @@ #define BARO_STARTUP_COUNTER 200 uint16_t startup_cnt; -void baro_init( void ) { +void baro_init(void) +{ ads1114_init(); #ifdef BARO_LED LED_OFF(BARO_LED); @@ -54,7 +55,8 @@ void baro_init( void ) { startup_cnt = BARO_STARTUP_COUNTER; } -void baro_periodic( void ) { +void baro_periodic(void) +{ // Run some loops to get correct readings from the adc if (startup_cnt > 0) { @@ -70,11 +72,12 @@ void baro_periodic( void ) { ads1114_read(&BARO_ABS_ADS); } -void umarim_baro_event(void) { +void umarim_baro_event(void) +{ Ads1114Event(); if (BARO_ABS_ADS.data_available) { if (startup_cnt == 0) { - float pressure = UMARIM_BARO_SENS*Ads1114GetValue(BARO_ABS_ADS); + float pressure = UMARIM_BARO_SENS * Ads1114GetValue(BARO_ABS_ADS); AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure); } BARO_ABS_ADS.data_available = FALSE; diff --git a/sw/airborne/boards/umarim/imu_umarim.c b/sw/airborne/boards/umarim/imu_umarim.c index e43024fac9..ae8e4ed655 100644 --- a/sw/airborne/boards/umarim/imu_umarim.c +++ b/sw/airborne/boards/umarim/imu_umarim.c @@ -82,7 +82,7 @@ void imu_impl_init(void) imu_umarim.acc_valid = FALSE; } -void imu_periodic( void ) +void imu_periodic(void) { // Start reading the latest gyroscope data itg3200_periodic(&imu_umarim.itg); @@ -93,14 +93,16 @@ void imu_periodic( void ) //RunOnceEvery(10,imu_umarim_downlink_raw()); } -void imu_umarim_downlink_raw( void ) +void imu_umarim_downlink_raw(void) { - DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice,&imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r); - DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice,&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z); + DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, + &imu.gyro_unscaled.r); + DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, &imu.accel_unscaled.x, &imu.accel_unscaled.y, + &imu.accel_unscaled.z); } -void imu_umarim_event( void ) +void imu_umarim_event(void) { // If the itg3200 I2C transaction has succeeded: convert the data itg3200_event(&imu_umarim.itg); @@ -113,7 +115,8 @@ void imu_umarim_event( void ) // If the adxl345 I2C transaction has succeeded: convert the data adxl345_i2c_event(&imu_umarim.adxl); if (imu_umarim.adxl.data_available) { - VECT3_ASSIGN(imu.accel_unscaled, imu_umarim.adxl.data.vect.y, -imu_umarim.adxl.data.vect.x, imu_umarim.adxl.data.vect.z); + VECT3_ASSIGN(imu.accel_unscaled, imu_umarim.adxl.data.vect.y, -imu_umarim.adxl.data.vect.x, + imu_umarim.adxl.data.vect.z); imu_umarim.adxl.data_available = FALSE; imu_umarim.acc_valid = TRUE; } diff --git a/sw/airborne/boards/umarim/imu_umarim.h b/sw/airborne/boards/umarim/imu_umarim.h index 5da1f78b24..a15e1c87d8 100644 --- a/sw/airborne/boards/umarim/imu_umarim.h +++ b/sw/airborne/boards/umarim/imu_umarim.h @@ -121,11 +121,13 @@ extern void imu_periodic(void); */ /* Own Extra Functions */ -extern void imu_umarim_event( void ); -extern void imu_umarim_downlink_raw( void ); +extern void imu_umarim_event(void); +extern void imu_umarim_downlink_raw(void); -static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void) __attribute__((unused))) { +static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), + void (* _mag_handler)(void) __attribute__((unused))) +{ imu_umarim_event(); if (imu_umarim.gyr_valid) { imu_umarim.gyr_valid = FALSE; diff --git a/sw/airborne/filters/low_pass_filter.h b/sw/airborne/filters/low_pass_filter.h index 92f0f476a0..ba82e010eb 100644 --- a/sw/airborne/filters/low_pass_filter.h +++ b/sw/airborne/filters/low_pass_filter.h @@ -54,7 +54,9 @@ struct FirstOrderLowPass { * @param sample_time sampling period of the signal * @param value initial value of the filter */ -static inline void init_first_order_low_pass(struct FirstOrderLowPass * filter, float tau, float sample_time, float value) { +static inline void init_first_order_low_pass(struct FirstOrderLowPass *filter, float tau, float sample_time, + float value) +{ filter->last_in = value; filter->last_out = value; filter->time_const = 2.0f * tau / sample_time; @@ -66,7 +68,8 @@ static inline void init_first_order_low_pass(struct FirstOrderLowPass * filter, * @param value new input value of the filter * @return new filtered value */ -static inline float update_first_order_low_pass(struct FirstOrderLowPass * filter, float value) { +static inline float update_first_order_low_pass(struct FirstOrderLowPass *filter, float value) +{ float out = (value + filter->last_in + (filter->time_const - 1.0f) * filter->last_out) / (1.0f + filter->time_const); filter->last_in = value; filter->last_out = out; @@ -78,7 +81,8 @@ static inline float update_first_order_low_pass(struct FirstOrderLowPass * filte * @param filter first order low pass filter structure * @return current value of the filter */ -static inline float get_first_order_low_pass(struct FirstOrderLowPass * filter) { +static inline float get_first_order_low_pass(struct FirstOrderLowPass *filter) +{ return filter->last_out; } @@ -131,12 +135,14 @@ struct SecondOrderLowPass { * @param sample_time sampling period of the signal * @param value initial value of the filter */ -static inline void init_second_order_low_pass(struct SecondOrderLowPass * filter, float tau, float Q, float sample_time, float value) { +static inline void init_second_order_low_pass(struct SecondOrderLowPass *filter, float tau, float Q, float sample_time, + float value) +{ float K = sample_time / (2.0f * tau); - float poly = K*K + K/Q + 1.0f; - filter->a[0] = 2.0f * (K*K - 1.0f) / poly; - filter->a[1] = (K*K - K/Q + 1.0f) / poly; - filter->b[0] = K*K / poly; + float poly = K * K + K / Q + 1.0f; + filter->a[0] = 2.0f * (K * K - 1.0f) / poly; + filter->a[1] = (K * K - K / Q + 1.0f) / poly; + filter->b[0] = K * K / poly; filter->b[1] = 2.0f * filter->b[0]; filter->i[0] = filter->i[1] = filter->o[0] = filter->o[1] = value; } @@ -147,12 +153,13 @@ static inline void init_second_order_low_pass(struct SecondOrderLowPass * filter * @param value new input value of the filter * @return new filtered value */ -static inline float update_second_order_low_pass(struct SecondOrderLowPass * filter, float value) { +static inline float update_second_order_low_pass(struct SecondOrderLowPass *filter, float value) +{ float out = filter->b[0] * value - + filter->b[1] * filter->i[0] - + filter->b[0] * filter->i[1] - - filter->a[0] * filter->o[0] - - filter->a[1] * filter->o[1]; + + filter->b[1] * filter->i[0] + + filter->b[0] * filter->i[1] + - filter->a[0] * filter->o[0] + - filter->a[1] * filter->o[1]; filter->i[1] = filter->i[0]; filter->i[0] = value; filter->o[1] = filter->o[0]; @@ -165,7 +172,8 @@ static inline float update_second_order_low_pass(struct SecondOrderLowPass * fil * @param filter second order low pass filter structure * @return current value of the filter */ -static inline float get_second_order_low_pass(struct SecondOrderLowPass * filter) { +static inline float get_second_order_low_pass(struct SecondOrderLowPass *filter) +{ return filter->o[0]; } @@ -185,16 +193,18 @@ struct SecondOrderLowPass_int { * @param sample_time sampling period of the signal * @param value initial value of the filter */ -static inline void init_second_order_low_pass_int(struct SecondOrderLowPass_int* filter, float cut_off, float Q, float sample_time, int32_t value) { +static inline void init_second_order_low_pass_int(struct SecondOrderLowPass_int *filter, float cut_off, float Q, + float sample_time, int32_t value) +{ struct SecondOrderLowPass filter_temp; float tau = 7.0f / (44.0f * cut_off); float K = sample_time / (2.0f * tau); - float poly = K*K + K/Q + 1.0f; + float poly = K * K + K / Q + 1.0f; float loop_gain_f; - filter_temp.a[0] = 2.0f * (K*K - 1.0f) / poly; - filter_temp.a[1] = (K*K - K/Q + 1.0f) / poly; - filter_temp.b[0] = K*K / poly; + filter_temp.a[0] = 2.0f * (K * K - 1.0f) / poly; + filter_temp.a[1] = (K * K - K / Q + 1.0f) / poly; + filter_temp.b[0] = K * K / poly; filter_temp.b[1] = 2.0f * filter_temp.b[0]; loop_gain_f = 1.0f / filter_temp.b[0]; @@ -212,12 +222,13 @@ static inline void init_second_order_low_pass_int(struct SecondOrderLowPass_int* * @param value new input value of the filter * @return new filtered value */ -static inline int32_t update_second_order_low_pass_int(struct SecondOrderLowPass_int* filter, int32_t value) { +static inline int32_t update_second_order_low_pass_int(struct SecondOrderLowPass_int *filter, int32_t value) +{ int32_t out = filter->b[0] * value - + filter->b[1] * filter->i[0] - + filter->b[0] * filter->i[1] - - filter->a[0] * filter->o[0] - - filter->a[1] * filter->o[1]; + + filter->b[1] * filter->i[0] + + filter->b[0] * filter->i[1] + - filter->a[0] * filter->o[0] + - filter->a[1] * filter->o[1]; filter->i[1] = filter->i[0]; filter->i[0] = value; @@ -231,7 +242,8 @@ static inline int32_t update_second_order_low_pass_int(struct SecondOrderLowPass * @param filter second order low pass filter structure * @return current value of the filter */ -static inline int32_t get_second_order_low_pass_int(struct SecondOrderLowPass_int * filter) { +static inline int32_t get_second_order_low_pass_int(struct SecondOrderLowPass_int *filter) +{ return filter->o[0]; } @@ -251,8 +263,9 @@ typedef struct SecondOrderLowPass Butterworth2LowPass; * @param sample_time sampling period of the signal * @param value initial value of the filter */ -static inline void init_butterworth_2_low_pass(Butterworth2LowPass * filter, float tau, float sample_time, float value) { - init_second_order_low_pass((struct SecondOrderLowPass*)filter, tau, 0.7071, sample_time, value); +static inline void init_butterworth_2_low_pass(Butterworth2LowPass *filter, float tau, float sample_time, float value) +{ + init_second_order_low_pass((struct SecondOrderLowPass *)filter, tau, 0.7071, sample_time, value); } /** Update second order Butterworth low pass filter state with a new value. @@ -261,8 +274,9 @@ static inline void init_butterworth_2_low_pass(Butterworth2LowPass * filter, flo * @param value new input value of the filter * @return new filtered value */ -static inline float update_butterworth_2_low_pass(Butterworth2LowPass * filter, float value) { - return update_second_order_low_pass((struct SecondOrderLowPass*)filter, value); +static inline float update_butterworth_2_low_pass(Butterworth2LowPass *filter, float value) +{ + return update_second_order_low_pass((struct SecondOrderLowPass *)filter, value); } /** Get current value of the second order Butterworth low pass filter. @@ -270,7 +284,8 @@ static inline float update_butterworth_2_low_pass(Butterworth2LowPass * filter, * @param filter second order Butterworth low pass filter structure * @return current value of the filter */ -static inline float get_butterworth_2_low_pass(Butterworth2LowPass * filter) { +static inline float get_butterworth_2_low_pass(Butterworth2LowPass *filter) +{ return filter->o[0]; } @@ -290,8 +305,10 @@ typedef struct SecondOrderLowPass_int Butterworth2LowPass_int; * @param sample_time sampling period of the signal * @param value initial value of the filter */ -static inline void init_butterworth_2_low_pass_int(Butterworth2LowPass_int* filter, float cut_off, float sample_time, int32_t value) { - init_second_order_low_pass_int((struct SecondOrderLowPass_int*)filter, cut_off, 0.7071, sample_time, value); +static inline void init_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter, float cut_off, float sample_time, + int32_t value) +{ + init_second_order_low_pass_int((struct SecondOrderLowPass_int *)filter, cut_off, 0.7071, sample_time, value); } /** Update second order Butterworth low pass filter state with a new value(fixed point version). @@ -300,8 +317,9 @@ static inline void init_butterworth_2_low_pass_int(Butterworth2LowPass_int* filt * @param value new input value of the filter * @return new filtered value */ -static inline int32_t update_butterworth_2_low_pass_int(Butterworth2LowPass_int* filter, int32_t value) { - return update_second_order_low_pass_int((struct SecondOrderLowPass_int*)filter, value); +static inline int32_t update_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter, int32_t value) +{ + return update_second_order_low_pass_int((struct SecondOrderLowPass_int *)filter, value); } /** Get current value of the second order Butterworth low pass filter(fixed point version). @@ -309,7 +327,8 @@ static inline int32_t update_butterworth_2_low_pass_int(Butterworth2LowPass_int* * @param filter second order Butterworth low pass filter structure * @return current value of the filter */ -static inline int32_t get_butterworth_2_low_pass_int(Butterworth2LowPass_int* filter) { +static inline int32_t get_butterworth_2_low_pass_int(Butterworth2LowPass_int *filter) +{ return filter->o[0]; } @@ -335,7 +354,8 @@ typedef struct { * @param sample_time sampling period of the signal * @param value initial value of the filter */ -static inline void init_butterworth_4_low_pass(Butterworth4LowPass * filter, float tau, float sample_time, float value) { +static inline void init_butterworth_4_low_pass(Butterworth4LowPass *filter, float tau, float sample_time, float value) +{ init_second_order_low_pass(&filter->lp1, tau, 1.30651, sample_time, value); init_second_order_low_pass(&filter->lp2, tau, 0.51184, sample_time, value); } @@ -348,7 +368,8 @@ static inline void init_butterworth_4_low_pass(Butterworth4LowPass * filter, flo * @param value new input value of the filter * @return new filtered value */ -static inline float update_butterworth_4_low_pass(Butterworth4LowPass * filter, float value) { +static inline float update_butterworth_4_low_pass(Butterworth4LowPass *filter, float value) +{ float tmp = update_second_order_low_pass(&filter->lp1, value); return update_second_order_low_pass(&filter->lp2, tmp); } @@ -358,7 +379,8 @@ static inline float update_butterworth_4_low_pass(Butterworth4LowPass * filter, * @param filter fourth order Butterworth low pass filter structure * @return current value of the filter */ -static inline float get_butterworth_4_low_pass(Butterworth4LowPass * filter) { +static inline float get_butterworth_4_low_pass(Butterworth4LowPass *filter) +{ return filter->lp2.o[0]; } @@ -384,7 +406,9 @@ typedef struct { * @param sample_time sampling period of the signal * @param value initial value of the filter */ -static inline void init_butterworth_4_low_pass_int(Butterworth4LowPass_int * filter, float cut_off, float sample_time, int32_t value) { +static inline void init_butterworth_4_low_pass_int(Butterworth4LowPass_int *filter, float cut_off, float sample_time, + int32_t value) +{ init_second_order_low_pass_int(&filter->lp1, cut_off, 1.30651, sample_time, value); init_second_order_low_pass_int(&filter->lp2, cut_off, 0.51184, sample_time, value); } @@ -397,7 +421,8 @@ static inline void init_butterworth_4_low_pass_int(Butterworth4LowPass_int * fil * @param value new input value of the filter * @return new filtered value */ -static inline int32_t update_butterworth_4_low_pass_int(Butterworth4LowPass_int * filter, int32_t value) { +static inline int32_t update_butterworth_4_low_pass_int(Butterworth4LowPass_int *filter, int32_t value) +{ int32_t tmp = update_second_order_low_pass_int(&filter->lp1, value); return update_second_order_low_pass_int(&filter->lp2, tmp); } @@ -407,7 +432,8 @@ static inline int32_t update_butterworth_4_low_pass_int(Butterworth4LowPass_int * @param filter fourth order Butterworth low pass filter structure * @return current value of the filter */ -static inline int32_t get_butterworth_4_low_pass_int(Butterworth4LowPass_int * filter) { +static inline int32_t get_butterworth_4_low_pass_int(Butterworth4LowPass_int *filter) +{ return filter->lp2.o[0]; } diff --git a/sw/airborne/filters/median_filter.h b/sw/airborne/filters/median_filter.h index 188695a04e..4c1fb1791b 100644 --- a/sw/airborne/filters/median_filter.h +++ b/sw/airborne/filters/median_filter.h @@ -34,11 +34,12 @@ struct MedianFilterInt { int8_t dataIndex; }; -inline void init_median_filter(struct MedianFilterInt * filter); -inline int32_t update_median_filter(struct MedianFilterInt * filter, int32_t new_data); -inline int32_t get_median_filter(struct MedianFilterInt * filter); +inline void init_median_filter(struct MedianFilterInt *filter); +inline int32_t update_median_filter(struct MedianFilterInt *filter, int32_t new_data); +inline int32_t get_median_filter(struct MedianFilterInt *filter); -inline void init_median_filter(struct MedianFilterInt * filter) { +inline void init_median_filter(struct MedianFilterInt *filter) +{ int i; for (i = 0; i < MEDIAN_DATASIZE; i++) { filter->data[i] = 0; @@ -47,15 +48,15 @@ inline void init_median_filter(struct MedianFilterInt * filter) { filter->dataIndex = 0; } -inline int32_t update_median_filter(struct MedianFilterInt * filter, int32_t new_data) { +inline int32_t update_median_filter(struct MedianFilterInt *filter, int32_t new_data) +{ int temp, i, j; // used to sort array // Insert new data into raw data array round robin style filter->data[filter->dataIndex] = new_data; - if (filter->dataIndex < (MEDIAN_DATASIZE-1)) { + if (filter->dataIndex < (MEDIAN_DATASIZE - 1)) { filter->dataIndex++; - } - else { + } else { filter->dataIndex = 0; } @@ -63,20 +64,21 @@ inline int32_t update_median_filter(struct MedianFilterInt * filter, int32_t new memcpy(filter->sortData, filter->data, sizeof(filter->data)); // Insertion Sort - for(i = 1; i <= (MEDIAN_DATASIZE-1); i++) { + for (i = 1; i <= (MEDIAN_DATASIZE - 1); i++) { temp = filter->sortData[i]; - j = i-1; - while(temp < filter->sortData[j] && j>=0) { - filter->sortData[j+1] = filter->sortData[j]; - j = j-1; + j = i - 1; + while (temp < filter->sortData[j] && j >= 0) { + filter->sortData[j + 1] = filter->sortData[j]; + j = j - 1; } - filter->sortData[j+1] = temp; + filter->sortData[j + 1] = temp; } - return filter->sortData[(MEDIAN_DATASIZE)>>1]; // return data value in middle of sorted array + return filter->sortData[(MEDIAN_DATASIZE) >> 1]; // return data value in middle of sorted array } -inline int32_t get_median_filter(struct MedianFilterInt * filter) { - return filter->sortData[(MEDIAN_DATASIZE)>>1]; +inline int32_t get_median_filter(struct MedianFilterInt *filter) +{ + return filter->sortData[(MEDIAN_DATASIZE) >> 1]; } struct MedianFilter3Int { @@ -84,48 +86,48 @@ struct MedianFilter3Int { }; #define InitMedianFilterVect3Int(_f) { \ - for (int i = 0; i < 3; i++) { \ - init_median_filter(&(_f.mf[i])); \ - } \ -} + for (int i = 0; i < 3; i++) { \ + init_median_filter(&(_f.mf[i])); \ + } \ + } #define InitMedianFilterEulerInt(_f) InitMedianFilterVect3Int(_f) #define InitMedianFilterRatesInt(_f) InitMedianFilterVect3Int(_f) #define UpdateMedianFilterVect3Int(_f, _v) { \ - (_v).x = update_median_filter(&(_f.mf[0]), (_v).x); \ - (_v).y = update_median_filter(&(_f.mf[1]), (_v).y); \ - (_v).z = update_median_filter(&(_f.mf[2]), (_v).z); \ -} + (_v).x = update_median_filter(&(_f.mf[0]), (_v).x); \ + (_v).y = update_median_filter(&(_f.mf[1]), (_v).y); \ + (_v).z = update_median_filter(&(_f.mf[2]), (_v).z); \ + } #define UpdateMedianFilterEulerInt(_f, _v) { \ - (_v).phi = update_median_filter(&(_f.mf[0]), (_v).phi); \ - (_v).theta = update_median_filter(&(_f.mf[1]), (_v).theta); \ - (_v).psi = update_median_filter(&(_f.mf[2]), (_v).psi); \ -} + (_v).phi = update_median_filter(&(_f.mf[0]), (_v).phi); \ + (_v).theta = update_median_filter(&(_f.mf[1]), (_v).theta); \ + (_v).psi = update_median_filter(&(_f.mf[2]), (_v).psi); \ + } #define UpdateMedianFilterRatesInt(_f, _v) { \ - (_v).p = update_median_filter(&(_f.mf[0]), (_v).p); \ - (_v).q = update_median_filter(&(_f.mf[1]), (_v).q); \ - (_v).r = update_median_filter(&(_f.mf[2]), (_v).r); \ -} + (_v).p = update_median_filter(&(_f.mf[0]), (_v).p); \ + (_v).q = update_median_filter(&(_f.mf[1]), (_v).q); \ + (_v).r = update_median_filter(&(_f.mf[2]), (_v).r); \ + } #define GetMedianFilterVect3Int(_f, _v) { \ - (_v).x = get_median_filter(&(_f.mf[0])); \ - (_v).y = get_median_filter(&(_f.mf[1])); \ - (_v).z = get_median_filter(&(_f.mf[2])); \ -} + (_v).x = get_median_filter(&(_f.mf[0])); \ + (_v).y = get_median_filter(&(_f.mf[1])); \ + (_v).z = get_median_filter(&(_f.mf[2])); \ + } #define GetMedianFilterEulerInt(_f, _v) { \ - (_v).phi = get_median_filter(&(_f.mf[0])); \ - (_v).theta = get_median_filter(&(_f.mf[1])); \ - (_v).psi = get_median_filter(&(_f.mf[2])); \ -} + (_v).phi = get_median_filter(&(_f.mf[0])); \ + (_v).theta = get_median_filter(&(_f.mf[1])); \ + (_v).psi = get_median_filter(&(_f.mf[2])); \ + } #define GetMedianFilterRatesInt(_f, _v) { \ - (_v).p = get_median_filter(&(_f.mf[0])); \ - (_v).q = get_median_filter(&(_f.mf[1])); \ - (_v).r = get_median_filter(&(_f.mf[2])); \ -} + (_v).p = get_median_filter(&(_f.mf[0])); \ + (_v).q = get_median_filter(&(_f.mf[1])); \ + (_v).r = get_median_filter(&(_f.mf[2])); \ + } #endif diff --git a/sw/airborne/firmwares/beth/bench_sensors.h b/sw/airborne/firmwares/beth/bench_sensors.h index 772464b1e9..99dd1786e7 100644 --- a/sw/airborne/firmwares/beth/bench_sensors.h +++ b/sw/airborne/firmwares/beth/bench_sensors.h @@ -30,13 +30,13 @@ struct BenchSensors { extern struct BenchSensors bench_sensors; #if USE_I2C2 -#define BenchSensorsEvent( _handler) { \ - if (bench_sensors.status == BS_BUSY && bench_sensors.ready) { \ - bench_sensors.angle_1 = i2c2.buf[0] + (i2c2.buf[1] << 8); \ - bench_sensors.angle_2 = i2c2.buf[2] + (i2c2.buf[3] << 8); \ - bench_sensors.status = BS_IDLE; \ - _handler(); \ - } \ +#define BenchSensorsEvent( _handler) { \ + if (bench_sensors.status == BS_BUSY && bench_sensors.ready) { \ + bench_sensors.angle_1 = i2c2.buf[0] + (i2c2.buf[1] << 8); \ + bench_sensors.angle_2 = i2c2.buf[2] + (i2c2.buf[3] << 8); \ + bench_sensors.status = BS_IDLE; \ + _handler(); \ + } \ } #endif diff --git a/sw/airborne/firmwares/beth/bench_sensors_can.c b/sw/airborne/firmwares/beth/bench_sensors_can.c index ada5ccfca7..aba54661db 100644 --- a/sw/airborne/firmwares/beth/bench_sensors_can.c +++ b/sw/airborne/firmwares/beth/bench_sensors_can.c @@ -12,7 +12,8 @@ static uint8_t rx_bd1 = 0; static uint8_t rx_bd2 = 0; -void bench_sensors_init(void) { +void bench_sensors_init(void) +{ can_init(can_rx_callback); } @@ -20,11 +21,12 @@ void bench_sensors_init(void) { #define MAX_ALLOWED_SKIPS_CANBD1 (60) #define MAX_ALLOWED_SKIPS_CANBD2 (10) -void read_bench_sensors(void) { +void read_bench_sensors(void) +{ //rx_bd1/2 keep track of how long it's been since last receive (when it is set to 0) //if we've lost link for 0.5 second, stop counting (to avoid overflowing our uint8) - if (rx_bd1 < 255) rx_bd1++; - if (rx_bd2 < 255) rx_bd2++; + if (rx_bd1 < 255) { rx_bd1++; } + if (rx_bd2 < 255) { rx_bd2++; } //if we haven't heard from a board for 15 periods (15/512=29ms) //we flag a CAN error to show which board is at cause and how long it's been. @@ -34,22 +36,22 @@ void read_bench_sensors(void) { //if ((rx_bd1 > 15) && (rx_bd2 > 15)) {can_err_flags = 3000 + rx_bd2 + rx_bd1;} } -static void can_rx_callback(uint32_t id, uint8_t *buf, int len) { +static void can_rx_callback(uint32_t id, uint8_t *buf, int len) +{ //LED_TOGGLE(7); static uint16_t tempangle; - bench_sensors.current = id>>7; + bench_sensors.current = id >> 7; if (bench_sensors.current == 2) { - tempangle = (buf[1]<<8) | buf[0]; - if ((tempangle == 0 )||(tempangle > 6000)) {can_err_flags = 0x20;} else {bench_sensors.angle_2 = tempangle;} - tempangle = (buf[3]<<8) | buf[2]; - if ((tempangle == 0 )||(tempangle > 6000)) {can_err_flags = 0x20;} else {bench_sensors.angle_3 = tempangle;} + tempangle = (buf[1] << 8) | buf[0]; + if ((tempangle == 0) || (tempangle > 6000)) {can_err_flags = 0x20;} else {bench_sensors.angle_2 = tempangle;} + tempangle = (buf[3] << 8) | buf[2]; + if ((tempangle == 0) || (tempangle > 6000)) {can_err_flags = 0x20;} else {bench_sensors.angle_3 = tempangle;} rx_bd2 = 0; LED_TOGGLE(4); - } - else { - tempangle = (buf[3]<<8) | buf[2]; - if ((tempangle == 0 )||(tempangle > 6000)) {can_err_flags = 0x10;} else {bench_sensors.angle_1 = tempangle;} + } else { + tempangle = (buf[3] << 8) | buf[2]; + if ((tempangle == 0) || (tempangle > 6000)) {can_err_flags = 0x10;} else {bench_sensors.angle_1 = tempangle;} rx_bd1 = 0; //LED_TOGGLE(5); } diff --git a/sw/airborne/firmwares/beth/bench_sensors_i2c.c b/sw/airborne/firmwares/beth/bench_sensors_i2c.c index a3dbd415a6..eb41216a0f 100644 --- a/sw/airborne/firmwares/beth/bench_sensors_i2c.c +++ b/sw/airborne/firmwares/beth/bench_sensors_i2c.c @@ -1,20 +1,23 @@ #include "bench_sensors.h" -struct BenchSensors bench_sensors,bench_sensors2; +struct BenchSensors bench_sensors, bench_sensors2; -void bench_sensors_init(void) { +void bench_sensors_init(void) +{ bench_sensors.status = BS_IDLE; bench_sensors.i2c_done = TRUE; } -void bench_sensors2_init(void) { +void bench_sensors2_init(void) +{ bench_sensors2.status = BS_IDLE; bench_sensors2.i2c_done = TRUE; } -void read_bench_sensors(void) { +void read_bench_sensors(void) +{ const uint8_t bench_addr = 0x40; bench_sensors.status = BS_BUSY; @@ -24,7 +27,8 @@ void read_bench_sensors(void) { } -void read_bench_sensors2(void) { +void read_bench_sensors2(void) +{ const uint8_t bench_addr2 = 0x30; bench_sensors2.status = BS_BUSY; diff --git a/sw/airborne/firmwares/beth/main_beth.c b/sw/airborne/firmwares/beth/main_beth.c index 07e81ac89b..1801bcb4bf 100644 --- a/sw/airborne/firmwares/beth/main_beth.c +++ b/sw/airborne/firmwares/beth/main_beth.c @@ -7,29 +7,33 @@ #include "beth/bench_sensors.h" -static inline void main_init( void ); -static inline void main_periodic_task( void ); -static inline void main_event_task( void ); +static inline void main_init(void); +static inline void main_periodic_task(void); +static inline void main_event_task(void); -static inline void main_on_bench_sensors( void ); +static inline void main_on_bench_sensors(void); -int main( void ) { +int main(void) +{ main_init(); - while(1) { - if (sys_time_check_and_ack_timer(0)) + while (1) { + if (sys_time_check_and_ack_timer(0)) { main_periodic_task(); + } } return 0; } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); mcu_int_enable(); } -static inline void main_periodic_task( void ) { +static inline void main_periodic_task(void) +{ RunOnceEvery(512, { DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);}); @@ -38,17 +42,19 @@ static inline void main_periodic_task( void ) { } -static inline void main_event_task( void ) { +static inline void main_event_task(void) +{ BenchSensorsEvent(main_on_bench_sensors); } -static inline void main_on_bench_sensors( void ) { +static inline void main_on_bench_sensors(void) +{ DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, &bench_sensors_angle_1, - &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 67ff1ab189..e79c59f1bc 100644 --- a/sw/airborne/firmwares/beth/main_coders.c +++ b/sw/airborne/firmwares/beth/main_coders.c @@ -29,9 +29,9 @@ Paul : using channel 10 instead of 14 */ -static inline void main_init( void ); -static inline void main_periodic( void ); -static inline void main_event( void ); +static inline void main_init(void); +static inline void main_periodic(void); +static inline void main_event(void); static inline void main_init_adc(void); //static inline void main_on_bench_sensors( void ); @@ -55,7 +55,8 @@ static uint16_t coder_values[3]; uint16_t servos[4]; -int main(void) { +int main(void) +{ main_init(); servos[0] = 1; @@ -64,23 +65,26 @@ int main(void) { servos[3] = 4; while (1) { - if (sys_time_check_and_ack_timer(0)) + if (sys_time_check_and_ack_timer(0)) { main_periodic(); + } main_event(); } return 0; } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); main_init_adc(); bench_sensors_init(); mcu_int_enable(); } -static inline void main_periodic( void ) { +static inline void main_periodic(void) +{ /*RunOnceEvery(10, {DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);});*/ @@ -90,15 +94,16 @@ static inline void main_periodic( void ) { /*RunOnceEvery(5, {DOWNLINK_SEND_BETH(DefaultChannel, &bench_sensors.angle_1, &bench_sensors.angle_2,&bench_sensors.angle_3, &bench_sensors.current);});*/ - servos[0]=coder_values[0]; - servos[1]=coder_values[1]; + servos[0] = coder_values[0]; + servos[1] = coder_values[1]; //use id=1 for azimuth board can_transmit(1, (uint8_t *)servos, 8); LED_TOGGLE(5); } -static inline void main_event( void ) { +static inline void main_event(void) +{ //BenchSensorsEvent(main_on_bench_sensors); } @@ -114,7 +119,8 @@ static inline void main_event( void ) { * I2C2 : autopilot link * */ -void i2c2_init(void) { +void i2c2_init(void) +{ // static inline void main_init_i2c2(void) { /* System clocks configuration ---------------------------------------------*/ @@ -165,35 +171,36 @@ void i2c2_init(void) { } -void i2c2_ev_irq_handler(void) { - switch (I2C_GetLastEvent(I2C2)) - { - /* Slave Transmitter ---------------------------------------------------*/ - case I2C_EVENT_SLAVE_TRANSMITTER_ADDRESS_MATCHED: /* EV1 */ - memcpy(i2c2_buf, coder_values, MY_I2C2_BUF_LEN); - i2c2_idx = 0; +void i2c2_ev_irq_handler(void) +{ + switch (I2C_GetLastEvent(I2C2)) { + /* Slave Transmitter ---------------------------------------------------*/ + case I2C_EVENT_SLAVE_TRANSMITTER_ADDRESS_MATCHED: /* EV1 */ + memcpy(i2c2_buf, coder_values, MY_I2C2_BUF_LEN); + i2c2_idx = 0; - case I2C_EVENT_SLAVE_BYTE_TRANSMITTED: /* EV3 */ - /* Transmit I2C2 data */ - if (i2c2_idx < MY_I2C2_BUF_LEN) { - I2C_SendData(I2C2, i2c2_buf[i2c2_idx]); - i2c2_idx++; - } - break; + case I2C_EVENT_SLAVE_BYTE_TRANSMITTED: /* EV3 */ + /* Transmit I2C2 data */ + if (i2c2_idx < MY_I2C2_BUF_LEN) { + I2C_SendData(I2C2, i2c2_buf[i2c2_idx]); + i2c2_idx++; + } + break; - case I2C_EVENT_SLAVE_STOP_DETECTED: /* EV4 */ - LED_ON(1); - /* Clear I2C2 STOPF flag: read of I2C_SR1 followed by a write on I2C_CR1 */ - (void)(I2C_GetITStatus(I2C2, I2C_IT_STOPF)); - I2C_Cmd(I2C2, ENABLE); - break; + case I2C_EVENT_SLAVE_STOP_DETECTED: /* EV4 */ + LED_ON(1); + /* Clear I2C2 STOPF flag: read of I2C_SR1 followed by a write on I2C_CR1 */ + (void)(I2C_GetITStatus(I2C2, I2C_IT_STOPF)); + I2C_Cmd(I2C2, ENABLE); + break; } } -void i2c2_er_irq_handler(void) { +void i2c2_er_irq_handler(void) +{ /* Check on I2C2 AF flag and clear it */ if (I2C_GetITStatus(I2C2, I2C_IT_AF)) { I2C_ClearITPendingBit(I2C2, I2C_IT_AF); @@ -212,14 +219,15 @@ void i2c2_er_irq_handler(void) { -static inline void main_init_adc(void) { +static inline void main_init_adc(void) +{ /* Enable DMA1 clock */ RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE); /* 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; @@ -227,7 +235,7 @@ static inline void main_init_adc(void) { GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN; GPIO_Init(GPIOC, &GPIO_InitStructure); - /* DMA1 channel1 configuration ----------------------------------------------*/ + /* DMA1 channel1 configuration ----------------------------------------------*/ DMA_InitTypeDef DMA_InitStructure; DMA_DeInit(DMA1_Channel1); DMA_InitStructure.DMA_PeripheralBaseAddr = ADC1_DR_Address; @@ -285,12 +293,12 @@ static inline void main_init_adc(void) { /* Enable ADC1 reset calibaration register */ ADC_ResetCalibration(ADC1); /* Check the end of ADC1 reset calibration register */ - while(ADC_GetResetCalibrationStatus(ADC1)); + while (ADC_GetResetCalibrationStatus(ADC1)); /* Start ADC1 calibaration */ ADC_StartCalibration(ADC1); /* Check the end of ADC1 calibration */ - while(ADC_GetCalibrationStatus(ADC1)); + while (ADC_GetCalibrationStatus(ADC1)); /* Enable ADC2 */ ADC_Cmd(ADC2, ENABLE); @@ -298,12 +306,12 @@ static inline void main_init_adc(void) { /* Enable ADC2 reset calibaration register */ ADC_ResetCalibration(ADC2); /* Check the end of ADC2 reset calibration register */ - while(ADC_GetResetCalibrationStatus(ADC2)); + while (ADC_GetResetCalibrationStatus(ADC2)); /* Start ADC2 calibaration */ ADC_StartCalibration(ADC2); /* Check the end of ADC2 calibration */ - while(ADC_GetCalibrationStatus(ADC2)); + while (ADC_GetCalibrationStatus(ADC2)); /* Start ADC1 Software Conversion */ diff --git a/sw/airborne/firmwares/beth/main_overo.c b/sw/airborne/firmwares/beth/main_overo.c index dd8c8416a5..256af99c59 100644 --- a/sw/airborne/firmwares/beth/main_overo.c +++ b/sw/airborne/firmwares/beth/main_overo.c @@ -61,7 +61,8 @@ static uint32_t foo = 0; static uint8_t spi_crc_ok = 1; static uint8_t last_state = 1; -int main(int argc, char *argv[]) { +int main(int argc, char *argv[]) +{ (void) signal(SIGINT, main_exit); @@ -94,24 +95,25 @@ int main(int argc, char *argv[]) { event_dispatch(); //should never occur! - printf("goodbye! (%d)\n",foo); + printf("goodbye! (%d)\n", foo); return 0; } #define PITCH_MAGIC_NUMBER (121) -static void main_periodic(int my_sig_num) { +static void main_periodic(int my_sig_num) +{ -/* static int bar=0; - if (!(foo%2000)) { - if (bar) { - controller.tilt_sp = -0.4; bar=0; - }else{ - controller.tilt_sp = 0.4; bar=1; + /* static int bar=0; + if (!(foo%2000)) { + if (bar) { + controller.tilt_sp = -0.4; bar=0; + }else{ + controller.tilt_sp = 0.4; bar=1; + } } - } -*/ + */ //if (foo >2000 ) { controller.armed=1;} RunOnceEvery(50, {DOWNLINK_SEND_ALIVE(gcs_com.udp_transport, 16, MD5SUM);}); @@ -121,25 +123,26 @@ static void main_periodic(int my_sig_num) { imu_scale_gyro(&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); + 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); - 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); - 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); + 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); + 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); //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; + 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); } @@ -151,51 +154,53 @@ 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);}); - 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);}) - 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);}); + /* 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);}); + 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);}) + 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);}); - RunOnceEvery(50, {DOWNLINK_SEND_AHRS_EULER(gcs_com.udp_transport, - &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);});*/ + RunOnceEvery(50, {DOWNLINK_SEND_AHRS_EULER(gcs_com.udp_transport, + &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);});*/ RunOnceEvery(33, gcs_com_periodic()); } #if 0 -static void main_parse_cmd_line(int argc, char *argv[]) { +static void main_parse_cmd_line(int argc, char *argv[]) +{ - if (argc>1){ + if (argc > 1) { controller.kp = atof(argv[1]); // printf("kp set to %f\n",kp); - if (argc>2) { + if (argc > 2) { controller.kd = atof(argv[2]); // printf("kd set to %f\n",kd); } else { - controller.kd=1.0; + controller.kd = 1.0; // printf("using default value of kd %f\n",kd); } } else { controller.kp = 0.05; // printf("using default value of kp %f\n",kp); } -/* - if (argc>1){ - printf("args not currently supported\n"); - }*/ + /* + if (argc>1){ + printf("args not currently supported\n"); + }*/ } #endif -static void drive_output() { +static void drive_output() +{ switch (controller.armed) { case 0: if (last_state == 2) { @@ -209,13 +214,13 @@ static void drive_output() { } controller.cmd_pitch = 1; controller.cmd_thrust = 1; - last_state=0; + last_state = 0; } break; case 1: if (last_state != 1) { printf("Entering standby mode.\n"); - last_state=1; + last_state = 1; if (last_state == 0) { controller.elevation_ref = estimator.elevation; controller.elevation_dot_ref = estimator.elevation_dot; @@ -235,7 +240,7 @@ static void drive_output() { } //controller.elevation_sp = ((int32_t)(foo/2048.)%2) ? RadOfDeg(-20) : RadOfDeg(10); control_run(); - last_state=2; + last_state = 2; } break; default: @@ -243,7 +248,8 @@ static void drive_output() { } } -static void main_exit(int sig) { +static void main_exit(int sig) +{ printf("Initiating BETH shutdown...\n"); //since the periodic event is no longer running when we get here right now, @@ -251,7 +257,7 @@ static void main_exit(int sig) { #if 0 printf("Zeroing setpoints..."); uint32_t startfoo = foo; - while (foo < (startfoo +2000)) { + while (foo < (startfoo + 2000)) { controller.tilt_sp = 0; controller.elevation_sp = RadOfDeg(-25); } @@ -266,7 +272,8 @@ static void main_exit(int sig) { -static void main_talk_with_stm32() { +static void main_talk_with_stm32() +{ //static int8_t adder = 1; //uint8_t *fooptr; @@ -279,7 +286,7 @@ static void main_talk_with_stm32() { //msg_out.payload.msg_down.pitch = 0; - spi_link_send(&msg_out, sizeof(struct AutopilotMessageCRCFrame) , &msg_in,&spi_crc_ok); + spi_link_send(&msg_out, sizeof(struct AutopilotMessageCRCFrame) , &msg_in, &spi_crc_ok); /* for debugging overo/stm spi link if (msg_in.payload.msg_up.bench_sensor.x == 0){ @@ -291,12 +298,12 @@ static void main_talk_with_stm32() { foo++; - imu.gyro_unscaled.p = (msg_in.payload.msg_up.gyro.p&0xFFFF); - imu.gyro_unscaled.q = (msg_in.payload.msg_up.gyro.q&0xFFFF); - imu.gyro_unscaled.r = (msg_in.payload.msg_up.gyro.r&0xFFFF); - imu.accel_unscaled.x = (msg_in.payload.msg_up.accel.x&0xFFFF); - imu.accel_unscaled.y = (msg_in.payload.msg_up.accel.y&0xFFFF); - imu.accel_unscaled.z = (msg_in.payload.msg_up.accel.z&0xFFFF); + imu.gyro_unscaled.p = (msg_in.payload.msg_up.gyro.p & 0xFFFF); + imu.gyro_unscaled.q = (msg_in.payload.msg_up.gyro.q & 0xFFFF); + imu.gyro_unscaled.r = (msg_in.payload.msg_up.gyro.r & 0xFFFF); + imu.accel_unscaled.x = (msg_in.payload.msg_up.accel.x & 0xFFFF); + imu.accel_unscaled.y = (msg_in.payload.msg_up.accel.y & 0xFFFF); + imu.accel_unscaled.z = (msg_in.payload.msg_up.accel.z & 0xFFFF); } diff --git a/sw/airborne/firmwares/beth/main_stm32.c b/sw/airborne/firmwares/beth/main_stm32.c index f6f3433620..1b284d03d2 100644 --- a/sw/airborne/firmwares/beth/main_stm32.c +++ b/sw/airborne/firmwares/beth/main_stm32.c @@ -33,9 +33,9 @@ #include "lisa/lisa_overo_link.h" #include "bench_sensors.h" -static inline void main_init( void ); -static inline void main_periodic( void ); -static inline void main_event( void ); +static inline void main_init(void); +static inline void main_periodic(void); +static inline void main_event(void); static inline void on_gyro_accel_event(void); static inline void on_accel_event(void); @@ -48,20 +48,23 @@ static inline void main_on_overo_link_error(void); static uint32_t spi_msg_cnt = 0; static uint16_t spi_errors = 0; -int main(void) { +int main(void) +{ main_init(); while (1) { - if (sys_time_check_and_ack_timer(0)) + if (sys_time_check_and_ack_timer(0)) { main_periodic(); + } main_event(); } return 0; } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); actuators_init(); //radio_control_init(); imu_init(); @@ -73,8 +76,9 @@ static inline void main_init( void ) { #define PITCH_MAGIC_NUMBER (121) -static inline void main_periodic( void ) { - int8_t pitch_out,thrust_out; +static inline void main_periodic(void) +{ + int8_t pitch_out, thrust_out; imu_periodic(); OveroLinkPeriodic(main_on_overo_link_lost) @@ -91,8 +95,8 @@ static inline void main_periodic( void ) { pitch_out = (int8_t)((0xFF) & overo_link.down.msg.pitch); thrust_out = (int8_t)((0xFF) & overo_link.down.msg.thrust); - Bound(pitch_out,-80,80); - Bound(thrust_out,0,100); + Bound(pitch_out, -80, 80); + Bound(thrust_out, 0, 100); overo_link.up.msg.thrust_out = thrust_out; overo_link.up.msg.pitch_out = pitch_out; @@ -112,12 +116,14 @@ static inline void main_periodic( void ) { } } -static inline void main_event( void ) { +static inline void main_event(void) +{ ImuEvent(on_gyro_accel_event, on_accel_event, on_mag_event); - OveroLinkEvent(main_on_overo_msg_received,main_on_overo_link_error); + OveroLinkEvent(main_on_overo_msg_received, main_on_overo_link_error); } -static inline void main_on_overo_msg_received(void) { +static inline void main_on_overo_msg_received(void) +{ overo_link.up.msg.bench_sensor.x = bench_sensors.angle_1; overo_link.up.msg.bench_sensor.y = bench_sensors.angle_2; @@ -143,70 +149,73 @@ static inline void main_on_overo_msg_received(void) { } -static inline void main_on_overo_link_lost(void) { +static inline void main_on_overo_link_lost(void) +{ //actuators_set(FALSE); spi_msg_cnt = 0; } -static inline void on_accel_event(void) { +static inline void on_accel_event(void) +{ } -static inline void on_gyro_accel_event(void) { +static inline void on_gyro_accel_event(void) +{ imu_scale_gyro(&imu); imu_scale_accel(&imu); //LED_TOGGLE(2); static uint8_t cnt; cnt++; - if (cnt > 15) cnt = 0; + if (cnt > 15) { cnt = 0; } if (cnt == 0) { DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, - &imu.gyro_unscaled.p, - &imu.gyro_unscaled.q, - &imu.gyro_unscaled.r); + &imu.gyro_unscaled.p, + &imu.gyro_unscaled.q, + &imu.gyro_unscaled.r); DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, - &imu.accel_unscaled.x, - &imu.accel_unscaled.y, - &imu.accel_unscaled.z); - } - else if (cnt == 7) { + &imu.accel_unscaled.x, + &imu.accel_unscaled.y, + &imu.accel_unscaled.z); + } else if (cnt == 7) { DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, - &imu.gyro.p, - &imu.gyro.q, - &imu.gyro.r); + &imu.gyro.p, + &imu.gyro.q, + &imu.gyro.r); DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, - &imu.accel.x, - &imu.accel.y, - &imu.accel.z); + &imu.accel.x, + &imu.accel.y, + &imu.accel.z); } } -static inline void on_mag_event(void) { +static inline void on_mag_event(void) +{ imu_scale_mag(&imu); static uint8_t cnt; cnt++; - if (cnt > 1) cnt = 0; + if (cnt > 1) { cnt = 0; } - if (cnt%2) { + if (cnt % 2) { DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, - &imu.mag.x, - &imu.mag.y, - &imu.mag.z); - } - else { + &imu.mag.x, + &imu.mag.y, + &imu.mag.z); + } else { DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, - &imu.mag_unscaled.x, - &imu.mag_unscaled.y, - &imu.mag_unscaled.z); + &imu.mag_unscaled.x, + &imu.mag_unscaled.y, + &imu.mag_unscaled.z); } } -static inline void main_on_overo_link_error(void){ +static inline void main_on_overo_link_error(void) +{ spi_errors++; } diff --git a/sw/airborne/firmwares/beth/overo_controller.c b/sw/airborne/firmwares/beth/overo_controller.c index d548106640..f07c89ae4c 100644 --- a/sw/airborne/firmwares/beth/overo_controller.c +++ b/sw/airborne/firmwares/beth/overo_controller.c @@ -9,7 +9,8 @@ struct OveroController controller; -void control_init(void) { +void control_init(void) +{ // controller.kp = 0.05; // controller.kd = 0.01; @@ -54,41 +55,45 @@ void control_init(void) { controller.armed = 0; } -void control_send_messages(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) { +void control_run(void) +{ /* * propagate reference */ - const float dt_ctl = 1./512.; + const float dt_ctl = 1. / 512.; const float thrust_constant = 40.; controller.tilt_ref = controller.tilt_ref + controller.tilt_dot_ref * dt_ctl; controller.tilt_dot_ref = controller.tilt_dot_ref + controller.tilt_ddot_ref * dt_ctl; - controller.tilt_ddot_ref = -2*controller.omega_tilt_ref*controller.xi_ref*controller.tilt_dot_ref - - controller.omega_tilt_ref*controller.omega_tilt_ref*(controller.tilt_ref - controller.tilt_sp); + controller.tilt_ddot_ref = -2 * controller.omega_tilt_ref * controller.xi_ref * controller.tilt_dot_ref + - controller.omega_tilt_ref * controller.omega_tilt_ref * (controller.tilt_ref - controller.tilt_sp); controller.elevation_ref = controller.elevation_ref + controller.elevation_dot_ref * dt_ctl; controller.elevation_dot_ref = controller.elevation_dot_ref + controller.elevation_ddot_ref * dt_ctl; - controller.elevation_ddot_ref = -2*controller.omega_elevation_ref*controller.xi_ref*controller.elevation_dot_ref - - controller.omega_elevation_ref*controller.omega_elevation_ref*(controller.elevation_ref - controller.elevation_sp); + controller.elevation_ddot_ref = -2 * controller.omega_elevation_ref * controller.xi_ref * controller.elevation_dot_ref + - controller.omega_elevation_ref * controller.omega_elevation_ref * (controller.elevation_ref - + controller.elevation_sp); controller.azimuth_ref = controller.azimuth_ref + controller.azimuth_dot_ref * dt_ctl; controller.azimuth_dot_ref = controller.azimuth_dot_ref + controller.azimuth_ddot_ref * dt_ctl; - controller.azimuth_ddot_ref = -2*controller.omega_azimuth_ref*controller.xi_ref*controller.azimuth_dot_ref - - controller.omega_azimuth_ref*controller.omega_azimuth_ref*(controller.azimuth_ref - controller.azimuth_sp); + controller.azimuth_ddot_ref = -2 * controller.omega_azimuth_ref * controller.xi_ref * controller.azimuth_dot_ref + - controller.omega_azimuth_ref * controller.omega_azimuth_ref * (controller.azimuth_ref - controller.azimuth_sp); - static int foo=0; + static int foo = 0; /* * calculate errors @@ -109,27 +114,27 @@ 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) + - controller.one_over_J * (controller.omega_cl * controller.omega_cl * err_azimuth); + controller.one_over_J * (controller.omega_cl * controller.omega_cl * err_azimuth); controller.cmd_pitch = controller.cmd_pitch_ff + controller.cmd_pitch_fb;// + controller.tilt_sp = - controller.azim_gain * (-controller.cmd_azimuth_fb );//+ controller.cmd_azimuth_ff); + controller.azim_gain * (-controller.cmd_azimuth_fb); //+ controller.cmd_azimuth_ff); controller.cmd_thrust = controller.cmd_thrust_ff + controller.cmd_thrust_fb + thrust_constant; - controller.cmd_thrust = controller.cmd_thrust*(1/cos(estimator.elevation)); + controller.cmd_thrust = controller.cmd_thrust * (1 / cos(estimator.elevation)); //if (controller.cmd_thrust<0.) controller.cmd_thrust = 0; - Bound(controller.cmd_thrust,0,100); - Bound(controller.cmd_pitch,-100,100); + Bound(controller.cmd_thrust, 0, 100); + Bound(controller.cmd_pitch, -100, 100); - if (!(foo%100)) { + if (!(foo % 100)) { //printf("pitch : ff:%f fb:%f (%f)\n",controller.cmd_pitch_ff, controller.cmd_pitch_fb,estimator.tilt_dot); //printf("thrust: ff:%f fb:%f (%f %f)\n",controller.cmd_thrust_ff, controller.cmd_thrust_fb,estimator.elevation,estimator.elevation_dot); //printf("%f %f %f\n",controller.tilt_ref,controller.tilt_dot_ref,controller.tilt_ddot_ref); diff --git a/sw/airborne/firmwares/beth/overo_estimator.c b/sw/airborne/firmwares/beth/overo_estimator.c index 46efedf58b..1ea449029a 100644 --- a/sw/airborne/firmwares/beth/overo_estimator.c +++ b/sw/airborne/firmwares/beth/overo_estimator.c @@ -8,36 +8,40 @@ struct OveroEstimator estimator; -void estimator_init(void) { +void estimator_init(void) +{ estimator.tilt_lp_coeff = 0.5; estimator.elevation_lp_coeff = 0.5; estimator.azimuth_lp_coeff = 0.5; } -void estimator_send_messages(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 -void estimator_run(uint16_t tilt_measure, uint16_t elevation_measure, uint16_t azimuth_measure) { +void estimator_run(uint16_t tilt_measure, uint16_t elevation_measure, uint16_t azimuth_measure) +{ const int32_t tilt_neutral = 1970; - const float tilt_scale = 1./580.; + const float tilt_scale = 1. / 580.; const int32_t azimuth_neutral = 2875; - const float azimuth_scale = 1./580.; + const float azimuth_scale = 1. / 580.; const int32_t elevation_neutral = 670; - const float elevation_scale = 1./580.; + const float elevation_scale = 1. / 580.; - estimator.tilt = -(tilt_neutral - (int32_t)tilt_measure ) * tilt_scale; - Bound(estimator.tilt,-89,89); + estimator.tilt = -(tilt_neutral - (int32_t)tilt_measure) * tilt_scale; + Bound(estimator.tilt, -89, 89); //low pass filter tilt gyro estimator.tilt_dot = estimator.tilt_dot + - estimator.tilt_lp_coeff * (RATE_FLOAT_OF_BFP(imu.gyro.q) - estimator.tilt_dot); + estimator.tilt_lp_coeff * (RATE_FLOAT_OF_BFP(imu.gyro.q) - estimator.tilt_dot); /* Second order filter yet to be tested estimator.tilt_dot = estimator.tilt_dot * (2 - estimator.tilt_lp_coeff1 - estimator.tilt_lp_coeff2) + estimator.tilt_lp_coeff1 * estimator.tilt_lp_coeff2 * RATE_FLOAT_OF_BFP(imu.gyro.q) - @@ -45,22 +49,22 @@ void estimator_run(uint16_t tilt_measure, uint16_t elevation_measure, uint16_t a estimator.tilt_lp_coeff1*estimator.tilt_lp_coeff2); */ - estimator.elevation = (elevation_neutral - (int32_t)elevation_measure ) * elevation_scale; - Bound(estimator.elevation,-45,45); + estimator.elevation = (elevation_neutral - (int32_t)elevation_measure) * elevation_scale; + Bound(estimator.elevation, -45, 45); //rotation compensation (mixing of two gyro values to generate a reading that reflects rate along beth axes float rotated_elev_dot = RATE_FLOAT_OF_BFP(imu.gyro.p) * cos(estimator.tilt) + - RATE_FLOAT_OF_BFP(imu.gyro.r) * sin(estimator.tilt); + RATE_FLOAT_OF_BFP(imu.gyro.r) * sin(estimator.tilt); //low pass filter -- should probably increase order and maybe move filtering to measured values. estimator.elevation_dot = estimator.elevation_dot + - estimator.elevation_lp_coeff * (rotated_elev_dot - estimator.elevation_dot); + estimator.elevation_lp_coeff * (rotated_elev_dot - estimator.elevation_dot); - estimator.azimuth = (azimuth_neutral - (int32_t)azimuth_measure ) * azimuth_scale; + estimator.azimuth = (azimuth_neutral - (int32_t)azimuth_measure) * azimuth_scale; //low pass filter azimuth gyro //TODO: compensate rotation and increase order estimator.azimuth_dot = estimator.azimuth_dot + - estimator.azimuth_lp_coeff * (RATE_FLOAT_OF_BFP(imu.gyro.r) - estimator.azimuth_dot); + estimator.azimuth_lp_coeff * (RATE_FLOAT_OF_BFP(imu.gyro.r) - estimator.azimuth_dot); } diff --git a/sw/airborne/firmwares/beth/overo_file_logger.c b/sw/airborne/firmwares/beth/overo_file_logger.c index 2dca111fde..be1ece3a10 100644 --- a/sw/airborne/firmwares/beth/overo_file_logger.c +++ b/sw/airborne/firmwares/beth/overo_file_logger.c @@ -4,23 +4,28 @@ struct FileLogger file_logger; -void file_logger_init(char* filename) { +void file_logger_init(char *filename) +{ - file_logger.outfile = fopen(filename,"w+"); + file_logger.outfile = fopen(filename, "w+"); } -void file_logger_periodic(void) { +void file_logger_periodic(void) +{ static uint32_t foo = 0; foo++; - fprintf(file_logger.outfile,"%f %d IMU_ACCEL_RAW %d %d %d\n",foo/512.,42,imu.accel_unscaled.x,imu.accel_unscaled.y,imu.accel_unscaled.z); - fprintf(file_logger.outfile,"%f %d IMU_GYRO_RAW %d %d %d\n",foo/512.,42,imu.gyro_unscaled.p,imu.gyro_unscaled.q,imu.gyro_unscaled.r); + fprintf(file_logger.outfile, "%f %d IMU_ACCEL_RAW %d %d %d\n", foo / 512., 42, imu.accel_unscaled.x, + imu.accel_unscaled.y, imu.accel_unscaled.z); + fprintf(file_logger.outfile, "%f %d IMU_GYRO_RAW %d %d %d\n", foo / 512., 42, imu.gyro_unscaled.p, imu.gyro_unscaled.q, + imu.gyro_unscaled.r); } -void file_logger_exit(void) { +void file_logger_exit(void) +{ fclose(file_logger.outfile); diff --git a/sw/airborne/firmwares/beth/overo_file_logger.h b/sw/airborne/firmwares/beth/overo_file_logger.h index 0bc4ce5112..11425c8ce9 100644 --- a/sw/airborne/firmwares/beth/overo_file_logger.h +++ b/sw/airborne/firmwares/beth/overo_file_logger.h @@ -4,12 +4,12 @@ #include struct FileLogger { - FILE* outfile; + FILE *outfile; }; extern struct FileLogger file_logger; -extern void file_logger_init(char* filename); +extern void file_logger_init(char *filename); extern void file_logger_periodic(void); extern void file_logger_exit(void); diff --git a/sw/airborne/firmwares/beth/overo_gcs_com.c b/sw/airborne/firmwares/beth/overo_gcs_com.c index edd87f0fca..2427f1b609 100644 --- a/sw/airborne/firmwares/beth/overo_gcs_com.c +++ b/sw/airborne/firmwares/beth/overo_gcs_com.c @@ -25,23 +25,28 @@ static void on_datalink_event(int fd, short event __attribute__((unused)), void struct OveroGcsCom gcs_com; -void gcs_com_init(void) { +void gcs_com_init(void) +{ - gcs_com.network = network_new(GCS_HOST,GCS_PORT,DATALINK_PORT,FALSE); + gcs_com.network = network_new(GCS_HOST, GCS_PORT, DATALINK_PORT, FALSE); gcs_com.udp_transport = udp_transport_new(gcs_com.network); - event_set(&gcs_com.datalink_event, gcs_com.network->socket_in, EV_READ| EV_PERSIST, on_datalink_event, gcs_com.udp_transport); + event_set(&gcs_com.datalink_event, gcs_com.network->socket_in, EV_READ | EV_PERSIST, on_datalink_event, + gcs_com.udp_transport); event_add(&gcs_com.datalink_event, NULL); } -void gcs_com_periodic(void) { - if (gcs_com.udp_transport->Periodic) +void gcs_com_periodic(void) +{ + if (gcs_com.udp_transport->Periodic) { gcs_com.udp_transport->Periodic(gcs_com.udp_transport->impl); + } } -static void on_datalink_event(int fd, short event __attribute__((unused)), void *arg) { +static void on_datalink_event(int fd, short event __attribute__((unused)), void *arg) +{ char buf[255]; int bytes_read; @@ -50,7 +55,7 @@ static void on_datalink_event(int fd, short event __attribute__((unused)), void struct udp_transport *udp_impl = tp->impl; //printf("on datalink event: %d bytes\n",bytes_read); int i = 0; - while (iudp_dl_msg_received) { @@ -64,27 +69,27 @@ static void on_datalink_event(int fd, short event __attribute__((unused)), void #define IdOfMsg(x) (x[1]) -static void dl_handle_msg(struct DownlinkTransport *tp) { +static void dl_handle_msg(struct DownlinkTransport *tp) +{ uint8_t msg_id = IdOfMsg(gcs_com.my_dl_buffer); switch (msg_id) { - case DL_PING: - DOWNLINK_SEND_PONG(tp); - break; + case DL_PING: + DOWNLINK_SEND_PONG(tp); + break; - case DL_SETTING : - { + case DL_SETTING : { uint8_t i = DL_SETTING_index(gcs_com.my_dl_buffer); float var = DL_SETTING_value(gcs_com.my_dl_buffer); DlSetting(i, var); - printf("datalink : %d %f\n",i,var); + printf("datalink : %d %f\n", i, var); DOWNLINK_SEND_DL_VALUE(tp, &i, &var); } break; - default : - printf("datalink did nothing\n"); - break; + default : + printf("datalink did nothing\n"); + break; } } diff --git a/sw/airborne/firmwares/beth/overo_gcs_com.h b/sw/airborne/firmwares/beth/overo_gcs_com.h index f956dcc403..b55abb054d 100644 --- a/sw/airborne/firmwares/beth/overo_gcs_com.h +++ b/sw/airborne/firmwares/beth/overo_gcs_com.h @@ -11,11 +11,11 @@ struct OveroGcsCom { - struct FmsNetwork* network; + struct FmsNetwork *network; struct DownlinkTransport *udp_transport; struct event datalink_event; /* bool_t my_dl_msg_available; */ - uint8_t my_dl_buffer[GCS_COM_DL_BUF_SIZE] __attribute__ ((aligned)); + uint8_t my_dl_buffer[GCS_COM_DL_BUF_SIZE] __attribute__((aligned)); }; diff --git a/sw/airborne/firmwares/beth/overo_sfb_controller.c b/sw/airborne/firmwares/beth/overo_sfb_controller.c index 66730bbbc9..3d4145a075 100644 --- a/sw/airborne/firmwares/beth/overo_sfb_controller.c +++ b/sw/airborne/firmwares/beth/overo_sfb_controller.c @@ -14,17 +14,19 @@ struct OveroController _CO; #define GAIN (RadOfDeg(15)) -static float z0=0, z1=GAIN, z2=0, z3=-GAIN, z4=0; +static float z0 = 0, z1 = GAIN, z2 = 0, z3 = -GAIN, z4 = 0; //static float x2=0, x3=-GAIN, x4=0; -static float x0=GAIN, x1=0, x2=-GAIN, x3=0, x4=0; +static float x0 = GAIN, x1 = 0, x2 = -GAIN, x3 = 0, x4 = 0; -void control_send_messages(void) { +void control_send_messages(void) +{ RunOnceEvery(15, {DOWNLINK_SEND_BETH_CONTROLLER_TWIST(gcs_com.udp_transport, &z0, &z1, &z2, &z3);}); } -void control_init(void) { +void control_init(void) +{ _CO.tilt_sp = 0.; _CO.elevation_sp = RadOfDeg(10); @@ -68,9 +70,10 @@ void control_init(void) { -void control_run(void) { +void control_run(void) +{ - static int foo=0; + static int foo = 0; calc_df_cmd(); @@ -81,71 +84,75 @@ void control_run(void) { _CO.cmd_pitch = _CO.cmd_sfb_pitch + _CO.cmd_df_pitch; _CO.cmd_thrust = _CO.cmd_sfb_thrust + _CO.cmd_df_thrust; - if (!(foo%100)) { - printf("P:%f T:%f \n",_CO.cmd_df_pitch,_CO.cmd_df_thrust); + if (!(foo % 100)) { + printf("P:%f T:%f \n", _CO.cmd_df_pitch, _CO.cmd_df_thrust); } foo++; - Bound(_CO.cmd_thrust,0,100); - Bound(_CO.cmd_pitch,-100,100); + Bound(_CO.cmd_thrust, 0, 100); + Bound(_CO.cmd_pitch, -100, 100); } -void calc_df_cmd(void) { +void calc_df_cmd(void) +{ static uint32_t timecnt = 0; static float time = 0; - const float dt = 1./512.; + const float dt = 1. / 512.; const float g = 9.8; - const float freq1 = 1./(2. * 3.14159); - const float freq2 = 1./(1. * 3.14159); - const float const1 = 9.8/75.; + const float freq1 = 1. / (2. * 3.14159); + const float freq2 = 1. / (1. * 3.14159); + const float const1 = 9.8 / 75.; const float const2 = 0.04; - if (_CO.armed) + if (_CO.armed) { time = timecnt++ * dt; + } -/* x2 = x2 + x3*dt; - x3 = x3 + x4*dt;*/ + /* x2 = x2 + x3*dt; + x3 = x3 + x4*dt;*/ //x4 = GAIN*sin (2 * 3.14159 * freq2 * time); - x0 = GAIN*cos (2 * 3.14159 * freq2 * time); - x1 = -GAIN*sin (2 * 3.14159 * freq2 * time); - x2 = -GAIN*cos (2 * 3.14159 * freq2 * time); - x3 = GAIN*sin (2 * 3.14159 * freq2 * time); - x4 = GAIN*cos (2 * 3.14159 * freq2 * time); + x0 = GAIN * cos(2 * 3.14159 * freq2 * time); + x1 = -GAIN * sin(2 * 3.14159 * freq2 * time); + x2 = -GAIN * cos(2 * 3.14159 * freq2 * time); + x3 = GAIN * sin(2 * 3.14159 * freq2 * time); + x4 = GAIN * cos(2 * 3.14159 * freq2 * time); -/* z0 = z0 + z1*dt ; - z1 = z1 + z2*dt ; - z2 = z2 + z3*dt ; - z3 = z3 + z4*dt ;*/ - z0 = GAIN*sin (2 * 3.14159 * freq1 * time); - z1 = GAIN*cos (2 * 3.14159 * freq1 * time); - z2 = -GAIN*sin (2 * 3.14159 * freq1 * time); - z3 = -GAIN*cos (2 * 3.14159 * freq1 * time); - z4 = GAIN*sin (2 * 3.14159 * freq1 * time); + /* z0 = z0 + z1*dt ; + z1 = z1 + z2*dt ; + z2 = z2 + z3*dt ; + z3 = z3 + z4*dt ;*/ + z0 = GAIN * sin(2 * 3.14159 * freq1 * time); + z1 = GAIN * cos(2 * 3.14159 * freq1 * time); + z2 = -GAIN * sin(2 * 3.14159 * freq1 * time); + z3 = -GAIN * cos(2 * 3.14159 * freq1 * time); + z4 = GAIN * sin(2 * 3.14159 * freq1 * time); - _CO.cmd_df_thrust = (1/const1) * sqrt( powf( x2,2) + powf( (z2 + g) ,2) ) ; - _CO.cmd_df_pitch = (1/const2) * - ( ( x4*(z2+1) - z4*x2 )*( powf(z2 + g,2) + powf(x2,2) ) - ( 2 * (z2 + g) * z3 + 2*x2*x3 ) * ( x3*(z2+g)-z3*x2 ) ) / - powf( ( powf(z2+g,2) + powf(x2,2) ) , 2); + _CO.cmd_df_thrust = (1 / const1) * sqrt(powf(x2, 2) + powf((z2 + g) , 2)) ; + _CO.cmd_df_pitch = (1 / const2) * + ((x4 * (z2 + 1) - z4 * x2) * (powf(z2 + g, 2) + powf(x2, + 2)) - (2 * (z2 + g) * z3 + 2 * x2 * x3) * (x3 * (z2 + g) - z3 * x2)) / + powf((powf(z2 + g, 2) + powf(x2, 2)) , 2); - Bound(_CO.cmd_df_thrust,0,100); - Bound(_CO.cmd_df_pitch,-100,100); + Bound(_CO.cmd_df_thrust, 0, 100); + Bound(_CO.cmd_df_pitch, -100, 100); } -void calc_sfb_cmd(void) { +void calc_sfb_cmd(void) +{ /* * calculate errors */ -/* const float err_tilt = estimator.tilt - _CO.tilt_ref; - const float err_tilt_dot = estimator.tilt_dot - _CO.tilt_dot_ref;*/ + /* const float err_tilt = estimator.tilt - _CO.tilt_ref; + const float err_tilt_dot = estimator.tilt_dot - _CO.tilt_dot_ref;*/ const float err_tilt = estimator.tilt - x0; const float err_tilt_dot = estimator.tilt_dot - x1; -/* const float err_elevation = estimator.elevation - _CO.elevation_ref; - const float err_elevation_dot = estimator.elevation_dot - _CO.elevation_dot_ref;*/ + /* const float err_elevation = estimator.elevation - _CO.elevation_ref; + const float err_elevation_dot = estimator.elevation_dot - _CO.elevation_dot_ref;*/ const float err_elevation = estimator.elevation - z0; const float err_elevation_dot = estimator.elevation_dot - z1; @@ -157,24 +164,24 @@ void calc_sfb_cmd(void) { * Compute state feedback */ - _CO.cmd_sfb_pitch = -1*( -1* - err_azimuth - * ( _CO.o_tilt * _CO.o_tilt * _CO.o_azim * _CO.o_azim * cos(estimator.tilt) ) - / ( _CO.b * _CO.a * _CO.u_t_ref) + - err_elevation - * ( _CO.o_tilt * _CO.o_tilt * _CO.o_elev * _CO.o_elev * sin(estimator.tilt) ) - / ( _CO.b * _CO.a * _CO.u_t_ref) - - err_tilt - * ( _CO.o_tilt * _CO.o_tilt ) / ( _CO.b ) -//+ - err_azimuth_dot - * ( _CO.o_tilt * _CO.o_tilt * 2 * _CO.z_azim * _CO.o_azim * cos(estimator.tilt) ) - / ( _CO.b * _CO.a * _CO.u_t_ref) + - err_elevation_dot - * ( _CO.o_tilt * _CO.o_tilt * 2 * _CO.z_elev * _CO.o_elev * sin(estimator.tilt) ) - / ( _CO.b * _CO.a * _CO.u_t_ref) - - err_tilt_dot - * ( 2 * _CO.o_tilt * _CO.z_tilt ) - / ( _CO.b ) ); + _CO.cmd_sfb_pitch = -1 * (-1 * + err_azimuth + * (_CO.o_tilt * _CO.o_tilt * _CO.o_azim * _CO.o_azim * cos(estimator.tilt)) + / (_CO.b * _CO.a * _CO.u_t_ref) + + err_elevation + * (_CO.o_tilt * _CO.o_tilt * _CO.o_elev * _CO.o_elev * sin(estimator.tilt)) + / (_CO.b * _CO.a * _CO.u_t_ref) - + err_tilt + * (_CO.o_tilt * _CO.o_tilt) / (_CO.b) - //+ + err_azimuth_dot + * (_CO.o_tilt * _CO.o_tilt * 2 * _CO.z_azim * _CO.o_azim * cos(estimator.tilt)) + / (_CO.b * _CO.a * _CO.u_t_ref) + + err_elevation_dot + * (_CO.o_tilt * _CO.o_tilt * 2 * _CO.z_elev * _CO.o_elev * sin(estimator.tilt)) + / (_CO.b * _CO.a * _CO.u_t_ref) - + err_tilt_dot + * (2 * _CO.o_tilt * _CO.z_tilt) + / (_CO.b)); _CO.cmd_sfb_thrust = err_azimuth * _CO.o_azim * _CO.o_azim * sin(estimator.tilt) / _CO.a - @@ -182,9 +189,9 @@ void calc_sfb_cmd(void) { err_azimuth_dot * 2 * _CO.z_azim * _CO.o_azim * sin(estimator.tilt) / _CO.a - err_elevation_dot * 2 * _CO.z_elev * _CO.o_elev * cos(estimator.tilt) / _CO.a ; - _CO.cmd_sfb_thrust = _CO.cmd_sfb_thrust*(1/cos(estimator.elevation)); + _CO.cmd_sfb_thrust = _CO.cmd_sfb_thrust * (1 / cos(estimator.elevation)); //if (_CO.cmd_thrust<0.) _CO.cmd_thrust = 0; - Bound(_CO.cmd_sfb_thrust,0,100); - Bound(_CO.cmd_sfb_pitch,-100,100); + Bound(_CO.cmd_sfb_thrust, 0, 100); + Bound(_CO.cmd_sfb_pitch, -100, 100); } diff --git a/sw/airborne/firmwares/beth/overo_test_uart.c b/sw/airborne/firmwares/beth/overo_test_uart.c index 1cceade1cc..7f98cdb296 100644 --- a/sw/airborne/firmwares/beth/overo_test_uart.c +++ b/sw/airborne/firmwares/beth/overo_test_uart.c @@ -76,25 +76,26 @@ extern uint16_t energy; extern uint16_t adc1; extern uint16_t adc2; -int main(int argc, char *argv[]) { +int main(int argc, char *argv[]) +{ portnum = 0; if (argc > 1) { portnum = atoi(argv[1]); - if (portnum > 10 ) { + if (portnum > 10) { printf("Port number must be <11\n"); return -1; } - if (argc > 2) configgps = atoi(argv[2]); + if (argc > 2) { configgps = atoi(argv[2]); } if (configgps) #ifdef GPS_CONFIGURE - printf("Will configure GPS.\n"); + printf("Will configure GPS.\n"); #else - printf("Rebuild with GPS configure support.\n"); + printf("Rebuild with GPS configure support.\n"); #endif } - printf("Using /dev/ttyUSB%d for GPS\n",portnum); + printf("Using /dev/ttyUSB%d for GPS\n", portnum); (void) signal(SIGINT, main_exit); @@ -114,21 +115,22 @@ int main(int argc, char *argv[]) { #ifdef GPS_CONFIGURE //periodic task is launched so we are now ready to use uart to request gps baud change... - if (configgps) gps_configure_uart(); + if (configgps) { gps_configure_uart(); } #endif event_dispatch(); //should never occur! - printf("goodbye! (%d)\n",foo); + printf("goodbye! (%d)\n", foo); return 0; } -static void main_periodic(int my_sig_num) { +static void main_periodic(int my_sig_num) +{ RunOnceEvery(50, {DOWNLINK_SEND_ALIVE(gcs_com.udp_transport, 16, MD5SUM);}); - RunOnceEvery(5, {DOWNLINK_SEND_ADC_GENERIC(gcs_com.udp_transport,&adc1,&adc2);}); + RunOnceEvery(5, {DOWNLINK_SEND_ADC_GENERIC(gcs_com.udp_transport, &adc1, &adc2);}); #if USE_UART0 uart0_handler(); @@ -165,56 +167,57 @@ uint16_t downlink.nb_msgs; #define DownlinkCountBytes(_chan, _n) downlink.nb_bytes += _n; #define DownlinkStartMessage(_chan, _name, msg_id, payload_len) { \ - downlink.nb_msgs++; \ - Transport(_chan, Header(DownlinkIDsSize(_chan, payload_len))); \ - Transport(_chan, PutUint8(AC_ID)); \ - Transport(_chan, PutNamedUint8(_name, msg_id)); \ -} + downlink.nb_msgs++; \ + Transport(_chan, Header(DownlinkIDsSize(_chan, payload_len))); \ + Transport(_chan, PutUint8(AC_ID)); \ + Transport(_chan, PutNamedUint8(_name, msg_id)); \ + } #define DownlinkEndMessage(_chan) Transport(_chan, Trailer()) #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 -void check_gps(void){ +void check_gps(void) +{ -/* if (GpsTimeoutError) { - printf("gps timeout\n"); - }*/ + /* if (GpsTimeoutError) { + printf("gps timeout\n"); + }*/ if (GpsBuffer()) { ReadGpsBuffer(); } if (gps_msg_received) { #ifdef GPS_CONFIGURE - if (gps_configuring) + if (gps_configuring) { gps_configure(); - else { + } else { if (!donegpsconf) { printf("Finished GPS configuration.\n"); - donegpsconf=1; + donegpsconf = 1; } parse_gps_msg(); } #else parse_gps_msg(); #endif - printf("gps msg rx %x %x\n",ubx_class,ubx_id); + printf("gps msg rx %x %x\n", ubx_class, ubx_id); const uint8_t ac_id = 3; //DOWNLINK_SEND_HITL_UBX(gcs_com.udp_transport, &ubx_class, &ubx_id, &ac_id, &ubx_len ,ubx_msg_buf); - DOWNLINK_SEND_HITL_UBX(PprzTransport, &ubx_class, &ubx_id, &ac_id, ubx_len ,ubx_msg_buf); + DOWNLINK_SEND_HITL_UBX(PprzTransport, &ubx_class, &ubx_id, &ac_id, ubx_len , ubx_msg_buf); gps_msg_received = FALSE; if (gps_pos_available) { printf("gps pos avail\n"); @@ -226,12 +229,14 @@ void check_gps(void){ } } -static void main_exit(int sig) { +static void main_exit(int sig) +{ printf("Application Exiting...\n"); exit(EXIT_SUCCESS); } -static void main_talk_with_tiny() { +static void main_talk_with_tiny() +{ //unsigned char c='D'; //write(spfd,&c,1); //if (read(spfd,&c,1)>0) write(STDOUT_FILENO,&c,1); diff --git a/sw/airborne/firmwares/beth/overo_twist_controller.c b/sw/airborne/firmwares/beth/overo_twist_controller.c index bc9b23fb20..35dece277a 100644 --- a/sw/airborne/firmwares/beth/overo_twist_controller.c +++ b/sw/airborne/firmwares/beth/overo_twist_controller.c @@ -10,22 +10,26 @@ struct OveroTwistController controller; -void control_send_messages(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); + }); 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); + }); } -void control_init(void) { +void control_init(void) +{ printf("Twisting controller initializing\n"); @@ -33,7 +37,7 @@ void control_init(void) { controller.elevation_sp = RadOfDeg(10); controller.azimuth_sp = 0.; - // controller.omega_tilt_ref = RadOfDeg(600); +// controller.omega_tilt_ref = RadOfDeg(600); controller.omega_tilt_ref = RadOfDeg(1200); controller.omega_elevation_ref = RadOfDeg(120); controller.omega_azimuth_ref = RadOfDeg(60); @@ -87,37 +91,40 @@ void control_init(void) { controller.c = 4.5; controller.error = 0; - printf("Vm=%f VM=%f satval1=%f satval2=%f c=%f\n",controller.Vm,controller.VM,controller.satval1,controller.satval2,controller.c); + printf("Vm=%f VM=%f satval1=%f satval2=%f c=%f\n", controller.Vm, controller.VM, controller.satval1, controller.satval2, + controller.c); } -void control_run(void) { - static int foo=0; +void control_run(void) +{ + static int foo = 0; /* * propagate reference */ - const float dt_ctl = 1./512.; + const float dt_ctl = 1. / 512.; const float thrust_constant = 40.; //make setpoint a cosine ; for testing - float new_sp = controller.tilt_sp * cos(6.28/5.*foo/512.); + float new_sp = controller.tilt_sp * cos(6.28 / 5.*foo / 512.); controller.tilt_ref = controller.tilt_ref + controller.tilt_dot_ref * dt_ctl; controller.tilt_dot_ref = controller.tilt_dot_ref + controller.tilt_ddot_ref * dt_ctl; - controller.tilt_ddot_ref = -2*controller.omega_tilt_ref*controller.xi_ref*controller.tilt_dot_ref - - controller.omega_tilt_ref*controller.omega_tilt_ref*(controller.tilt_ref - new_sp); + controller.tilt_ddot_ref = -2 * controller.omega_tilt_ref * controller.xi_ref * controller.tilt_dot_ref + - controller.omega_tilt_ref * controller.omega_tilt_ref * (controller.tilt_ref - new_sp); controller.elevation_ref = controller.elevation_ref + controller.elevation_dot_ref * dt_ctl; controller.elevation_dot_ref = controller.elevation_dot_ref + controller.elevation_ddot_ref * dt_ctl; - controller.elevation_ddot_ref = -2*controller.omega_elevation_ref*controller.xi_ref*controller.elevation_dot_ref - - controller.omega_elevation_ref*controller.omega_elevation_ref*(controller.elevation_ref - controller.elevation_sp); + controller.elevation_ddot_ref = -2 * controller.omega_elevation_ref * controller.xi_ref * controller.elevation_dot_ref + - controller.omega_elevation_ref * controller.omega_elevation_ref * (controller.elevation_ref - + controller.elevation_sp); #if USE_AZIMUTH controller.azimuth_ref = controller.azimuth_ref + controller.azimuth_dot_ref * dt_ctl; controller.azimuth_dot_ref = controller.azimuth_dot_ref + controller.azimuth_ddot_ref * dt_ctl; - controller.azimuth_ddot_ref = -2*controller.omega_azimuth_ref*controller.xi_ref*controller.azimuth_dot_ref - - controller.omega_azimuth_ref*controller.omega_azimuth_ref*(controller.azimuth_ref - controller.azimuth_sp); + controller.azimuth_ddot_ref = -2 * controller.omega_azimuth_ref * controller.xi_ref * controller.azimuth_dot_ref + - controller.omega_azimuth_ref * controller.omega_azimuth_ref * (controller.azimuth_ref - controller.azimuth_sp); #endif @@ -125,8 +132,8 @@ void control_run(void) { * calculate errors */ -/* const float err_tilt = estimator.tilt - controller.tilt_ref; - const float err_tilt_dot = estimator.tilt_dot - controller.tilt_dot_ref;*/ + /* const float err_tilt = estimator.tilt - controller.tilt_ref; + const float err_tilt_dot = estimator.tilt_dot - controller.tilt_dot_ref;*/ const float err_elevation = estimator.elevation - controller.elevation_ref; const float err_elevation_dot = estimator.elevation_dot - controller.elevation_dot_ref; @@ -142,25 +149,25 @@ 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.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.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; controller.cmd_azimuth_fb = controller.one_over_J * (2 * controller.xi_cl * controller.omega_cl * err_azimuth_dot) + - controller.one_over_J * (controller.omega_cl * controller.omega_cl * err_azimuth); + controller.one_over_J * (controller.omega_cl * controller.omega_cl * err_azimuth); #endif controller.cmd_pitch = /*controller.cmd_pitch_ff*/ + controller.cmd_pitch_fb; #if USE_AZIMUTH - controller.tilt_sp = controller.azim_gain * (-controller.cmd_azimuth_fb ); + controller.tilt_sp = controller.azim_gain * (-controller.cmd_azimuth_fb); #endif controller.cmd_thrust = get_U_twt2(); @@ -169,10 +176,10 @@ void control_run(void) { // controller.cmd_thrust = controller.cmd_thrust*(1/cos(estimator.elevation)); //if (controller.cmd_thrust<0.) controller.cmd_thrust = 0; - Bound(controller.cmd_thrust,0,100); - Bound(controller.cmd_pitch,-100,100); + Bound(controller.cmd_thrust, 0, 100); + Bound(controller.cmd_pitch, -100, 100); - if (!(foo%128)) { + if (!(foo % 128)) { //printf("pitch : ff:%f fb:%f (%f)\n",controller.cmd_pitch_ff, controller.cmd_pitch_fb,estimator.tilt_dot); //printf("thrust: ff:%f fb:%f (%f %f)\n",controller.cmd_thrust_ff, controller.cmd_thrust_fb,estimator.elevation,estimator.elevation_dot); //printf("%f %f %f\n",controller.tilt_ref,controller.tilt_dot_ref,controller.tilt_ddot_ref); @@ -190,12 +197,12 @@ float get_U_twt() /**Definition des constantes du modèle**/ const float Gain = -65; - const float Te = 1/512.; + 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 + 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; @@ -215,45 +222,43 @@ float get_U_twt() //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[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 -/* 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; + /* 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; + } - 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**/ - 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); } /********************************/ @@ -273,19 +278,19 @@ float get_U_twt2() /**Definition des constantes du modèle**/ const float Gain = 800.; - const float Te = 1/512.; + 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 + 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}; + 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; @@ -301,45 +306,43 @@ float get_U_twt2() //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[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 -/* 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; + /* 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; + } - 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**/ - 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); } /********************************/ @@ -358,9 +361,9 @@ float get_U_twt2() sum = sum + retval; - if (i == (NUMSAMPS-1)) { + if (i == (NUMSAMPS - 1)) { i = 0; - printf("avg: %f\n",sum/(float)NUMSAMPS); + printf("avg: %f\n", sum / (float)NUMSAMPS); sum = 0; } else { i++; diff --git a/sw/airborne/firmwares/beth/overo_twist_controller.h b/sw/airborne/firmwares/beth/overo_twist_controller.h index 0a0ace3bda..11e6819a8f 100644 --- a/sw/airborne/firmwares/beth/overo_twist_controller.h +++ b/sw/airborne/firmwares/beth/overo_twist_controller.h @@ -50,7 +50,7 @@ struct OveroTwistController { int armed; -/***Twisting stuff***/ + /***Twisting stuff***/ float S[2]; float S_dot; diff --git a/sw/airborne/firmwares/beth/rcv_telemetry.c b/sw/airborne/firmwares/beth/rcv_telemetry.c index 6c8a4d37d3..6b2de3f7c7 100644 --- a/sw/airborne/firmwares/beth/rcv_telemetry.c +++ b/sw/airborne/firmwares/beth/rcv_telemetry.c @@ -69,28 +69,29 @@ uint16_t adc2; 37 ENERGY 42 ESTIMATOR */ -void dl_parse_msg(void) { +void dl_parse_msg(void) +{ datalink_time = 0; uint8_t msg_id = IdOfMsg(dl_buffer); - printf("Tiny rx id: %d\n",msg_id); + printf("Tiny rx id: %d\n", msg_id); if (msg_id == DL_ATTITUDE) { - phi = DL_ATTITUDE_phi(dl_buffer); - psi = DL_ATTITUDE_psi(dl_buffer); - theta = DL_ATTITUDE_theta(dl_buffer); - printf("Attitude: %f %f %f\n", phi, psi, theta); + phi = DL_ATTITUDE_phi(dl_buffer); + psi = DL_ATTITUDE_psi(dl_buffer); + theta = DL_ATTITUDE_theta(dl_buffer); + printf("Attitude: %f %f %f\n", phi, psi, theta); } if (msg_id == DL_BAT) { - throttle = DL_BAT_throttle(dl_buffer); - voltage = DL_BAT_voltage(dl_buffer); - amps = DL_BAT_amps(dl_buffer); - energy = DL_BAT_energy(dl_buffer); - printf("BAT: %d %d %d %d\n", throttle,voltage,amps,energy); + throttle = DL_BAT_throttle(dl_buffer); + voltage = DL_BAT_voltage(dl_buffer); + amps = DL_BAT_amps(dl_buffer); + energy = DL_BAT_energy(dl_buffer); + printf("BAT: %d %d %d %d\n", throttle, voltage, amps, energy); } if (msg_id == DL_ADC_GENERIC) { - adc1 = DL_ADC_GENERIC_val1(dl_buffer); - adc2 = DL_ADC_GENERIC_val2(dl_buffer); - printf("ADC: %d %d\n",adc1,adc2); + adc1 = DL_ADC_GENERIC_val1(dl_buffer); + adc2 = DL_ADC_GENERIC_val2(dl_buffer); + printf("ADC: %d %d\n", adc1, adc2); } else { diff --git a/sw/airborne/firmwares/beth/uart_hw.c b/sw/airborne/firmwares/beth/uart_hw.c index 0bb7413531..2afcc1a18c 100644 --- a/sw/airborne/firmwares/beth/uart_hw.c +++ b/sw/airborne/firmwares/beth/uart_hw.c @@ -39,31 +39,32 @@ volatile uint16_t uart0_tx_insert_idx, uart0_tx_extract_idx; volatile bool_t uart0_tx_running; uint8_t uart0_tx_buffer[UART0_TX_BUFFER_SIZE]; -struct SerialPort* fmssp0; +struct SerialPort *fmssp0; int uart0_fd; extern uint8_t portnum; //This function will close our UART and reopen with the new baud rate #ifdef GPS_CONFIGURE -void uart0_init_param( uint16_t baud, uint8_t mode, uint8_t fmode) { +void uart0_init_param(uint16_t baud, uint8_t mode, uint8_t fmode) +{ //serial_port_flush_output(fmssp0); serial_port_close(fmssp0); fmssp0 = serial_port_new(); if (portnum == 0) { - printf("opening ttyUSB0 on uart0 at %d\n",GPS_BAUD); - serial_port_open_raw(fmssp0,"/dev/ttyUSB0",GPS_BAUD); + printf("opening ttyUSB0 on uart0 at %d\n", GPS_BAUD); + serial_port_open_raw(fmssp0, "/dev/ttyUSB0", GPS_BAUD); } if (portnum == 1) { - printf("opening ttyUSB1 on uart0 at %d\n",GPS_BAUD); - serial_port_open_raw(fmssp0,"/dev/ttyUSB1",GPS_BAUD); + printf("opening ttyUSB1 on uart0 at %d\n", GPS_BAUD); + serial_port_open_raw(fmssp0, "/dev/ttyUSB1", GPS_BAUD); } uart0_fd = (int)fmssp0->fd; - // initialize the transmit data queue + // initialize the transmit data queue uart0_tx_extract_idx = 0; uart0_tx_insert_idx = 0; uart0_tx_running = FALSE; @@ -75,19 +76,20 @@ void uart0_init_param( uint16_t baud, uint8_t mode, uint8_t fmode) { } #endif -void uart0_init( void ) { +void uart0_init(void) +{ fmssp0 = serial_port_new(); //TODO: set device name in application and pass as argument if (portnum == 0) { - printf("opening ttyUSB0 on uart0 at %d\n",UART0_BAUD); - serial_port_open_raw(fmssp0,"/dev/ttyUSB0",UART0_BAUD); + printf("opening ttyUSB0 on uart0 at %d\n", UART0_BAUD); + serial_port_open_raw(fmssp0, "/dev/ttyUSB0", UART0_BAUD); } if (portnum == 1) { - printf("opening ttyUSB1 on uart0 at %d\n",UART0_BAUD); - serial_port_open_raw(fmssp0,"/dev/ttyUSB1",UART0_BAUD); + printf("opening ttyUSB1 on uart0 at %d\n", UART0_BAUD); + serial_port_open_raw(fmssp0, "/dev/ttyUSB1", UART0_BAUD); } uart0_fd = (int)fmssp0->fd; @@ -102,54 +104,58 @@ void uart0_init( void ) { } -void uart0_transmit( uint8_t data ) { +void uart0_transmit(uint8_t data) +{ uint16_t temp = (uart0_tx_insert_idx + 1) % UART0_TX_BUFFER_SIZE; - if (temp == uart0_tx_extract_idx) - return; // no room + if (temp == uart0_tx_extract_idx) { + return; // no room + } // check if in process of sending data if (uart0_tx_running) { // yes, add to queue uart0_tx_buffer[uart0_tx_insert_idx] = data; uart0_tx_insert_idx = temp; - } - else { // no, set running flag and write to output register + } else { // no, set running flag and write to output register uart0_tx_running = TRUE; - write(uart0_fd,&data,1); + write(uart0_fd, &data, 1); //printf("w %x\n",data); } } -bool_t uart0_check_free_space( uint8_t len) { +bool_t uart0_check_free_space(uint8_t len) +{ int16_t space = uart0_tx_extract_idx - uart0_tx_insert_idx; - if (space <= 0) + if (space <= 0) { space += UART0_TX_BUFFER_SIZE; + } return (uint16_t)(space - 1) >= len; } -void uart0_handler(void) { - unsigned char c='D'; +void uart0_handler(void) +{ + unsigned char c = 'D'; - // check if more data to send - if (uart0_tx_insert_idx != uart0_tx_extract_idx) { - write(uart0_fd,&uart0_tx_buffer[uart0_tx_extract_idx],1); - //printf("w %x\n",uart0_tx_buffer[uart0_tx_extract_idx]); - uart0_tx_extract_idx++; - uart0_tx_extract_idx %= UART0_TX_BUFFER_SIZE; - } - else { - uart0_tx_running = FALSE; // clear running flag - } + // check if more data to send + if (uart0_tx_insert_idx != uart0_tx_extract_idx) { + write(uart0_fd, &uart0_tx_buffer[uart0_tx_extract_idx], 1); + //printf("w %x\n",uart0_tx_buffer[uart0_tx_extract_idx]); + uart0_tx_extract_idx++; + uart0_tx_extract_idx %= UART0_TX_BUFFER_SIZE; + } else { + uart0_tx_running = FALSE; // clear running flag + } - if(read(uart0_fd,&c,1) > 0){ + if (read(uart0_fd, &c, 1) > 0) { //printf("r %x %c\n",c,c); uint16_t temp = (uart0_rx_insert_idx + 1) % UART0_RX_BUFFER_SIZE; uart0_rx_buffer[uart0_rx_insert_idx] = c; // check for more room in queue - if (temp != uart0_rx_extract_idx) - uart0_rx_insert_idx = temp; // update insert index + if (temp != uart0_rx_extract_idx) { + uart0_rx_insert_idx = temp; // update insert index + } } } @@ -165,20 +171,21 @@ volatile uint16_t uart1_tx_insert_idx, uart1_tx_extract_idx; volatile bool_t uart1_tx_running; uint8_t uart1_tx_buffer[UART1_TX_BUFFER_SIZE]; -struct SerialPort* fmssp1; +struct SerialPort *fmssp1; int uart1_fd; -void uart1_init( void ) { +void uart1_init(void) +{ fmssp1 = serial_port_new(); if (portnum == 0) { - printf("opening ttyUSB1 on uart1 at %d\n",UART1_BAUD); - serial_port_open_raw(fmssp1,"/dev/ttyUSB1",UART1_BAUD); + printf("opening ttyUSB1 on uart1 at %d\n", UART1_BAUD); + serial_port_open_raw(fmssp1, "/dev/ttyUSB1", UART1_BAUD); } if (portnum == 1) { - printf("opening ttyUSB0 on uart1 at %d\n",UART1_BAUD); - serial_port_open_raw(fmssp1,"/dev/ttyUSB0",UART1_BAUD); + printf("opening ttyUSB0 on uart1 at %d\n", UART1_BAUD); + serial_port_open_raw(fmssp1, "/dev/ttyUSB0", UART1_BAUD); } uart1_fd = (int)fmssp1->fd; @@ -194,61 +201,65 @@ void uart1_init( void ) { } -void uart1_transmit( uint8_t data ) { +void uart1_transmit(uint8_t data) +{ uint16_t temp = (uart1_tx_insert_idx + 1) % UART1_TX_BUFFER_SIZE; - if (temp == uart1_tx_extract_idx) - return; // no room + if (temp == uart1_tx_extract_idx) { + return; // no room + } // check if in process of sending data if (uart1_tx_running) { // yes, add to queue uart1_tx_buffer[uart1_tx_insert_idx] = data; uart1_tx_insert_idx = temp; - } - else { // no, set running flag and write to output register + } else { // no, set running flag and write to output register uart1_tx_running = TRUE; //printf("z %x\n",data); - write(uart1_fd,&data,1); + write(uart1_fd, &data, 1); } } -bool_t uart1_check_free_space( uint8_t len) { +bool_t uart1_check_free_space(uint8_t len) +{ int16_t space = uart1_tx_extract_idx - uart1_tx_insert_idx; - if (space <= 0) + if (space <= 0) { space += UART1_TX_BUFFER_SIZE; + } return (uint16_t)(space - 1) >= len; } -void uart1_handler(void) { - unsigned char c='D'; +void uart1_handler(void) +{ + unsigned char c = 'D'; - // check if more data to send - if (uart1_tx_insert_idx != uart1_tx_extract_idx) { - write(uart1_fd,&uart1_tx_buffer[uart1_tx_extract_idx],1); - //printf("z %x\n",uart1_tx_buffer[uart1_tx_extract_idx]); - uart1_tx_extract_idx++; - uart1_tx_extract_idx %= UART1_TX_BUFFER_SIZE; - } - else { - uart1_tx_running = FALSE; // clear running flag - } + // check if more data to send + if (uart1_tx_insert_idx != uart1_tx_extract_idx) { + write(uart1_fd, &uart1_tx_buffer[uart1_tx_extract_idx], 1); + //printf("z %x\n",uart1_tx_buffer[uart1_tx_extract_idx]); + uart1_tx_extract_idx++; + uart1_tx_extract_idx %= UART1_TX_BUFFER_SIZE; + } else { + uart1_tx_running = FALSE; // clear running flag + } - if(read(uart1_fd,&c,1) > 0){ + if (read(uart1_fd, &c, 1) > 0) { //printf("s %x %c\n",c,c); uint16_t temp = (uart1_rx_insert_idx + 1) % UART1_RX_BUFFER_SIZE;; uart1_rx_buffer[uart1_rx_insert_idx] = c; // check for more room in queue - if (temp != uart1_rx_extract_idx) - uart1_rx_insert_idx = temp; // update insert index + if (temp != uart1_rx_extract_idx) { + uart1_rx_insert_idx = temp; // update insert index + } } } #endif /* USE_UART1 */ -void uart_init( void ) +void uart_init(void) { #if USE_UART0 uart0_init(); diff --git a/sw/airborne/firmwares/beth/uart_hw.h b/sw/airborne/firmwares/beth/uart_hw.h index ae57ca153f..e36d1fad48 100644 --- a/sw/airborne/firmwares/beth/uart_hw.h +++ b/sw/airborne/firmwares/beth/uart_hw.h @@ -56,11 +56,11 @@ extern volatile bool_t uart0_tx_running; extern uint8_t uart0_tx_buffer[UART0_TX_BUFFER_SIZE]; #define UART0ChAvailable() (uart0_rx_insert_idx != uart0_rx_extract_idx) -#define UART0Getch() ({ \ - uint8_t ret = uart0_rx_buffer[uart0_rx_extract_idx]; \ - uart0_rx_extract_idx = (uart0_rx_extract_idx + 1)%UART0_RX_BUFFER_SIZE; \ - ret; \ - }) +#define UART0Getch() ({ \ + uint8_t ret = uart0_rx_buffer[uart0_rx_extract_idx]; \ + uart0_rx_extract_idx = (uart0_rx_extract_idx + 1)%UART0_RX_BUFFER_SIZE; \ + ret; \ + }) #endif /* USE_UART0 */ @@ -81,16 +81,16 @@ extern volatile bool_t uart1_tx_running; extern uint8_t uart1_tx_buffer[UART1_TX_BUFFER_SIZE]; #define UART1ChAvailable() (uart1_rx_insert_idx != uart1_rx_extract_idx) -#define UART1Getch() ({ \ - uint8_t ret = uart1_rx_buffer[uart1_rx_extract_idx]; \ - uart1_rx_extract_idx = (uart1_rx_extract_idx + 1)%UART1_RX_BUFFER_SIZE; \ - ret; \ - }) +#define UART1Getch() ({ \ + uint8_t ret = uart1_rx_buffer[uart1_rx_extract_idx]; \ + uart1_rx_extract_idx = (uart1_rx_extract_idx + 1)%UART1_RX_BUFFER_SIZE; \ + ret; \ + }) #endif /* USE_UART1 */ -void uart_init( void ); -void uart0_init_param( uint16_t baud, uint8_t mode, uint8_t fmode); +void uart_init(void); +void uart0_init_param(uint16_t baud, uint8_t mode, uint8_t fmode); #endif /* UART_HW_H */ diff --git a/sw/airborne/firmwares/fixedwing/autopilot.c b/sw/airborne/firmwares/fixedwing/autopilot.c index 5c4c437dc6..8f491a8416 100644 --- a/sw/airborne/firmwares/fixedwing/autopilot.c +++ b/sw/airborne/firmwares/fixedwing/autopilot.c @@ -56,53 +56,62 @@ bool_t gps_lost; bool_t power_switch; -static void send_alive(struct transport_tx *trans, struct link_device *dev) { +static void send_alive(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_ALIVE(trans, dev, AC_ID, 16, MD5SUM); } #if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS #include "rc_settings.h" -static void send_rc_settings(struct transport_tx *trans, struct link_device *dev) { - if (!RcSettingsOff()) +static void send_rc_settings(struct transport_tx *trans, struct link_device *dev) +{ + if (!RcSettingsOff()) { pprz_msg_send_SETTINGS(trans, dev, AC_ID, &slider_1_val, &slider_2_val); + } } #else uint8_t rc_settings_mode = 0; #endif -static void send_mode(struct transport_tx *trans, struct link_device *dev) { +static void send_mode(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_PPRZ_MODE(trans, dev, AC_ID, - &pprz_mode, &v_ctl_mode, &lateral_mode, &horizontal_mode, &rc_settings_mode, &mcu1_status); + &pprz_mode, &v_ctl_mode, &lateral_mode, &horizontal_mode, &rc_settings_mode, &mcu1_status); } -void autopilot_send_mode(void) { +void autopilot_send_mode(void) +{ // use default telemetry here #if DOWNLINK send_mode(&(DefaultChannel).trans_tx, &(DefaultDevice).device); #endif } -static void send_attitude(struct transport_tx *trans, struct link_device *dev) { - struct FloatEulers* att = stateGetNedToBodyEulers_f(); +static void send_attitude(struct transport_tx *trans, struct link_device *dev) +{ + struct FloatEulers *att = stateGetNedToBodyEulers_f(); pprz_msg_send_ATTITUDE(trans, dev, AC_ID, - &(att->phi), &(att->psi), &(att->theta)); + &(att->phi), &(att->psi), &(att->theta)); }; -static void send_estimator(struct transport_tx *trans, struct link_device *dev) { +static void send_estimator(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_ESTIMATOR(trans, dev, AC_ID, - &(stateGetPositionUtm_f()->alt), &(stateGetSpeedEnu_f()->z)); + &(stateGetPositionUtm_f()->alt), &(stateGetSpeedEnu_f()->z)); } -static void send_bat(struct transport_tx *trans, struct link_device *dev) { - int16_t amps = (int16_t) (current/10); +static void send_bat(struct transport_tx *trans, struct link_device *dev) +{ + int16_t amps = (int16_t)(current / 10); int16_t e = energy; pprz_msg_send_BAT(trans, dev, AC_ID, - &v_ctl_throttle_slewed, &vsupply, &s, - &autopilot_flight_time, (uint8_t*)(&kill_throttle), - &block_time, &stage_time, &e); + &v_ctl_throttle_slewed, &vsupply, &s, + &autopilot_flight_time, (uint8_t *)(&kill_throttle), + &block_time, &stage_time, &e); } -static void send_energy(struct transport_tx *trans, struct link_device *dev) { +static void send_energy(struct transport_tx *trans, struct link_device *dev) +{ uint16_t e = energy; float vsup = ((float)vsupply) / 10.0f; float curs = ((float)current) / 1000.0f; @@ -110,35 +119,40 @@ static void send_energy(struct transport_tx *trans, struct link_device *dev) { pprz_msg_send_ENERGY(trans, dev, AC_ID, &vsup, &curs, &e, &power); } -static void send_dl_value(struct transport_tx *trans, struct link_device *dev) { +static void send_dl_value(struct transport_tx *trans, struct link_device *dev) +{ PeriodicSendDlValue(trans, dev); } // FIXME not the best place #include "firmwares/fixedwing/stabilization/stabilization_attitude.h" #include CTRL_TYPE_H -static void send_desired(struct transport_tx *trans, struct link_device *dev) { +static void send_desired(struct transport_tx *trans, struct link_device *dev) +{ #ifndef USE_AIRSPEED float v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED; #endif pprz_msg_send_DESIRED(trans, dev, AC_ID, - &h_ctl_roll_setpoint, &h_ctl_pitch_loop_setpoint, &h_ctl_course_setpoint, - &desired_x, &desired_y, &v_ctl_altitude_setpoint, &v_ctl_climb_setpoint, - &v_ctl_auto_airspeed_setpoint); + &h_ctl_roll_setpoint, &h_ctl_pitch_loop_setpoint, &h_ctl_course_setpoint, + &desired_x, &desired_y, &v_ctl_altitude_setpoint, &v_ctl_climb_setpoint, + &v_ctl_auto_airspeed_setpoint); } -static void send_airspeed(struct transport_tx *trans __attribute__((unused)), struct link_device *dev __attribute__((unused))) { +static void send_airspeed(struct transport_tx *trans __attribute__((unused)), + struct link_device *dev __attribute__((unused))) +{ #ifdef MEASURE_AIRSPEED - float* airspeed = stateGetAirspeed_f(); + float *airspeed = stateGetAirspeed_f(); pprz_msg_send_AIRSPEED(trans, dev, AC_ID, airspeed, airspeed, airspeed, airspeed); #elif USE_AIRSPEED pprz_msg_send_AIRSPEED(trans, dev, AC_ID, - stateGetAirspeed_f(), &v_ctl_auto_airspeed_setpoint, - &v_ctl_auto_airspeed_controlled, &v_ctl_auto_groundspeed_setpoint); + stateGetAirspeed_f(), &v_ctl_auto_airspeed_setpoint, + &v_ctl_auto_airspeed_controlled, &v_ctl_auto_groundspeed_setpoint); #endif } -void autopilot_init(void) { +void autopilot_init(void) +{ pprz_mode = PPRZ_MODE_AUTO2; kill_throttle = FALSE; launch = FALSE; diff --git a/sw/airborne/firmwares/fixedwing/autopilot.h b/sw/airborne/firmwares/fixedwing/autopilot.h index 4d969224ba..676c22e37c 100644 --- a/sw/airborne/firmwares/fixedwing/autopilot.h +++ b/sw/airborne/firmwares/fixedwing/autopilot.h @@ -50,13 +50,13 @@ extern void autopilot_init(void); #define PPRZ_MODE_MANUAL 0 #define PPRZ_MODE_AUTO1 1 #define PPRZ_MODE_AUTO2 2 -#define PPRZ_MODE_HOME 3 +#define PPRZ_MODE_HOME 3 #define PPRZ_MODE_GPS_OUT_OF_ORDER 4 #define PPRZ_MODE_NB 5 #define PPRZ_MODE_OF_PULSE(pprz) \ (pprz > THRESHOLD2 ? PPRZ_MODE_AUTO2 : \ - (pprz > THRESHOLD1 ? PPRZ_MODE_AUTO1 : PPRZ_MODE_MANUAL)) + (pprz > THRESHOLD1 ? PPRZ_MODE_AUTO1 : PPRZ_MODE_MANUAL)) extern uint8_t pprz_mode; extern bool_t kill_throttle; @@ -66,8 +66,8 @@ extern uint8_t mcu1_status; extern uint16_t autopilot_flight_time; #define autopilot_ResetFlightTimeAndLaunch(_) { \ - autopilot_flight_time = 0; launch = FALSE; \ -} + autopilot_flight_time = 0; launch = FALSE; \ + } // FIXME, move to control @@ -91,7 +91,7 @@ extern uint16_t vsupply; /** Supply current in milliAmpere. * This the ap copy of the measurement from fbw */ -extern int32_t current; // milliAmpere +extern int32_t current; // milliAmpere /** Energy consumption (mAh) * This is the ap copy of the measurement from fbw @@ -105,9 +105,9 @@ extern bool_t gps_lost; /** Assignment, returning _old_value != _value * Using GCC expression statements */ #define ModeUpdate(_mode, _value) ({ \ - uint8_t new_mode = _value; \ - (_mode != new_mode ? _mode = new_mode, TRUE : FALSE); \ -}) + uint8_t new_mode = _value; \ + (_mode != new_mode ? _mode = new_mode, TRUE : FALSE); \ + }) /** Send mode over telemetry */ @@ -120,10 +120,10 @@ extern bool_t power_switch; #ifdef POWER_SWITCH_GPIO #include "mcu_periph/gpio.h" #define autopilot_SetPowerSwitch(_x) { \ - power_switch = _x; \ - if (_x) { gpio_set(POWER_SWITCH_GPIO); } \ - else { gpio_clear(POWER_SWITCH_GPIO); } \ -} + power_switch = _x; \ + if (_x) { gpio_set(POWER_SWITCH_GPIO); } \ + else { gpio_clear(POWER_SWITCH_GPIO); } \ + } #else // POWER_SWITCH_GPIO #define autopilot_SetPowerSwitch(_x) { power_switch = _x; } #endif // POWER_SWITCH_GPIO diff --git a/sw/airborne/firmwares/fixedwing/datalink.c b/sw/airborne/firmwares/fixedwing/datalink.c index b37fd5b324..456a1bab12 100644 --- a/sw/airborne/firmwares/fixedwing/datalink.c +++ b/sw/airborne/firmwares/fixedwing/datalink.c @@ -79,7 +79,8 @@ uint8_t joystick_block; #define SenderIdOfMsg(x) (x[0]) #define IdOfMsg(x) (x[1]) -void dl_parse_msg(void) { +void dl_parse_msg(void) +{ datalink_time = 0; uint8_t msg_id = IdOfMsg(dl_buffer); #if 0 // not ready yet @@ -89,13 +90,12 @@ void dl_parse_msg(void) { if (sender_id != 0) { switch (msg_id) { #ifdef TCAS - case DL_TCAS_RA: - { - if (DL_TCAS_RESOLVE_ac_id(dl_buffer) == AC_ID && SenderIdOfMsg(dl_buffer) != AC_ID) { - uint8_t ac_id_conflict = SenderIdOfMsg(dl_buffer); - tcas_acs_status[the_acs_id[ac_id_conflict]].resolve = DL_TCAS_RA_resolve(dl_buffer); - } + case DL_TCAS_RA: { + if (DL_TCAS_RESOLVE_ac_id(dl_buffer) == AC_ID && SenderIdOfMsg(dl_buffer) != AC_ID) { + uint8_t ac_id_conflict = SenderIdOfMsg(dl_buffer); + tcas_acs_status[the_acs_id[ac_id_conflict]].resolve = DL_TCAS_RA_resolve(dl_buffer); } + } #endif } return; @@ -106,122 +106,121 @@ void dl_parse_msg(void) { DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice); } else #ifdef TRAFFIC_INFO - if (msg_id == DL_ACINFO && DL_ACINFO_ac_id(dl_buffer) != AC_ID) { - uint8_t id = DL_ACINFO_ac_id(dl_buffer); - float ux = MOfCm(DL_ACINFO_utm_east(dl_buffer)); - float uy = MOfCm(DL_ACINFO_utm_north(dl_buffer)); - float a = MOfCm(DL_ACINFO_alt(dl_buffer)); - float c = RadOfDeg(((float)DL_ACINFO_course(dl_buffer))/ 10.); - float s = MOfCm(DL_ACINFO_speed(dl_buffer)); - float cl = MOfCm(DL_ACINFO_climb(dl_buffer)); - uint32_t t = DL_ACINFO_itow(dl_buffer); - SetAcInfo(id, ux, uy, c, a, s, cl, t); - } else + if (msg_id == DL_ACINFO && DL_ACINFO_ac_id(dl_buffer) != AC_ID) { + uint8_t id = DL_ACINFO_ac_id(dl_buffer); + float ux = MOfCm(DL_ACINFO_utm_east(dl_buffer)); + float uy = MOfCm(DL_ACINFO_utm_north(dl_buffer)); + float a = MOfCm(DL_ACINFO_alt(dl_buffer)); + float c = RadOfDeg(((float)DL_ACINFO_course(dl_buffer)) / 10.); + float s = MOfCm(DL_ACINFO_speed(dl_buffer)); + float cl = MOfCm(DL_ACINFO_climb(dl_buffer)); + uint32_t t = DL_ACINFO_itow(dl_buffer); + SetAcInfo(id, ux, uy, c, a, s, cl, t); + } else #endif #ifdef NAV - if (msg_id == DL_MOVE_WP && DL_MOVE_WP_ac_id(dl_buffer) == AC_ID) { - uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer); - float a = MOfCm(DL_MOVE_WP_alt(dl_buffer)); + if (msg_id == DL_MOVE_WP && DL_MOVE_WP_ac_id(dl_buffer) == AC_ID) { + uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer); + float a = MOfCm(DL_MOVE_WP_alt(dl_buffer)); - /* Computes from (lat, long) in the referenced UTM zone */ - struct LlaCoor_f lla; - lla.lat = RadOfDeg((float)(DL_MOVE_WP_lat(dl_buffer) / 1e7)); - lla.lon = RadOfDeg((float)(DL_MOVE_WP_lon(dl_buffer) / 1e7)); - struct UtmCoor_f utm; - utm.zone = nav_utm_zone0; - utm_of_lla_f(&utm, &lla); - nav_move_waypoint(wp_id, utm.east, utm.north, a); + /* Computes from (lat, long) in the referenced UTM zone */ + struct LlaCoor_f lla; + lla.lat = RadOfDeg((float)(DL_MOVE_WP_lat(dl_buffer) / 1e7)); + lla.lon = RadOfDeg((float)(DL_MOVE_WP_lon(dl_buffer) / 1e7)); + struct UtmCoor_f utm; + utm.zone = nav_utm_zone0; + utm_of_lla_f(&utm, &lla); + nav_move_waypoint(wp_id, utm.east, utm.north, a); - /* Waypoint range is limited. Computes the UTM pos back from the relative - coordinates */ - utm.east = waypoints[wp_id].x + nav_utm_east0; - utm.north = waypoints[wp_id].y + nav_utm_north0; - DOWNLINK_SEND_WP_MOVED(DefaultChannel, DefaultDevice, &wp_id, &utm.east, &utm.north, &a, &nav_utm_zone0); - } else if (msg_id == DL_BLOCK && DL_BLOCK_ac_id(dl_buffer) == AC_ID) { - nav_goto_block(DL_BLOCK_block_id(dl_buffer)); - SEND_NAVIGATION(&(DefaultChannel).trans_tx, &(DefaultDevice).device); - } else + /* Waypoint range is limited. Computes the UTM pos back from the relative + coordinates */ + utm.east = waypoints[wp_id].x + nav_utm_east0; + utm.north = waypoints[wp_id].y + nav_utm_north0; + DOWNLINK_SEND_WP_MOVED(DefaultChannel, DefaultDevice, &wp_id, &utm.east, &utm.north, &a, &nav_utm_zone0); + } else if (msg_id == DL_BLOCK && DL_BLOCK_ac_id(dl_buffer) == AC_ID) { + nav_goto_block(DL_BLOCK_block_id(dl_buffer)); + SEND_NAVIGATION(&(DefaultChannel).trans_tx, &(DefaultDevice).device); + } else #endif /** NAV */ #ifdef WIND_INFO - if (msg_id == DL_WIND_INFO && DL_WIND_INFO_ac_id(dl_buffer) == AC_ID) { - struct FloatVect2 wind; - wind.x = DL_WIND_INFO_north(dl_buffer); - wind.y = DL_WIND_INFO_east(dl_buffer); - stateSetHorizontalWindspeed_f(&wind); + if (msg_id == DL_WIND_INFO && DL_WIND_INFO_ac_id(dl_buffer) == AC_ID) { + struct FloatVect2 wind; + wind.x = DL_WIND_INFO_north(dl_buffer); + wind.y = DL_WIND_INFO_east(dl_buffer); + stateSetHorizontalWindspeed_f(&wind); #if !USE_AIRSPEED - float airspeed = DL_WIND_INFO_airspeed(dl_buffer); - stateSetAirspeed_f(&airspeed); + float airspeed = DL_WIND_INFO_airspeed(dl_buffer); + stateSetAirspeed_f(&airspeed); #endif #ifdef WIND_INFO_RET - DOWNLINK_SEND_WIND_INFO_RET(DefaultChannel, DefaultDevice, &wind.y, &wind.x, stateGetAirspeed_f()); + DOWNLINK_SEND_WIND_INFO_RET(DefaultChannel, DefaultDevice, &wind.y, &wind.x, stateGetAirspeed_f()); #endif - } else + } else #endif /** WIND_INFO */ #ifdef HITL - /** Infrared and GPS sensors are replaced by messages on the datalink */ - if (msg_id == DL_HITL_INFRARED) { - /** This code simulates infrared.c:ir_update() */ - infrared.roll = DL_HITL_INFRARED_roll(dl_buffer); - infrared.pitch = DL_HITL_INFRARED_pitch(dl_buffer); - infrared.top = DL_HITL_INFRARED_top(dl_buffer); - } else if (msg_id == DL_HITL_UBX) { - /** This code simulates gps_ubx.c:parse_ubx() */ - if (gps_msg_received) { - gps_nb_ovrn++; - } else { - ubx_class = DL_HITL_UBX_class(dl_buffer); - ubx_id = DL_HITL_UBX_id(dl_buffer); - uint8_t l = DL_HITL_UBX_ubx_payload_length(dl_buffer); - uint8_t *ubx_payload = DL_HITL_UBX_ubx_payload(dl_buffer); - memcpy(ubx_msg_buf, ubx_payload, l); - gps_msg_received = TRUE; - } - } else + /** Infrared and GPS sensors are replaced by messages on the datalink */ + if (msg_id == DL_HITL_INFRARED) { + /** This code simulates infrared.c:ir_update() */ + infrared.roll = DL_HITL_INFRARED_roll(dl_buffer); + infrared.pitch = DL_HITL_INFRARED_pitch(dl_buffer); + infrared.top = DL_HITL_INFRARED_top(dl_buffer); + } else if (msg_id == DL_HITL_UBX) { + /** This code simulates gps_ubx.c:parse_ubx() */ + if (gps_msg_received) { + gps_nb_ovrn++; + } else { + ubx_class = DL_HITL_UBX_class(dl_buffer); + ubx_id = DL_HITL_UBX_id(dl_buffer); + uint8_t l = DL_HITL_UBX_ubx_payload_length(dl_buffer); + uint8_t *ubx_payload = DL_HITL_UBX_ubx_payload(dl_buffer); + memcpy(ubx_msg_buf, ubx_payload, l); + gps_msg_received = TRUE; + } + } else #endif #ifdef DlSetting - if (msg_id == DL_SETTING && DL_SETTING_ac_id(dl_buffer) == AC_ID) { - uint8_t i = DL_SETTING_index(dl_buffer); - float val = DL_SETTING_value(dl_buffer); - DlSetting(i, val); - DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val); - } else if (msg_id == DL_GET_SETTING && DL_GET_SETTING_ac_id(dl_buffer) == AC_ID) { - uint8_t i = DL_GET_SETTING_index(dl_buffer); - float val = settings_get_value(i); - DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val); - } else + if (msg_id == DL_SETTING && DL_SETTING_ac_id(dl_buffer) == AC_ID) { + uint8_t i = DL_SETTING_index(dl_buffer); + float val = DL_SETTING_value(dl_buffer); + DlSetting(i, val); + DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val); + } else if (msg_id == DL_GET_SETTING && DL_GET_SETTING_ac_id(dl_buffer) == AC_ID) { + uint8_t i = DL_GET_SETTING_index(dl_buffer); + float val = settings_get_value(i); + DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val); + } else #endif /** Else there is no dl_settings section in the flight plan */ #if USE_JOYSTICK - if (msg_id == DL_JOYSTICK_RAW && DL_JOYSTICK_RAW_ac_id(dl_buffer) == AC_ID) { - JoystickHandeDatalink(DL_JOYSTICK_RAW_roll(dl_buffer), - DL_JOYSTICK_RAW_pitch(dl_buffer), - DL_JOYSTICK_RAW_throttle(dl_buffer)); - } else + if (msg_id == DL_JOYSTICK_RAW && DL_JOYSTICK_RAW_ac_id(dl_buffer) == AC_ID) { + JoystickHandeDatalink(DL_JOYSTICK_RAW_roll(dl_buffer), + DL_JOYSTICK_RAW_pitch(dl_buffer), + DL_JOYSTICK_RAW_throttle(dl_buffer)); + } else #endif // USE_JOYSTICK #if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK - if (msg_id == DL_RC_3CH /*&& DL_RC_3CH_ac_id(dl_buffer) == TX_ID*/) { + if (msg_id == DL_RC_3CH /*&& DL_RC_3CH_ac_id(dl_buffer) == TX_ID*/) { #ifdef RADIO_CONTROL_DATALINK_LED - LED_TOGGLE(RADIO_CONTROL_DATALINK_LED); + LED_TOGGLE(RADIO_CONTROL_DATALINK_LED); #endif - parse_rc_3ch_datalink( - DL_RC_3CH_throttle_mode(dl_buffer), - DL_RC_3CH_roll(dl_buffer), - DL_RC_3CH_pitch(dl_buffer)); - } else - if (msg_id == DL_RC_4CH && DL_RC_4CH_ac_id(dl_buffer) == AC_ID) { + parse_rc_3ch_datalink( + DL_RC_3CH_throttle_mode(dl_buffer), + DL_RC_3CH_roll(dl_buffer), + DL_RC_3CH_pitch(dl_buffer)); + } else if (msg_id == DL_RC_4CH && DL_RC_4CH_ac_id(dl_buffer) == AC_ID) { #ifdef RADIO_CONTROL_DATALINK_LED - LED_TOGGLE(RADIO_CONTROL_DATALINK_LED); + LED_TOGGLE(RADIO_CONTROL_DATALINK_LED); #endif - parse_rc_4ch_datalink( - DL_RC_4CH_mode(dl_buffer), - DL_RC_4CH_throttle(dl_buffer), - DL_RC_4CH_roll(dl_buffer), - DL_RC_4CH_pitch(dl_buffer), - DL_RC_4CH_yaw(dl_buffer)); - } else + parse_rc_4ch_datalink( + DL_RC_4CH_mode(dl_buffer), + DL_RC_4CH_throttle(dl_buffer), + DL_RC_4CH_roll(dl_buffer), + DL_RC_4CH_pitch(dl_buffer), + DL_RC_4CH_yaw(dl_buffer)); + } else #endif // RC_DATALINK - { /* Last else */ - /* Parse modules datalink */ - modules_parse_datalink(msg_id); - } + { /* Last else */ + /* Parse modules datalink */ + modules_parse_datalink(msg_id); + } } diff --git a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c index ed50e91934..246b02ebf5 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c +++ b/sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c @@ -166,38 +166,33 @@ float ac_char_cruise_throttle = 0.0f; float ac_char_cruise_pitch = 0.0f; int ac_char_cruise_count = 0; -static void ac_char_average(float* last_v, float new_v, int count) +static void ac_char_average(float *last_v, float new_v, int count) { *last_v = (((*last_v) * (((float)count) - 1.0f)) + new_v) / ((float) count); } static void ac_char_update(float throttle, float pitch, float climb, float accelerate) { - if ((accelerate > -0.02) && (accelerate < 0.02)) - { - if (throttle >= 1.0f) - { + if ((accelerate > -0.02) && (accelerate < 0.02)) { + if (throttle >= 1.0f) { ac_char_climb_count++; - ac_char_average(&ac_char_climb_pitch, pitch * 57.6f, ac_char_climb_count ); - ac_char_average(&ac_char_climb_max , stateGetSpeedEnu_f()->z, ac_char_climb_count ); - } - else if (throttle <= 0.0f) - { + ac_char_average(&ac_char_climb_pitch, pitch * 57.6f, ac_char_climb_count); + ac_char_average(&ac_char_climb_max , stateGetSpeedEnu_f()->z, ac_char_climb_count); + } else if (throttle <= 0.0f) { ac_char_descend_count++; - ac_char_average(&ac_char_descend_pitch, pitch * 57.6f , ac_char_descend_count ); - ac_char_average(&ac_char_descend_max , stateGetSpeedEnu_f()->z , ac_char_descend_count ); - } - else if ((climb > -0.125) && (climb < 0.125)) - { + ac_char_average(&ac_char_descend_pitch, pitch * 57.6f , ac_char_descend_count); + ac_char_average(&ac_char_descend_max , stateGetSpeedEnu_f()->z , ac_char_descend_count); + } else if ((climb > -0.125) && (climb < 0.125)) { ac_char_cruise_count++; - ac_char_average(&ac_char_cruise_throttle , throttle , ac_char_cruise_count ); - ac_char_average(&ac_char_cruise_pitch , pitch * 57.6f , ac_char_cruise_count ); + ac_char_average(&ac_char_cruise_throttle , throttle , ac_char_cruise_count); + ac_char_average(&ac_char_cruise_pitch , pitch * 57.6f , ac_char_cruise_count); } } } -void v_ctl_init( void ) { +void v_ctl_init(void) +{ /* mode */ v_ctl_mode = V_CTL_MODE_MANUAL; @@ -271,10 +266,10 @@ const float dt_navigation = 1.0 / ((float)NAVIGATION_FREQUENCY); * \brief Computes v_ctl_climb_setpoint and sets v_ctl_auto_throttle_submode */ -void v_ctl_altitude_loop( void ) +void v_ctl_altitude_loop(void) { // Airspeed Command Saturation - if (v_ctl_auto_airspeed_setpoint <= STALL_AIRSPEED * 1.23) v_ctl_auto_airspeed_setpoint = STALL_AIRSPEED * 1.23; + if (v_ctl_auto_airspeed_setpoint <= STALL_AIRSPEED * 1.23) { v_ctl_auto_airspeed_setpoint = STALL_AIRSPEED * 1.23; } // Altitude Controller v_ctl_altitude_error = v_ctl_altitude_setpoint - stateGetPositionUtm_f()->alt; @@ -309,7 +304,7 @@ static float low_pass_vdot(float v) * Auto-throttle inner loop * \brief */ -void v_ctl_climb_loop( void ) +void v_ctl_climb_loop(void) { // Airspeed setpoint rate limiter: // AIRSPEED_SETPOINT_SLEW in m/s/s - a change from 15m/s to 18m/s takes 3s with the default value of 1 @@ -318,20 +313,22 @@ void v_ctl_climb_loop( void ) v_ctl_auto_airspeed_setpoint_slew += airspeed_incr; #ifdef V_CTL_AUTO_GROUNDSPEED_SETPOINT - // Ground speed control loop (input: groundspeed error, output: airspeed controlled) +// Ground speed control loop (input: groundspeed error, output: airspeed controlled) float err_groundspeed = (v_ctl_auto_groundspeed_setpoint - (*stateGetHorizontalSpeedNorm_f())); v_ctl_auto_groundspeed_sum_err += err_groundspeed; BoundAbs(v_ctl_auto_groundspeed_sum_err, V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR); - v_ctl_auto_airspeed_controlled = (err_groundspeed + v_ctl_auto_groundspeed_sum_err * v_ctl_auto_groundspeed_igain) * v_ctl_auto_groundspeed_pgain; + v_ctl_auto_airspeed_controlled = (err_groundspeed + v_ctl_auto_groundspeed_sum_err * v_ctl_auto_groundspeed_igain) * + v_ctl_auto_groundspeed_pgain; // Do not allow controlled airspeed below the setpoint if (v_ctl_auto_airspeed_controlled < v_ctl_auto_airspeed_setpoint_slew) { v_ctl_auto_airspeed_controlled = v_ctl_auto_airspeed_setpoint_slew; // reset integrator of ground speed loop - v_ctl_auto_groundspeed_sum_err = v_ctl_auto_airspeed_controlled/(v_ctl_auto_groundspeed_pgain*v_ctl_auto_groundspeed_igain); + v_ctl_auto_groundspeed_sum_err = v_ctl_auto_airspeed_controlled / (v_ctl_auto_groundspeed_pgain * + v_ctl_auto_groundspeed_igain); } #else - v_ctl_auto_airspeed_controlled = v_ctl_auto_airspeed_setpoint_slew; + v_ctl_auto_airspeed_controlled = v_ctl_auto_airspeed_setpoint_slew; #endif // Airspeed outerloop: positive means we need to accelerate @@ -352,7 +349,7 @@ void v_ctl_climb_loop( void ) #endif // Acceleration Error: positive means UAV needs to accelerate: needs extra energy - float vdot_err = low_pass_vdot( v_ctl_desired_acceleration - vdot ); + float vdot_err = low_pass_vdot(v_ctl_desired_acceleration - vdot); // Flight Path Outerloop: positive means needs to climb more: needs extra energy float gamma_err = (v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z) / v_ctl_auto_airspeed_controlled; @@ -364,48 +361,45 @@ void v_ctl_climb_loop( void ) float en_dis_err = gamma_err - vdot_err; // Auto Cruise Throttle - if (launch && (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)) - { + if (launch && (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)) { v_ctl_auto_throttle_nominal_cruise_throttle += - v_ctl_auto_throttle_of_airspeed_igain * speed_error * dt_attidude + v_ctl_auto_throttle_of_airspeed_igain * speed_error * dt_attidude + en_tot_err * v_ctl_energy_total_igain * dt_attidude; - Bound(v_ctl_auto_throttle_nominal_cruise_throttle,0.0f,1.0f); + Bound(v_ctl_auto_throttle_nominal_cruise_throttle, 0.0f, 1.0f); } // Total Controller float controlled_throttle = v_ctl_auto_throttle_nominal_cruise_throttle - + v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint - + v_ctl_auto_throttle_of_airspeed_pgain * speed_error - + v_ctl_energy_total_pgain * en_tot_err; + + v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint + + v_ctl_auto_throttle_of_airspeed_pgain * speed_error + + v_ctl_energy_total_pgain * en_tot_err; - if ((controlled_throttle >= 1.0f) || (controlled_throttle <= 0.0f) || (kill_throttle==1)) - { + if ((controlled_throttle >= 1.0f) || (controlled_throttle <= 0.0f) || (kill_throttle == 1)) { // If your energy supply is not sufficient, then neglect the climb requirement en_dis_err = -vdot_err; // adjust climb_setpoint to maintain airspeed in case of an engine failure or an unrestriced climb - if(v_ctl_climb_setpoint>0) v_ctl_climb_setpoint += - 30. * dt_attidude; - if(v_ctl_climb_setpoint<0) v_ctl_climb_setpoint += 30. * dt_attidude; + if (v_ctl_climb_setpoint > 0) { v_ctl_climb_setpoint += - 30. * dt_attidude; } + if (v_ctl_climb_setpoint < 0) { v_ctl_climb_setpoint += 30. * dt_attidude; } } /* pitch pre-command */ - if (launch && (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)) - { + if (launch && (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)) { v_ctl_auto_throttle_nominal_cruise_pitch += v_ctl_auto_pitch_of_airspeed_igain * (-speed_error) * dt_attidude - + v_ctl_energy_diff_igain * en_dis_err * dt_attidude; - Bound(v_ctl_auto_throttle_nominal_cruise_pitch,H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT); + + v_ctl_energy_diff_igain * en_dis_err * dt_attidude; + Bound(v_ctl_auto_throttle_nominal_cruise_pitch, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT); } float v_ctl_pitch_of_vz = - + (v_ctl_climb_setpoint /*+ d_err * v_ctl_auto_throttle_pitch_of_vz_dgain*/) * v_ctl_auto_throttle_pitch_of_vz_pgain - - v_ctl_auto_pitch_of_airspeed_pgain * speed_error - + v_ctl_auto_pitch_of_airspeed_dgain * vdot - + v_ctl_energy_diff_pgain * en_dis_err - + v_ctl_auto_throttle_nominal_cruise_pitch; -if(kill_throttle) v_ctl_pitch_of_vz = v_ctl_pitch_of_vz - 1/V_CTL_GLIDE_RATIO; + + (v_ctl_climb_setpoint /*+ d_err * v_ctl_auto_throttle_pitch_of_vz_dgain*/) * v_ctl_auto_throttle_pitch_of_vz_pgain + - v_ctl_auto_pitch_of_airspeed_pgain * speed_error + + v_ctl_auto_pitch_of_airspeed_dgain * vdot + + v_ctl_energy_diff_pgain * en_dis_err + + v_ctl_auto_throttle_nominal_cruise_pitch; + if (kill_throttle) { v_ctl_pitch_of_vz = v_ctl_pitch_of_vz - 1 / V_CTL_GLIDE_RATIO; } v_ctl_pitch_setpoint = v_ctl_pitch_of_vz + nav_pitch; - Bound(v_ctl_pitch_setpoint,H_CTL_PITCH_MIN_SETPOINT,H_CTL_PITCH_MAX_SETPOINT) + Bound(v_ctl_pitch_setpoint, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT) ac_char_update(controlled_throttle, v_ctl_pitch_of_vz, v_ctl_climb_setpoint, v_ctl_desired_acceleration); @@ -423,8 +417,9 @@ if(kill_throttle) v_ctl_pitch_of_vz = v_ctl_pitch_of_vz - 1/V_CTL_GLIDE_RATIO; /** \brief Computes slewed throttle from throttle setpoint called at 20Hz */ -void v_ctl_throttle_slew( void ) { +void v_ctl_throttle_slew(void) +{ pprz_t diff_throttle = v_ctl_throttle_setpoint - v_ctl_throttle_slewed; - BoundAbs(diff_throttle, TRIM_PPRZ(V_CTL_THROTTLE_SLEW*MAX_PPRZ)); + BoundAbs(diff_throttle, TRIM_PPRZ(V_CTL_THROTTLE_SLEW * MAX_PPRZ)); v_ctl_throttle_slewed += diff_throttle; } diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_common.h b/sw/airborne/firmwares/fixedwing/guidance/guidance_common.h index fe8fb7e564..eb2d6c67c1 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/guidance_common.h +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_common.h @@ -66,21 +66,21 @@ extern pprz_t v_ctl_throttle_setpoint; extern pprz_t v_ctl_throttle_slewed; extern float v_ctl_pitch_setpoint; -extern void v_ctl_init( void ); -extern void v_ctl_altitude_loop( void ); -extern void v_ctl_climb_loop ( void ); +extern void v_ctl_init(void); +extern void v_ctl_altitude_loop(void); +extern void v_ctl_climb_loop(void); /** Computes throttle_slewed from throttle_setpoint */ -extern void v_ctl_throttle_slew( void ); +extern void v_ctl_throttle_slew(void); #define guidance_v_SetCruiseThrottle(_v) { \ - v_ctl_auto_throttle_cruise_throttle = (_v ? _v : v_ctl_auto_throttle_nominal_cruise_throttle); \ - Bound(v_ctl_auto_throttle_cruise_throttle, v_ctl_auto_throttle_min_cruise_throttle, v_ctl_auto_throttle_max_cruise_throttle); \ -} + v_ctl_auto_throttle_cruise_throttle = (_v ? _v : v_ctl_auto_throttle_nominal_cruise_throttle); \ + Bound(v_ctl_auto_throttle_cruise_throttle, v_ctl_auto_throttle_min_cruise_throttle, v_ctl_auto_throttle_max_cruise_throttle); \ + } -#define guidance_v_SetAutoThrottleIgain(_v) { \ - v_ctl_auto_throttle_igain = _v; \ - v_ctl_auto_throttle_sum_err = 0; \ +#define guidance_v_SetAutoThrottleIgain(_v) { \ + v_ctl_auto_throttle_igain = _v; \ + v_ctl_auto_throttle_sum_err = 0; \ } diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c index 2d95ded2ff..33c391e87f 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c @@ -93,9 +93,9 @@ float v_ctl_pitch_setpoint; #endif float v_ctl_pitch_trim; -inline static void v_ctl_climb_auto_throttle_loop( void ); +inline static void v_ctl_climb_auto_throttle_loop(void); #ifdef V_CTL_AUTO_PITCH_PGAIN -inline static void v_ctl_climb_auto_pitch_loop( void ); +inline static void v_ctl_climb_auto_pitch_loop(void); #endif #if USE_AIRSPEED @@ -119,7 +119,8 @@ float v_ctl_auto_groundspeed_sum_err; #endif -void v_ctl_init( void ) { +void v_ctl_init(void) +{ /* mode */ v_ctl_mode = V_CTL_MODE_MANUAL; @@ -174,26 +175,26 @@ void v_ctl_init( void ) { v_ctl_throttle_setpoint = 0; -/*agressive tuning*/ + /*agressive tuning*/ #ifdef TUNE_AGRESSIVE_CLIMB agr_climb_throttle = AGR_CLIMB_THROTTLE; - #undef AGR_CLIMB_THROTTLE - #define AGR_CLIMB_THROTTLE agr_climb_throttle +#undef AGR_CLIMB_THROTTLE +#define AGR_CLIMB_THROTTLE agr_climb_throttle agr_climb_pitch = AGR_CLIMB_PITCH; - #undef AGR_CLIMB_PITCH - #define AGR_CLIMB_PITCH agr_climb_pitch +#undef AGR_CLIMB_PITCH +#define AGR_CLIMB_PITCH agr_climb_pitch agr_climb_nav_ratio = AGR_CLIMB_NAV_RATIO; - #undef AGR_CLIMB_NAV_RATIO - #define AGR_CLIMB_NAV_RATIO agr_climb_nav_ratio +#undef AGR_CLIMB_NAV_RATIO +#define AGR_CLIMB_NAV_RATIO agr_climb_nav_ratio agr_descent_throttle = AGR_DESCENT_THROTTLE; - #undef AGR_DESCENT_THROTTLE - #define AGR_DESCENT_THROTTLE agr_descent_throttle +#undef AGR_DESCENT_THROTTLE +#define AGR_DESCENT_THROTTLE agr_descent_throttle agr_descent_pitch = AGR_DESCENT_PITCH; - #undef AGR_DESCENT_PITCH - #define AGR_DESCENT_PITCH agr_descent_pitch +#undef AGR_DESCENT_PITCH +#define AGR_DESCENT_PITCH agr_descent_pitch agr_descent_nav_ratio = AGR_DESCENT_NAV_RATIO; - #undef AGR_DESCENT_NAV_RATIO - #define AGR_DESCENT_NAV_RATIO agr_descent_nav_ratio +#undef AGR_DESCENT_NAV_RATIO +#define AGR_DESCENT_NAV_RATIO agr_descent_nav_ratio #endif } @@ -201,50 +202,51 @@ void v_ctl_init( void ) { * outer loop * \brief Computes v_ctl_climb_setpoint and sets v_ctl_auto_throttle_submode */ -void v_ctl_altitude_loop( void ) { +void v_ctl_altitude_loop(void) +{ float altitude_pgain_boost = 1.0; #if USE_AIRSPEED && defined(AGR_CLIMB) // Aggressive climb mode (boost gain of altitude loop) - if ( v_ctl_climb_mode == V_CTL_CLIMB_MODE_AUTO_THROTTLE) { + if (v_ctl_climb_mode == V_CTL_CLIMB_MODE_AUTO_THROTTLE) { float dist = fabs(v_ctl_altitude_error); - altitude_pgain_boost = 1.0 + (V_CTL_AUTO_AGR_CLIMB_GAIN-1.0)*(dist-AGR_BLEND_END)/(AGR_BLEND_START-AGR_BLEND_END); + altitude_pgain_boost = 1.0 + (V_CTL_AUTO_AGR_CLIMB_GAIN - 1.0) * (dist - AGR_BLEND_END) / + (AGR_BLEND_START - AGR_BLEND_END); Bound(altitude_pgain_boost, 1.0, V_CTL_AUTO_AGR_CLIMB_GAIN); } #endif v_ctl_altitude_error = v_ctl_altitude_setpoint - stateGetPositionUtm_f()->alt; v_ctl_climb_setpoint = altitude_pgain_boost * v_ctl_altitude_pgain * v_ctl_altitude_error - + v_ctl_altitude_pre_climb * v_ctl_altitude_pre_climb_correction; + + v_ctl_altitude_pre_climb * v_ctl_altitude_pre_climb_correction; BoundAbs(v_ctl_climb_setpoint, v_ctl_altitude_max_climb); #ifdef AGR_CLIMB - if ( v_ctl_climb_mode == V_CTL_CLIMB_MODE_AUTO_THROTTLE) { + if (v_ctl_climb_mode == V_CTL_CLIMB_MODE_AUTO_THROTTLE) { float dist = fabs(v_ctl_altitude_error); if (dist < AGR_BLEND_END) { v_ctl_auto_throttle_submode = V_CTL_AUTO_THROTTLE_STANDARD; - } - else if (dist > AGR_BLEND_START) { + } else if (dist > AGR_BLEND_START) { v_ctl_auto_throttle_submode = V_CTL_AUTO_THROTTLE_AGRESSIVE; - } - else { + } else { v_ctl_auto_throttle_submode = V_CTL_AUTO_THROTTLE_BLENDED; } } #endif } -void v_ctl_climb_loop ( void ) { +void v_ctl_climb_loop(void) +{ switch (v_ctl_climb_mode) { - case V_CTL_CLIMB_MODE_AUTO_THROTTLE: - default: - v_ctl_climb_auto_throttle_loop(); - break; + case V_CTL_CLIMB_MODE_AUTO_THROTTLE: + default: + v_ctl_climb_auto_throttle_loop(); + break; #ifdef V_CTL_AUTO_PITCH_PGAIN #pragma message "AUTO PITCH Enabled!" - case V_CTL_CLIMB_MODE_AUTO_PITCH: - v_ctl_climb_auto_pitch_loop(); - break; + case V_CTL_CLIMB_MODE_AUTO_PITCH: + v_ctl_climb_auto_pitch_loop(); + break; #endif } } @@ -256,7 +258,8 @@ void v_ctl_climb_loop ( void ) { #if !USE_AIRSPEED -inline static void v_ctl_climb_auto_throttle_loop(void) { +inline static void v_ctl_climb_auto_throttle_loop(void) +{ static float last_err; float f_throttle = 0; @@ -264,54 +267,54 @@ inline static void v_ctl_climb_auto_throttle_loop(void) { float d_err = err - last_err; last_err = err; float controlled_throttle = v_ctl_auto_throttle_cruise_throttle - + v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint - - v_ctl_auto_throttle_pgain * - (err + v_ctl_auto_throttle_igain * v_ctl_auto_throttle_sum_err - + v_ctl_auto_throttle_dgain * d_err); + + v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint + - v_ctl_auto_throttle_pgain * + (err + v_ctl_auto_throttle_igain * v_ctl_auto_throttle_sum_err + + v_ctl_auto_throttle_dgain * d_err); /* pitch pre-command */ - float v_ctl_pitch_of_vz = (v_ctl_climb_setpoint + d_err * v_ctl_auto_throttle_pitch_of_vz_dgain) * v_ctl_auto_throttle_pitch_of_vz_pgain; + float v_ctl_pitch_of_vz = (v_ctl_climb_setpoint + d_err * v_ctl_auto_throttle_pitch_of_vz_dgain) * + v_ctl_auto_throttle_pitch_of_vz_pgain; #if defined AGR_CLIMB switch (v_ctl_auto_throttle_submode) { - case V_CTL_AUTO_THROTTLE_AGRESSIVE: - if (v_ctl_climb_setpoint > 0) { /* Climbing */ - f_throttle = AGR_CLIMB_THROTTLE; - v_ctl_pitch_setpoint = AGR_CLIMB_PITCH; - } - else { /* Going down */ - f_throttle = AGR_DESCENT_THROTTLE; - v_ctl_pitch_setpoint = AGR_DESCENT_PITCH; - } - break; + case V_CTL_AUTO_THROTTLE_AGRESSIVE: + if (v_ctl_climb_setpoint > 0) { /* Climbing */ + f_throttle = AGR_CLIMB_THROTTLE; + v_ctl_pitch_setpoint = AGR_CLIMB_PITCH; + } else { /* Going down */ + f_throttle = AGR_DESCENT_THROTTLE; + v_ctl_pitch_setpoint = AGR_DESCENT_PITCH; + } + break; - case V_CTL_AUTO_THROTTLE_BLENDED: { - float ratio = (fabs(v_ctl_altitude_error) - AGR_BLEND_END) - / (AGR_BLEND_START - AGR_BLEND_END); - f_throttle = (1-ratio) * controlled_throttle; - v_ctl_pitch_setpoint = (1-ratio) * (v_ctl_pitch_of_vz + v_ctl_pitch_trim); - v_ctl_auto_throttle_sum_err += (1-ratio) * err; - BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR); - /* positive error -> too low */ - if (v_ctl_altitude_error > 0) { - f_throttle += ratio * AGR_CLIMB_THROTTLE; - v_ctl_pitch_setpoint += ratio * AGR_CLIMB_PITCH; - } else { - f_throttle += ratio * AGR_DESCENT_THROTTLE; - v_ctl_pitch_setpoint += ratio * AGR_DESCENT_PITCH; + case V_CTL_AUTO_THROTTLE_BLENDED: { + float ratio = (fabs(v_ctl_altitude_error) - AGR_BLEND_END) + / (AGR_BLEND_START - AGR_BLEND_END); + f_throttle = (1 - ratio) * controlled_throttle; + v_ctl_pitch_setpoint = (1 - ratio) * (v_ctl_pitch_of_vz + v_ctl_pitch_trim); + v_ctl_auto_throttle_sum_err += (1 - ratio) * err; + BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR); + /* positive error -> too low */ + if (v_ctl_altitude_error > 0) { + f_throttle += ratio * AGR_CLIMB_THROTTLE; + v_ctl_pitch_setpoint += ratio * AGR_CLIMB_PITCH; + } else { + f_throttle += ratio * AGR_DESCENT_THROTTLE; + v_ctl_pitch_setpoint += ratio * AGR_DESCENT_PITCH; + } + break; } - break; - } - case V_CTL_AUTO_THROTTLE_STANDARD: - default: + case V_CTL_AUTO_THROTTLE_STANDARD: + default: #endif - f_throttle = controlled_throttle; - v_ctl_auto_throttle_sum_err += err; - BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR); - v_ctl_pitch_setpoint = v_ctl_pitch_of_vz + v_ctl_pitch_trim + nav_pitch; + f_throttle = controlled_throttle; + v_ctl_auto_throttle_sum_err += err; + BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR); + v_ctl_pitch_setpoint = v_ctl_pitch_of_vz + v_ctl_pitch_trim + nav_pitch; #if defined AGR_CLIMB - break; + break; } /* switch submode */ #endif @@ -320,7 +323,8 @@ inline static void v_ctl_climb_auto_throttle_loop(void) { #else // USE_AIRSPEED -inline static void v_ctl_climb_auto_throttle_loop(void) { +inline static void v_ctl_climb_auto_throttle_loop(void) +{ float f_throttle = 0; float controlled_throttle; float v_ctl_pitch_of_vz; @@ -337,25 +341,28 @@ inline static void v_ctl_climb_auto_throttle_loop(void) { v_ctl_auto_pitch_sum_err += err; BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR); v_ctl_pitch_of_vz = -v_ctl_auto_pitch_pgain * - (err + v_ctl_auto_pitch_igain * v_ctl_auto_pitch_sum_err); + (err + v_ctl_auto_pitch_igain * v_ctl_auto_pitch_sum_err); // Ground speed control loop (input: groundspeed error, output: airspeed controlled) float err_groundspeed = (v_ctl_auto_groundspeed_setpoint - *stateGetHorizontalSpeedNorm_f()); v_ctl_auto_groundspeed_sum_err += err_groundspeed; BoundAbs(v_ctl_auto_groundspeed_sum_err, V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR); - v_ctl_auto_airspeed_controlled = (err_groundspeed + v_ctl_auto_groundspeed_sum_err * v_ctl_auto_groundspeed_igain) * v_ctl_auto_groundspeed_pgain; + v_ctl_auto_airspeed_controlled = (err_groundspeed + v_ctl_auto_groundspeed_sum_err * v_ctl_auto_groundspeed_igain) * + v_ctl_auto_groundspeed_pgain; // Do not allow controlled airspeed below the setpoint if (v_ctl_auto_airspeed_controlled < v_ctl_auto_airspeed_setpoint) { v_ctl_auto_airspeed_controlled = v_ctl_auto_airspeed_setpoint; - v_ctl_auto_groundspeed_sum_err = v_ctl_auto_airspeed_controlled/(v_ctl_auto_groundspeed_pgain*v_ctl_auto_groundspeed_igain); // reset integrator of ground speed loop + v_ctl_auto_groundspeed_sum_err = v_ctl_auto_airspeed_controlled / (v_ctl_auto_groundspeed_pgain * + v_ctl_auto_groundspeed_igain); // reset integrator of ground speed loop } // Airspeed control loop (input: airspeed controlled, output: throttle controlled) float err_airspeed = (v_ctl_auto_airspeed_controlled - *stateGetAirspeed_f()); v_ctl_auto_airspeed_sum_err += err_airspeed; BoundAbs(v_ctl_auto_airspeed_sum_err, V_CTL_AUTO_AIRSPEED_MAX_SUM_ERR); - controlled_throttle = (err_airspeed + v_ctl_auto_airspeed_sum_err * v_ctl_auto_airspeed_igain) * v_ctl_auto_airspeed_pgain; + controlled_throttle = (err_airspeed + v_ctl_auto_airspeed_sum_err * v_ctl_auto_airspeed_igain) * + v_ctl_auto_airspeed_pgain; // Done, set outputs Bound(controlled_throttle, 0, v_ctl_auto_throttle_max_cruise_throttle); @@ -373,13 +380,14 @@ inline static void v_ctl_climb_auto_throttle_loop(void) { * \brief computes a v_ctl_pitch_setpoint from a climb_setpoint given a fixed throttle */ #ifdef V_CTL_AUTO_PITCH_PGAIN -inline static void v_ctl_climb_auto_pitch_loop(void) { +inline static void v_ctl_climb_auto_pitch_loop(void) +{ float err = stateGetSpeedEnu_f()->z - v_ctl_climb_setpoint; v_ctl_throttle_setpoint = nav_throttle_setpoint; v_ctl_auto_pitch_sum_err += err; BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR); v_ctl_pitch_setpoint = v_ctl_pitch_trim - v_ctl_auto_pitch_pgain * - (err + v_ctl_auto_pitch_igain * v_ctl_auto_pitch_sum_err); + (err + v_ctl_auto_pitch_igain * v_ctl_auto_pitch_sum_err); Bound(v_ctl_pitch_setpoint, V_CTL_AUTO_PITCH_MIN_PITCH, V_CTL_AUTO_PITCH_MAX_PITCH); } #endif @@ -394,8 +402,9 @@ inline static void v_ctl_climb_auto_pitch_loop(void) { /** \brief Computes slewed throttle from throttle setpoint called at 20Hz */ -void v_ctl_throttle_slew( void ) { +void v_ctl_throttle_slew(void) +{ pprz_t diff_throttle = v_ctl_throttle_setpoint - v_ctl_throttle_slewed; - BoundAbs(diff_throttle, TRIM_PPRZ(V_CTL_THROTTLE_SLEW*MAX_PPRZ)); + BoundAbs(diff_throttle, TRIM_PPRZ(V_CTL_THROTTLE_SLEW * MAX_PPRZ)); v_ctl_throttle_slewed += diff_throttle; } diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c index ac19a524ea..a750cc82ca 100644 --- a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c +++ b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c @@ -118,7 +118,8 @@ float v_ctl_auto_groundspeed_sum_err; #define V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR 100 #endif -void v_ctl_init( void ) { +void v_ctl_init(void) +{ /* mode */ v_ctl_mode = V_CTL_MODE_MANUAL; v_ctl_speed_mode = V_CTL_SPEED_THROTTLE; @@ -188,7 +189,8 @@ void v_ctl_init( void ) { #define LEAD_CTRL_A 1. #define LEAD_CTRL_Te (1./60.) -void v_ctl_altitude_loop( void ) { +void v_ctl_altitude_loop(void) +{ static float v_ctl_climb_setpoint_last = 0.; //static float last_lead_input = 0.; @@ -210,11 +212,13 @@ void v_ctl_altitude_loop( void ) { v_ctl_climb_setpoint_last = v_ctl_climb_setpoint; } -static inline void v_ctl_set_pitch ( void ) { +static inline void v_ctl_set_pitch(void) +{ static float last_err = 0.; - if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) + if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) { v_ctl_auto_pitch_sum_err = 0; + } // Compute errors float err = v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z; @@ -222,25 +226,27 @@ static inline void v_ctl_set_pitch ( void ) { last_err = err; if (v_ctl_auto_pitch_igain > 0.) { - v_ctl_auto_pitch_sum_err += err*(1./60.); + v_ctl_auto_pitch_sum_err += err * (1. / 60.); BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR / v_ctl_auto_pitch_igain); } // PI loop + feedforward ctl v_ctl_pitch_setpoint = nav_pitch - + v_ctl_pitch_trim - + v_ctl_auto_throttle_pitch_of_vz_pgain * v_ctl_climb_setpoint - + v_ctl_auto_pitch_pgain * err - + v_ctl_auto_pitch_dgain * d_err - + v_ctl_auto_pitch_igain * v_ctl_auto_pitch_sum_err; + + v_ctl_pitch_trim + + v_ctl_auto_throttle_pitch_of_vz_pgain * v_ctl_climb_setpoint + + v_ctl_auto_pitch_pgain * err + + v_ctl_auto_pitch_dgain * d_err + + v_ctl_auto_pitch_igain * v_ctl_auto_pitch_sum_err; } -static inline void v_ctl_set_throttle( void ) { +static inline void v_ctl_set_throttle(void) +{ static float last_err = 0.; - if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) + if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) { v_ctl_auto_throttle_sum_err = 0; + } // Compute errors float err = v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z; @@ -248,16 +254,16 @@ static inline void v_ctl_set_throttle( void ) { last_err = err; if (v_ctl_auto_throttle_igain > 0.) { - v_ctl_auto_throttle_sum_err += err*(1./60.); + v_ctl_auto_throttle_sum_err += err * (1. / 60.); BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR / v_ctl_auto_throttle_igain); } // PID loop + feedforward ctl controlled_throttle = v_ctl_auto_throttle_cruise_throttle - + v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint - + v_ctl_auto_throttle_pgain * err - + v_ctl_auto_throttle_dgain * d_err - + v_ctl_auto_throttle_igain * v_ctl_auto_throttle_sum_err; + + v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint + + v_ctl_auto_throttle_pgain * err + + v_ctl_auto_throttle_dgain * d_err + + v_ctl_auto_throttle_igain * v_ctl_auto_throttle_sum_err; } @@ -265,7 +271,8 @@ static inline void v_ctl_set_throttle( void ) { #define AIRSPEED_LOOP_PERIOD (1./60.) // Airspeed control loop (input: [airspeed controlled, climb_setpoint], output: [throttle controlled, pitch setpoint]) -static inline void v_ctl_set_airspeed( void ) { +static inline void v_ctl_set_airspeed(void) +{ static float last_err_vz = 0.; static float last_err_as = 0.; @@ -274,26 +281,27 @@ static inline void v_ctl_set_airspeed( void ) { // Compute errors float err_vz = v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z; - float d_err_vz = (err_vz - last_err_vz)*AIRSPEED_LOOP_PERIOD; + float d_err_vz = (err_vz - last_err_vz) * AIRSPEED_LOOP_PERIOD; last_err_vz = err_vz; if (v_ctl_auto_throttle_igain > 0.) { - v_ctl_auto_throttle_sum_err += err_vz*AIRSPEED_LOOP_PERIOD; + v_ctl_auto_throttle_sum_err += err_vz * AIRSPEED_LOOP_PERIOD; BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR / v_ctl_auto_throttle_igain); } if (v_ctl_auto_pitch_igain > 0.) { - v_ctl_auto_pitch_sum_err += err_vz*AIRSPEED_LOOP_PERIOD; + v_ctl_auto_pitch_sum_err += err_vz * AIRSPEED_LOOP_PERIOD; BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR / v_ctl_auto_pitch_igain); } float err_airspeed = v_ctl_auto_airspeed_setpoint - *stateGetAirspeed_f(); - float d_err_airspeed = (err_airspeed - last_err_as)*AIRSPEED_LOOP_PERIOD; + float d_err_airspeed = (err_airspeed - last_err_as) * AIRSPEED_LOOP_PERIOD; last_err_as = err_airspeed; if (v_ctl_auto_airspeed_throttle_igain > 0.) { - v_ctl_auto_airspeed_throttle_sum_err += err_airspeed*AIRSPEED_LOOP_PERIOD; - BoundAbs(v_ctl_auto_airspeed_throttle_sum_err, V_CTL_AUTO_AIRSPEED_THROTTLE_MAX_SUM_ERR / v_ctl_auto_airspeed_throttle_igain); + v_ctl_auto_airspeed_throttle_sum_err += err_airspeed * AIRSPEED_LOOP_PERIOD; + BoundAbs(v_ctl_auto_airspeed_throttle_sum_err, + V_CTL_AUTO_AIRSPEED_THROTTLE_MAX_SUM_ERR / v_ctl_auto_airspeed_throttle_igain); } if (v_ctl_auto_airspeed_pitch_igain > 0.) { - v_ctl_auto_airspeed_pitch_sum_err += err_airspeed*AIRSPEED_LOOP_PERIOD; + v_ctl_auto_airspeed_pitch_sum_err += err_airspeed * AIRSPEED_LOOP_PERIOD; BoundAbs(v_ctl_auto_airspeed_pitch_sum_err, V_CTL_AUTO_AIRSPEED_PITCH_MAX_SUM_ERR / v_ctl_auto_airspeed_pitch_igain); } @@ -319,47 +327,50 @@ static inline void v_ctl_set_airspeed( void ) { // Throttle loop controlled_throttle = v_ctl_auto_throttle_cruise_throttle - + v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint - + v_ctl_auto_throttle_pgain * err_vz - + v_ctl_auto_throttle_dgain * d_err_vz - + v_ctl_auto_throttle_igain * v_ctl_auto_throttle_sum_err - + v_ctl_auto_airspeed_throttle_pgain * err_airspeed - + v_ctl_auto_airspeed_throttle_dgain * d_err_airspeed - + v_ctl_auto_airspeed_throttle_igain * v_ctl_auto_airspeed_throttle_sum_err; + + v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint + + v_ctl_auto_throttle_pgain * err_vz + + v_ctl_auto_throttle_dgain * d_err_vz + + v_ctl_auto_throttle_igain * v_ctl_auto_throttle_sum_err + + v_ctl_auto_airspeed_throttle_pgain * err_airspeed + + v_ctl_auto_airspeed_throttle_dgain * d_err_airspeed + + v_ctl_auto_airspeed_throttle_igain * v_ctl_auto_airspeed_throttle_sum_err; } -static inline void v_ctl_set_groundspeed( void ) { +static inline void v_ctl_set_groundspeed(void) +{ // Ground speed control loop (input: groundspeed error, output: airspeed controlled) float err_groundspeed = v_ctl_auto_groundspeed_setpoint - *stateGetHorizontalSpeedNorm_f(); v_ctl_auto_groundspeed_sum_err += err_groundspeed; BoundAbs(v_ctl_auto_groundspeed_sum_err, V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR); - v_ctl_auto_airspeed_setpoint = err_groundspeed * v_ctl_auto_groundspeed_pgain + v_ctl_auto_groundspeed_sum_err * v_ctl_auto_groundspeed_igain; + v_ctl_auto_airspeed_setpoint = err_groundspeed * v_ctl_auto_groundspeed_pgain + v_ctl_auto_groundspeed_sum_err * + v_ctl_auto_groundspeed_igain; } #endif -void v_ctl_climb_loop ( void ) { +void v_ctl_climb_loop(void) +{ switch (v_ctl_speed_mode) { - case V_CTL_SPEED_THROTTLE: - // Set pitch - v_ctl_set_pitch(); - // Set throttle - v_ctl_set_throttle(); - break; + case V_CTL_SPEED_THROTTLE: + // Set pitch + v_ctl_set_pitch(); + // Set throttle + v_ctl_set_throttle(); + break; #if USE_AIRSPEED - case V_CTL_SPEED_AIRSPEED: - v_ctl_set_airspeed(); - break; - case V_CTL_SPEED_GROUNDSPEED: - v_ctl_set_groundspeed(); - v_ctl_set_airspeed(); - break; + case V_CTL_SPEED_AIRSPEED: + v_ctl_set_airspeed(); + break; + case V_CTL_SPEED_GROUNDSPEED: + v_ctl_set_groundspeed(); + v_ctl_set_airspeed(); + break; #endif - default: - controlled_throttle = v_ctl_auto_throttle_cruise_throttle; // ??? - break; + default: + controlled_throttle = v_ctl_auto_throttle_cruise_throttle; // ??? + break; } // Set Pitch output @@ -379,8 +390,9 @@ void v_ctl_climb_loop ( void ) { /** \brief Computes slewed throttle from throttle setpoint called at 20Hz */ -void v_ctl_throttle_slew( void ) { +void v_ctl_throttle_slew(void) +{ pprz_t diff_throttle = v_ctl_throttle_setpoint - v_ctl_throttle_slewed; - BoundAbs(diff_throttle, TRIM_PPRZ(V_CTL_THROTTLE_SLEW*MAX_PPRZ)); + BoundAbs(diff_throttle, TRIM_PPRZ(V_CTL_THROTTLE_SLEW * MAX_PPRZ)); v_ctl_throttle_slewed += diff_throttle; } diff --git a/sw/airborne/firmwares/fixedwing/main.c b/sw/airborne/firmwares/fixedwing/main.c index 9bec94264f..a0020aa230 100644 --- a/sw/airborne/firmwares/fixedwing/main.c +++ b/sw/airborne/firmwares/fixedwing/main.c @@ -39,7 +39,8 @@ #define Ap(f) #endif -int main( void ) { +int main(void) +{ Fbw(init); Ap(init); while (1) { diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c index 18415ce48b..a599236aff 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.c +++ b/sw/airborne/firmwares/fixedwing/main_ap.c @@ -131,20 +131,21 @@ PRINT_CONFIG_VAR(BARO_PERIODIC_FREQUENCY) #ifdef AHRS_PROPAGATE_FREQUENCY #if (AHRS_PROPAGATE_FREQUENCY > PERIODIC_FREQUENCY) #warning "PERIODIC_FREQUENCY should be least equal or greater than AHRS_PROPAGATE_FREQUENCY" -INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY to at least ",AHRS_PROPAGATE_FREQUENCY) +INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY to at least ", AHRS_PROPAGATE_FREQUENCY) #endif #endif -static inline void on_gyro_event( void ); -static inline void on_accel_event( void ); -static inline void on_mag_event( void ); +static inline void on_gyro_event(void); +static inline void on_accel_event(void); +static inline void on_mag_event(void); volatile uint8_t ahrs_timeout_counter = 0; //FIXME not the correct place -static void send_filter_status(struct transport_tx *trans, struct link_device *dev) { +static void send_filter_status(struct transport_tx *trans, struct link_device *dev) +{ uint8_t mde = 3; - if (ahrs.status == AHRS_UNINIT) mde = 2; - if (ahrs_timeout_counter > 10) mde = 5; + if (ahrs.status == AHRS_UNINIT) { mde = 2; } + if (ahrs_timeout_counter > 10) { mde = 5; } uint16_t val = 0; pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &mde, &val); } @@ -152,7 +153,7 @@ static void send_filter_status(struct transport_tx *trans, struct link_device *d #endif // USE_AHRS && USE_IMU #if USE_GPS -static inline void on_gps_solution( void ); +static inline void on_gps_solution(void); #endif // what version is this ???? @@ -173,7 +174,8 @@ tid_t monitor_tid; ///< id for monitor_task() timer tid_t baro_tid; ///< id for baro_periodic() timer #endif -void init_ap( void ) { +void init_ap(void) +{ #ifndef SINGLE_MCU /** init done in main_fbw in single MCU */ mcu_init(); #endif /* SINGLE_MCU */ @@ -228,14 +230,14 @@ void init_ap( void ) { settings_init(); /**** start timers for periodic functions *****/ - sensors_tid = sys_time_register_timer(1./PERIODIC_FREQUENCY, NULL); - navigation_tid = sys_time_register_timer(1./NAVIGATION_FREQUENCY, NULL); - attitude_tid = sys_time_register_timer(1./CONTROL_FREQUENCY, NULL); - modules_tid = sys_time_register_timer(1./MODULES_FREQUENCY, NULL); - telemetry_tid = sys_time_register_timer(1./TELEMETRY_FREQUENCY, NULL); + sensors_tid = sys_time_register_timer(1. / PERIODIC_FREQUENCY, NULL); + navigation_tid = sys_time_register_timer(1. / NAVIGATION_FREQUENCY, NULL); + attitude_tid = sys_time_register_timer(1. / CONTROL_FREQUENCY, NULL); + modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL); + telemetry_tid = sys_time_register_timer(1. / TELEMETRY_FREQUENCY, NULL); monitor_tid = sys_time_register_timer(1.0, NULL); #if USE_BARO_BOARD - baro_tid = sys_time_register_timer(1./BARO_PERIODIC_FREQUENCY, NULL); + baro_tid = sys_time_register_timer(1. / BARO_PERIODIC_FREQUENCY, NULL); #endif /** - start interrupt task */ @@ -265,29 +267,36 @@ void init_ap( void ) { } -void handle_periodic_tasks_ap(void) { +void handle_periodic_tasks_ap(void) +{ - if (sys_time_check_and_ack_timer(sensors_tid)) + if (sys_time_check_and_ack_timer(sensors_tid)) { sensors_task(); + } #if USE_BARO_BOARD - if (sys_time_check_and_ack_timer(baro_tid)) + if (sys_time_check_and_ack_timer(baro_tid)) { baro_periodic(); + } #endif - if (sys_time_check_and_ack_timer(navigation_tid)) + if (sys_time_check_and_ack_timer(navigation_tid)) { navigation_task(); + } #ifndef AHRS_TRIGGERED_ATTITUDE_LOOP - if (sys_time_check_and_ack_timer(attitude_tid)) + if (sys_time_check_and_ack_timer(attitude_tid)) { attitude_loop(); + } #endif - if (sys_time_check_and_ack_timer(modules_tid)) + if (sys_time_check_and_ack_timer(modules_tid)) { modules_periodic_task(); + } - if (sys_time_check_and_ack_timer(monitor_tid)) + if (sys_time_check_and_ack_timer(monitor_tid)) { monitor_task(); + } if (sys_time_check_and_ack_timer(telemetry_tid)) { reporting_task(); @@ -302,13 +311,14 @@ void handle_periodic_tasks_ap(void) { /** Update paparazzi mode. */ #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1 -static inline uint8_t pprz_mode_update( void ) { +static inline uint8_t pprz_mode_update(void) +{ if ((pprz_mode != PPRZ_MODE_HOME && pprz_mode != PPRZ_MODE_GPS_OUT_OF_ORDER) #ifdef UNLOCKED_HOME_MODE || TRUE #endif - ) { + ) { #ifndef RADIO_AUTO_MODE return ModeUpdate(pprz_mode, PPRZ_MODE_OF_PULSE(fbw_state->channels[RADIO_MODE])); #else @@ -318,7 +328,7 @@ static inline uint8_t pprz_mode_update( void ) { * * This is mainly a cludge for entry level radios with no three-way switch but two available two-way switches which can be used. */ - if(PPRZ_MODE_OF_PULSE(fbw_state->channels[RADIO_MODE]) == PPRZ_MODE_MANUAL) { + if (PPRZ_MODE_OF_PULSE(fbw_state->channels[RADIO_MODE]) == PPRZ_MODE_MANUAL) { /* RADIO_MODE in MANUAL position */ return ModeUpdate(pprz_mode, PPRZ_MODE_MANUAL); } else { @@ -328,19 +338,22 @@ static inline uint8_t pprz_mode_update( void ) { return ModeUpdate(pprz_mode, (fbw_state->channels[RADIO_AUTO_MODE] > THRESHOLD2) ? PPRZ_MODE_AUTO2 : PPRZ_MODE_AUTO1); } #endif // RADIO_AUTO_MODE - } else + } else { return FALSE; + } } #else // not RADIO_CONTROL -static inline uint8_t pprz_mode_update( void ) { +static inline uint8_t pprz_mode_update(void) +{ return FALSE; } #endif -static inline uint8_t mcu1_status_update( void ) { +static inline uint8_t mcu1_status_update(void) +{ uint8_t new_status = fbw_state->status; if (mcu1_status != new_status) { - bool_t changed = ((mcu1_status&MASK_FBW_CHANGED) != (new_status&MASK_FBW_CHANGED)); + bool_t changed = ((mcu1_status & MASK_FBW_CHANGED) != (new_status & MASK_FBW_CHANGED)); mcu1_status = new_status; return changed; } @@ -350,7 +363,8 @@ static inline uint8_t mcu1_status_update( void ) { /** Send back uncontrolled channels. */ -static inline void copy_from_to_fbw ( void ) { +static inline void copy_from_to_fbw(void) +{ #ifdef SetAutoCommandsFromRC SetAutoCommandsFromRC(ap_state->commands, fbw_state->channels); #elif defined RADIO_YAW && defined COMMAND_YAW @@ -366,17 +380,19 @@ static inline void copy_from_to_fbw ( void ) { /** * Function to be called when a message from FBW is available */ -static inline void telecommand_task( void ) { +static inline void telecommand_task(void) +{ uint8_t mode_changed = FALSE; copy_from_to_fbw(); - uint8_t really_lost = bit_is_set(fbw_state->status, STATUS_RADIO_REALLY_LOST) && (pprz_mode == PPRZ_MODE_AUTO1 || pprz_mode == PPRZ_MODE_MANUAL); + uint8_t really_lost = bit_is_set(fbw_state->status, STATUS_RADIO_REALLY_LOST) && (pprz_mode == PPRZ_MODE_AUTO1 + || pprz_mode == PPRZ_MODE_MANUAL); if (pprz_mode != PPRZ_MODE_HOME && pprz_mode != PPRZ_MODE_GPS_OUT_OF_ORDER && launch) { - if (too_far_from_home) { + if (too_far_from_home) { pprz_mode = PPRZ_MODE_HOME; mode_changed = TRUE; } - if (really_lost) { + if (really_lost) { pprz_mode = RC_LOST_MODE; mode_changed = TRUE; } @@ -391,7 +407,7 @@ static inline void telecommand_task( void ) { #endif } mode_changed |= mcu1_status_update(); - if ( mode_changed ) autopilot_send_mode(); + if (mode_changed) { autopilot_send_mode(); } #if defined RADIO_CONTROL || defined RADIO_CONTROL_AUTO1 /** In AUTO1 mode, compute roll setpoint and pitch setpoint from @@ -439,7 +455,8 @@ static inline void telecommand_task( void ) { * Send a series of initialisation messages followed by a stream of periodic ones. * Called at 60Hz. */ -void reporting_task( void ) { +void reporting_task(void) +{ static uint8_t boot = TRUE; /** initialisation phase during boot */ @@ -465,7 +482,8 @@ void reporting_task( void ) { /** * Compute desired_course */ -void navigation_task( void ) { +void navigation_task(void) +{ #if defined FAILSAFE_DELAY_WITHOUT_GPS /** This section is used for the failsafe of GPS */ static uint8_t last_pprz_mode; @@ -490,12 +508,13 @@ void navigation_task( void ) { #endif /* GPS && FAILSAFE_DELAY_WITHOUT_GPS */ common_nav_periodic_task_4Hz(); - if (pprz_mode == PPRZ_MODE_HOME) + if (pprz_mode == PPRZ_MODE_HOME) { nav_home(); - else if (pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER) + } else if (pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER) { nav_without_gps(); - else + } else { nav_periodic_task(); + } #ifdef TCAS CallTCAS(); @@ -508,17 +527,19 @@ void navigation_task( void ) { /* The nav task computes only nav_altitude. However, we are interested by desired_altitude (= nav_alt+alt_shift) in any case. So we always run the altitude control loop */ - if (v_ctl_mode == V_CTL_MODE_AUTO_ALT) + if (v_ctl_mode == V_CTL_MODE_AUTO_ALT) { v_ctl_altitude_loop(); + } if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME - || pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER) { + || pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER) { #ifdef H_CTL_RATE_LOOP /* Be sure to be in attitude mode, not roll */ h_ctl_auto1_rate = FALSE; #endif - if (lateral_mode >=LATERAL_MODE_COURSE) - h_ctl_course_loop(); /* aka compute nav_desired_roll */ + if (lateral_mode >= LATERAL_MODE_COURSE) { + h_ctl_course_loop(); /* aka compute nav_desired_roll */ + } // climb_loop(); //4Hz } @@ -529,25 +550,23 @@ void navigation_task( void ) { volatile uint8_t new_ins_attitude = 0; #endif -void attitude_loop( void ) { +void attitude_loop(void) +{ #if USE_INFRARED ahrs_update_infrared(); #endif /* USE_INFRARED */ - if (pprz_mode >= PPRZ_MODE_AUTO2) - { + if (pprz_mode >= PPRZ_MODE_AUTO2) { if (v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE) { v_ctl_throttle_setpoint = nav_throttle_setpoint; v_ctl_pitch_setpoint = nav_pitch; - } - else if (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB) - { + } else if (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB) { v_ctl_climb_loop(); } #if defined V_CTL_THROTTLE_IDLE - Bound(v_ctl_throttle_setpoint, TRIM_PPRZ(V_CTL_THROTTLE_IDLE*MAX_PPRZ), MAX_PPRZ); + Bound(v_ctl_throttle_setpoint, TRIM_PPRZ(V_CTL_THROTTLE_IDLE * MAX_PPRZ), MAX_PPRZ); #endif #ifdef V_CTL_POWER_CTL_BAT_NOMINAL @@ -559,8 +578,9 @@ void attitude_loop( void ) { h_ctl_pitch_setpoint = v_ctl_pitch_setpoint; // Copy the pitch setpoint from the guidance to the stabilization control Bound(h_ctl_pitch_setpoint, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT); - if (kill_throttle || (!autopilot_flight_time && !launch)) + if (kill_throttle || (!autopilot_flight_time && !launch)) { v_ctl_throttle_setpoint = 0; + } } h_ctl_attitude_loop(); /* Set h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */ @@ -581,20 +601,22 @@ void attitude_loop( void ) { /** Run at PERIODIC_FREQUENCY (60Hz if not defined) */ -void sensors_task( void ) { +void sensors_task(void) +{ #if USE_IMU imu_periodic(); #if USE_AHRS - if (ahrs_timeout_counter < 255) + if (ahrs_timeout_counter < 255) { ahrs_timeout_counter ++; + } #endif // USE_AHRS #endif // USE_IMU //FIXME: this is just a kludge #if USE_AHRS && defined SITL && !USE_NPS // dt is not really used in ahrs_sim - ahrs_propagate(1./PERIODIC_FREQUENCY); + ahrs_propagate(1. / PERIODIC_FREQUENCY); #endif #if USE_GPS @@ -623,18 +645,21 @@ void sensors_task( void ) { #define MIN_SPEED_FOR_TAKEOFF 5. /** monitor stuff run at 1Hz */ -void monitor_task( void ) { - if (autopilot_flight_time) +void monitor_task(void) +{ + if (autopilot_flight_time) { autopilot_flight_time++; + } #if defined DATALINK || defined SITL datalink_time++; #endif static uint8_t t = 0; - if (vsupply < CATASTROPHIC_BAT_LEVEL*10) + if (vsupply < CATASTROPHIC_BAT_LEVEL * 10) { t++; - else + } else { t = 0; + } kill_throttle |= (t >= CATASTROPHIC_BAT_KILL_DELAY); kill_throttle |= launch && (dist2_to_home > Square(KILL_MODE_DISTANCE)); @@ -650,7 +675,8 @@ void monitor_task( void ) { /*********** EVENT ***********************************************************/ -void event_task_ap( void ) { +void event_task_ap(void) +{ #ifndef SINGLE_MCU #if USE_I2C0 || USE_I2C1 || USE_I2C2 || USE_I2C3 @@ -697,8 +723,7 @@ void event_task_ap( void ) { modules_event_task(); #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP - if (new_ins_attitude > 0) - { + if (new_ins_attitude > 0) { attitude_loop(); new_ins_attitude = 0; } @@ -708,7 +733,8 @@ void event_task_ap( void ) { #if USE_GPS -static inline void on_gps_solution( void ) { +static inline void on_gps_solution(void) +{ ins_update_gps(); #if USE_AHRS ahrs_update_gps(); @@ -722,9 +748,10 @@ static inline void on_gps_solution( void ) { #if USE_AHRS #if USE_IMU -static inline void on_accel_event( void ) { +static inline void on_accel_event(void) +{ #if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY) -PRINT_CONFIG_MSG("Calculating dt for AHRS accel update.") + PRINT_CONFIG_MSG("Calculating dt for AHRS accel update.") // timestamp in usec when last callback was received static uint32_t last_ts = 0; // current timestamp @@ -733,9 +760,9 @@ PRINT_CONFIG_MSG("Calculating dt for AHRS accel update.") float dt = (float)(now_ts - last_ts) / 1e6; last_ts = now_ts; #else -PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS accel update.") -PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY) - const float dt = 1./AHRS_CORRECT_FREQUENCY; + PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS accel update.") + PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY) + const float dt = 1. / AHRS_CORRECT_FREQUENCY; #endif imu_scale_accel(&imu); @@ -744,9 +771,10 @@ PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY) } } -static inline void on_gyro_event( void ) { +static inline void on_gyro_event(void) +{ #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY) -PRINT_CONFIG_MSG("Calculating dt for AHRS/INS propagation.") + PRINT_CONFIG_MSG("Calculating dt for AHRS/INS propagation.") // timestamp in usec when last callback was received static uint32_t last_ts = 0; // current timestamp @@ -755,8 +783,8 @@ PRINT_CONFIG_MSG("Calculating dt for AHRS/INS propagation.") float dt = (float)(now_ts - last_ts) / 1e6; last_ts = now_ts; #else -PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS/INS propagation.") -PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) + PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS/INS propagation.") + PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); #endif @@ -768,8 +796,9 @@ PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) // Run aligner on raw data as it also makes averages. if (ahrs.status == AHRS_UNINIT) { ahrs_aligner_run(); - if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) + if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) { ahrs_align(); + } return; } #endif @@ -777,7 +806,7 @@ PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) ahrs_propagate(dt); #if defined SITL && USE_NPS - if (nps_bypass_ahrs) sim_overwrite_ahrs(); + if (nps_bypass_ahrs) { sim_overwrite_ahrs(); } #endif #ifdef AHRS_TRIGGERED_ATTITUDE_LOOP @@ -790,7 +819,7 @@ static inline void on_mag_event(void) { #if USE_MAGNETOMETER #if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY) -PRINT_CONFIG_MSG("Calculating dt for AHRS mag update.") + PRINT_CONFIG_MSG("Calculating dt for AHRS mag update.") // timestamp in usec when last callback was received static uint32_t last_ts = 0; // current timestamp @@ -799,8 +828,8 @@ PRINT_CONFIG_MSG("Calculating dt for AHRS mag update.") float dt = (float)(now_ts - last_ts) / 1e6; last_ts = now_ts; #else -PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS mag update.") -PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) + PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS mag update.") + PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY); #endif diff --git a/sw/airborne/firmwares/fixedwing/main_ap.h b/sw/airborne/firmwares/fixedwing/main_ap.h index 358d44df37..eb23567218 100644 --- a/sw/airborne/firmwares/fixedwing/main_ap.h +++ b/sw/airborne/firmwares/fixedwing/main_ap.h @@ -29,14 +29,14 @@ #ifndef AP_H #define AP_H -extern void init_ap( void ); -extern void handle_periodic_tasks_ap( void ); -extern void event_task_ap( void ); +extern void init_ap(void); +extern void handle_periodic_tasks_ap(void); +extern void event_task_ap(void); -extern void sensors_task( void ); -extern void navigation_task( void ); -extern void monitor_task( void ); -extern void reporting_task( void ); -extern void attitude_loop( void ); +extern void sensors_task(void); +extern void navigation_task(void); +extern void monitor_task(void); +extern void reporting_task(void); +extern void attitude_loop(void); #endif diff --git a/sw/airborne/firmwares/fixedwing/main_chibios_libopencm3.c b/sw/airborne/firmwares/fixedwing/main_chibios_libopencm3.c index f3e879d44e..2452c6ccfa 100644 --- a/sw/airborne/firmwares/fixedwing/main_chibios_libopencm3.c +++ b/sw/airborne/firmwares/fixedwing/main_chibios_libopencm3.c @@ -46,11 +46,11 @@ #include "led.h" -static int32_t pprz_thd (void *arg); +static int32_t pprz_thd(void *arg); static bool_t sdlogOk ; bool_t pprzReady = FALSE; -int main( void ) +int main(void) { // Init sys_time_init(); @@ -64,7 +64,7 @@ int main( void ) chibios_chThdSleepMilliseconds(100); - launch_pprz_thd (&pprz_thd); + launch_pprz_thd(&pprz_thd); pprzReady = TRUE; // Call PPRZ periodic and event functions while (TRUE) { @@ -74,7 +74,7 @@ int main( void ) } -static int32_t pprz_thd (void *arg) +static int32_t pprz_thd(void *arg) { /* To be compatible with rtos architecture, each of this 4 workers should @@ -82,7 +82,7 @@ static int32_t pprz_thd (void *arg) periodic task should sleep, and event task should wait for event */ (void) arg; - chibios_chRegSetThreadName ("pprz big loop"); + chibios_chRegSetThreadName("pprz big loop"); while (!chThdShouldTerminate()) { Fbw(handle_periodic_tasks); diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.c b/sw/airborne/firmwares/fixedwing/main_fbw.c index 701bda65aa..488977c181 100644 --- a/sw/airborne/firmwares/fixedwing/main_fbw.c +++ b/sw/airborne/firmwares/fixedwing/main_fbw.c @@ -77,30 +77,35 @@ tid_t electrical_tid; ///< id for electrical_periodic() timer /********** PERIODIC MESSAGES ************************************************/ #if PERIODIC_TELEMETRY -static void send_commands(struct transport_tx *trans, struct link_device *dev) { +static void send_commands(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_COMMANDS(trans, dev, AC_ID, COMMANDS_NB, commands); } #ifdef RADIO_CONTROL -static void send_fbw_status(struct transport_tx *trans, struct link_device *dev) { +static void send_fbw_status(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_FBW_STATUS(trans, dev, AC_ID, - &(radio_control.status), &(radio_control.frame_rate), &fbw_mode, &electrical.vsupply, &electrical.current); + &(radio_control.status), &(radio_control.frame_rate), &fbw_mode, &electrical.vsupply, &electrical.current); } -static void send_rc(struct transport_tx *trans, struct link_device *dev) { +static void send_rc(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_RC(trans, dev, AC_ID, RADIO_CONTROL_NB_CHANNEL, radio_control.values); } #else -static void send_fbw_status(struct transport_tx *trans, struct link_device *dev) { +static void send_fbw_status(struct transport_tx *trans, struct link_device *dev) +{ uint8_t dummy = 0; pprz_msg_send_FBW_STATUS(trans, dev, AC_ID, - &dummy, &dummy, &fbw_mode, &electrical.vsupply, &electrical.current); + &dummy, &dummy, &fbw_mode, &electrical.vsupply, &electrical.current); } #endif #ifdef ACTUATORS -static void send_actuators(struct transport_tx *trans, struct link_device *dev) { +static void send_actuators(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_ACTUATORS(trans, dev, AC_ID , ACTUATORS_NB, actuators); } #endif @@ -108,7 +113,8 @@ static void send_actuators(struct transport_tx *trans, struct link_device *dev) #endif /********** INIT *************************************************************/ -void init_fbw( void ) { +void init_fbw(void) +{ mcu_init(); @@ -138,7 +144,7 @@ void init_fbw( void ) { fbw_mode = FBW_MODE_FAILSAFE; /**** start timers for periodic functions *****/ - fbw_periodic_tid = sys_time_register_timer((1./60.), NULL); + fbw_periodic_tid = sys_time_register_timer((1. / 60.), NULL); electrical_tid = sys_time_register_timer(0.1, NULL); #ifndef SINGLE_MCU @@ -159,7 +165,8 @@ void init_fbw( void ) { } -static inline void set_failsafe_mode( void ) { +static inline void set_failsafe_mode(void) +{ fbw_mode = FBW_MODE_FAILSAFE; SetCommands(commands_failsafe); fbw_new_actuators = 1; @@ -167,10 +174,10 @@ static inline void set_failsafe_mode( void ) { #ifdef RADIO_CONTROL -static inline void handle_rc_frame( void ) { +static inline void handle_rc_frame(void) +{ fbw_mode = FBW_MODE_OF_PPRZ(radio_control.values[RADIO_MODE]); - if (fbw_mode == FBW_MODE_MANUAL) - { + if (fbw_mode == FBW_MODE_MANUAL) { SetCommandsFromRC(commands, radio_control.values); fbw_new_actuators = 1; } @@ -180,7 +187,8 @@ static inline void handle_rc_frame( void ) { uint8_t ap_has_been_ok = FALSE; /********** EVENT ************************************************************/ -void event_task_fbw( void) { +void event_task_fbw(void) +{ #ifdef RADIO_CONTROL RadioControlEvent(handle_rc_frame); #endif @@ -229,8 +237,7 @@ void event_task_fbw( void) { SetCommands(ap_state->commands); } #ifdef SetApOnlyCommands - else - { + else { SetApOnlyCommands(ap_state->commands); } #endif @@ -245,42 +252,39 @@ void event_task_fbw( void) { #warning DANGER DANGER DANGER DANGER: Outback Challenge Rule FORCE-CRASH-RULE: DANGER DANGER: AP is now capable to FORCE your FBW in failsafe mode EVEN IF RC IS NOT LOST: Consider the consequences. // OUTBACK: JURY REQUEST FLIGHT TERMINATION int crash = 0; - if (commands[COMMAND_FORCECRASH] >= 8000) - { + if (commands[COMMAND_FORCECRASH] >= 8000) { set_failsafe_mode(); crash = 1; } #endif #ifdef ACTUATORS - if (fbw_new_actuators > 0) - { + if (fbw_new_actuators > 0) { pprz_t trimmed_commands[COMMANDS_NB]; int i; - for(i = 0; i < COMMANDS_NB; i++) trimmed_commands[i] = commands[i]; + for (i = 0; i < COMMANDS_NB; i++) { trimmed_commands[i] = commands[i]; } - #ifdef COMMAND_ROLL - trimmed_commands[COMMAND_ROLL] += ChopAbs(command_roll_trim, MAX_PPRZ/10); - #endif - #ifdef COMMAND_PITCH - trimmed_commands[COMMAND_PITCH] += ChopAbs(command_pitch_trim, MAX_PPRZ/10); - #endif - #ifdef COMMAND_YAW +#ifdef COMMAND_ROLL + trimmed_commands[COMMAND_ROLL] += ChopAbs(command_roll_trim, MAX_PPRZ / 10); +#endif +#ifdef COMMAND_PITCH + trimmed_commands[COMMAND_PITCH] += ChopAbs(command_pitch_trim, MAX_PPRZ / 10); +#endif +#ifdef COMMAND_YAW trimmed_commands[COMMAND_YAW] += ChopAbs(command_yaw_trim, MAX_PPRZ); - #endif +#endif SetActuatorsFromCommands(trimmed_commands, autopilot_mode); fbw_new_actuators = 0; - #if OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_AP_CAN_FORCE_FAILSAFE - if (crash == 1) - { +#if OUTBACK_CHALLENGE_VERY_DANGEROUS_RULE_AP_CAN_FORCE_FAILSAFE + if (crash == 1) { for (;;) { #if FBW_DATALINK fbw_datalink_event(); #endif } } - #endif +#endif } #endif @@ -302,7 +306,8 @@ void event_task_fbw( void) { /************* PERIODIC ******************************************************/ -void periodic_task_fbw( void ) { +void periodic_task_fbw(void) +{ #ifdef FBW_DATALINK fbw_datalink_periodic(); @@ -318,7 +323,7 @@ void periodic_task_fbw( void ) { #warning WARNING DANGER: OUTBACK_CHALLENGE_DANGEROUS_RULE_RC_LOST_NO_AP_IRREVERSIBLE defined. If you ever temporarly lost RC while in manual, you will failsafe forever even if RC is restored commands[COMMAND_FORCECRASH] = 9600; #endif - #else +#else fbw_mode = FBW_MODE_AUTO; #endif } @@ -326,8 +331,7 @@ void periodic_task_fbw( void ) { #ifdef INTER_MCU inter_mcu_periodic_task(); - if (fbw_mode == FBW_MODE_AUTO && !ap_ok) - { + if (fbw_mode == FBW_MODE_AUTO && !ap_ok) { set_failsafe_mode(); } #endif @@ -348,14 +352,17 @@ void periodic_task_fbw( void ) { } -void handle_periodic_tasks_fbw(void) { +void handle_periodic_tasks_fbw(void) +{ - if (sys_time_check_and_ack_timer(fbw_periodic_tid)) + if (sys_time_check_and_ack_timer(fbw_periodic_tid)) { periodic_task_fbw(); + } #if !(DISABLE_ELECTRICAL) - if (sys_time_check_and_ack_timer(electrical_tid)) + if (sys_time_check_and_ack_timer(electrical_tid)) { electrical_periodic(); + } #endif } diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.h b/sw/airborne/firmwares/fixedwing/main_fbw.h index b71bf5d57e..7c3b793613 100644 --- a/sw/airborne/firmwares/fixedwing/main_fbw.h +++ b/sw/airborne/firmwares/fixedwing/main_fbw.h @@ -40,9 +40,9 @@ extern uint8_t fbw_mode; extern bool_t failsafe_mode; -void init_fbw( void ); -void handle_periodic_tasks_fbw( void ); -void periodic_task_fbw( void ); -void event_task_fbw( void ); +void init_fbw(void); +void handle_periodic_tasks_fbw(void); +void periodic_task_fbw(void); +void event_task_fbw(void); #endif diff --git a/sw/airborne/firmwares/fixedwing/nav.c b/sw/airborne/firmwares/fixedwing/nav.c index 799ac77277..5c0a0e0cab 100644 --- a/sw/airborne/firmwares/fixedwing/nav.c +++ b/sw/airborne/firmwares/fixedwing/nav.c @@ -43,7 +43,7 @@ enum oval_status oval_status; float last_x, last_y; /** Index of last waypoint. Used only in "go" stage in "route" horiz mode */ -static uint8_t last_wp __attribute__ ((unused)); +static uint8_t last_wp __attribute__((unused)); float rc_pitch; float carrot_x, carrot_y; @@ -88,7 +88,8 @@ bool_t nav_survey_active; int nav_mode; -void nav_init_stage( void ) { +void nav_init_stage(void) +{ last_x = stateGetPositionEnu_f()->x; last_y = stateGetPositionEnu_f()->y; stage_time = 0; @@ -106,8 +107,9 @@ void nav_init_stage( void ) { /** Navigates around (x, y). Clockwise iff radius > 0 */ -void nav_circle_XY(float x, float y, float radius) { - struct EnuCoor_f* pos = stateGetPositionEnu_f(); +void nav_circle_XY(float x, float y, float radius) +{ + struct EnuCoor_f *pos = stateGetPositionEnu_f(); float last_trigo_qdr = nav_circle_trigo_qdr; nav_circle_trigo_qdr = atan2f(pos->y - y, pos->x - x); float sign_radius = radius > 0 ? 1 : -1; @@ -117,12 +119,13 @@ void nav_circle_XY(float x, float y, float radius) { NormRadAngle(trigo_diff); nav_circle_radians += trigo_diff; trigo_diff *= - sign_radius; - if (trigo_diff > 0) // do not rewind if the change in angle is in the opposite sense than nav_radius + if (trigo_diff > 0) { // do not rewind if the change in angle is in the opposite sense than nav_radius nav_circle_radians_no_rewind += trigo_diff; + } } float dist2_center = DistanceSquare(pos->x, pos->y, x, y); - float dist_carrot = CARROT*NOMINAL_AIRSPEED; + float dist_carrot = CARROT * NOMINAL_AIRSPEED; radius += -nav_shift; @@ -133,19 +136,19 @@ void nav_circle_XY(float x, float y, float radius) { (dist2_center > Square(abs_radius + dist_carrot) || dist2_center < Square(abs_radius - dist_carrot)) ? 0 : - atanf((*stateGetHorizontalSpeedNorm_f())*(*stateGetHorizontalSpeedNorm_f()) / (NAV_GRAVITY*radius)); + atanf((*stateGetHorizontalSpeedNorm_f()) * (*stateGetHorizontalSpeedNorm_f()) / (NAV_GRAVITY * radius)); float carrot_angle = dist_carrot / abs_radius; - carrot_angle = Min(carrot_angle, M_PI/4); - carrot_angle = Max(carrot_angle, M_PI/16); + carrot_angle = Min(carrot_angle, M_PI / 4); + carrot_angle = Max(carrot_angle, M_PI / 16); float alpha_carrot = nav_circle_trigo_qdr - sign_radius * carrot_angle; horizontal_mode = HORIZONTAL_MODE_CIRCLE; float radius_carrot = abs_radius; if (nav_mode == NAV_MODE_COURSE) { radius_carrot += (abs_radius / cosf(carrot_angle) - abs_radius); } - fly_to_xy(x+cosf(alpha_carrot)*radius_carrot, - y+sinf(alpha_carrot)*radius_carrot); + fly_to_xy(x + cosf(alpha_carrot)*radius_carrot, + y + sinf(alpha_carrot)*radius_carrot); nav_in_circle = TRUE; nav_circle_x = x; nav_circle_y = y; @@ -198,19 +201,21 @@ void nav_circle_XY(float x, float y, float radius) { nav_follow(_ac_id, _distance, _height); -static unit_t unit __attribute__ ((unused)); +static unit_t unit __attribute__((unused)); static inline void nav_follow(uint8_t _ac_id, float _distance, float _height); #ifdef NAV_GROUND_SPEED_PGAIN /** \brief Computes cruise throttle from ground speed setpoint */ -static void nav_ground_speed_loop( void ) { +static void nav_ground_speed_loop(void) +{ if (MINIMUM_AIRSPEED < nav_ground_speed_setpoint && nav_ground_speed_setpoint < MAXIMUM_AIRSPEED) { float err = nav_ground_speed_setpoint - (*stateGetHorizontalSpeedNorm_f()); - v_ctl_auto_throttle_cruise_throttle += nav_ground_speed_pgain*err; - Bound(v_ctl_auto_throttle_cruise_throttle, V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE); + v_ctl_auto_throttle_cruise_throttle += nav_ground_speed_pgain * err; + Bound(v_ctl_auto_throttle_cruise_throttle, V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, + V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE); } else { /* Reset cruise throttle to nominal value */ v_ctl_auto_throttle_cruise_throttle = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE; @@ -219,14 +224,15 @@ static void nav_ground_speed_loop( void ) { #endif static float baseleg_out_qdr; -static inline bool_t nav_compute_baseleg(uint8_t wp_af, uint8_t wp_td, uint8_t wp_baseleg, float radius ) { +static inline bool_t nav_compute_baseleg(uint8_t wp_af, uint8_t wp_td, uint8_t wp_baseleg, float radius) +{ nav_radius = radius; float x_0 = waypoints[wp_td].x - waypoints[wp_af].x; float y_0 = waypoints[wp_td].y - waypoints[wp_af].y; /* Unit vector from AF to TD */ - float d = sqrtf(x_0*x_0+y_0*y_0); + float d = sqrtf(x_0 * x_0 + y_0 * y_0); float x_1 = x_0 / d; float y_1 = y_0 / d; @@ -234,20 +240,22 @@ static inline bool_t nav_compute_baseleg(uint8_t wp_af, uint8_t wp_td, uint8_t w waypoints[wp_baseleg].y = waypoints[wp_af].y - x_1 * nav_radius; waypoints[wp_baseleg].a = waypoints[wp_af].a; baseleg_out_qdr = M_PI - atan2f(-y_1, -x_1); - if (nav_radius < 0) + if (nav_radius < 0) { baseleg_out_qdr += M_PI; + } return FALSE; } -static inline bool_t nav_compute_final_from_glide(uint8_t wp_af, uint8_t wp_td, float glide ) { +static inline bool_t nav_compute_final_from_glide(uint8_t wp_af, uint8_t wp_td, float glide) +{ float x_0 = waypoints[wp_td].x - waypoints[wp_af].x; float y_0 = waypoints[wp_td].y - waypoints[wp_af].y; float h_0 = waypoints[wp_td].a - waypoints[wp_af].a; /* Unit vector from AF to TD */ - float d = sqrtf(x_0*x_0+y_0*y_0); + float d = sqrtf(x_0 * x_0 + y_0 * y_0); float x_1 = x_0 / d; float y_1 = y_0 / d; @@ -262,12 +270,14 @@ static inline bool_t nav_compute_final_from_glide(uint8_t wp_af, uint8_t wp_td, /* For a landing UPWIND. Computes Top Of Descent waypoint from Touch Down and Approach Fix waypoints, using glide airspeed, glide vertical speed and wind */ -static inline bool_t compute_TOD(uint8_t _af, uint8_t _td, uint8_t _tod, float glide_airspeed, float glide_vspeed) { - struct FloatVect2* wind = stateGetHorizontalWindspeed_f(); +static inline bool_t compute_TOD(uint8_t _af, uint8_t _td, uint8_t _tod, float glide_airspeed, float glide_vspeed) +{ + struct FloatVect2 *wind = stateGetHorizontalWindspeed_f(); float td_af_x = WaypointX(_af) - WaypointX(_td); float td_af_y = WaypointY(_af) - WaypointY(_td); - float td_af = sqrtf( td_af_x*td_af_x + td_af_y*td_af_y); - float td_tod = (WaypointAlt(_af) - WaypointAlt(_td)) / glide_vspeed * (glide_airspeed - sqrtf(wind->x*wind->x + wind->y*wind->y)); + float td_af = sqrtf(td_af_x * td_af_x + td_af_y * td_af_y); + float td_tod = (WaypointAlt(_af) - WaypointAlt(_td)) / glide_vspeed * (glide_airspeed - sqrtf( + wind->x * wind->x + wind->y * wind->y)); WaypointX(_tod) = WaypointX(_td) + td_af_x / td_af * td_tod; WaypointY(_tod) = WaypointY(_td) + td_af_y / td_af * td_tod; WaypointAlt(_tod) = WaypointAlt(_af); @@ -287,18 +297,19 @@ static inline bool_t compute_TOD(uint8_t _af, uint8_t _td, uint8_t _tod, float g -static inline void nav_follow(uint8_t _ac_id, float _distance, float _height) { +static inline void nav_follow(uint8_t _ac_id, float _distance, float _height) +{ struct ac_info_ * ac = get_ac_info(_ac_id); NavVerticalAutoThrottleMode(0.); - NavVerticalAltitudeMode(Max(ac->alt + _height, ground_alt+SECURITY_HEIGHT), 0.); - float alpha = M_PI/2 - ac->course; + NavVerticalAltitudeMode(Max(ac->alt + _height, ground_alt + SECURITY_HEIGHT), 0.); + float alpha = M_PI / 2 - ac->course; float ca = cosf(alpha), sa = sinf(alpha); - float x = ac->east - _distance*ca; - float y = ac->north - _distance*sa; + float x = ac->east - _distance * ca; + float y = ac->north - _distance * sa; fly_to_xy(x, y); #ifdef NAV_FOLLOW_PGAIN - float s = (stateGetPositionEnu_f()->x - x)*ca + (stateGetPositionEnu_f()->y - y)*sa; - nav_ground_speed_setpoint = ac->gspeed + NAV_FOLLOW_PGAIN*s; + float s = (stateGetPositionEnu_f()->x - x) * ca + (stateGetPositionEnu_f()->y - y) * sa; + nav_ground_speed_setpoint = ac->gspeed + NAV_FOLLOW_PGAIN * s; nav_ground_speed_loop(); #endif } @@ -319,7 +330,8 @@ float fp_pitch; /* deg */ * * @return true if the position (x, y) is reached */ -bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float approaching_time) { +bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float approaching_time) +{ /** distance to waypoint in x */ float pw_x = x - stateGetPositionEnu_f()->x; /** distance to waypoint in y */ @@ -333,12 +345,11 @@ bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float ap float exceed_dist = approaching_time * (*stateGetHorizontalSpeedNorm_f()); // negative value float scal_prod = (leg_x * pw_x + leg_y * pw_y) / leg; return (scal_prod < exceed_dist); - } - else { + } else { // fly close enough of the waypoint or cross it - dist2_to_wp = pw_x*pw_x + pw_y *pw_y; + dist2_to_wp = pw_x * pw_x + pw_y * pw_y; float min_dist = approaching_time * (*stateGetHorizontalSpeedNorm_f()); - if (dist2_to_wp < min_dist*min_dist) { + if (dist2_to_wp < min_dist * min_dist) { return TRUE; } float scal_prod = (x - from_x) * pw_x + (y - from_y) * pw_y; @@ -351,22 +362,24 @@ bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float ap * \brief Computes \a desired_x, \a desired_y and \a desired_course. */ //static inline void fly_to_xy(float x, float y) { -void fly_to_xy(float x, float y) { - struct EnuCoor_f* pos = stateGetPositionEnu_f(); +void fly_to_xy(float x, float y) +{ + struct EnuCoor_f *pos = stateGetPositionEnu_f(); desired_x = x; desired_y = y; if (nav_mode == NAV_MODE_COURSE) { h_ctl_course_setpoint = atan2f(x - pos->x, y - pos->y); - if (h_ctl_course_setpoint < 0.) + if (h_ctl_course_setpoint < 0.) { h_ctl_course_setpoint += 2 * M_PI; + } lateral_mode = LATERAL_MODE_COURSE; } else { float diff = atan2f(x - pos->x, y - pos->y) - (*stateGetHorizontalSpeedDir_f()); NormRadAngle(diff); - BoundAbs(diff,M_PI/2.); + BoundAbs(diff, M_PI / 2.); float s = sinf(diff); float speed = *stateGetHorizontalSpeedNorm_f(); - h_ctl_roll_setpoint = atanf(2 * speed * speed * s * h_ctl_course_pgain / (CARROT * NOMINAL_AIRSPEED * 9.81) ); + h_ctl_roll_setpoint = atanf(2 * speed * speed * s * h_ctl_course_pgain / (CARROT * NOMINAL_AIRSPEED * 9.81)); BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint); lateral_mode = LATERAL_MODE_ROLL; } @@ -375,11 +388,13 @@ void fly_to_xy(float x, float y) { /** * \brief Computes the carrot position along the desired segment. */ -void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y) { +void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y) +{ float leg_x = wp_x - last_wp_x; float leg_y = wp_y - last_wp_y; float leg2 = Max(leg_x * leg_x + leg_y * leg_y, 1.); - nav_leg_progress = ((stateGetPositionEnu_f()->x - last_wp_x) * leg_x + (stateGetPositionEnu_f()->y - last_wp_y) * leg_y) / leg2; + nav_leg_progress = ((stateGetPositionEnu_f()->x - last_wp_x) * leg_x + (stateGetPositionEnu_f()->y - last_wp_y) * + leg_y) / leg2; nav_leg_length = sqrtf(leg2); /** distance of carrot (in meter) */ @@ -393,7 +408,8 @@ void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y) { nav_segment_y_2 = wp_y; horizontal_mode = HORIZONTAL_MODE_ROUTE; - fly_to_xy(last_wp_x + nav_carrot_leg_progress*leg_x +nav_shift*leg_y/nav_leg_length, last_wp_y + nav_carrot_leg_progress*leg_y-nav_shift*leg_x/nav_leg_length); + fly_to_xy(last_wp_x + nav_carrot_leg_progress * leg_x + nav_shift * leg_y / nav_leg_length, + last_wp_y + nav_carrot_leg_progress * leg_y - nav_shift * leg_x / nav_leg_length); } #include "subsystems/navigation/common_nav.c" @@ -402,7 +418,8 @@ void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y) { #define FAILSAFE_HOME_RADIUS DEFAULT_CIRCLE_RADIUS #endif -static void nav_set_altitude(void) { +static void nav_set_altitude(void) +{ static float last_nav_altitude; if (fabs(nav_altitude - last_nav_altitude) > 1.) { flight_altitude = nav_altitude; @@ -412,12 +429,13 @@ static void nav_set_altitude(void) { } /** \brief Home mode navigation (circle around HOME) */ -void nav_home(void) { +void nav_home(void) +{ NavCircleWaypoint(WP_HOME, FAILSAFE_HOME_RADIUS); /** Nominal speed */ nav_pitch = 0.; v_ctl_mode = V_CTL_MODE_AUTO_ALT; - nav_altitude = ground_alt+HOME_MODE_HEIGHT; + nav_altitude = ground_alt + HOME_MODE_HEIGHT; compute_dist2_to_home(); dist2_to_wp = dist2_to_home; nav_set_altitude(); @@ -427,7 +445,8 @@ void nav_home(void) { * \brief Navigation main: call to the code generated from the XML flight * plan */ -void nav_periodic_task(void) { +void nav_periodic_task(void) +{ nav_survey_active = FALSE; compute_dist2_to_home(); @@ -438,8 +457,9 @@ void nav_periodic_task(void) { h_ctl_course_pre_bank = nav_in_circle ? circle_bank : 0; #ifdef AGR_CLIMB - if ( v_ctl_mode == V_CTL_MODE_AUTO_CLIMB) + if (v_ctl_mode == V_CTL_MODE_AUTO_CLIMB) { v_ctl_auto_throttle_submode = V_CTL_AUTO_THROTTLE_STANDARD; + } #endif nav_set_altitude(); @@ -451,18 +471,22 @@ void nav_periodic_task(void) { #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_nav_ref(struct transport_tx *trans, struct link_device *dev) { +static void send_nav_ref(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_NAVIGATION_REF(trans, dev, AC_ID, - &nav_utm_east0, &nav_utm_north0, &nav_utm_zone0, &ground_alt); + &nav_utm_east0, &nav_utm_north0, &nav_utm_zone0, &ground_alt); } -static void send_nav(struct transport_tx *trans, struct link_device *dev) { +static void send_nav(struct transport_tx *trans, struct link_device *dev) +{ SEND_NAVIGATION(trans, dev); } -static void send_wp_moved(struct transport_tx *trans, struct link_device *dev) { +static void send_wp_moved(struct transport_tx *trans, struct link_device *dev) +{ static uint8_t i; - i++; if (i >= nb_waypoint) i = 0; + i++; + if (i >= nb_waypoint) { i = 0; } DownlinkSendWp(trans, dev, i); } @@ -473,24 +497,27 @@ bool_t DownlinkSendWpNr(uint8_t _wp) } -static void send_circle(struct transport_tx *trans, struct link_device *dev) { +static void send_circle(struct transport_tx *trans, struct link_device *dev) +{ if (nav_in_circle) { pprz_msg_send_CIRCLE(trans, dev, AC_ID, - &nav_circle_x, &nav_circle_y, &nav_circle_radius); + &nav_circle_x, &nav_circle_y, &nav_circle_radius); } } -static void send_segment(struct transport_tx *trans, struct link_device *dev) { +static void send_segment(struct transport_tx *trans, struct link_device *dev) +{ if (nav_in_segment) { pprz_msg_send_SEGMENT(trans, dev, AC_ID, - &nav_segment_x_1, &nav_segment_y_1, &nav_segment_x_2, &nav_segment_y_2); + &nav_segment_x_1, &nav_segment_y_1, &nav_segment_x_2, &nav_segment_y_2); } } -static void send_survey(struct transport_tx *trans, struct link_device *dev) { +static void send_survey(struct transport_tx *trans, struct link_device *dev) +{ if (nav_survey_active) { pprz_msg_send_SURVEY(trans, dev, AC_ID, - &nav_survey_east, &nav_survey_north, &nav_survey_west, &nav_survey_south); + &nav_survey_east, &nav_survey_north, &nav_survey_west, &nav_survey_south); } } #endif @@ -498,13 +525,14 @@ static void send_survey(struct transport_tx *trans, struct link_device *dev) { /** * \brief Navigation Initialisation */ -void nav_init(void) { +void nav_init(void) +{ nav_block = 0; nav_stage = 0; ground_alt = GROUND_ALT; nav_glide_pitch_trim = NAV_GLIDE_PITCH_TRIM; nav_radius = DEFAULT_CIRCLE_RADIUS; - nav_survey_shift = 2*DEFAULT_CIRCLE_RADIUS; + nav_survey_shift = 2 * DEFAULT_CIRCLE_RADIUS; nav_mode = NAV_MODE_COURSE; #ifdef NAV_GROUND_SPEED_PGAIN @@ -528,18 +556,19 @@ void nav_init(void) { * Just set attitude and throttle to FAILSAFE values * to prevent the plane from crashing. */ -void nav_without_gps(void) { +void nav_without_gps(void) +{ lateral_mode = LATERAL_MODE_ROLL; v_ctl_mode = V_CTL_MODE_AUTO_THROTTLE; #ifdef SECTION_FAILSAFE h_ctl_roll_setpoint = FAILSAFE_DEFAULT_ROLL; nav_pitch = FAILSAFE_DEFAULT_PITCH; - nav_throttle_setpoint = TRIM_UPPRZ((FAILSAFE_DEFAULT_THROTTLE)*MAX_PPRZ); + nav_throttle_setpoint = TRIM_UPPRZ((FAILSAFE_DEFAULT_THROTTLE) * MAX_PPRZ); #else h_ctl_roll_setpoint = 0; nav_pitch = 0; - nav_throttle_setpoint = TRIM_UPPRZ((V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE)*MAX_PPRZ); + nav_throttle_setpoint = TRIM_UPPRZ((V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE) * MAX_PPRZ); #endif } @@ -550,7 +579,8 @@ void nav_without_gps(void) { enum eight_status { R1T, RT2, C2, R2T, RT1, C1 }; static enum eight_status eight_status; -void nav_eight_init( void ) { +void nav_eight_init(void) +{ eight_status = C1; } @@ -562,14 +592,15 @@ void nav_eight_init( void ) { If necessary, the [c1] waypoint is moved in the direction of [target] to be not far than [2*radius]. */ -void nav_eight(uint8_t target, uint8_t c1, float radius) { +void nav_eight(uint8_t target, uint8_t c1, float radius) +{ float aradius = fabs(radius); float alt = waypoints[target].a; waypoints[c1].a = alt; float target_c1_x = waypoints[c1].x - waypoints[target].x; float target_c1_y = waypoints[c1].y - waypoints[target].y; - float d = sqrtf(target_c1_x*target_c1_x+target_c1_y*target_c1_y); + float d = sqrtf(target_c1_x * target_c1_x + target_c1_y * target_c1_y); d = Max(d, 1.); /* To prevent a division by zero */ /* Unit vector from target to c1 */ @@ -578,90 +609,96 @@ void nav_eight(uint8_t target, uint8_t c1, float radius) { /* Move [c1] closer if needed */ if (d > 2 * aradius) { - d = 2*aradius; - waypoints[c1].x = waypoints[target].x + d*u_x; - waypoints[c1].y = waypoints[target].y + d*u_y; + d = 2 * aradius; + waypoints[c1].x = waypoints[target].x + d * u_x; + waypoints[c1].y = waypoints[target].y + d * u_y; } /* The other center */ struct point c2 = { - waypoints[target].x - d*u_x, - waypoints[target].y - d*u_y, - alt }; + waypoints[target].x - d * u_x, + waypoints[target].y - d * u_y, + alt + }; struct point c1_in = { waypoints[c1].x + radius * -u_y, waypoints[c1].y + radius * u_x, - alt }; + alt + }; struct point c1_out = { waypoints[c1].x - radius * -u_y, waypoints[c1].y - radius * u_x, - alt }; + alt + }; struct point c2_in = { c2.x + radius * -u_y, c2.y + radius * u_x, - alt }; + alt + }; struct point c2_out = { c2.x - radius * -u_y, c2.y - radius * u_x, - alt }; + alt + }; float qdr_out = M_PI - atan2f(u_y, u_x); - if (radius < 0) + if (radius < 0) { qdr_out += M_PI; + } switch (eight_status) { - case C1 : - NavCircleWaypoint(c1, radius); - if (NavQdrCloseTo(DegOfRad(qdr_out)-10)) { - eight_status = R1T; - InitStage(); - } - return; + case C1 : + NavCircleWaypoint(c1, radius); + if (NavQdrCloseTo(DegOfRad(qdr_out) - 10)) { + eight_status = R1T; + InitStage(); + } + return; - case R1T: - nav_route_xy(c1_out.x, c1_out.y, c2_in.x, c2_in.y); - if (nav_approaching_xy(waypoints[target].x, waypoints[target].y, c1_out.x, c1_out.y, 0)) { - eight_status = RT2; - InitStage(); - } - return; + case R1T: + nav_route_xy(c1_out.x, c1_out.y, c2_in.x, c2_in.y); + if (nav_approaching_xy(waypoints[target].x, waypoints[target].y, c1_out.x, c1_out.y, 0)) { + eight_status = RT2; + InitStage(); + } + return; - case RT2: - nav_route_xy(c1_out.x, c1_out.y, c2_in.x, c2_in.y); - if (nav_approaching_xy(c2_in.x, c2_in.y, c1_out.x, c1_out.y, CARROT)) { - eight_status = C2; - InitStage(); - } - return; + case RT2: + nav_route_xy(c1_out.x, c1_out.y, c2_in.x, c2_in.y); + if (nav_approaching_xy(c2_in.x, c2_in.y, c1_out.x, c1_out.y, CARROT)) { + eight_status = C2; + InitStage(); + } + return; - case C2 : - nav_circle_XY(c2.x, c2.y, -radius); - if (NavQdrCloseTo(DegOfRad(qdr_out)+10)) { - eight_status = R2T; - InitStage(); - } - return; + case C2 : + nav_circle_XY(c2.x, c2.y, -radius); + if (NavQdrCloseTo(DegOfRad(qdr_out) + 10)) { + eight_status = R2T; + InitStage(); + } + return; - case R2T: - nav_route_xy(c2_out.x, c2_out.y, c1_in.x, c1_in.y); - if (nav_approaching_xy(waypoints[target].x, waypoints[target].y, c2_out.x, c2_out.y, 0)) { - eight_status = RT1; - InitStage(); - } - return; + case R2T: + nav_route_xy(c2_out.x, c2_out.y, c1_in.x, c1_in.y); + if (nav_approaching_xy(waypoints[target].x, waypoints[target].y, c2_out.x, c2_out.y, 0)) { + eight_status = RT1; + InitStage(); + } + return; - case RT1: - nav_route_xy(c2_out.x, c2_out.y, c1_in.x, c1_in.y); - if (nav_approaching_xy(c1_in.x, c1_in.y, c2_out.x, c2_out.y, CARROT)) { - eight_status = C1; - InitStage(); - } - return; + case RT1: + nav_route_xy(c2_out.x, c2_out.y, c1_in.x, c1_in.y); + if (nav_approaching_xy(c1_in.x, c1_in.y, c2_out.x, c2_out.y, CARROT)) { + eight_status = C1; + InitStage(); + } + return; - default:/* Should not occur !!! Doing nothing */ - return; + default:/* Should not occur !!! Doing nothing */ + return; } /* switch */ } @@ -678,12 +715,14 @@ void nav_eight(uint8_t target, uint8_t c1, float radius) { uint8_t nav_oval_count; -void nav_oval_init( void ) { +void nav_oval_init(void) +{ oval_status = OC2; nav_oval_count = 0; } -void nav_oval(uint8_t p1, uint8_t p2, float radius) { +void nav_oval(uint8_t p1, uint8_t p2, float radius) +{ radius = - radius; /* Historical error ? */ float alt = waypoints[p1].a; @@ -691,7 +730,7 @@ void nav_oval(uint8_t p1, uint8_t p2, float radius) { float p2_p1_x = waypoints[p1].x - waypoints[p2].x; float p2_p1_y = waypoints[p1].y - waypoints[p2].y; - float d = sqrtf(p2_p1_x*p2_p1_x+p2_p1_y*p2_p1_y); + float d = sqrtf(p2_p1_x * p2_p1_x + p2_p1_y * p2_p1_y); /* Unit vector from p1 to p2 */ float u_x = p2_p1_x / d; @@ -699,18 +738,22 @@ void nav_oval(uint8_t p1, uint8_t p2, float radius) { /* The half circle centers and the other leg */ struct point p1_center = { waypoints[p1].x + radius * -u_y, - waypoints[p1].y + radius * u_x, - alt }; - struct point p1_out = { waypoints[p1].x + 2*radius * -u_y, - waypoints[p1].y + 2*radius * u_x, - alt }; + waypoints[p1].y + radius * u_x, + alt + }; + struct point p1_out = { waypoints[p1].x + 2 * radius * -u_y, + waypoints[p1].y + 2 * radius * u_x, + alt + }; - struct point p2_in = { waypoints[p2].x + 2*radius * -u_y, - waypoints[p2].y + 2*radius * u_x, - alt }; + struct point p2_in = { waypoints[p2].x + 2 * radius * -u_y, + waypoints[p2].y + 2 * radius * u_x, + alt + }; struct point p2_center = { waypoints[p2].x + radius * -u_y, - waypoints[p2].y + radius * u_x, - alt }; + waypoints[p2].y + radius * u_x, + alt + }; float qdr_out_2 = M_PI - atan2f(u_y, u_x); float qdr_out_1 = qdr_out_2 + M_PI; @@ -721,44 +764,44 @@ void nav_oval(uint8_t p1, uint8_t p2, float radius) { float qdr_anticipation = (radius > 0 ? -15 : 15); switch (oval_status) { - case OC1 : - nav_circle_XY(p1_center.x,p1_center.y, -radius); - if (NavQdrCloseTo(DegOfRad(qdr_out_1)-qdr_anticipation)) { - oval_status = OR12; - InitStage(); - LINE_START_FUNCTION; - } - return; + case OC1 : + nav_circle_XY(p1_center.x, p1_center.y, -radius); + if (NavQdrCloseTo(DegOfRad(qdr_out_1) - qdr_anticipation)) { + oval_status = OR12; + InitStage(); + LINE_START_FUNCTION; + } + return; - case OR12: - nav_route_xy(p1_out.x, p1_out.y, p2_in.x, p2_in.y); - if (nav_approaching_xy(p2_in.x, p2_in.y, p1_out.x, p1_out.y, CARROT)) { - oval_status = OC2; - nav_oval_count++; - InitStage(); - LINE_STOP_FUNCTION; - } - return; + case OR12: + nav_route_xy(p1_out.x, p1_out.y, p2_in.x, p2_in.y); + if (nav_approaching_xy(p2_in.x, p2_in.y, p1_out.x, p1_out.y, CARROT)) { + oval_status = OC2; + nav_oval_count++; + InitStage(); + LINE_STOP_FUNCTION; + } + return; - case OC2 : - nav_circle_XY(p2_center.x, p2_center.y, -radius); - if (NavQdrCloseTo(DegOfRad(qdr_out_2)-qdr_anticipation)) { - oval_status = OR21; - InitStage(); - LINE_START_FUNCTION; - } - return; + case OC2 : + nav_circle_XY(p2_center.x, p2_center.y, -radius); + if (NavQdrCloseTo(DegOfRad(qdr_out_2) - qdr_anticipation)) { + oval_status = OR21; + InitStage(); + LINE_START_FUNCTION; + } + return; - case OR21: - nav_route_xy(waypoints[p2].x, waypoints[p2].y, waypoints[p1].x, waypoints[p1].y); - if (nav_approaching_xy(waypoints[p1].x, waypoints[p1].y, waypoints[p2].x, waypoints[p2].y, CARROT)) { - oval_status = OC1; - InitStage(); - LINE_STOP_FUNCTION; - } - return; + case OR21: + nav_route_xy(waypoints[p2].x, waypoints[p2].y, waypoints[p1].x, waypoints[p1].y); + if (nav_approaching_xy(waypoints[p1].x, waypoints[p1].y, waypoints[p2].x, waypoints[p2].y, CARROT)) { + oval_status = OC1; + InitStage(); + LINE_STOP_FUNCTION; + } + return; - default: /* Should not occur !!! Doing nothing */ - return; + default: /* Should not occur !!! Doing nothing */ + return; } } diff --git a/sw/airborne/firmwares/fixedwing/nav.h b/sw/airborne/firmwares/fixedwing/nav.h index 4cbbfa4d3c..07fc13ec21 100644 --- a/sw/airborne/firmwares/fixedwing/nav.h +++ b/sw/airborne/firmwares/fixedwing/nav.h @@ -80,16 +80,16 @@ extern uint8_t horizontal_mode; extern void fly_to_xy(float x, float y); #define NavGotoWaypoint(_wp) { \ - horizontal_mode = HORIZONTAL_MODE_WAYPOINT; \ - fly_to_xy(waypoints[_wp].x, waypoints[_wp].y); \ -} + horizontal_mode = HORIZONTAL_MODE_WAYPOINT; \ + fly_to_xy(waypoints[_wp].x, waypoints[_wp].y); \ + } -extern void nav_eight_init( void ); +extern void nav_eight_init(void); extern void nav_eight(uint8_t, uint8_t, float); #define Eight(a, b, c) nav_eight((a), (b), (c)) -extern void nav_oval_init( void ); +extern void nav_oval_init(void); extern void nav_oval(uint8_t, uint8_t, float); extern uint8_t nav_oval_count; #define Oval(a, b, c) nav_oval((b), (a), (c)) @@ -119,10 +119,10 @@ extern void nav_circle_XY(float x, float y, float radius); /** Normalize a degree angle between 0 and 359 */ #define NormCourse(x) { \ - uint8_t dont_loop_forever = 0; \ - while (x < 0 && ++dont_loop_forever) x += 360; \ - while (x >= 360 && ++dont_loop_forever) x -= 360; \ -} + uint8_t dont_loop_forever = 0; \ + while (x < 0 && ++dont_loop_forever) x += 360; \ + while (x >= 360 && ++dont_loop_forever) x -= 360; \ + } #define NavCircleCountNoRewind() (nav_circle_radians_no_rewind / (2*M_PI)) #define NavCircleCount() (fabs(nav_circle_radians) / (2*M_PI)) @@ -148,47 +148,47 @@ bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float ap /** Set the climb control to auto-throttle with the specified pitch pre-command */ #define NavVerticalAutoThrottleMode(_pitch) { \ - v_ctl_climb_mode = V_CTL_CLIMB_MODE_AUTO_THROTTLE; \ - nav_pitch = _pitch; \ -} + v_ctl_climb_mode = V_CTL_CLIMB_MODE_AUTO_THROTTLE; \ + nav_pitch = _pitch; \ + } /** Set the climb control to auto-pitch with the specified throttle pre-command */ #define NavVerticalAutoPitchMode(_throttle) { \ - v_ctl_climb_mode = V_CTL_CLIMB_MODE_AUTO_PITCH; \ - nav_throttle_setpoint = _throttle; \ -} + v_ctl_climb_mode = V_CTL_CLIMB_MODE_AUTO_PITCH; \ + nav_throttle_setpoint = _throttle; \ + } /** Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command. */ #define NavVerticalAltitudeMode(_alt, _pre_climb) { \ - v_ctl_mode = V_CTL_MODE_AUTO_ALT; \ - nav_altitude = _alt; \ - v_ctl_altitude_pre_climb = _pre_climb; \ -} + v_ctl_mode = V_CTL_MODE_AUTO_ALT; \ + nav_altitude = _alt; \ + v_ctl_altitude_pre_climb = _pre_climb; \ + } /** Set the vertical mode to climb control with the specified climb setpoint */ #define NavVerticalClimbMode(_climb) { \ - v_ctl_mode = V_CTL_MODE_AUTO_CLIMB; \ - v_ctl_climb_setpoint = _climb; \ -} + v_ctl_mode = V_CTL_MODE_AUTO_CLIMB; \ + v_ctl_climb_setpoint = _climb; \ + } /** Set the vertical mode to fixed throttle with the specified setpoint */ #define NavVerticalThrottleMode(_throttle) { \ - v_ctl_mode = V_CTL_MODE_AUTO_THROTTLE; \ - nav_throttle_setpoint = _throttle; \ -} + v_ctl_mode = V_CTL_MODE_AUTO_THROTTLE; \ + nav_throttle_setpoint = _throttle; \ + } #define NavHeading(_course) { \ - lateral_mode = LATERAL_MODE_COURSE; \ - h_ctl_course_setpoint = _course; \ -} + lateral_mode = LATERAL_MODE_COURSE; \ + h_ctl_course_setpoint = _course; \ + } #define NavAttitude(_roll) { \ - lateral_mode = LATERAL_MODE_ROLL; \ - if(pprz_mode != PPRZ_MODE_AUTO1) \ - {h_ctl_roll_setpoint = _roll;} \ -} + lateral_mode = LATERAL_MODE_ROLL; \ + if(pprz_mode != PPRZ_MODE_AUTO1) \ + {h_ctl_roll_setpoint = _roll;} \ + } #define nav_IncreaseShift(x) { if (x==0) nav_shift = 0; else nav_shift += x; } @@ -207,14 +207,14 @@ bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float ap float dist_wp = sqrtf(dist2_to_wp); \ float dist_home = sqrtf(dist2_to_home); \ pprz_msg_send_NAVIGATION(_trans, _dev, AC_ID, &nav_block, &nav_stage, &(pos->x), &(pos->y), &dist_wp, &dist_home, &_circle_count, &nav_oval_count); \ -} + } extern bool_t DownlinkSendWpNr(uint8_t _wp); -#define DownlinkSendWp(_trans, _dev, i) { \ - float x = nav_utm_east0 + waypoints[i].x; \ - float y = nav_utm_north0 + waypoints[i].y; \ - pprz_msg_send_WP_MOVED(_trans, _dev, AC_ID, &i, &x, &y, &(waypoints[i].a),&nav_utm_zone0); \ -} +#define DownlinkSendWp(_trans, _dev, i) { \ + float x = nav_utm_east0 + waypoints[i].x; \ + float y = nav_utm_north0 + waypoints[i].y; \ + pprz_msg_send_WP_MOVED(_trans, _dev, AC_ID, &i, &x, &y, &(waypoints[i].a),&nav_utm_zone0); \ + } #endif /* NAV_H */ diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c index 17824ba230..6fb055f8d4 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c @@ -143,8 +143,8 @@ float v_ctl_pitch_dash_trim; #ifndef PITCH_TRIM_RATE_LIMITER #define PITCH_TRIM_RATE_LIMITER 3. #endif -inline static void h_ctl_roll_loop( void ); -inline static void h_ctl_pitch_loop( void ); +inline static void h_ctl_roll_loop(void); +inline static void h_ctl_pitch_loop(void); // Some default course gains // H_CTL_COURSE_PGAIN needs to be define in airframe @@ -192,31 +192,35 @@ inline static void h_ctl_pitch_loop( void ); #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_calibration(struct transport_tx *trans, struct link_device *dev) { +static void send_calibration(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_CALIBRATION(trans, dev, AC_ID, &v_ctl_auto_throttle_sum_err, &v_ctl_auto_throttle_submode); } -static void send_tune_roll(struct transport_tx *trans, struct link_device *dev) { +static void send_tune_roll(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_TUNE_ROLL(trans, dev, AC_ID, - &(stateGetBodyRates_f()->p), &(stateGetNedToBodyEulers_f()->phi), &h_ctl_roll_setpoint); + &(stateGetBodyRates_f()->p), &(stateGetNedToBodyEulers_f()->phi), &h_ctl_roll_setpoint); } -static void send_ctl_a(struct transport_tx *trans, struct link_device *dev) { +static void send_ctl_a(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_H_CTL_A(trans, dev, AC_ID, - &h_ctl_roll_sum_err, - &h_ctl_roll_setpoint, - &h_ctl_ref.roll_angle, - &(stateGetNedToBodyEulers_f()->phi), - &h_ctl_aileron_setpoint, - &h_ctl_pitch_sum_err, - &h_ctl_pitch_loop_setpoint, - &h_ctl_ref.pitch_angle, - &(stateGetNedToBodyEulers_f()->theta), - &h_ctl_elevator_setpoint); + &h_ctl_roll_sum_err, + &h_ctl_roll_setpoint, + &h_ctl_ref.roll_angle, + &(stateGetNedToBodyEulers_f()->phi), + &h_ctl_aileron_setpoint, + &h_ctl_pitch_sum_err, + &h_ctl_pitch_loop_setpoint, + &h_ctl_ref.pitch_angle, + &(stateGetNedToBodyEulers_f()->theta), + &h_ctl_elevator_setpoint); } #endif -void h_ctl_init( void ) { +void h_ctl_init(void) +{ h_ctl_ref.max_p = H_CTL_REF_MAX_P; h_ctl_ref.max_p_dot = H_CTL_REF_MAX_P_DOT; h_ctl_ref.max_q = H_CTL_REF_MAX_Q; @@ -279,7 +283,8 @@ void h_ctl_init( void ) { * \brief * */ -void h_ctl_course_loop ( void ) { +void h_ctl_course_loop(void) +{ static float last_err; // Ground path error @@ -290,33 +295,35 @@ void h_ctl_course_loop ( void ) { last_err = err; NormRadAngle(d_err); - float speed_depend_nav = (*stateGetHorizontalSpeedNorm_f())/NOMINAL_AIRSPEED; + float speed_depend_nav = (*stateGetHorizontalSpeedNorm_f()) / NOMINAL_AIRSPEED; Bound(speed_depend_nav, 0.66, 1.5); h_ctl_roll_setpoint = h_ctl_course_pre_bank_correction * h_ctl_course_pre_bank - + h_ctl_course_pgain * speed_depend_nav * err - + h_ctl_course_dgain * d_err; + + h_ctl_course_pgain * speed_depend_nav * err + + h_ctl_course_dgain * d_err; BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint); } #if USE_AIRSPEED -static inline void compute_airspeed_ratio( void ) { +static inline void compute_airspeed_ratio(void) +{ if (use_airspeed_ratio) { // low pass airspeed static float airspeed = 0.; - airspeed = ( 15*airspeed + (*stateGetAirspeed_f()) ) / 16; + airspeed = (15 * airspeed + (*stateGetAirspeed_f())) / 16; // compute ratio float airspeed_ratio = airspeed / NOMINAL_AIRSPEED; Bound(airspeed_ratio, AIRSPEED_RATIO_MIN, AIRSPEED_RATIO_MAX); - airspeed_ratio2 = airspeed_ratio*airspeed_ratio; + airspeed_ratio2 = airspeed_ratio * airspeed_ratio; } else { airspeed_ratio2 = 1.; } } #endif -void h_ctl_attitude_loop ( void ) { +void h_ctl_attitude_loop(void) +{ if (!h_ctl_disabled) { #if USE_AIRSPEED compute_airspeed_ratio(); @@ -344,7 +351,8 @@ void h_ctl_attitude_loop ( void ) { #define KFFA_UPDATE 0.1 #define KFFD_UPDATE 0.05 -inline static void h_ctl_roll_loop( void ) { +inline static void h_ctl_roll_loop(void) +{ static float cmd_fb = 0.; @@ -352,16 +360,16 @@ inline static void h_ctl_roll_loop( void ) { // Update reference setpoints for roll h_ctl_ref.roll_angle += h_ctl_ref.roll_rate * H_CTL_REF_DT; h_ctl_ref.roll_rate += h_ctl_ref.roll_accel * H_CTL_REF_DT; - h_ctl_ref.roll_accel = H_CTL_REF_W_P*H_CTL_REF_W_P * (h_ctl_roll_setpoint - h_ctl_ref.roll_angle) - 2*H_CTL_REF_XI_P*H_CTL_REF_W_P * h_ctl_ref.roll_rate; + h_ctl_ref.roll_accel = H_CTL_REF_W_P * H_CTL_REF_W_P * (h_ctl_roll_setpoint - h_ctl_ref.roll_angle) - 2 * H_CTL_REF_XI_P + * H_CTL_REF_W_P * h_ctl_ref.roll_rate; // Saturation on references BoundAbs(h_ctl_ref.roll_accel, h_ctl_ref.max_p_dot); if (h_ctl_ref.roll_rate > h_ctl_ref.max_p) { h_ctl_ref.roll_rate = h_ctl_ref.max_p; - if (h_ctl_ref.roll_accel > 0.) h_ctl_ref.roll_accel = 0.; - } - else if (h_ctl_ref.roll_rate < - h_ctl_ref.max_p) { + if (h_ctl_ref.roll_accel > 0.) { h_ctl_ref.roll_accel = 0.; } + } else if (h_ctl_ref.roll_rate < - h_ctl_ref.max_p) { h_ctl_ref.roll_rate = - h_ctl_ref.max_p; - if (h_ctl_ref.roll_accel < 0.) h_ctl_ref.roll_accel = 0.; + if (h_ctl_ref.roll_accel < 0.) { h_ctl_ref.roll_accel = 0.; } } #else h_ctl_ref.roll_angle = h_ctl_roll_setpoint; @@ -371,8 +379,8 @@ inline static void h_ctl_roll_loop( void ) { #ifdef USE_KFF_UPDATE // update Kff gains - h_ctl_roll_Kffa += KFFA_UPDATE * h_ctl_ref.roll_accel * cmd_fb / (h_ctl_ref.max_p_dot*h_ctl_ref.max_p_dot); - h_ctl_roll_Kffd += KFFD_UPDATE * h_ctl_ref.roll_rate * cmd_fb / (h_ctl_ref.max_p*h_ctl_ref.max_p); + h_ctl_roll_Kffa += KFFA_UPDATE * h_ctl_ref.roll_accel * cmd_fb / (h_ctl_ref.max_p_dot * h_ctl_ref.max_p_dot); + h_ctl_roll_Kffd += KFFD_UPDATE * h_ctl_ref.roll_rate * cmd_fb / (h_ctl_ref.max_p * h_ctl_ref.max_p); #ifdef SITL printf("%f %f %f\n", h_ctl_roll_Kffa, h_ctl_roll_Kffd, cmd_fb); #endif @@ -382,13 +390,12 @@ inline static void h_ctl_roll_loop( void ) { // Compute errors float err = h_ctl_ref.roll_angle - stateGetNedToBodyEulers_f()->phi; - struct FloatRates* body_rate = stateGetBodyRates_f(); + struct FloatRates *body_rate = stateGetBodyRates_f(); float d_err = h_ctl_ref.roll_rate - body_rate->p; if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) { h_ctl_roll_sum_err = 0.; - } - else { + } else { if (h_ctl_roll_igain > 0.) { h_ctl_roll_sum_err += err * H_CTL_REF_DT; BoundAbs(h_ctl_roll_sum_err, H_CTL_ROLL_SUM_ERR_MAX / h_ctl_roll_igain); @@ -399,11 +406,11 @@ inline static void h_ctl_roll_loop( void ) { cmd_fb = h_ctl_roll_attitude_gain * err; float cmd = - h_ctl_roll_Kffa * h_ctl_ref.roll_accel - - h_ctl_roll_Kffd * h_ctl_ref.roll_rate - - cmd_fb - - h_ctl_roll_rate_gain * d_err - - h_ctl_roll_igain * h_ctl_roll_sum_err - + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle; + - h_ctl_roll_Kffd * h_ctl_ref.roll_rate + - cmd_fb + - h_ctl_roll_rate_gain * d_err + - h_ctl_roll_igain * h_ctl_roll_sum_err + + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle; cmd /= airspeed_ratio2; @@ -413,15 +420,16 @@ inline static void h_ctl_roll_loop( void ) { #if USE_PITCH_TRIM -inline static void loiter(void) { +inline static void loiter(void) +{ float pitch_trim; static float last_pitch_trim; #if USE_AIRSPEED if (stateGetAirspeed_f() > NOMINAL_AIRSPEED) { - pitch_trim = v_ctl_pitch_dash_trim * (airspeed_ratio2-1) / ((AIRSPEED_RATIO_MAX * AIRSPEED_RATIO_MAX) - 1); + pitch_trim = v_ctl_pitch_dash_trim * (airspeed_ratio2 - 1) / ((AIRSPEED_RATIO_MAX * AIRSPEED_RATIO_MAX) - 1); } else { - pitch_trim = v_ctl_pitch_loiter_trim * (airspeed_ratio2-1) / ((AIRSPEED_RATIO_MIN * AIRSPEED_RATIO_MIN) - 1); + pitch_trim = v_ctl_pitch_loiter_trim * (airspeed_ratio2 - 1) / ((AIRSPEED_RATIO_MIN * AIRSPEED_RATIO_MIN) - 1); } #else float throttle_diff = v_ctl_auto_throttle_cruise_throttle - v_ctl_auto_throttle_nominal_cruise_throttle; @@ -435,7 +443,7 @@ inline static void loiter(void) { #endif // Pitch trim rate limiter - float max_change = (v_ctl_pitch_loiter_trim - v_ctl_pitch_dash_trim) * H_CTL_REF_DT/ PITCH_TRIM_RATE_LIMITER; + float max_change = (v_ctl_pitch_loiter_trim - v_ctl_pitch_dash_trim) * H_CTL_REF_DT / PITCH_TRIM_RATE_LIMITER; Bound(pitch_trim, last_pitch_trim - max_change, last_pitch_trim + max_change); last_pitch_trim = pitch_trim; @@ -444,14 +452,16 @@ inline static void loiter(void) { #endif -inline static void h_ctl_pitch_loop( void ) { +inline static void h_ctl_pitch_loop(void) +{ #if !USE_GYRO_PITCH_RATE static float last_err; #endif /* sanity check */ - if (h_ctl_pitch_of_roll <0.) + if (h_ctl_pitch_of_roll < 0.) { h_ctl_pitch_of_roll = 0.; + } h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_pitch_of_roll * fabs(stateGetNedToBodyEulers_f()->phi); #if USE_PITCH_TRIM @@ -462,16 +472,16 @@ inline static void h_ctl_pitch_loop( void ) { // Update reference setpoints for pitch h_ctl_ref.pitch_angle += h_ctl_ref.pitch_rate * H_CTL_REF_DT; h_ctl_ref.pitch_rate += h_ctl_ref.pitch_accel * H_CTL_REF_DT; - h_ctl_ref.pitch_accel = H_CTL_REF_W_Q*H_CTL_REF_W_Q * (h_ctl_pitch_loop_setpoint - h_ctl_ref.pitch_angle) - 2*H_CTL_REF_XI_Q*H_CTL_REF_W_Q * h_ctl_ref.pitch_rate; + h_ctl_ref.pitch_accel = H_CTL_REF_W_Q * H_CTL_REF_W_Q * (h_ctl_pitch_loop_setpoint - h_ctl_ref.pitch_angle) - 2 * + H_CTL_REF_XI_Q * H_CTL_REF_W_Q * h_ctl_ref.pitch_rate; // Saturation on references BoundAbs(h_ctl_ref.pitch_accel, h_ctl_ref.max_q_dot); if (h_ctl_ref.pitch_rate > h_ctl_ref.max_q) { h_ctl_ref.pitch_rate = h_ctl_ref.max_q; - if (h_ctl_ref.pitch_accel > 0.) h_ctl_ref.pitch_accel = 0.; - } - else if (h_ctl_ref.pitch_rate < - h_ctl_ref.max_q) { + if (h_ctl_ref.pitch_accel > 0.) { h_ctl_ref.pitch_accel = 0.; } + } else if (h_ctl_ref.pitch_rate < - h_ctl_ref.max_q) { h_ctl_ref.pitch_rate = - h_ctl_ref.max_q; - if (h_ctl_ref.pitch_accel < 0.) h_ctl_ref.pitch_accel = 0.; + if (h_ctl_ref.pitch_accel < 0.) { h_ctl_ref.pitch_accel = 0.; } } #else h_ctl_ref.pitch_angle = h_ctl_pitch_loop_setpoint; @@ -484,14 +494,13 @@ inline static void h_ctl_pitch_loop( void ) { #if USE_GYRO_PITCH_RATE float d_err = h_ctl_ref.pitch_rate - stateGetBodyRates_f()->q; #else // soft derivation - float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref.pitch_rate; + float d_err = (err - last_err) / H_CTL_REF_DT - h_ctl_ref.pitch_rate; last_err = err; #endif if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) { h_ctl_pitch_sum_err = 0.; - } - else { + } else { if (h_ctl_pitch_igain > 0.) { h_ctl_pitch_sum_err += err * H_CTL_REF_DT; BoundAbs(h_ctl_pitch_sum_err, H_CTL_PITCH_SUM_ERR_MAX / h_ctl_pitch_igain); @@ -501,10 +510,10 @@ inline static void h_ctl_pitch_loop( void ) { } float cmd = - h_ctl_pitch_Kffa * h_ctl_ref.pitch_accel - - h_ctl_pitch_Kffd * h_ctl_ref.pitch_rate - + h_ctl_pitch_pgain * err - + h_ctl_pitch_dgain * d_err - + h_ctl_pitch_igain * h_ctl_pitch_sum_err; + - h_ctl_pitch_Kffd * h_ctl_ref.pitch_rate + + h_ctl_pitch_pgain * err + + h_ctl_pitch_dgain * d_err + + h_ctl_pitch_igain * h_ctl_pitch_sum_err; cmd /= airspeed_ratio2; diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h index 4c8155f2ee..5b464769fd 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.h @@ -48,13 +48,13 @@ extern float h_ctl_pitch_of_roll; #define H_CTL_PITCH_SUM_ERR_MAX (MAX_PPRZ/2.) #define stabilization_adaptive_SetRollIGain(_gain) { \ - h_ctl_roll_sum_err = 0.; \ - h_ctl_roll_igain = _gain; \ + h_ctl_roll_sum_err = 0.; \ + h_ctl_roll_igain = _gain; \ } #define stabilization_adaptive_SetPitchIGain(_gain) { \ - h_ctl_pitch_sum_err = 0.; \ - h_ctl_pitch_igain = _gain; \ + h_ctl_pitch_sum_err = 0.; \ + h_ctl_pitch_igain = _gain; \ } extern bool_t use_airspeed_ratio; diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c index 5f1ffc409c..9a28930a42 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c @@ -87,10 +87,10 @@ float h_ctl_course_slew_increment; #endif -inline static void h_ctl_roll_loop( void ); -inline static void h_ctl_pitch_loop( void ); +inline static void h_ctl_roll_loop(void); +inline static void h_ctl_pitch_loop(void); #ifdef H_CTL_RATE_LOOP -static inline void h_ctl_roll_rate_loop( void ); +static inline void h_ctl_roll_rate_loop(void); #endif #ifndef H_CTL_COURSE_PRE_BANK_CORRECTION @@ -115,12 +115,14 @@ static float nav_ratio; #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_calibration(struct transport_tx *trans, struct link_device *dev) { +static void send_calibration(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_CALIBRATION(trans, dev, AC_ID, &v_ctl_auto_throttle_sum_err, &v_ctl_auto_throttle_submode); } #endif -void h_ctl_init( void ) { +void h_ctl_init(void) +{ h_ctl_course_setpoint = 0.; h_ctl_course_pre_bank = 0.; h_ctl_course_pre_bank_correction = H_CTL_COURSE_PRE_BANK_CORRECTION; @@ -173,7 +175,7 @@ void h_ctl_init( void ) { #endif #ifdef AGR_CLIMB - nav_ratio=0; + nav_ratio = 0; #endif #if PERIODIC_TELEMETRY @@ -185,7 +187,8 @@ void h_ctl_init( void ) { * \brief * */ -void h_ctl_course_loop ( void ) { +void h_ctl_course_loop(void) +{ static float last_err; // Ground path error @@ -198,10 +201,9 @@ void h_ctl_course_loop ( void ) { float advance = cos(err) * (*stateGetHorizontalSpeedNorm_f()) / reference_advance; if ( - (advance < 1.) && // Path speed is small - ((*stateGetHorizontalSpeedNorm_f()) < reference_advance) // Small path speed is due to wind (small groundspeed) - ) - { + (advance < 1.) && // Path speed is small + ((*stateGetHorizontalSpeedNorm_f()) < reference_advance) // Small path speed is due to wind (small groundspeed) + ) { /* // rough crabangle approximation float wind_mod = sqrt(wind_east*wind_east + wind_north*wind_north); @@ -221,16 +223,11 @@ void h_ctl_course_loop ( void ) { float herr = stateGetNedToBodyEulers_f()->psi - h_ctl_course_setpoint; //+crab); NormRadAngle(herr); - if (advance < -0.5) // 90 degree turn - { + if (advance < -0.5) { // 90 degree turn err = herr; - } - else if (advance < 0.) // half_nav_angle_saturation) { h_ctl_course_slew_rate = Max(h_ctl_course_slew_rate, 0.); - err = Min(err,(half_nav_angle_saturation + h_ctl_course_slew_rate)); - h_ctl_course_slew_rate +=h_ctl_course_slew_increment; - } - else if ( err < -half_nav_angle_saturation) { + err = Min(err, (half_nav_angle_saturation + h_ctl_course_slew_rate)); + h_ctl_course_slew_rate += h_ctl_course_slew_increment; + } else if (err < -half_nav_angle_saturation) { h_ctl_course_slew_rate = Min(h_ctl_course_slew_rate, 0.); - err = Max(err,(-half_nav_angle_saturation + h_ctl_course_slew_rate)); - h_ctl_course_slew_rate -=h_ctl_course_slew_increment; - } - else { + err = Max(err, (-half_nav_angle_saturation + h_ctl_course_slew_rate)); + h_ctl_course_slew_rate -= h_ctl_course_slew_increment; + } else { h_ctl_course_slew_rate = 0.; } } #endif - float speed_depend_nav = (*stateGetHorizontalSpeedNorm_f())/NOMINAL_AIRSPEED; + float speed_depend_nav = (*stateGetHorizontalSpeedNorm_f()) / NOMINAL_AIRSPEED; Bound(speed_depend_nav, 0.66, 1.5); float cmd = -h_ctl_course_pgain * speed_depend_nav * (err + d_err * h_ctl_course_dgain); @@ -290,11 +285,13 @@ void h_ctl_course_loop ( void ) { BoundAbs(cmd, h_ctl_roll_max_setpoint); /* bound cmd before NAV_RATIO and again after */ /* altitude: z-up is positive -> positive error -> too low */ if (v_ctl_altitude_error > 0) { - nav_ratio = AGR_CLIMB_NAV_RATIO + (1 - AGR_CLIMB_NAV_RATIO) * (1 - (fabs(v_ctl_altitude_error) - AGR_BLEND_END) / (AGR_BLEND_START - AGR_BLEND_END)); - Bound (nav_ratio, AGR_CLIMB_NAV_RATIO, 1); + nav_ratio = AGR_CLIMB_NAV_RATIO + (1 - AGR_CLIMB_NAV_RATIO) * (1 - (fabs(v_ctl_altitude_error) - AGR_BLEND_END) / + (AGR_BLEND_START - AGR_BLEND_END)); + Bound(nav_ratio, AGR_CLIMB_NAV_RATIO, 1); } else { - nav_ratio = AGR_DESCENT_NAV_RATIO + (1 - AGR_DESCENT_NAV_RATIO) * (1 - (fabs(v_ctl_altitude_error) - AGR_BLEND_END) / (AGR_BLEND_START - AGR_BLEND_END)); - Bound (nav_ratio, AGR_DESCENT_NAV_RATIO, 1); + nav_ratio = AGR_DESCENT_NAV_RATIO + (1 - AGR_DESCENT_NAV_RATIO) * (1 - (fabs(v_ctl_altitude_error) - AGR_BLEND_END) / + (AGR_BLEND_START - AGR_BLEND_END)); + Bound(nav_ratio, AGR_DESCENT_NAV_RATIO, 1); } cmd *= nav_ratio; } @@ -314,7 +311,8 @@ void h_ctl_course_loop ( void ) { BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint); } -void h_ctl_attitude_loop ( void ) { +void h_ctl_attitude_loop(void) +{ if (!h_ctl_disabled) { h_ctl_roll_loop(); h_ctl_pitch_loop(); @@ -323,17 +321,18 @@ void h_ctl_attitude_loop ( void ) { #ifdef H_CTL_ROLL_ATTITUDE_GAIN -inline static void h_ctl_roll_loop( void ) { +inline static void h_ctl_roll_loop(void) +{ float err = stateGetNedToBodyEulers_f()->phi - h_ctl_roll_setpoint; - struct FloatRates* body_rate = stateGetBodyRates_f(); + struct FloatRates *body_rate = stateGetBodyRates_f(); #ifdef SITL static float last_err = 0; - body_rate->p = (err - last_err)/(1/60.); + body_rate->p = (err - last_err) / (1 / 60.); last_err = err; #endif float cmd = h_ctl_roll_attitude_gain * err - + h_ctl_roll_rate_gain * body_rate->p - + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle; + + h_ctl_roll_rate_gain * body_rate->p + + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle; h_ctl_aileron_setpoint = TRIM_PPRZ(cmd); } @@ -341,10 +340,11 @@ inline static void h_ctl_roll_loop( void ) { #else // H_CTL_ROLL_ATTITUDE_GAIN /** Computes h_ctl_aileron_setpoint from h_ctl_roll_setpoint */ -inline static void h_ctl_roll_loop( void ) { +inline static void h_ctl_roll_loop(void) +{ float err = stateGetNedToBodyEulers_f()->phi - h_ctl_roll_setpoint; float cmd = h_ctl_roll_pgain * err - + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle; + + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle; h_ctl_aileron_setpoint = TRIM_PPRZ(cmd); #ifdef H_CTL_RATE_LOOP @@ -365,7 +365,8 @@ inline static void h_ctl_roll_loop( void ) { #ifdef H_CTL_RATE_LOOP -static inline void h_ctl_roll_rate_loop() { +static inline void h_ctl_roll_rate_loop() +{ float err = stateGetBodyRates_f()->p - h_ctl_roll_rate_setpoint; /* I term calculation */ @@ -377,7 +378,7 @@ static inline void h_ctl_roll_rate_loop() { roll_rate_sum_values[roll_rate_sum_idx] = err; roll_rate_sum_err += err; roll_rate_sum_idx++; - if (roll_rate_sum_idx >= H_CTL_ROLL_RATE_SUM_NB_SAMPLES) roll_rate_sum_idx = 0; + if (roll_rate_sum_idx >= H_CTL_ROLL_RATE_SUM_NB_SAMPLES) { roll_rate_sum_idx = 0; } /* D term calculations */ static float last_err = 0; @@ -385,8 +386,10 @@ static inline void h_ctl_roll_rate_loop() { last_err = err; float throttle_dep_pgain = - Blend(h_ctl_hi_throttle_roll_rate_pgain, h_ctl_lo_throttle_roll_rate_pgain, v_ctl_throttle_setpoint/((float)MAX_PPRZ)); - float cmd = throttle_dep_pgain * ( err + h_ctl_roll_rate_igain * roll_rate_sum_err / H_CTL_ROLL_RATE_SUM_NB_SAMPLES + h_ctl_roll_rate_dgain * d_err); + Blend(h_ctl_hi_throttle_roll_rate_pgain, h_ctl_lo_throttle_roll_rate_pgain, + v_ctl_throttle_setpoint / ((float)MAX_PPRZ)); + float cmd = throttle_dep_pgain * (err + h_ctl_roll_rate_igain * roll_rate_sum_err / H_CTL_ROLL_RATE_SUM_NB_SAMPLES + + h_ctl_roll_rate_dgain * d_err); h_ctl_aileron_setpoint = TRIM_PPRZ(cmd); } @@ -404,7 +407,8 @@ static inline void h_ctl_roll_rate_loop() { float v_ctl_auto_throttle_loiter_trim = V_CTL_AUTO_THROTTLE_LOITER_TRIM; float v_ctl_auto_throttle_dash_trim = V_CTL_AUTO_THROTTLE_DASH_TRIM; -inline static float loiter(void) { +inline static float loiter(void) +{ static float last_elevator_trim; float elevator_trim; @@ -426,19 +430,21 @@ inline static float loiter(void) { #endif -inline static void h_ctl_pitch_loop( void ) { +inline static void h_ctl_pitch_loop(void) +{ static float last_err; - struct FloatEulers* att = stateGetNedToBodyEulers_f(); + struct FloatEulers *att = stateGetNedToBodyEulers_f(); /* sanity check */ - if (h_ctl_elevator_of_roll <0.) + if (h_ctl_elevator_of_roll < 0.) { h_ctl_elevator_of_roll = 0.; + } h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_elevator_of_roll / h_ctl_pitch_pgain * fabs(att->phi); float err = 0; #ifdef USE_AOA - switch (h_ctl_pitch_mode){ + switch (h_ctl_pitch_mode) { case H_CTL_PITCH_MODE_THETA: err = att->theta - h_ctl_pitch_loop_setpoint; break; diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.h b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.h index 4b94f56c9e..9ea67bdf05 100644 --- a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.h +++ b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.h @@ -61,8 +61,8 @@ extern float h_ctl_roll_slew; #ifdef USE_AOA /* Pitch mode */ -#define H_CTL_PITCH_MODE_THETA 0 -#define H_CTL_PITCH_MODE_AOA 1 +#define H_CTL_PITCH_MODE_THETA 0 +#define H_CTL_PITCH_MODE_AOA 1 extern uint8_t h_ctl_pitch_mode; #endif @@ -91,9 +91,9 @@ extern float h_ctl_roll_rate_dgain; #define stabilization_attitude_SetRollRatePGain(v) { h_ctl_hi_throttle_roll_rate_pgain = v; h_ctl_lo_throttle_roll_rate_pgain = v; } #endif -extern void h_ctl_init( void ); -extern void h_ctl_course_loop ( void ); -extern void h_ctl_attitude_loop ( void ); +extern void h_ctl_init(void); +extern void h_ctl_course_loop(void); +extern void h_ctl_attitude_loop(void); extern float h_ctl_roll_attitude_gain; extern float h_ctl_roll_rate_gain; diff --git a/sw/airborne/firmwares/logger/main_logger.c b/sw/airborne/firmwares/logger/main_logger.c index 46388b126c..32f8ab4da9 100644 --- a/sw/airborne/firmwares/logger/main_logger.c +++ b/sw/airborne/firmwares/logger/main_logger.c @@ -27,53 +27,53 @@ * to a (micro) SD card through the efsl library */ - /* XBee-message: ABCDxxxxxxxE - A XBEE_START (0x7E) - B LENGTH_MSB (D->D) - C LENGTH_LSB - D XBEE_PAYLOAD - 0 XBEE_TX16 (0x01) / XBEE_RX16 (0x81) - 1 FRAME_ID (0) / SRC_ID_MSB - 2 DEST_ID_MSB / SRC_ID_LSB - 3 DEST_ID_LSB / XBEE_RSSI - 4 TX16_OPTIONS (0) / RX16_OPTIONS - 5 PPRZ_DATA - 0 SENDER_ID - 1 MSG_ID - 2 MSG_PAYLOAD - . DATA (messages.xml) - E XBEE_CHECKSUM (sum[A->D]) - - ID is AC_ID for aircraft, 0x100 for ground station - */ - - /* PPRZ-message: ABCxxxxxxxDE - A PPRZ_STX (0x99) - B LENGTH (A->E) - C PPRZ_DATA +/* XBee-message: ABCDxxxxxxxE + A XBEE_START (0x7E) + B LENGTH_MSB (D->D) + C LENGTH_LSB + D XBEE_PAYLOAD + 0 XBEE_TX16 (0x01) / XBEE_RX16 (0x81) + 1 FRAME_ID (0) / SRC_ID_MSB + 2 DEST_ID_MSB / SRC_ID_LSB + 3 DEST_ID_LSB / XBEE_RSSI + 4 TX16_OPTIONS (0) / RX16_OPTIONS + 5 PPRZ_DATA 0 SENDER_ID 1 MSG_ID 2 MSG_PAYLOAD . DATA (messages.xml) - D PPRZ_CHECKSUM_A (sum[B->C]) - E PPRZ_CHECKSUM_B (sum[ck_a]) - */ + E XBEE_CHECKSUM (sum[A->D]) - /* LOG-message: ABCDEFGHxxxxxxxI - A PPRZ_STX (0x99) - B LENGTH (H->H) - C SOURCE (0=uart0, 1=uart1, 2=i2c0, ...) - D TIMESTAMP_LSB (100 microsec raster) - E TIMESTAMP - F TIMESTAMP - G TIMESTAMP_MSB - H PPRZ_DATA - 0 SENDER_ID - 1 MSG_ID - 2 MSG_PAYLOAD - . DATA (messages.xml) - I CHECKSUM (sum[B->H]) - */ + ID is AC_ID for aircraft, 0x100 for ground station +*/ + +/* PPRZ-message: ABCxxxxxxxDE + A PPRZ_STX (0x99) + B LENGTH (A->E) + C PPRZ_DATA + 0 SENDER_ID + 1 MSG_ID + 2 MSG_PAYLOAD + . DATA (messages.xml) + D PPRZ_CHECKSUM_A (sum[B->C]) + E PPRZ_CHECKSUM_B (sum[ck_a]) +*/ + +/* LOG-message: ABCDEFGHxxxxxxxI + A PPRZ_STX (0x99) + B LENGTH (H->H) + C SOURCE (0=uart0, 1=uart1, 2=i2c0, ...) + D TIMESTAMP_LSB (100 microsec raster) + E TIMESTAMP + F TIMESTAMP + G TIMESTAMP_MSB + H PPRZ_DATA + 0 SENDER_ID + 1 MSG_ID + 2 MSG_PAYLOAD + . DATA (messages.xml) + I CHECKSUM (sum[B->H]) +*/ #include "std.h" #include "mcu.h" @@ -118,15 +118,15 @@ */ #ifndef LED_GREEN -#define LED_GREEN 3 +#define LED_GREEN 3 #endif #ifndef LED_YELLOW -#define LED_YELLOW 2 +#define LED_YELLOW 2 #endif #ifndef LED_RED -#define LED_RED 1 +#define LED_RED 1 #endif @@ -173,12 +173,12 @@ #define LOG_SOURCE_I2C0 2 #define LOG_SOURCE_I2C1 3 -static inline void main_init( void ); -static inline void main_periodic_task( void ); +static inline void main_init(void); +static inline void main_periodic_task(void); int main_log(void); -void set_filename(unsigned int local, char* name); -unsigned char checksum(unsigned char start, unsigned char* data, int length); +void set_filename(unsigned int local, char *name); +unsigned char checksum(unsigned char start, unsigned char *data, int length); unsigned int getclock(void); void log_payload(int len, unsigned char source, unsigned int timestamp); void log_xbee(unsigned char c, unsigned char source); @@ -196,7 +196,7 @@ volatile unsigned char xbeel_payload_len; volatile unsigned char pprzl_payload_len; unsigned char xbeel_error; unsigned char pprzl_error; -unsigned char log_buffer[MSG_SIZE] __attribute__ ((aligned)); +unsigned char log_buffer[MSG_SIZE] __attribute__((aligned)); static unsigned int xbeel_timestamp = 0; static unsigned int pprzl_timestamp = 0; unsigned int nb_messages = 0; @@ -205,23 +205,23 @@ int bytes = 0; unsigned int clock_msb = 0; unsigned int clock_lsb_last = 0; -void set_filename(unsigned int local, char* name) +void set_filename(unsigned int local, char *name) { /* do not use sprintf or similar */ int i; - for (i=7; i>=0; 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; + name[8] = '.'; name[9] = 't'; name[10] = 'l'; name[11] = 'm'; name[12] = 0; } -unsigned char checksum(unsigned char start, unsigned char* data, int length) +unsigned char checksum(unsigned char start, unsigned char *data, int length) { int i; unsigned char retval = start; - for (i=0;i>LOG_STOP_KEY) - { + while ((IO0PIN & (1 << LOG_STOP_KEY)) >> LOG_STOP_KEY) { #ifdef USE_MAX11040 if ((max11040_data == MAX11040_DATA_AVAILABLE) && @@ -428,20 +429,20 @@ int do_log(void) 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; + i = max11040_buf_out + 1; + if (i >= MAX11040_BUF_SIZE) { i = 0; } max11040_buf_out = i; } } @@ -449,9 +450,8 @@ int do_log(void) #if USE_UART0 temp = 0; - while (uart_char_available(&uart0) && (temp++ < 128)) - { - // LED_TOGGLE(LED_GREEN); + while (uart_char_available(&uart0) && (temp++ < 128)) { + // LED_TOGGLE(LED_GREEN); inc = uart_getch(&uart1); #ifdef LOG_XBEE log_xbee(inc, LOG_SOURCE_UART0); @@ -466,9 +466,8 @@ int do_log(void) #endif #if USE_UART1 temp = 0; - while (uart_char_available(&uart1) && (temp++ < 128)) - { - // LED_TOGGLE(LED_GREEN); + while (uart_char_available(&uart1) && (temp++ < 128)) { + // LED_TOGGLE(LED_GREEN); inc = uart_getch(&uart1); #ifdef LOG_XBEE log_xbee(inc, LOG_SOURCE_UART1); @@ -484,8 +483,8 @@ int do_log(void) } LED_OFF(LED_GREEN); - file_fclose( &filew ); - fs_umount( &efs.myFs ) ; + file_fclose(&filew); + fs_umount(&efs.myFs) ; return 0; } @@ -496,23 +495,16 @@ int main(void) main_init(); #ifdef _DEBUG_BOARD_ - while(1) - { - if (IO0PIN & (1 << LOG_STOP_KEY)) - { + while (1) { + if (IO0PIN & (1 << LOG_STOP_KEY)) { LED_ON(LED_YELLOW); - } - else - { + } else { LED_OFF(LED_YELLOW); } - if (IO1PIN & (1 << CARD_DETECT_PIN)) - { + if (IO1PIN & (1 << CARD_DETECT_PIN)) { LED_OFF(LED_GREEN); - } - else - { + } else { LED_ON(LED_GREEN); } @@ -520,9 +512,7 @@ int main(void) // if (IO0PIN & (1 << VBUS_PIN)) { LED_ON(LED_RED); - } - else - { + } else { LED_OFF(LED_RED); } } @@ -530,23 +520,16 @@ int main(void) #ifdef _DEBUG_BOARD_ - while(1) - { - if (IO0PIN & (1 << LOG_STOP_KEY)) - { + while (1) { + if (IO0PIN & (1 << LOG_STOP_KEY)) { LED_ON(LED_YELLOW); - } - else - { + } else { LED_OFF(LED_YELLOW); } - if (IO1PIN & (1 << CARD_DETECT_PIN)) - { + if (IO1PIN & (1 << CARD_DETECT_PIN)) { LED_OFF(LED_GREEN); - } - else - { + } else { LED_ON(LED_GREEN); } @@ -554,24 +537,20 @@ int main(void) // if (IO0PIN & (1 << VBUS_PIN)) { LED_ON(LED_RED); - } - else - { + } else { LED_OFF(LED_RED); } } #endif // Direct SD Reader Mode - if ((IO0PIN & _BV(VBUS_PIN))>>VBUS_PIN) - { + if ((IO0PIN & _BV(VBUS_PIN)) >> VBUS_PIN) { LED_OFF(LED_YELLOW); LED_ON(LED_RED); main_mass_storage(); } - while(1) - { + while (1) { LED_ON(LED_YELLOW); do_log(); LED_OFF(LED_YELLOW); @@ -579,39 +558,38 @@ int main(void) waitloop = 0; ledcount = 0; - while (waitloop < 20) - { + while (waitloop < 20) { sys_time_usleep(100000); { if (ledcount++ > 9) { - ledcount=0; + ledcount = 0; LED_ON(LED_YELLOW); } else { LED_OFF(LED_YELLOW); } - if (((IO0PIN & _BV(LOG_STOP_KEY))>>LOG_STOP_KEY) == 1) { - waitloop=0; + if (((IO0PIN & _BV(LOG_STOP_KEY)) >> LOG_STOP_KEY) == 1) { + waitloop = 0; } else { waitloop++; } } - if ((IO0PIN & _BV(VBUS_PIN))>>VBUS_PIN) - { + if ((IO0PIN & _BV(VBUS_PIN)) >> VBUS_PIN) { LED_OFF(LED_YELLOW); LED_ON(LED_RED); main_mass_storage(); } } LED_ON(LED_YELLOW); - while (((IO0PIN & _BV(LOG_STOP_KEY))>>LOG_STOP_KEY) == 0); + while (((IO0PIN & _BV(LOG_STOP_KEY)) >> LOG_STOP_KEY) == 0); } return 0; } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); sys_time_init(); led_init(); @@ -624,8 +602,9 @@ static inline void main_init( void ) { mcu_int_enable(); - PINSEL2 = ~ (0x0c); + PINSEL2 = ~(0x0c); } -static inline void main_periodic_task( void ) { +static inline void main_periodic_task(void) +{ } diff --git a/sw/airborne/firmwares/motor_bench/main_motor_bench.c b/sw/airborne/firmwares/motor_bench/main_motor_bench.c index 4bf01b9ec2..cf6369ca86 100644 --- a/sw/airborne/firmwares/motor_bench/main_motor_bench.c +++ b/sw/airborne/firmwares/motor_bench/main_motor_bench.c @@ -22,26 +22,29 @@ #include "mb_modes.h" //#include "mb_static.h" -static inline void main_init( void ); -static inline void main_periodic_task( void ); -static inline void main_event_task( void ); +static inline void main_init(void); +static inline void main_periodic_task(void); +static inline void main_event_task(void); -static inline void main_dl_parse_msg( void ); +static inline void main_dl_parse_msg(void); -int main( void ) { +int main(void) +{ main_init(); - while(1) { - if (sys_time_check_and_ack_timer(0)) + while (1) { + if (sys_time_check_and_ack_timer(0)) { main_periodic_task(); + } main_event_task(); } return 0; } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); mb_tacho_init(); #if defined USE_TWI_CONTROLLER @@ -50,7 +53,7 @@ static inline void main_init( void ) { #endif mb_servo_init(); - mb_servo_set_range( 1090000, 1910000 ); + mb_servo_set_range(1090000, 1910000); mb_current_init(); mb_scale_init(); @@ -61,7 +64,8 @@ static inline void main_init( void ) { mcu_int_enable(); } -static inline void main_periodic_task( void ) { +static inline void main_periodic_task(void) +{ float rpm = mb_tacho_get_averaged(); mb_current_periodic(); float amps = mb_current_amp; @@ -79,18 +83,20 @@ static inline void main_periodic_task( void ) { RunOnceEvery(125, { - DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM); - PeriodicSendDlValue(DefaultChannel); - }); + DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM); + PeriodicSendDlValue(DefaultChannel); + }); uint16_t time_secs = sys_time.nb_sec; - DOWNLINK_SEND_MOTOR_BENCH_STATUS(DefaultChannel, DefaultDevice, &sys_time.nb_sec_rem, &throttle, &rpm, &s , &thrust, &torque, &time_secs, &mb_modes_mode); + DOWNLINK_SEND_MOTOR_BENCH_STATUS(DefaultChannel, DefaultDevice, &sys_time.nb_sec_rem, &throttle, &rpm, &s , &thrust, + &torque, &time_secs, &mb_modes_mode); } -static inline void main_event_task( void ) { +static inline void main_event_task(void) +{ if (PprzBuffer()) { ReadPprzBuffer(); if (pprz_msg_received) { @@ -107,12 +113,13 @@ static inline void main_event_task( void ) { bool_t dl_msg_available; #define MSG_SIZE 128 -uint8_t dl_buffer[MSG_SIZE] __attribute__ ((aligned)); +uint8_t dl_buffer[MSG_SIZE] __attribute__((aligned)); #define IdOfMsg(x) (x[1]) -static inline void main_dl_parse_msg(void) { +static inline void main_dl_parse_msg(void) +{ uint8_t msg_id = IdOfMsg(dl_buffer); // if (msg_id == DL_PING) {} if (msg_id == DL_SETTING) { diff --git a/sw/airborne/firmwares/motor_bench/main_turntable.c b/sw/airborne/firmwares/motor_bench/main_turntable.c index a94f4542f6..d97354751f 100644 --- a/sw/airborne/firmwares/motor_bench/main_turntable.c +++ b/sw/airborne/firmwares/motor_bench/main_turntable.c @@ -16,9 +16,9 @@ #define NB_STEP 256 #endif -static inline void main_init( void ); -static inline void main_periodic( void ); -static inline void main_event( void ); +static inline void main_init(void); +static inline void main_periodic(void); +static inline void main_event(void); static inline void main_init_tacho(void); uint32_t lp_pulse; @@ -26,38 +26,44 @@ uint32_t nb_pulse = 0; static float omega_rad; -int main( void ) { +int main(void) +{ main_init(); - while(1) { - if (sys_time_check_and_ack_timer(0)) + while (1) { + if (sys_time_check_and_ack_timer(0)) { main_periodic(); + } main_event(); } return 0; } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); - sys_time_register_timer(1./PERIODIC_FREQUENCY, NULL); + sys_time_register_timer(1. / PERIODIC_FREQUENCY, NULL); main_init_tacho(); mcu_int_enable(); } -static inline void main_periodic( void ) { +static inline void main_periodic(void) +{ RunOnceEvery(50, { - const float tach_to_rpm = 15000000.*2*M_PI/(float)NB_STEP; - omega_rad = tach_to_rpm / lp_pulse; - DOWNLINK_SEND_IMU_TURNTABLE(DefaultChannel, DefaultDevice, &omega_rad);} - // float foo = nb_pulse; - // DOWNLINK_SEND_IMU_TURNTABLE(DefaultChannel, DefaultDevice, &foo);} - ); + const float tach_to_rpm = 15000000.*2 * M_PI / (float)NB_STEP; + omega_rad = tach_to_rpm / lp_pulse; + DOWNLINK_SEND_IMU_TURNTABLE(DefaultChannel, DefaultDevice, &omega_rad); + } + // float foo = nb_pulse; + // DOWNLINK_SEND_IMU_TURNTABLE(DefaultChannel, DefaultDevice, &foo);} + ); RunOnceEvery(100, {DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);}); } -static inline void main_event( void ) { +static inline void main_event(void) +{ } @@ -69,7 +75,8 @@ static inline void main_event( void ) { #define TT_TACHO_PINSEL_VAL 0x02 #define TT_TACHO_PINSEL_BIT 12 -static inline void main_init_tacho(void) { +static inline void main_init_tacho(void) +{ /* select pin for capture */ TT_TACHO_PINSEL |= TT_TACHO_PINSEL_VAL << TT_TACHO_PINSEL_BIT; /* enable capture 0.2 on falling edge + trigger interrupt */ diff --git a/sw/airborne/firmwares/motor_bench/mb_current.c b/sw/airborne/firmwares/motor_bench/mb_current.c index 2089e9f814..e3f1425b3e 100644 --- a/sw/airborne/firmwares/motor_bench/mb_current.c +++ b/sw/airborne/firmwares/motor_bench/mb_current.c @@ -6,12 +6,14 @@ static struct adc_buf mb_current_buf; float mb_current_amp; -void mb_current_init(void) { +void mb_current_init(void) +{ adc_buf_channel(4, &mb_current_buf, 16); } -void mb_current_periodic(void) { +void mb_current_periodic(void) +{ mb_current_amp = (float)mb_current_buf.sum * 0.00113607 - 2.8202; } diff --git a/sw/airborne/firmwares/motor_bench/mb_modes.c b/sw/airborne/firmwares/motor_bench/mb_modes.c index 2c90233bf5..f7644edd75 100644 --- a/sw/airborne/firmwares/motor_bench/mb_modes.c +++ b/sw/airborne/firmwares/motor_bench/mb_modes.c @@ -21,15 +21,16 @@ float mb_modes_sine_freq; float mb_modes_sine_mean; float mb_modes_sine_ampl; -static void mb_modes_manual( void ); -static void mb_modes_ramp( void ); -static void mb_modes_step( void ); -static void mb_modes_prbs( void ); -static void mb_modes_sine( void ); +static void mb_modes_manual(void); +static void mb_modes_ramp(void); +static void mb_modes_step(void); +static void mb_modes_prbs(void); +static void mb_modes_sine(void); static struct adc_buf mb_modes_adc_buf; /* manual mode */ -void mb_mode_init(void) { +void mb_mode_init(void) +{ adc_buf_channel(1, &mb_modes_adc_buf, 16); mb_modes_mode = MB_MODES_IDLE; mb_modes_throttle = 0.; @@ -50,68 +51,73 @@ void mb_mode_init(void) { void mb_mode_event(void) {} -void mb_mode_periodic(float rpm, float thrust, float current) { +void mb_mode_periodic(float rpm, float thrust, float current) +{ switch (mb_modes_mode) { - case MB_MODES_IDLE : - mb_modes_throttle = 0.; - break; - case MB_MODES_MANUAL : - mb_modes_manual(); - break; - case MB_MODES_RAMP : - // mb_static_periodic(rpm, thrust, current); - // mb_modes_throttle = (float)mb_static_throttle / (float)MB_STATIC_MAX_THROTTLE; - mb_modes_ramp(); - break; - case MB_MODES_STEP : - mb_modes_step(); - break; - case MB_MODES_SINE : - mb_modes_sine(); - break; - // case MB_MODES_FIXED_RPM : - // mb_mode_fixed_rpm_periodic(rpm, thrust, current); - // mb_modes_throttle = mb_mode_fixed_rpm_throttle; - // break; - default: - mb_modes_throttle = 0.; + case MB_MODES_IDLE : + mb_modes_throttle = 0.; + break; + case MB_MODES_MANUAL : + mb_modes_manual(); + break; + case MB_MODES_RAMP : + // mb_static_periodic(rpm, thrust, current); + // mb_modes_throttle = (float)mb_static_throttle / (float)MB_STATIC_MAX_THROTTLE; + mb_modes_ramp(); + break; + case MB_MODES_STEP : + mb_modes_step(); + break; + case MB_MODES_SINE : + mb_modes_sine(); + break; + // case MB_MODES_FIXED_RPM : + // mb_mode_fixed_rpm_periodic(rpm, thrust, current); + // mb_modes_throttle = mb_mode_fixed_rpm_throttle; + // break; + default: + mb_modes_throttle = 0.; } } -static void mb_modes_manual( void ) { - uint16_t poti = mb_modes_adc_buf.sum; - mb_modes_throttle = (float)poti/(16.*1024.); +static void mb_modes_manual(void) +{ + uint16_t poti = mb_modes_adc_buf.sum; + mb_modes_throttle = (float)poti / (16.*1024.); } -static void mb_modes_ramp( void ) { +static void mb_modes_ramp(void) +{ float now = get_sys_time_float(); float elapsed = now - mb_modes_last_change_time; - if ( elapsed < mb_modes_ramp_duration) - mb_modes_throttle = elapsed/mb_modes_ramp_duration; - else if ( elapsed < 2 * mb_modes_ramp_duration) - mb_modes_throttle = 2 - elapsed/mb_modes_ramp_duration; - else { + if (elapsed < mb_modes_ramp_duration) { + mb_modes_throttle = elapsed / mb_modes_ramp_duration; + } else if (elapsed < 2 * mb_modes_ramp_duration) { + mb_modes_throttle = 2 - elapsed / mb_modes_ramp_duration; + } else { mb_modes_last_change_time = now; mb_modes_throttle = 0.; } } -static void mb_modes_step( void ) { +static void mb_modes_step(void) +{ float now = get_sys_time_float(); float elapsed = now - mb_modes_last_change_time; - if ( elapsed < mb_modes_step_duration) + if (elapsed < mb_modes_step_duration) { mb_modes_throttle = mb_modes_step_low_throttle; - else if ( elapsed < 2 * mb_modes_step_duration) + } else if (elapsed < 2 * mb_modes_step_duration) { mb_modes_throttle = mb_modes_step_high_throttle; - else { + } else { mb_modes_last_change_time = now; mb_modes_throttle = mb_modes_step_low_throttle; } } -static void mb_modes_sine( void ) { +static void mb_modes_sine(void) +{ float now = get_sys_time_float(); float alpha = 2. * M_PI * mb_modes_sine_freq * now; mb_modes_throttle = mb_modes_sine_mean + mb_modes_sine_ampl * sin(alpha); diff --git a/sw/airborne/firmwares/motor_bench/mb_modes.h b/sw/airborne/firmwares/motor_bench/mb_modes.h index ea1ef53e3c..ac952b4c3f 100644 --- a/sw/airborne/firmwares/motor_bench/mb_modes.h +++ b/sw/airborne/firmwares/motor_bench/mb_modes.h @@ -29,13 +29,13 @@ extern void mb_mode_init(void); extern void mb_mode_event(void); extern void mb_mode_periodic(float rpm, float thrust, float current); -#define mb_modes_SetMode(_val) { \ - mb_modes_mode = _val; \ +#define mb_modes_SetMode(_val) { \ + mb_modes_mode = _val; \ mb_modes_last_change_time = get_sys_time_float(); \ - /*if (mb_modes_mode == MB_MODES_RAMP) \ - mb_static_init(); \ - if (mb_modes_mode == MB_MODES_FIXED_RPM) \ - mb_mode_fixed_rpm_init();*/ \ + /*if (mb_modes_mode == MB_MODES_RAMP) \ + mb_static_init(); \ + if (mb_modes_mode == MB_MODES_FIXED_RPM) \ + mb_mode_fixed_rpm_init();*/ \ } #endif /* MB_MODES_H */ diff --git a/sw/airborne/firmwares/motor_bench/mb_scale.c b/sw/airborne/firmwares/motor_bench/mb_scale.c index 1b0110ad6f..ca4245241e 100644 --- a/sw/airborne/firmwares/motor_bench/mb_scale.c +++ b/sw/airborne/firmwares/motor_bench/mb_scale.c @@ -8,7 +8,8 @@ volatile uint32_t mb_scale_neutral = 2892000; //2944640; float mb_scale_gain = 0.0018584; //1; volatile uint8_t mb_scale_calib; -void mb_scale_init ( void ) { +void mb_scale_init(void) +{ /* select pin for capture */ ICP_PINSEL |= ICP_PINSEL_VAL << ICP_PINSEL_BIT; /* enable capture 0.3 on falling edge + trigger interrupt */ diff --git a/sw/airborne/firmwares/motor_bench/mb_scale.h b/sw/airborne/firmwares/motor_bench/mb_scale.h index 2f73bd13b8..37f91cdaa8 100644 --- a/sw/airborne/firmwares/motor_bench/mb_scale.h +++ b/sw/airborne/firmwares/motor_bench/mb_scale.h @@ -20,7 +20,7 @@ extern volatile uint8_t mb_scale_calib; #define MB_SCALE_NB_CALIB 50 -void mb_scale_init ( void ); +void mb_scale_init(void); #define MB_SCALE_IT TIR_CR3I #define MB_SCALE_ICP_ISR() { \ diff --git a/sw/airborne/firmwares/motor_bench/mb_servo.c b/sw/airborne/firmwares/motor_bench/mb_servo.c index 6cedd46d63..b8d9d62dc8 100644 --- a/sw/airborne/firmwares/motor_bench/mb_servo.c +++ b/sw/airborne/firmwares/motor_bench/mb_servo.c @@ -7,7 +7,8 @@ uint32_t mb_servo_max_pulse_ns, mb_servo_min_pulse_ns; void mb_servo_set_ns(uint32_t duration_ns); -void mb_servo_init( void ) { +void mb_servo_init(void) +{ /* set P0.21 as PWM5 output */ PINSEL1 |= (0X01 << 10); /* enable and select the type of PWM channel */ @@ -22,41 +23,46 @@ void mb_servo_init( void ) { mb_servo_max_pulse_ns = MAX_SERVO_NS; } -void mb_servo_set_range( uint32_t min_pulse_ns, uint32_t max_pulse_ns ) { +void mb_servo_set_range(uint32_t min_pulse_ns, uint32_t max_pulse_ns) +{ mb_servo_min_pulse_ns = min_pulse_ns; mb_servo_max_pulse_ns = max_pulse_ns; } -void mb_servo_set_us(uint32_t duration_us) { +void mb_servo_set_us(uint32_t duration_us) +{ /* set Match5 value (pulse duration )*/ PWMMR5 = MY_NB_CLOCK_TIMER_PWM(duration_us); - /* commit PWMMRx changes */ + /* commit PWMMRx changes */ PWMLER = PWMLER_LATCH5; } -void mb_servo_set_ns(uint32_t duration_ns) { +void mb_servo_set_ns(uint32_t duration_ns) +{ /* set Match5 value (pulse duration )*/ PWMMR5 = cpu_ticks_of_nsec(duration_ns); - /* commit PWMMRx changes */ + /* commit PWMMRx changes */ PWMLER = PWMLER_LATCH5; } /* normalized throttle between 0. and 1. */ -void mb_servo_set(float throttle) { +void mb_servo_set(float throttle) +{ uint32_t range = mb_servo_max_pulse_ns - mb_servo_min_pulse_ns; uint32_t pulse_ns = mb_servo_min_pulse_ns + throttle * (range); mb_servo_set_ns(pulse_ns); } -#define FOO_DELAY() { \ - uint32_t foo = 0; \ - while (foo<10000000) foo++; \ +#define FOO_DELAY() { \ + uint32_t foo = 0; \ + while (foo<10000000) foo++; \ } /* arm the brushless controller */ -void mb_servo_arm (void) { +void mb_servo_arm(void) +{ mb_servo_set(0.); FOO_DELAY(); mb_servo_set(1.); diff --git a/sw/airborne/firmwares/motor_bench/mb_servo.h b/sw/airborne/firmwares/motor_bench/mb_servo.h index 5f7bcc0249..075a272a52 100644 --- a/sw/airborne/firmwares/motor_bench/mb_servo.h +++ b/sw/airborne/firmwares/motor_bench/mb_servo.h @@ -10,10 +10,10 @@ #define MAX_SERVO_NS 2000000 #define RANGE_SERVO_US (MAX_SERVO_US - MIN_SERVO_US) -void mb_servo_init( void ); -void mb_servo_set_range( uint32_t min_pulse_ns, uint32_t max_pulse_ns ); +void mb_servo_init(void); +void mb_servo_set_range(uint32_t min_pulse_ns, uint32_t max_pulse_ns); void mb_servo_set_us(uint32_t duration_us); //void mb_servo_set_ns(uint32_t duration_ns); void mb_servo_set(float throttle); -void mb_servo_arm (void); +void mb_servo_arm(void); #endif /* MB_SERVO_H */ diff --git a/sw/airborne/firmwares/motor_bench/mb_tacho.c b/sw/airborne/firmwares/motor_bench/mb_tacho.c index 37d37c1c52..be5d231a1c 100644 --- a/sw/airborne/firmwares/motor_bench/mb_tacho.c +++ b/sw/airborne/firmwares/motor_bench/mb_tacho.c @@ -18,14 +18,16 @@ volatile uint16_t mb_tacho_nb_pulse; #define MB_TACHO_NB_SLOT 65 //#define MB_TACHO_NB_SLOT 36 -void mb_tacho_init ( void ) { +void mb_tacho_init(void) +{ /* select pin for capture */ MB_TACHO_PINSEL |= MB_TACHO_PINSEL_VAL << MB_TACHO_PINSEL_BIT; /* enable capture 0.2 on falling edge + trigger interrupt */ T0CCR |= TCCR_CR0_F | TCCR_CR0_I; } -uint32_t mb_tacho_get_duration( void ) { +uint32_t mb_tacho_get_duration(void) +{ int_disable(); uint32_t my_duration = 0; if (got_one_pulse) { @@ -36,21 +38,24 @@ uint32_t mb_tacho_get_duration( void ) { return my_duration; } -float mb_tacho_get_averaged( void ) { +float mb_tacho_get_averaged(void) +{ int_disable(); float ret; float tacho; - const float tach_to_rpm = 15000000.*60./(float)MB_TACHO_NB_SLOT; - if (mb_tacho_nb_pulse) + const float tach_to_rpm = 15000000.*60. / (float)MB_TACHO_NB_SLOT; + if (mb_tacho_nb_pulse) { tacho = mb_tacho_averaged / (float)mb_tacho_nb_pulse ; - else - tacho = 0.; + } else { + tacho = 0.; + } - if (tacho ==0) + if (tacho == 0) { ret = 0; - else - ret = tach_to_rpm/tacho; + } else { + ret = tach_to_rpm / tacho; + } mb_tacho_averaged = 0.; mb_tacho_nb_pulse = 0; diff --git a/sw/airborne/firmwares/motor_bench/mb_tacho.h b/sw/airborne/firmwares/motor_bench/mb_tacho.h index 08a40004be..b003726c51 100644 --- a/sw/airborne/firmwares/motor_bench/mb_tacho.h +++ b/sw/airborne/firmwares/motor_bench/mb_tacho.h @@ -3,9 +3,9 @@ #include "std.h" -extern void mb_tacho_init ( void ); -extern uint32_t mb_tacho_get_duration( void ); -extern float mb_tacho_get_averaged( void ); +extern void mb_tacho_init(void); +extern uint32_t mb_tacho_get_duration(void); +extern float mb_tacho_get_averaged(void); extern volatile uint32_t mb_tacho_duration; extern volatile uint8_t got_one_pulse; @@ -13,14 +13,14 @@ extern volatile float mb_tacho_averaged; extern volatile uint16_t mb_tacho_nb_pulse; #define MB_TACHO_IT TIR_CR0I -#define MB_TACHO_ISR() { \ - static uint32_t tmb_last; \ - uint32_t t_now = T0CR0; \ - uint32_t diff = t_now - tmb_last; \ - mb_tacho_duration = diff; \ - mb_tacho_averaged += diff; \ - mb_tacho_nb_pulse++; \ - tmb_last = t_now; \ +#define MB_TACHO_ISR() { \ + static uint32_t tmb_last; \ + uint32_t t_now = T0CR0; \ + uint32_t diff = t_now - tmb_last; \ + mb_tacho_duration = diff; \ + mb_tacho_averaged += diff; \ + mb_tacho_nb_pulse++; \ + tmb_last = t_now; \ got_one_pulse = TRUE; \ } diff --git a/sw/airborne/firmwares/motor_bench/mb_twi_controller.c b/sw/airborne/firmwares/motor_bench/mb_twi_controller.c index 19b0407151..1053c22609 100644 --- a/sw/airborne/firmwares/motor_bench/mb_twi_controller.c +++ b/sw/airborne/firmwares/motor_bench/mb_twi_controller.c @@ -10,22 +10,24 @@ uint8_t mb_twi_nb_overun; uint8_t mb_twi_i2c_done; -void mb_twi_controller_init(void) { +void mb_twi_controller_init(void) +{ mb_twi_nb_overun = 0; mb_twi_i2c_done = TRUE; } -void mb_twi_controller_set( float throttle ) { +void mb_twi_controller_set(float throttle) +{ LED_TOGGLE(1); if (mb_twi_i2c_done) { mb_twi_command = throttle * MB_TWI_CONTROLLER_MAX_CMD; - i2c_buf[0] = (uint8_t)(mb_twi_command&0xFF); - i2c_buf[1] = (uint8_t)(mb_twi_command>>8); + i2c_buf[0] = (uint8_t)(mb_twi_command & 0xFF); + i2c_buf[1] = (uint8_t)(mb_twi_command >> 8); i2c_transmit(MB_TWI_CONTROLLER_ADDR, 2, &mb_twi_i2c_done); - } - else + } else { mb_twi_nb_overun++; + } } diff --git a/sw/airborne/firmwares/motor_bench/mb_twi_controller.h b/sw/airborne/firmwares/motor_bench/mb_twi_controller.h index 4e49e027b7..01c333df00 100644 --- a/sw/airborne/firmwares/motor_bench/mb_twi_controller.h +++ b/sw/airborne/firmwares/motor_bench/mb_twi_controller.h @@ -5,7 +5,7 @@ extern void mb_twi_controller_init(void); -extern void mb_twi_controller_set( float throttle ); +extern void mb_twi_controller_set(float throttle); #define MB_TWI_CONTROLLER_MAX_CMD 65535 /* 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 4963dc3d85..1a77ff92d7 100644 --- a/sw/airborne/firmwares/motor_bench/mb_twi_controller_asctech.c +++ b/sw/airborne/firmwares/motor_bench/mb_twi_controller_asctech.c @@ -22,7 +22,8 @@ uint8_t mb_twi_i2c_done; #define MB_TWI_CONTROLLER_MAX_CMD 200 #define MB_TWI_CONTROLLER_ADDR 0x02 -void mb_twi_controller_init(void) { +void mb_twi_controller_init(void) +{ mb_twi_nb_overun = 0; mb_twi_i2c_done = TRUE; mb_twi_controller_asctech_command = FALSE; @@ -30,7 +31,8 @@ void mb_twi_controller_init(void) { mb_twi_controller_asctech_addr = MB_TWI_CONTROLLER_ASCTECH_ADDR_FRONT; } -void mb_twi_controller_set( float throttle ) { +void mb_twi_controller_set(float throttle) +{ if (mb_twi_i2c_done) { if (mb_twi_controller_asctech_command) { @@ -42,7 +44,7 @@ void mb_twi_controller_set( float throttle ) { 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; + // mb_twi_i2c_done = FALSE; i2c0_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done); break; @@ -51,7 +53,7 @@ void mb_twi_controller_set( float throttle ) { 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; + // mb_twi_i2c_done = FALSE; i2c0_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done); break; @@ -60,15 +62,14 @@ void mb_twi_controller_set( float throttle ) { 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_new_addr; mb_twi_controller_asctech_addr = mb_twi_controller_asctech_new_addr; - // mb_twi_i2c_done = FALSE; + // mb_twi_i2c_done = FALSE; i2c0_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done); break; } - } - else { + } else { uint8_t pitch = 100; uint8_t roll = 100; @@ -81,12 +82,13 @@ void mb_twi_controller_set( float throttle ) { // mb_twi_i2c_done = FALSE; i2c0_transmit(MB_TWI_CONTROLLER_ADDR, 4, &mb_twi_i2c_done); } - } - else + } else { mb_twi_nb_overun++; + } } -void mb_twi_controller_set_raw( uint8_t throttle ) { +void mb_twi_controller_set_raw(uint8_t throttle) +{ } diff --git a/sw/airborne/firmwares/motor_bench/mb_twi_controller_asctech.h b/sw/airborne/firmwares/motor_bench/mb_twi_controller_asctech.h index aa030c014d..b65af55d57 100644 --- a/sw/airborne/firmwares/motor_bench/mb_twi_controller_asctech.h +++ b/sw/airborne/firmwares/motor_bench/mb_twi_controller_asctech.h @@ -4,8 +4,8 @@ #include "std.h" extern void mb_twi_controller_init(void); -extern void mb_twi_controller_set( float throttle ); -extern void mb_twi_controller_set_raw( uint8_t throttle ); +extern void mb_twi_controller_set(float throttle); +extern void mb_twi_controller_set_raw(uint8_t throttle); #define MB_TWI_CONTROLLER_COMMAND_NONE 0 #define MB_TWI_CONTROLLER_COMMAND_TEST 1 @@ -19,14 +19,14 @@ extern uint8_t mb_twi_controller_asctech_addr; extern uint8_t mb_twi_controller_asctech_new_addr; #define mb_twi_controller_asctech_SetCommand(value) { \ - mb_twi_controller_asctech_command = TRUE; \ + mb_twi_controller_asctech_command = TRUE; \ mb_twi_controller_asctech_command_type = value; \ } -#define mb_twi_controller_asctech_SetAddr(value) { \ - mb_twi_controller_asctech_command = TRUE; \ +#define mb_twi_controller_asctech_SetAddr(value) { \ + mb_twi_controller_asctech_command = TRUE; \ mb_twi_controller_asctech_command_type = MB_TWI_CONTROLLER_COMMAND_SET_ADDR; \ - mb_twi_controller_asctech_new_addr = value; \ + mb_twi_controller_asctech_new_addr = value; \ } diff --git a/sw/airborne/firmwares/motor_bench/mb_twi_controller_mkk.c b/sw/airborne/firmwares/motor_bench/mb_twi_controller_mkk.c index 83cbc0b8cd..76916365bd 100644 --- a/sw/airborne/firmwares/motor_bench/mb_twi_controller_mkk.c +++ b/sw/airborne/firmwares/motor_bench/mb_twi_controller_mkk.c @@ -20,17 +20,19 @@ uint8_t mb_buss_twi_i2c_done; */ #define MB_BUSS_TWI_CONTROLLER_ADDR 0x56 -void mb_twi_controller_init(void) { +void mb_twi_controller_init(void) +{ mb_buss_twi_nb_overun = 0; mb_buss_twi_i2c_done = TRUE; } -void mb_twi_controller_set( float throttle ) { +void mb_twi_controller_set(float throttle) +{ if (mb_buss_twi_i2c_done) { mb_buss_twi_command = throttle * MB_BUSS_TWI_CONTROLLER_MAX_CMD; i2c_buf[0] = mb_buss_twi_command; i2c_transmit(MB_BUSS_TWI_CONTROLLER_ADDR, 1, &mb_buss_twi_i2c_done); - } - else + } else { mb_buss_twi_nb_overun++; + } } diff --git a/sw/airborne/firmwares/motor_bench/mb_twi_controller_mkk.h b/sw/airborne/firmwares/motor_bench/mb_twi_controller_mkk.h index a7e91b933e..ee20bfef00 100644 --- a/sw/airborne/firmwares/motor_bench/mb_twi_controller_mkk.h +++ b/sw/airborne/firmwares/motor_bench/mb_twi_controller_mkk.h @@ -5,7 +5,7 @@ extern void mb_twi_controller_init(void); -extern void mb_twi_controller_set( float throttle ); +extern void mb_twi_controller_set(float throttle); #endif /* MB_TWI_CONTROLLER_MKK_H */ diff --git a/sw/airborne/firmwares/motor_bench/turntable_systime.c b/sw/airborne/firmwares/motor_bench/turntable_systime.c index 676f71d58e..78da429609 100644 --- a/sw/airborne/firmwares/motor_bench/turntable_systime.c +++ b/sw/airborne/firmwares/motor_bench/turntable_systime.c @@ -7,7 +7,8 @@ #define TIMER0_IT_MASK (SYS_TICK_IT | \ TIR_CR0I) -void sys_time_arch_init( void ) { +void sys_time_arch_init(void) +{ /* setup Timer 0 to count forever */ /* reset & disable timer 0 */ T0TCR = TCR_RESET; @@ -35,7 +36,8 @@ void sys_time_arch_init( void ) { _VIC_ADDR(TIMER0_VIC_SLOT) = (uint32_t)TIMER0_ISR; } -static inline void sys_tick_irq_handler(void) { +static inline void sys_tick_irq_handler(void) +{ /* set match register for next interrupt */ T0MR0 += ticks_resolution - 1; @@ -48,12 +50,12 @@ static inline void sys_tick_irq_handler(void) { LED_TOGGLE(SYS_TIME_LED); #endif } - for (unsigned int i=0; i= sys_time.timer[i].end_time) { sys_time.timer[i].end_time += sys_time.timer[i].duration; sys_time.timer[i].elapsed = TRUE; - if (sys_time.timer[i].cb) sys_time.timer[i].cb(i); + if (sys_time.timer[i].cb) { sys_time.timer[i].cb(i); } } } } @@ -61,7 +63,8 @@ static inline void sys_tick_irq_handler(void) { extern uint32_t lp_pulse; extern uint32_t nb_pulse; -void TIMER0_ISR ( void ) { +void TIMER0_ISR(void) +{ ISR_ENTRY(); while (T0IR & TIMER0_IT_MASK) { @@ -75,7 +78,7 @@ void TIMER0_ISR ( void ) { static uint32_t pulse_last_t; uint32_t t_now = T0CR0; uint32_t diff = t_now - pulse_last_t; - lp_pulse = (lp_pulse + diff)/2; + lp_pulse = (lp_pulse + diff) / 2; pulse_last_t = t_now; nb_pulse++; // got_one_pulse = TRUE; 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 770cfe3c5d..744e04a77c 100644 --- a/sw/airborne/firmwares/non_ap/geiger_counter/geiger_counter.c +++ b/sw/airborne/firmwares/non_ap/geiger_counter/geiger_counter.c @@ -38,7 +38,8 @@ enum stats { FOUND_2, FOUND_3, FOUND_4, - FOUND_5 }; + FOUND_5 +}; int received data = 0; int stat = 0, received data = 0; @@ -46,21 +47,23 @@ unsigned long count_geiger_1 = 0; unsigned long count_geiger_2 = 0; unsigned short volt_geiger = 0; -void read_i2c() { +void read_i2c() +{ unsigned char dat[10]; digitalWrite(LED_GR_PIN, LOW); received_data = 0; memcpy(dat, count_geiger_1, 4); - memcpy(dat+4, count_geiger_2, 4); - memcpy(dat+8, volt_geiger, 2); + memcpy(dat + 4, count_geiger_2, 4); + memcpy(dat + 8, volt_geiger, 2); Wire.send(dat, 2); } -void setup() { +void setup() +{ /* serial port */ Serial.begin(2400); - pinMode(2,OUTPUT); - digitalWrite(2,HIGH); + pinMode(2, OUTPUT); + digitalWrite(2, HIGH); #ifdef DEBUG Serial.println("geiger counter init"); #endif @@ -76,7 +79,8 @@ void setup() { stat = INIT; } -void loop() { +void loop() +{ unsigned char ser; int i; @@ -96,74 +100,74 @@ void loop() { break; case FOUND_SYNC: if ((b <= '9') && (b >= '0')) { - count_geiger_1 = count_geiger_1 * 10 + (b-'0'); - if (++i > 7) state = IDLE; + 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; - } 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; } + } 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/firmwares/non_ap/led_flasher/blitzer.c b/sw/airborne/firmwares/non_ap/led_flasher/blitzer.c index 570bac501b..0cfc8b9866 100644 --- a/sw/airborne/firmwares/non_ap/led_flasher/blitzer.c +++ b/sw/airborne/firmwares/non_ap/led_flasher/blitzer.c @@ -38,31 +38,30 @@ void wait(int msec_time) /* roughly based on internal oscillator with divider by 8 enabled */ for (cnta = 0; cnta < msec_time; cnta++) { - for (cntb = 0; cntb < 38;cntb++) cntb=cntb; + for (cntb = 0; cntb < 38; cntb++) { cntb = cntb; } } } int main(void) { - DDRB |= (1 << PB2); // PB2 output - DDRB |= (1 << PB1); // PB1 output + DDRB |= (1 << PB2); // PB2 output + DDRB |= (1 << PB1); // PB1 output DDRB &= ~(1 << PB0); // PB0 input - while (1) - { - PORTB |= (1 << PB2); - PORTB |= (1 << PB1); + while (1) { + PORTB |= (1 << PB2); + PORTB |= (1 << PB1); wait(25); PORTB &= ~(1 << PB2); PORTB &= ~(1 << PB1); wait(110); - PORTB |= (1 << PB2); - PORTB |= (1 << PB1); + PORTB |= (1 << PB2); + PORTB |= (1 << PB1); wait(25); PORTB &= ~(1 << PB2); PORTB &= ~(1 << PB1); wait(780); } - return(0); + return (0); } diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index d3a6e9e899..0c50fd65ba 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot.c @@ -88,12 +88,14 @@ bool_t autopilot_detect_ground_once; #ifndef AUTOPILOT_DISABLE_AHRS_KILL #include "subsystems/ahrs.h" -static inline int ahrs_is_aligned(void) { +static inline int ahrs_is_aligned(void) +{ return (ahrs.status == AHRS_RUNNING); } #else PRINT_CONFIG_MSG("Using AUTOPILOT_DISABLE_AHRS_KILL") -static inline int ahrs_is_aligned(void) { +static inline int ahrs_is_aligned(void) +{ return TRUE; } #endif @@ -142,7 +144,8 @@ PRINT_CONFIG_MSG("Enabled UNLOCKED_HOME_MODE since MODE_AUTO2 is AP_MODE_HOME") #error "MODE_MANUAL mustn't be AP_MODE_NAV" #endif -static void send_alive(struct transport_tx *trans, struct link_device *dev) { +static void send_alive(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_ALIVE(trans, dev, AC_ID, 16, MD5SUM); } @@ -150,7 +153,8 @@ static void send_alive(struct transport_tx *trans, struct link_device *dev) { #include "subsystems/actuators/motor_mixing.h" #endif -static void send_status(struct transport_tx *trans, struct link_device *dev) { +static void send_status(struct transport_tx *trans, struct link_device *dev) +{ uint32_t imu_nb_err = 0; #if USE_MOTOR_MIXING uint8_t _motor_nb_err = motor_mixing.nb_saturation + motor_mixing.nb_failure * 10; @@ -164,15 +168,16 @@ static void send_status(struct transport_tx *trans, struct link_device *dev) { #endif uint16_t time_sec = sys_time.nb_sec; pprz_msg_send_ROTORCRAFT_STATUS(trans, dev, AC_ID, - &imu_nb_err, &_motor_nb_err, - &radio_control.status, &radio_control.frame_rate, - &fix, &autopilot_mode, - &autopilot_in_flight, &autopilot_motors_on, - &guidance_h_mode, &guidance_v_mode, - &electrical.vsupply, &time_sec); + &imu_nb_err, &_motor_nb_err, + &radio_control.status, &radio_control.frame_rate, + &fix, &autopilot_mode, + &autopilot_in_flight, &autopilot_motors_on, + &guidance_h_mode, &guidance_v_mode, + &electrical.vsupply, &time_sec); } -static void send_energy(struct transport_tx *trans, struct link_device *dev) { +static void send_energy(struct transport_tx *trans, struct link_device *dev) +{ uint16_t e = electrical.energy; float vsup = ((float)electrical.vsupply) / 10.0f; float curs = ((float)electrical.current) / 1000.0f; @@ -180,68 +185,75 @@ static void send_energy(struct transport_tx *trans, struct link_device *dev) { pprz_msg_send_ENERGY(trans, dev, AC_ID, &vsup, &curs, &e, &power); } -static void send_fp(struct transport_tx *trans, struct link_device *dev) { +static void send_fp(struct transport_tx *trans, struct link_device *dev) +{ int32_t carrot_up = -guidance_v_z_sp; pprz_msg_send_ROTORCRAFT_FP(trans, dev, AC_ID, - &(stateGetPositionEnu_i()->x), - &(stateGetPositionEnu_i()->y), - &(stateGetPositionEnu_i()->z), - &(stateGetSpeedEnu_i()->x), - &(stateGetSpeedEnu_i()->y), - &(stateGetSpeedEnu_i()->z), - &(stateGetNedToBodyEulers_i()->phi), - &(stateGetNedToBodyEulers_i()->theta), - &(stateGetNedToBodyEulers_i()->psi), - &guidance_h_pos_sp.y, - &guidance_h_pos_sp.x, - &carrot_up, - &guidance_h_heading_sp, - &stabilization_cmd[COMMAND_THRUST], - &autopilot_flight_time); + &(stateGetPositionEnu_i()->x), + &(stateGetPositionEnu_i()->y), + &(stateGetPositionEnu_i()->z), + &(stateGetSpeedEnu_i()->x), + &(stateGetSpeedEnu_i()->y), + &(stateGetSpeedEnu_i()->z), + &(stateGetNedToBodyEulers_i()->phi), + &(stateGetNedToBodyEulers_i()->theta), + &(stateGetNedToBodyEulers_i()->psi), + &guidance_h_pos_sp.y, + &guidance_h_pos_sp.x, + &carrot_up, + &guidance_h_heading_sp, + &stabilization_cmd[COMMAND_THRUST], + &autopilot_flight_time); } #ifdef RADIO_CONTROL -static void send_rc(struct transport_tx *trans, struct link_device *dev) { +static void send_rc(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_RC(trans, dev, AC_ID, RADIO_CONTROL_NB_CHANNEL, radio_control.values); } -static void send_rotorcraft_rc(struct transport_tx *trans, struct link_device *dev) { +static void send_rotorcraft_rc(struct transport_tx *trans, struct link_device *dev) +{ #ifdef RADIO_KILL_SWITCH int16_t _kill_switch = radio_control.values[RADIO_KILL_SWITCH]; #else int16_t _kill_switch = 42; #endif pprz_msg_send_ROTORCRAFT_RADIO_CONTROL(trans, dev, AC_ID, - &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], - &_kill_switch, - &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], + &_kill_switch, + &radio_control.status); } #endif #ifdef ACTUATORS -static void send_actuators(struct transport_tx *trans, struct link_device *dev) { +static void send_actuators(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_ACTUATORS(trans, dev, AC_ID , ACTUATORS_NB, actuators); } #endif -static void send_dl_value(struct transport_tx *trans, struct link_device *dev) { +static void send_dl_value(struct transport_tx *trans, struct link_device *dev) +{ PeriodicSendDlValue(trans, dev); } -static void send_rotorcraft_cmd(struct transport_tx *trans, struct link_device *dev) { +static void send_rotorcraft_cmd(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_ROTORCRAFT_CMD(trans, dev, AC_ID, - &stabilization_cmd[COMMAND_ROLL], - &stabilization_cmd[COMMAND_PITCH], - &stabilization_cmd[COMMAND_YAW], - &stabilization_cmd[COMMAND_THRUST]); + &stabilization_cmd[COMMAND_ROLL], + &stabilization_cmd[COMMAND_PITCH], + &stabilization_cmd[COMMAND_YAW], + &stabilization_cmd[COMMAND_THRUST]); } -void autopilot_init(void) { +void autopilot_init(void) +{ /* mode is finally set at end of init if MODE_STARTUP is not KILL */ autopilot_mode = AP_MODE_KILL; autopilot_motors_on = FALSE; @@ -290,23 +302,24 @@ void autopilot_init(void) { #define NAV_PRESCALER (PERIODIC_FREQUENCY / NAV_FREQ) -void autopilot_periodic(void) { +void autopilot_periodic(void) +{ RunOnceEvery(NAV_PRESCALER, compute_dist2_to_home()); if (autopilot_in_flight && autopilot_mode == AP_MODE_NAV) { if (too_far_from_home) { - if (dist2_to_home > failsafe_mode_dist2) + if (dist2_to_home > failsafe_mode_dist2) { autopilot_set_mode(FAILSAFE_MODE_TOO_FAR_FROM_HOME); - else + } else { autopilot_set_mode(AP_MODE_HOME); + } } } if (autopilot_mode == AP_MODE_HOME) { RunOnceEvery(NAV_PRESCALER, nav_home()); - } - else { + } else { // otherwise always call nav_periodic_task so that carrot is always updated in GCS for other modes RunOnceEvery(NAV_PRESCALER, nav_periodic_task()); } @@ -316,13 +329,15 @@ void autopilot_periodic(void) { * or just "detected" ground, go to KILL mode. */ if (autopilot_mode == AP_MODE_FAILSAFE) { - if (!autopilot_in_flight) + if (!autopilot_in_flight) { autopilot_set_mode(AP_MODE_KILL); + } #if FAILSAFE_GROUND_DETECT -INFO("Using FAILSAFE_GROUND_DETECT: KILL") - if (autopilot_ground_detected) + INFO("Using FAILSAFE_GROUND_DETECT: KILL") + if (autopilot_ground_detected) { autopilot_set_mode(AP_MODE_KILL); + } #endif } @@ -339,21 +354,22 @@ INFO("Using FAILSAFE_GROUND_DETECT: KILL") */ if (autopilot_mode == AP_MODE_KILL) { SetCommands(commands_failsafe); - } - else { - guidance_v_run( autopilot_in_flight ); - guidance_h_run( autopilot_in_flight ); + } else { + guidance_v_run(autopilot_in_flight); + guidance_h_run(autopilot_in_flight); SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight, autopilot_motors_on); } } -void autopilot_set_mode(uint8_t new_autopilot_mode) { +void autopilot_set_mode(uint8_t new_autopilot_mode) +{ /* force startup mode (default is kill) as long as AHRS is not aligned */ - if (!ahrs_is_aligned()) + if (!ahrs_is_aligned()) { new_autopilot_mode = MODE_STARTUP; + } if (new_autopilot_mode != autopilot_mode) { /* horizontal mode */ @@ -447,37 +463,34 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) { } -void autopilot_check_in_flight(bool_t motors_on) { +void autopilot_check_in_flight(bool_t motors_on) +{ if (autopilot_in_flight) { if (autopilot_in_flight_counter > 0) { /* probably in_flight if thrust, speed and accel above IN_FLIGHT_MIN thresholds */ if ((stabilization_cmd[COMMAND_THRUST] <= AUTOPILOT_IN_FLIGHT_MIN_THRUST) && (abs(stateGetSpeedNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_SPEED) && - (abs(stateGetAccelNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_ACCEL)) - { + (abs(stateGetAccelNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_ACCEL)) { autopilot_in_flight_counter--; if (autopilot_in_flight_counter == 0) { autopilot_in_flight = FALSE; } - } - else { /* thrust, speed or accel not above min threshold, reset counter */ + } else { /* thrust, speed or accel not above min threshold, reset counter */ autopilot_in_flight_counter = AUTOPILOT_IN_FLIGHT_TIME; } } - } - else { /* currently not in flight */ + } else { /* currently not in flight */ if (autopilot_in_flight_counter < AUTOPILOT_IN_FLIGHT_TIME && - motors_on) - { + motors_on) { /* if thrust above min threshold, assume in_flight. * Don't check for velocity and acceleration above threshold here... */ if (stabilization_cmd[COMMAND_THRUST] > AUTOPILOT_IN_FLIGHT_MIN_THRUST) { autopilot_in_flight_counter++; - if (autopilot_in_flight_counter == AUTOPILOT_IN_FLIGHT_TIME) + if (autopilot_in_flight_counter == AUTOPILOT_IN_FLIGHT_TIME) { autopilot_in_flight = TRUE; - } - else { /* currently not in_flight and thrust below threshold, reset counter */ + } + } else { /* currently not in_flight and thrust below threshold, reset counter */ autopilot_in_flight_counter = 0; } } @@ -485,17 +498,20 @@ void autopilot_check_in_flight(bool_t motors_on) { } -void autopilot_set_motors_on(bool_t motors_on) { - if (autopilot_mode != AP_MODE_KILL && ahrs_is_aligned() && motors_on) +void autopilot_set_motors_on(bool_t motors_on) +{ + if (autopilot_mode != AP_MODE_KILL && ahrs_is_aligned() && motors_on) { autopilot_motors_on = TRUE; - else + } else { autopilot_motors_on = FALSE; + } kill_throttle = ! autopilot_motors_on; autopilot_arming_set(autopilot_motors_on); } -void autopilot_on_rc_frame(void) { +void autopilot_on_rc_frame(void) +{ if (kill_switch_is_on()) { autopilot_set_mode(AP_MODE_KILL); @@ -515,8 +531,7 @@ void autopilot_on_rc_frame(void) { /* Allowed to leave home mode when UNLOCKED_HOME_MODE */ || !too_far_from_home #endif - ) - { + ) { autopilot_set_mode(new_autopilot_mode); } } diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.h b/sw/airborne/firmwares/rotorcraft/autopilot.h index 5320f3db0a..70b5371bbe 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.h +++ b/sw/airborne/firmwares/rotorcraft/autopilot.h @@ -47,7 +47,7 @@ #define AP_MODE_HOVER_CLIMB 11 #define AP_MODE_HOVER_Z_HOLD 12 #define AP_MODE_NAV 13 -#define AP_MODE_RC_DIRECT 14 // Safety Pilot Direct Commands for helicopter direct control +#define AP_MODE_RC_DIRECT 14 // Safety Pilot Direct Commands for helicopter direct control #define AP_MODE_CARE_FREE_DIRECT 15 #define AP_MODE_FORWARD 16 @@ -123,21 +123,21 @@ extern uint16_t autopilot_flight_time; */ #ifndef ROTORCRAFT_COMMANDS_YAW_ALWAYS_ENABLED #define SetRotorcraftCommands(_cmd, _in_flight, _motor_on) { \ - if (!(_in_flight)) { _cmd[COMMAND_YAW] = 0; } \ - if (!(_motor_on)) { _cmd[COMMAND_THRUST] = 0; } \ - commands[COMMAND_ROLL] = _cmd[COMMAND_ROLL]; \ - commands[COMMAND_PITCH] = _cmd[COMMAND_PITCH]; \ - commands[COMMAND_YAW] = _cmd[COMMAND_YAW]; \ - commands[COMMAND_THRUST] = _cmd[COMMAND_THRUST]; \ -} + if (!(_in_flight)) { _cmd[COMMAND_YAW] = 0; } \ + if (!(_motor_on)) { _cmd[COMMAND_THRUST] = 0; } \ + commands[COMMAND_ROLL] = _cmd[COMMAND_ROLL]; \ + commands[COMMAND_PITCH] = _cmd[COMMAND_PITCH]; \ + commands[COMMAND_YAW] = _cmd[COMMAND_YAW]; \ + commands[COMMAND_THRUST] = _cmd[COMMAND_THRUST]; \ + } #else #define SetRotorcraftCommands(_cmd, _in_flight, _motor_on) { \ - if (!(_motor_on)) { _cmd[COMMAND_THRUST] = 0; } \ - commands[COMMAND_ROLL] = _cmd[COMMAND_ROLL]; \ - commands[COMMAND_PITCH] = _cmd[COMMAND_PITCH]; \ - commands[COMMAND_YAW] = _cmd[COMMAND_YAW]; \ - commands[COMMAND_THRUST] = _cmd[COMMAND_THRUST]; \ -} + if (!(_motor_on)) { _cmd[COMMAND_THRUST] = 0; } \ + commands[COMMAND_ROLL] = _cmd[COMMAND_ROLL]; \ + commands[COMMAND_PITCH] = _cmd[COMMAND_PITCH]; \ + commands[COMMAND_YAW] = _cmd[COMMAND_YAW]; \ + commands[COMMAND_THRUST] = _cmd[COMMAND_THRUST]; \ + } #endif /** Z-acceleration threshold to detect ground in m/s^2 */ @@ -146,9 +146,10 @@ extern uint16_t autopilot_flight_time; #endif /** Ground detection based on vertical acceleration. */ -static inline void DetectGroundEvent(void) { +static inline void DetectGroundEvent(void) +{ if (autopilot_mode == AP_MODE_FAILSAFE || autopilot_detect_ground_once) { - struct NedCoor_f* accel = stateGetAccelNed_f(); + struct NedCoor_f *accel = stateGetAccelNed_f(); if (accel->z < -THRESHOLD_GROUND_DETECT || accel->z > THRESHOLD_GROUND_DETECT) { autopilot_ground_detected = TRUE; diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_arming_switch.h b/sw/airborne/firmwares/rotorcraft/autopilot_arming_switch.h index afc889cc64..67395e0fa4 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot_arming_switch.h +++ b/sw/airborne/firmwares/rotorcraft/autopilot_arming_switch.h @@ -45,23 +45,23 @@ enum arming_state { enum arming_state autopilot_arming_state; bool_t autopilot_unarmed_in_auto; -static inline void autopilot_arming_init(void) { +static inline void autopilot_arming_init(void) +{ autopilot_arming_state = STATE_UNINIT; autopilot_unarmed_in_auto = FALSE; } -static inline void autopilot_arming_set(bool_t motors_on) { +static inline void autopilot_arming_set(bool_t motors_on) +{ if (motors_on) { autopilot_arming_state = STATE_MOTORS_ON; - } - else { + } else { if (autopilot_arming_state == STATE_MOTORS_ON) { autopilot_arming_state = STATE_STARTABLE; /* if turned off in an AUTO mode, remember it so it can be turned on again in AUTO */ if (autopilot_mode != MODE_MANUAL) { autopilot_unarmed_in_auto = TRUE; - } - else { + } else { autopilot_unarmed_in_auto = FALSE; } } @@ -75,48 +75,47 @@ static inline void autopilot_arming_set(bool_t motors_on) { * Also to start the motors, throttle needs to be down, other sticks centered, * AHRS aligned and you need be in manual mode. */ -static inline void autopilot_arming_check_motors_on( void ) { - switch(autopilot_arming_state) { - case STATE_UNINIT: - autopilot_motors_on = FALSE; - if (kill_switch_is_on()) { - autopilot_arming_state = STATE_STARTABLE; - } - else { - autopilot_arming_state = STATE_WAITING; - } - break; - case STATE_WAITING: - autopilot_motors_on = FALSE; - if (kill_switch_is_on()) { - autopilot_arming_state = STATE_STARTABLE; - } - break; - case STATE_STARTABLE: - autopilot_motors_on = FALSE; - if (!kill_switch_is_on() && - THROTTLE_STICK_DOWN() && - rc_attitude_sticks_centered() && - (autopilot_mode == MODE_MANUAL || autopilot_unarmed_in_auto)) { - autopilot_arming_state = STATE_MOTORS_ON; - } - break; - case STATE_MOTORS_ON: - autopilot_motors_on = TRUE; - if (kill_switch_is_on()) { - /* if killed, go to STATE_STARTABLE where motors will be turned off */ - autopilot_arming_state = STATE_STARTABLE; - /* if turned off in an AUTO mode, remember it so it can be turned on again in AUTO */ - if (autopilot_mode != MODE_MANUAL) { - autopilot_unarmed_in_auto = TRUE; +static inline void autopilot_arming_check_motors_on(void) +{ + switch (autopilot_arming_state) { + case STATE_UNINIT: + autopilot_motors_on = FALSE; + if (kill_switch_is_on()) { + autopilot_arming_state = STATE_STARTABLE; + } else { + autopilot_arming_state = STATE_WAITING; } - else { - autopilot_unarmed_in_auto = FALSE; + break; + case STATE_WAITING: + autopilot_motors_on = FALSE; + if (kill_switch_is_on()) { + autopilot_arming_state = STATE_STARTABLE; } - } - break; - default: - break; + break; + case STATE_STARTABLE: + autopilot_motors_on = FALSE; + if (!kill_switch_is_on() && + THROTTLE_STICK_DOWN() && + rc_attitude_sticks_centered() && + (autopilot_mode == MODE_MANUAL || autopilot_unarmed_in_auto)) { + autopilot_arming_state = STATE_MOTORS_ON; + } + break; + case STATE_MOTORS_ON: + autopilot_motors_on = TRUE; + if (kill_switch_is_on()) { + /* if killed, go to STATE_STARTABLE where motors will be turned off */ + autopilot_arming_state = STATE_STARTABLE; + /* if turned off in an AUTO mode, remember it so it can be turned on again in AUTO */ + if (autopilot_mode != MODE_MANUAL) { + autopilot_unarmed_in_auto = TRUE; + } else { + autopilot_unarmed_in_auto = FALSE; + } + } + break; + default: + break; } } diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_arming_throttle.h b/sw/airborne/firmwares/rotorcraft/autopilot_arming_throttle.h index 87e83d7229..9ba5021f5e 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot_arming_throttle.h +++ b/sw/airborne/firmwares/rotorcraft/autopilot_arming_throttle.h @@ -46,17 +46,18 @@ enum arming_throttle_state autopilot_arming_state; uint8_t autopilot_arming_delay_counter; bool_t autopilot_unarmed_in_auto; -static inline void autopilot_arming_init(void) { +static inline void autopilot_arming_init(void) +{ autopilot_arming_state = STATE_UNINIT; autopilot_arming_delay_counter = 0; autopilot_unarmed_in_auto = FALSE; } -static inline void autopilot_arming_set(bool_t motors_on) { +static inline void autopilot_arming_set(bool_t motors_on) +{ if (motors_on) { autopilot_arming_state = STATE_MOTORS_ON; - } - else { + } else { if (autopilot_arming_state == STATE_MOTORS_ON) { autopilot_arming_state = STATE_WAITING; } @@ -70,19 +71,19 @@ static inline void autopilot_arming_set(bool_t motors_on) { * - other sticks need to be centered to start motors * - need to be in manual mode to start the motors */ -static inline void autopilot_arming_check_motors_on( void ) { +static inline void autopilot_arming_check_motors_on(void) +{ /* only allow switching motor if not in KILL mode */ if (autopilot_mode != AP_MODE_KILL) { - switch(autopilot_arming_state) { + switch (autopilot_arming_state) { case STATE_UNINIT: autopilot_motors_on = FALSE; autopilot_arming_delay_counter = 0; if (THROTTLE_STICK_DOWN()) { autopilot_arming_state = STATE_MOTORS_OFF_READY; - } - else { + } else { autopilot_arming_state = STATE_WAITING; } break; @@ -109,8 +110,7 @@ static inline void autopilot_arming_check_motors_on( void ) { !rc_attitude_sticks_centered() || (autopilot_mode != MODE_MANUAL && !autopilot_unarmed_in_auto)) { autopilot_arming_state = STATE_MOTORS_OFF_READY; - } - else if (autopilot_arming_delay_counter >= AUTOPILOT_ARMING_DELAY) { + } else if (autopilot_arming_delay_counter >= AUTOPILOT_ARMING_DELAY) { autopilot_arming_state = STATE_MOTORS_ON; } break; @@ -126,13 +126,11 @@ static inline void autopilot_arming_check_motors_on( void ) { autopilot_arming_delay_counter--; if (!THROTTLE_STICK_DOWN()) { autopilot_arming_state = STATE_MOTORS_ON; - } - else if (autopilot_arming_delay_counter == 0) { + } else if (autopilot_arming_delay_counter == 0) { autopilot_arming_state = STATE_MOTORS_OFF_READY; if (autopilot_mode != MODE_MANUAL) { autopilot_unarmed_in_auto = TRUE; - } - else { + } else { autopilot_unarmed_in_auto = FALSE; } } diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_arming_yaw.h b/sw/airborne/firmwares/rotorcraft/autopilot_arming_yaw.h index d14b9f488c..1557142751 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot_arming_yaw.h +++ b/sw/airborne/firmwares/rotorcraft/autopilot_arming_yaw.h @@ -53,7 +53,8 @@ uint32_t autopilot_motors_on_counter; enum arming_state autopilot_check_motor_status; -static inline void autopilot_arming_init(void) { +static inline void autopilot_arming_init(void) +{ autopilot_motors_on_counter = 0; autopilot_check_motor_status = STATUS_MOTORS_OFF; } @@ -61,11 +62,13 @@ static inline void autopilot_arming_init(void) { /** Update the status of the check_motors state machine. */ -static inline void autopilot_arming_set(bool_t motors_on) { - if (motors_on) +static inline void autopilot_arming_set(bool_t motors_on) +{ + if (motors_on) { autopilot_check_motor_status = STATUS_MOTORS_ON; - else + } else { autopilot_check_motor_status = STATUS_MOTORS_OFF; + } } /** @@ -74,50 +77,57 @@ static inline void autopilot_arming_set(bool_t motors_on) { * An intermediate state prevents oscillating between ON and OFF while keeping the stick pushed. * The stick must return to a neutral position before starting/stoping again. */ -static inline void autopilot_arming_check_motors_on( void ) { +static inline void autopilot_arming_check_motors_on(void) +{ /* only allow switching motor if not in KILL mode */ if (autopilot_mode != AP_MODE_KILL) { - switch(autopilot_check_motor_status) { + switch (autopilot_check_motor_status) { case STATUS_MOTORS_OFF: autopilot_motors_on = FALSE; autopilot_motors_on_counter = 0; - if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED()) // stick pushed + if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED()) { // stick pushed autopilot_check_motor_status = STATUS_M_OFF_STICK_PUSHED; + } break; case STATUS_M_OFF_STICK_PUSHED: autopilot_motors_on = FALSE; autopilot_motors_on_counter++; - if (autopilot_motors_on_counter >= MOTOR_ARMING_DELAY) + if (autopilot_motors_on_counter >= MOTOR_ARMING_DELAY) { autopilot_check_motor_status = STATUS_START_MOTORS; - else if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // stick released too soon + } else if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) { // stick released too soon autopilot_check_motor_status = STATUS_MOTORS_OFF; + } break; case STATUS_START_MOTORS: autopilot_motors_on = TRUE; autopilot_motors_on_counter = MOTOR_ARMING_DELAY; - if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // wait until stick released + if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) { // wait until stick released autopilot_check_motor_status = STATUS_MOTORS_ON; + } break; case STATUS_MOTORS_ON: autopilot_motors_on = TRUE; autopilot_motors_on_counter = MOTOR_ARMING_DELAY; - if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED()) // stick pushed + if (THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED()) { // stick pushed autopilot_check_motor_status = STATUS_M_ON_STICK_PUSHED; + } break; case STATUS_M_ON_STICK_PUSHED: autopilot_motors_on = TRUE; autopilot_motors_on_counter--; - if (autopilot_motors_on_counter == 0) + if (autopilot_motors_on_counter == 0) { autopilot_check_motor_status = STATUS_STOP_MOTORS; - else if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // stick released too soon + } else if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) { // stick released too soon autopilot_check_motor_status = STATUS_MOTORS_ON; + } break; case STATUS_STOP_MOTORS: autopilot_motors_on = FALSE; autopilot_motors_on_counter = 0; - if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) // wait until stick released + if (!(THROTTLE_STICK_DOWN() && YAW_STICK_PUSHED())) { // wait until stick released autopilot_check_motor_status = STATUS_MOTORS_OFF; + } break; default: break; diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_rc_helpers.h b/sw/airborne/firmwares/rotorcraft/autopilot_rc_helpers.h index 23a8c0ff7a..43482ccb1a 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot_rc_helpers.h +++ b/sw/airborne/firmwares/rotorcraft/autopilot_rc_helpers.h @@ -52,19 +52,23 @@ (radio_control.values[RADIO_ROLL] < AUTOPILOT_STICK_CENTER_THRESHOLD && \ radio_control.values[RADIO_ROLL] > -AUTOPILOT_STICK_CENTER_THRESHOLD) -static inline bool_t rc_attitude_sticks_centered(void) { +static inline bool_t rc_attitude_sticks_centered(void) +{ return ROLL_STICK_CENTERED() && PITCH_STICK_CENTERED() && YAW_STICK_CENTERED(); } #ifdef RADIO_KILL_SWITCH -static inline bool_t kill_switch_is_on(void) { - if (radio_control.values[RADIO_KILL_SWITCH] < 0) +static inline bool_t kill_switch_is_on(void) +{ + if (radio_control.values[RADIO_KILL_SWITCH] < 0) { return TRUE; - else + } else { return FALSE; + } } #else -static inline bool_t kill_switch_is_on(void) { +static inline bool_t kill_switch_is_on(void) +{ return FALSE; } #endif @@ -72,10 +76,11 @@ static inline bool_t kill_switch_is_on(void) { static inline uint8_t percent_from_rc(int channel) { int per = (MAX_PPRZ + (int32_t)radio_control.values[channel]) * 50 / MAX_PPRZ; - if (per < 0) + if (per < 0) { per = 0; - else if (per > 100) + } else if (per > 100) { per = 100; + } return per; } diff --git a/sw/airborne/firmwares/rotorcraft/datalink.c b/sw/airborne/firmwares/rotorcraft/datalink.c index b2ec1c871c..fdf0aeac9a 100644 --- a/sw/airborne/firmwares/rotorcraft/datalink.c +++ b/sw/airborne/firmwares/rotorcraft/datalink.c @@ -58,22 +58,21 @@ #define IdOfMsg(x) (x[1]) -void dl_parse_msg(void) { +void dl_parse_msg(void) +{ datalink_time = 0; uint8_t msg_id = IdOfMsg(dl_buffer); switch (msg_id) { - case DL_PING: - { + case DL_PING: { DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice); } break; - case DL_SETTING : - { - if (DL_SETTING_ac_id(dl_buffer) != AC_ID) break; + case DL_SETTING : { + if (DL_SETTING_ac_id(dl_buffer) != AC_ID) { break; } uint8_t i = DL_SETTING_index(dl_buffer); float var = DL_SETTING_value(dl_buffer); DlSetting(i, var); @@ -81,9 +80,8 @@ void dl_parse_msg(void) { } break; - case DL_GET_SETTING : - { - if (DL_GET_SETTING_ac_id(dl_buffer) != AC_ID) break; + case DL_GET_SETTING : { + if (DL_GET_SETTING_ac_id(dl_buffer) != AC_ID) { break; } uint8_t i = DL_GET_SETTING_index(dl_buffer); float val = settings_get_value(i); DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val); @@ -91,17 +89,15 @@ void dl_parse_msg(void) { break; #if defined USE_NAVIGATION - case DL_BLOCK : - { - if (DL_BLOCK_ac_id(dl_buffer) != AC_ID) break; + case DL_BLOCK : { + if (DL_BLOCK_ac_id(dl_buffer) != AC_ID) { break; } nav_goto_block(DL_BLOCK_block_id(dl_buffer)); } break; - case DL_MOVE_WP : - { + case DL_MOVE_WP : { uint8_t ac_id = DL_MOVE_WP_ac_id(dl_buffer); - if (ac_id != AC_ID) break; + if (ac_id != AC_ID) { break; } if (stateIsLocalCoordinateValid()) { uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer); struct LlaCoor_i lla; @@ -110,60 +106,60 @@ void dl_parse_msg(void) { /* WP_alt from message is alt above MSL in cm * lla.alt is above ellipsoid in mm */ - lla.alt = DL_MOVE_WP_alt(dl_buffer)*10 - state.ned_origin_i.hmsl + - state.ned_origin_i.lla.alt; + lla.alt = DL_MOVE_WP_alt(dl_buffer) * 10 - state.ned_origin_i.hmsl + + state.ned_origin_i.lla.alt; nav_move_waypoint_lla(wp_id, &lla); } } break; #endif /* USE_NAVIGATION */ #ifdef RADIO_CONTROL_TYPE_DATALINK - case DL_RC_3CH : -#ifdef RADIO_CONTROL_DATALINK_LED - LED_TOGGLE(RADIO_CONTROL_DATALINK_LED); -#endif - parse_rc_3ch_datalink( - DL_RC_3CH_throttle_mode(dl_buffer), - DL_RC_3CH_roll(dl_buffer), - DL_RC_3CH_pitch(dl_buffer)); - break; - case DL_RC_4CH : - if (DL_RC_4CH_ac_id(dl_buffer) == AC_ID) { + case DL_RC_3CH : #ifdef RADIO_CONTROL_DATALINK_LED LED_TOGGLE(RADIO_CONTROL_DATALINK_LED); #endif - parse_rc_4ch_datalink(DL_RC_4CH_mode(dl_buffer), - DL_RC_4CH_throttle(dl_buffer), - DL_RC_4CH_roll(dl_buffer), - DL_RC_4CH_pitch(dl_buffer), - DL_RC_4CH_yaw(dl_buffer)); - } - break; + parse_rc_3ch_datalink( + DL_RC_3CH_throttle_mode(dl_buffer), + DL_RC_3CH_roll(dl_buffer), + DL_RC_3CH_pitch(dl_buffer)); + break; + case DL_RC_4CH : + if (DL_RC_4CH_ac_id(dl_buffer) == AC_ID) { +#ifdef RADIO_CONTROL_DATALINK_LED + LED_TOGGLE(RADIO_CONTROL_DATALINK_LED); +#endif + parse_rc_4ch_datalink(DL_RC_4CH_mode(dl_buffer), + DL_RC_4CH_throttle(dl_buffer), + DL_RC_4CH_roll(dl_buffer), + DL_RC_4CH_pitch(dl_buffer), + DL_RC_4CH_yaw(dl_buffer)); + } + break; #endif // RADIO_CONTROL_TYPE_DATALINK #if defined GPS_DATALINK - case DL_REMOTE_GPS : - // Check if the GPS is for this AC - if (DL_REMOTE_GPS_ac_id(dl_buffer) != AC_ID) break; + case DL_REMOTE_GPS : + // Check if the GPS is for this AC + if (DL_REMOTE_GPS_ac_id(dl_buffer) != AC_ID) { break; } - // Parse the GPS - parse_gps_datalink( - DL_REMOTE_GPS_numsv(dl_buffer), - DL_REMOTE_GPS_ecef_x(dl_buffer), - DL_REMOTE_GPS_ecef_y(dl_buffer), - DL_REMOTE_GPS_ecef_z(dl_buffer), - DL_REMOTE_GPS_lat(dl_buffer), - DL_REMOTE_GPS_lon(dl_buffer), - DL_REMOTE_GPS_alt(dl_buffer), - DL_REMOTE_GPS_hmsl(dl_buffer), - DL_REMOTE_GPS_ecef_xd(dl_buffer), - DL_REMOTE_GPS_ecef_yd(dl_buffer), - DL_REMOTE_GPS_ecef_zd(dl_buffer), - DL_REMOTE_GPS_tow(dl_buffer), - DL_REMOTE_GPS_course(dl_buffer)); - break; + // Parse the GPS + parse_gps_datalink( + DL_REMOTE_GPS_numsv(dl_buffer), + DL_REMOTE_GPS_ecef_x(dl_buffer), + DL_REMOTE_GPS_ecef_y(dl_buffer), + DL_REMOTE_GPS_ecef_z(dl_buffer), + DL_REMOTE_GPS_lat(dl_buffer), + DL_REMOTE_GPS_lon(dl_buffer), + DL_REMOTE_GPS_alt(dl_buffer), + DL_REMOTE_GPS_hmsl(dl_buffer), + DL_REMOTE_GPS_ecef_xd(dl_buffer), + DL_REMOTE_GPS_ecef_yd(dl_buffer), + DL_REMOTE_GPS_ecef_zd(dl_buffer), + DL_REMOTE_GPS_tow(dl_buffer), + DL_REMOTE_GPS_course(dl_buffer)); + break; #endif - default: - break; + default: + break; } /* Parse modules datalink */ modules_parse_datalink(msg_id); diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 3bc9287532..bfed116024 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -109,18 +109,20 @@ static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool_t in_flig #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_gh(struct transport_tx *trans, struct link_device *dev) { - struct NedCoor_i* pos = stateGetPositionNed_i(); +static void send_gh(struct transport_tx *trans, struct link_device *dev) +{ + struct NedCoor_i *pos = stateGetPositionNed_i(); pprz_msg_send_GUIDANCE_H_INT(trans, dev, AC_ID, - &guidance_h_pos_sp.x, &guidance_h_pos_sp.y, - &guidance_h_pos_ref.x, &guidance_h_pos_ref.y, - &(pos->x), &(pos->y)); + &guidance_h_pos_sp.x, &guidance_h_pos_sp.y, + &guidance_h_pos_ref.x, &guidance_h_pos_ref.y, + &(pos->x), &(pos->y)); } -static void send_hover_loop(struct transport_tx *trans, struct link_device *dev) { - struct NedCoor_i* pos = stateGetPositionNed_i(); - struct NedCoor_i* speed = stateGetSpeedNed_i(); - struct NedCoor_i* accel = stateGetAccelNed_i(); +static void send_hover_loop(struct transport_tx *trans, struct link_device *dev) +{ + struct NedCoor_i *pos = stateGetPositionNed_i(); + struct NedCoor_i *speed = stateGetSpeedNed_i(); + struct NedCoor_i *accel = stateGetAccelNed_i(); pprz_msg_send_HOVER_LOOP(trans, dev, AC_ID, &guidance_h_pos_sp.x, &guidance_h_pos_sp.y, @@ -138,33 +140,36 @@ static void send_hover_loop(struct transport_tx *trans, struct link_device *dev) &guidance_h_heading_sp); } -static void send_href(struct transport_tx *trans, struct link_device *dev) { +static void send_href(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_GUIDANCE_H_REF_INT(trans, dev, AC_ID, - &guidance_h_pos_sp.x, &guidance_h_pos_ref.x, - &guidance_h_speed_sp.x, &guidance_h_speed_ref.x, - &guidance_h_accel_ref.x, - &guidance_h_pos_sp.y, &guidance_h_pos_ref.y, - &guidance_h_speed_sp.y, &guidance_h_speed_ref.y, - &guidance_h_accel_ref.y); + &guidance_h_pos_sp.x, &guidance_h_pos_ref.x, + &guidance_h_speed_sp.x, &guidance_h_speed_ref.x, + &guidance_h_accel_ref.x, + &guidance_h_pos_sp.y, &guidance_h_pos_ref.y, + &guidance_h_speed_sp.y, &guidance_h_speed_ref.y, + &guidance_h_accel_ref.y); } -static void send_tune_hover(struct transport_tx *trans, struct link_device *dev) { +static void send_tune_hover(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_ROTORCRAFT_TUNE_HOVER(trans, dev, AC_ID, - &radio_control.values[RADIO_ROLL], - &radio_control.values[RADIO_PITCH], - &radio_control.values[RADIO_YAW], - &stabilization_cmd[COMMAND_ROLL], - &stabilization_cmd[COMMAND_PITCH], - &stabilization_cmd[COMMAND_YAW], - &stabilization_cmd[COMMAND_THRUST], - &(stateGetNedToBodyEulers_i()->phi), - &(stateGetNedToBodyEulers_i()->theta), - &(stateGetNedToBodyEulers_i()->psi)); + &radio_control.values[RADIO_ROLL], + &radio_control.values[RADIO_PITCH], + &radio_control.values[RADIO_YAW], + &stabilization_cmd[COMMAND_ROLL], + &stabilization_cmd[COMMAND_PITCH], + &stabilization_cmd[COMMAND_YAW], + &stabilization_cmd[COMMAND_THRUST], + &(stateGetNedToBodyEulers_i()->phi), + &(stateGetNedToBodyEulers_i()->theta), + &(stateGetNedToBodyEulers_i()->psi)); } #endif -void guidance_h_init(void) { +void guidance_h_init(void) +{ guidance_h_mode = GUIDANCE_H_MODE_KILL; guidance_h_use_ref = GUIDANCE_H_USE_REF; @@ -193,7 +198,8 @@ void guidance_h_init(void) { } -static inline void reset_guidance_reference_from_current_position(void) { +static inline void reset_guidance_reference_from_current_position(void) +{ VECT2_COPY(guidance_h_pos_ref, *stateGetPositionNed_i()); VECT2_COPY(guidance_h_speed_ref, *stateGetSpeedNed_i()); INT_VECT2_ZERO(guidance_h_accel_ref); @@ -202,14 +208,16 @@ static inline void reset_guidance_reference_from_current_position(void) { INT_VECT2_ZERO(guidance_h_trim_att_integrator); } -void guidance_h_mode_changed(uint8_t new_mode) { - if (new_mode == guidance_h_mode) +void guidance_h_mode_changed(uint8_t new_mode) +{ + if (new_mode == guidance_h_mode) { return; + } if (new_mode != GUIDANCE_H_MODE_FORWARD && new_mode != GUIDANCE_H_MODE_RATE) { - transition_percentage = 0; - transition_theta_offset = 0; - } + transition_percentage = 0; + transition_theta_offset = 0; + } switch (new_mode) { case GUIDANCE_H_MODE_RC_DIRECT: @@ -264,9 +272,10 @@ void guidance_h_mode_changed(uint8_t new_mode) { } -void guidance_h_read_rc(bool_t in_flight) { +void guidance_h_read_rc(bool_t in_flight) +{ - switch ( guidance_h_mode ) { + switch (guidance_h_mode) { case GUIDANCE_H_MODE_RC_DIRECT: stabilization_none_read_rc(); @@ -298,8 +307,7 @@ void guidance_h_read_rc(bool_t in_flight) { case GUIDANCE_H_MODE_NAV: if (radio_control.status == RC_OK) { stabilization_attitude_read_rc_setpoint_eulers(&guidance_h_rc_sp, in_flight, FALSE, FALSE); - } - else { + } else { INT_EULERS_ZERO(guidance_h_rc_sp); } break; @@ -310,8 +318,9 @@ void guidance_h_read_rc(bool_t in_flight) { } -void guidance_h_run(bool_t in_flight) { - switch ( guidance_h_mode ) { +void guidance_h_run(bool_t in_flight) +{ + switch (guidance_h_mode) { case GUIDANCE_H_MODE_RC_DIRECT: stabilization_none_run(in_flight); @@ -322,7 +331,7 @@ void guidance_h_run(bool_t in_flight) { break; case GUIDANCE_H_MODE_FORWARD: - if(transition_percentage < (100<psi; stabilization_attitude_set_rpy_setpoint_i(&sp_cmd_i); - } - else { + } else { INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, navigation_carrot); guidance_h_update_reference(); @@ -383,21 +393,22 @@ void guidance_h_run(bool_t in_flight) { } -static void guidance_h_update_reference(void) { +static void guidance_h_update_reference(void) +{ /* compute reference even if usage temporarily disabled via guidance_h_use_ref */ #if GUIDANCE_H_USE_REF #if GUIDANCE_H_USE_SPEED_REF - if(guidance_h_mode == GUIDANCE_H_MODE_HOVER) + if (guidance_h_mode == GUIDANCE_H_MODE_HOVER) { gh_update_ref_from_speed_sp(guidance_h_speed_sp); - else + } else #endif - gh_update_ref_from_pos_sp(guidance_h_pos_sp); + gh_update_ref_from_pos_sp(guidance_h_pos_sp); #endif /* either use the reference or simply copy the pos setpoint */ if (guidance_h_use_ref) { /* convert our reference to generic representation */ - INT32_VECT2_RSHIFT(guidance_h_pos_ref, gh_ref.pos, (GH_POS_REF_FRAC - INT32_POS_FRAC)); + INT32_VECT2_RSHIFT(guidance_h_pos_ref, gh_ref.pos, (GH_POS_REF_FRAC - INT32_POS_FRAC)); INT32_VECT2_LSHIFT(guidance_h_speed_ref, gh_ref.speed, (INT32_SPEED_FRAC - GH_SPEED_REF_FRAC)); INT32_VECT2_LSHIFT(guidance_h_accel_ref, gh_ref.accel, (INT32_ACCEL_FRAC - GH_ACCEL_REF_FRAC)); } else { @@ -425,10 +436,11 @@ static void guidance_h_update_reference(void) { * you get an angle of 5.6 degrees for 1m pos error */ #define GH_GAIN_SCALE 2 -static void guidance_h_traj_run(bool_t in_flight) { +static void guidance_h_traj_run(bool_t in_flight) +{ /* maximum bank angle: default 20 deg, max 40 deg*/ static const int32_t traj_max_bank = Max(BFP_OF_REAL(GUIDANCE_H_MAX_BANK, INT32_ANGLE_FRAC), - BFP_OF_REAL(RadOfDeg(40), INT32_ANGLE_FRAC)); + BFP_OF_REAL(RadOfDeg(40), INT32_ANGLE_FRAC)); static const int32_t total_max_bank = BFP_OF_REAL(RadOfDeg(45), INT32_ANGLE_FRAC); /* compute position error */ @@ -481,15 +493,19 @@ static void guidance_h_traj_run(bool_t in_flight) { if (guidance_h_approx_force_by_thrust && in_flight) { static int32_t thrust_cmd_filt; int32_t vertical_thrust = (stabilization_cmd[COMMAND_THRUST] * guidance_v_thrust_coeff) >> INT32_TRIG_FRAC; - thrust_cmd_filt = (thrust_cmd_filt * GUIDANCE_H_THRUST_CMD_FILTER + vertical_thrust) / (GUIDANCE_H_THRUST_CMD_FILTER + 1); - guidance_h_cmd_earth.x = ANGLE_BFP_OF_REAL(atan2f((guidance_h_cmd_earth.x * MAX_PPRZ / INT32_ANGLE_PI_2), thrust_cmd_filt)); - guidance_h_cmd_earth.y = ANGLE_BFP_OF_REAL(atan2f((guidance_h_cmd_earth.y * MAX_PPRZ / INT32_ANGLE_PI_2), thrust_cmd_filt)); + thrust_cmd_filt = (thrust_cmd_filt * GUIDANCE_H_THRUST_CMD_FILTER + vertical_thrust) / + (GUIDANCE_H_THRUST_CMD_FILTER + 1); + guidance_h_cmd_earth.x = ANGLE_BFP_OF_REAL(atan2f((guidance_h_cmd_earth.x * MAX_PPRZ / INT32_ANGLE_PI_2), + thrust_cmd_filt)); + guidance_h_cmd_earth.y = ANGLE_BFP_OF_REAL(atan2f((guidance_h_cmd_earth.y * MAX_PPRZ / INT32_ANGLE_PI_2), + thrust_cmd_filt)); } VECT2_STRIM(guidance_h_cmd_earth, -total_max_bank, total_max_bank); } -static void guidance_h_hover_enter(void) { +static void guidance_h_hover_enter(void) +{ /* set horizontal setpoint to current position */ VECT2_COPY(guidance_h_pos_sp, *stateGetPositionNed_i()); @@ -499,7 +515,8 @@ static void guidance_h_hover_enter(void) { guidance_h_rc_sp.psi = stateGetNedToBodyEulers_i()->psi; } -static void guidance_h_nav_enter(void) { +static void guidance_h_nav_enter(void) +{ /* horizontal position setpoint from navigation/flightplan */ INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, navigation_carrot); @@ -509,24 +526,27 @@ static void guidance_h_nav_enter(void) { nav_heading = stateGetNedToBodyEulers_i()->psi; } -static inline void transition_run(void) { +static inline void transition_run(void) +{ //Add 0.00625% - transition_percentage += 1<<(INT32_PERCENTAGE_FRAC-4); + transition_percentage += 1 << (INT32_PERCENTAGE_FRAC - 4); #ifdef TRANSITION_MAX_OFFSET const int32_t max_offset = ANGLE_BFP_OF_REAL(TRANSITION_MAX_OFFSET); - transition_theta_offset = INT_MULT_RSHIFT((transition_percentage<<(INT32_ANGLE_FRAC-INT32_PERCENTAGE_FRAC))/100, max_offset, INT32_ANGLE_FRAC); + transition_theta_offset = INT_MULT_RSHIFT((transition_percentage << (INT32_ANGLE_FRAC - INT32_PERCENTAGE_FRAC)) / 100, + max_offset, INT32_ANGLE_FRAC); #endif } /// read speed setpoint from RC -static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool_t in_flight) { +static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool_t in_flight) +{ if (in_flight) { // negative pitch is forward int64_t rc_x = -radio_control.values[RADIO_PITCH]; int64_t rc_y = radio_control.values[RADIO_ROLL]; - DeadBand(rc_x, MAX_PPRZ/20); - DeadBand(rc_y, MAX_PPRZ/20); + DeadBand(rc_x, MAX_PPRZ / 20); + DeadBand(rc_y, MAX_PPRZ / 20); // convert input from MAX_PPRZ range to SPEED_BFP int32_t max_speed = SPEED_BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED); @@ -541,10 +561,9 @@ static void read_rc_setpoint_speed_i(struct Int32Vect2 *speed_sp, bool_t in_flig int32_t s_psi, c_psi; PPRZ_ITRIG_SIN(s_psi, psi); PPRZ_ITRIG_COS(c_psi, psi); - speed_sp->x = (int32_t)(( (int64_t)c_psi * rc_x + (int64_t)s_psi * rc_y) >> INT32_TRIG_FRAC); + speed_sp->x = (int32_t)(((int64_t)c_psi * rc_x + (int64_t)s_psi * rc_y) >> INT32_TRIG_FRAC); speed_sp->y = (int32_t)((-(int64_t)s_psi * rc_x + (int64_t)c_psi * rc_y) >> INT32_TRIG_FRAC); - } - else { + } else { speed_sp->x = 0; speed_sp->y = 0; } diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h index adc1ac5da1..ac9b064017 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h @@ -103,7 +103,7 @@ extern void guidance_h_run(bool_t in_flight); #define guidance_h_SetKi(_val) { \ guidance_h_igain = _val; \ - INT_VECT2_ZERO(guidance_h_trim_att_integrator); \ + INT_VECT2_ZERO(guidance_h_trim_att_integrator); \ } /* Make sure that ref can only be temporarily disabled for testing, @@ -113,19 +113,23 @@ extern void guidance_h_run(bool_t in_flight); guidance_h_use_ref = _val && GUIDANCE_H_USE_REF; \ } -static inline void guidance_h_SetMaxSpeed(float speed) { +static inline void guidance_h_SetMaxSpeed(float speed) +{ gh_set_max_speed(speed); } -static inline void guidance_h_SetOmega(float omega) { +static inline void guidance_h_SetOmega(float omega) +{ gh_set_omega(omega); } -static inline void guidance_h_SetZeta(float zeta) { +static inline void guidance_h_SetZeta(float zeta) +{ gh_set_zeta(zeta); } -static inline void guidance_h_SetTau(float tau) { +static inline void guidance_h_SetTau(float tau) +{ gh_set_tau(tau); } diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c index 13f813710a..26c2c63b7e 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.c @@ -52,62 +52,69 @@ static const int32_t gh_max_accel = BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, GH_ACC #endif #define GH_REF_INV_TAU_FRAC 16 -static void gh_compute_route_ref(struct Int32Vect2* ref_vector); -static void gh_compute_ref_max(struct Int32Vect2* ref_vector); -static void gh_compute_ref_max_accel(struct Int32Vect2* ref_vector); -static void gh_compute_ref_max_speed(struct Int32Vect2* ref_vector); +static void gh_compute_route_ref(struct Int32Vect2 *ref_vector); +static void gh_compute_ref_max(struct Int32Vect2 *ref_vector); +static void gh_compute_ref_max_accel(struct Int32Vect2 *ref_vector); +static void gh_compute_ref_max_speed(struct Int32Vect2 *ref_vector); static void gh_saturate_ref_accel(void); static void gh_saturate_ref_speed(void); -void gh_ref_init(void) { +void gh_ref_init(void) +{ gh_ref.omega = GUIDANCE_H_REF_OMEGA; gh_ref.zeta = GUIDANCE_H_REF_ZETA; - gh_ref.zeta_omega = BFP_OF_REAL((GUIDANCE_H_REF_ZETA*GUIDANCE_H_REF_OMEGA), GH_ZETA_OMEGA_FRAC); - gh_ref.omega_2 = BFP_OF_REAL((GUIDANCE_H_REF_OMEGA*GUIDANCE_H_REF_OMEGA), GH_OMEGA_2_FRAC); + gh_ref.zeta_omega = BFP_OF_REAL((GUIDANCE_H_REF_ZETA * GUIDANCE_H_REF_OMEGA), GH_ZETA_OMEGA_FRAC); + gh_ref.omega_2 = BFP_OF_REAL((GUIDANCE_H_REF_OMEGA * GUIDANCE_H_REF_OMEGA), GH_OMEGA_2_FRAC); gh_set_tau(GUIDANCE_H_REF_TAU); gh_set_max_speed(GUIDANCE_H_REF_MAX_SPEED); } -float gh_set_max_speed(float max_speed) { +float gh_set_max_speed(float max_speed) +{ /* limit to 100m/s as int version would overflow at 2^14 = 128 m/s */ gh_ref.max_speed = Min(fabs(max_speed), 100.0f); gh_ref.max_speed_int = BFP_OF_REAL(gh_ref.max_speed, GH_MAX_SPEED_REF_FRAC); return gh_ref.max_speed; } -float gh_set_tau(float tau) { +float gh_set_tau(float tau) +{ gh_ref.tau = tau; Bound(gh_ref.tau, 0.01f, 2.0f); - gh_ref.inv_tau = BFP_OF_REAL((1./gh_ref.tau), GH_REF_INV_TAU_FRAC); + gh_ref.inv_tau = BFP_OF_REAL((1. / gh_ref.tau), GH_REF_INV_TAU_FRAC); return gh_ref.tau; } -float gh_set_omega(float omega) { +float gh_set_omega(float omega) +{ gh_ref.omega = omega; Bound(gh_ref.omega, 0.1f, 5.0f); - gh_ref.omega_2 = BFP_OF_REAL((gh_ref.omega*gh_ref.omega), GH_OMEGA_2_FRAC); - gh_ref.zeta_omega = BFP_OF_REAL((gh_ref.zeta*gh_ref.omega), GH_ZETA_OMEGA_FRAC); + gh_ref.omega_2 = BFP_OF_REAL((gh_ref.omega * gh_ref.omega), GH_OMEGA_2_FRAC); + gh_ref.zeta_omega = BFP_OF_REAL((gh_ref.zeta * gh_ref.omega), GH_ZETA_OMEGA_FRAC); return gh_ref.omega; } -float gh_set_zeta(float zeta) { +float gh_set_zeta(float zeta) +{ gh_ref.zeta = zeta; Bound(gh_ref.zeta, 0.7f, 1.2f); - gh_ref.zeta_omega = BFP_OF_REAL((gh_ref.zeta*gh_ref.omega), GH_ZETA_OMEGA_FRAC); + gh_ref.zeta_omega = BFP_OF_REAL((gh_ref.zeta * gh_ref.omega), GH_ZETA_OMEGA_FRAC); return gh_ref.zeta; } -void gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel) { +void gh_set_ref(struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel) +{ struct Int64Vect2 new_pos; - new_pos.x = ((int64_t)pos.x)<<(GH_POS_REF_FRAC - INT32_POS_FRAC); - new_pos.y = ((int64_t)pos.y)<<(GH_POS_REF_FRAC - INT32_POS_FRAC); + new_pos.x = ((int64_t)pos.x) << (GH_POS_REF_FRAC - INT32_POS_FRAC); + new_pos.y = ((int64_t)pos.y) << (GH_POS_REF_FRAC - INT32_POS_FRAC); gh_ref.pos = new_pos; INT32_VECT2_RSHIFT(gh_ref.speed, speed, (INT32_SPEED_FRAC - GH_SPEED_REF_FRAC)); INT32_VECT2_RSHIFT(gh_ref.accel, accel, (INT32_ACCEL_FRAC - GH_ACCEL_REF_FRAC)); } -void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp) { +void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp) +{ VECT2_ADD(gh_ref.pos, gh_ref.speed); VECT2_ADD(gh_ref.speed, gh_ref.accel); @@ -138,8 +145,9 @@ void gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp) { } -void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) { -/* WARNING: SPEED SATURATION UNTESTED */ +void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) +{ + /* WARNING: SPEED SATURATION UNTESTED */ VECT2_ADD(gh_ref.pos, gh_ref.speed); VECT2_ADD(gh_ref.speed, gh_ref.accel); @@ -161,7 +169,8 @@ void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) { gh_saturate_ref_speed(); } -static void gh_compute_route_ref(struct Int32Vect2* ref_vector) { +static void gh_compute_route_ref(struct Int32Vect2 *ref_vector) +{ float f_route_ref = atan2f(-ref_vector->y, -ref_vector->x); gh_ref.route_ref = ANGLE_BFP_OF_REAL(f_route_ref); /* Compute North and East route components */ @@ -171,15 +180,15 @@ static void gh_compute_route_ref(struct Int32Vect2* ref_vector) { gh_ref.s_route_ref = abs(gh_ref.s_route_ref); } -static void gh_compute_ref_max(struct Int32Vect2* ref_vector) { +static void gh_compute_ref_max(struct Int32Vect2 *ref_vector) +{ /* Bound ref to max speed/accel along route reference angle. * If angle can't be computed, simply set both axes to max magnitude/sqrt(2). */ if (ref_vector->x == 0 && ref_vector->y == 0) { gh_ref.max_accel.x = gh_ref.max_accel.y = gh_max_accel * 0.707; gh_ref.max_vel.x = gh_ref.max_vel.y = gh_ref.max_speed_int * 0.707; - } - else { + } else { gh_compute_route_ref(ref_vector); /* Compute maximum acceleration*/ gh_ref.max_accel.x = INT_MULT_RSHIFT(gh_max_accel, gh_ref.c_route_ref, INT32_TRIG_FRAC); @@ -192,14 +201,14 @@ static void gh_compute_ref_max(struct Int32Vect2* ref_vector) { INT32_VECT2_LSHIFT(gh_ref.max_vel, gh_ref.max_vel, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC)); } -static void gh_compute_ref_max_accel(struct Int32Vect2* ref_vector) { +static void gh_compute_ref_max_accel(struct Int32Vect2 *ref_vector) +{ /* Bound ref to max accel along reference vector. * If angle can't be computed, simply set both axes to max magnitude/sqrt(2). */ if (ref_vector->x == 0 && ref_vector->y == 0) { gh_ref.max_accel.x = gh_ref.max_accel.y = gh_max_accel * 0.707; - } - else { + } else { gh_compute_route_ref(ref_vector); /* Compute maximum acceleration*/ gh_ref.max_accel.x = INT_MULT_RSHIFT(gh_max_accel, gh_ref.c_route_ref, INT32_TRIG_FRAC); @@ -207,14 +216,14 @@ static void gh_compute_ref_max_accel(struct Int32Vect2* ref_vector) { } } -static void gh_compute_ref_max_speed(struct Int32Vect2* ref_vector) { +static void gh_compute_ref_max_speed(struct Int32Vect2 *ref_vector) +{ /* Bound ref to max speed along reference vector. * If angle can't be computed, simply set both axes to max magnitude/sqrt(2). */ if (ref_vector->x == 0 && ref_vector->y == 0) { gh_ref.max_vel.x = gh_ref.max_vel.y = gh_ref.max_speed_int * 0.707; - } - else { + } else { gh_compute_route_ref(ref_vector); /* Compute maximum reference x/y velocity from absolute max_speed */ gh_ref.max_vel.x = INT_MULT_RSHIFT(gh_ref.max_speed_int, gh_ref.c_route_ref, INT32_TRIG_FRAC); @@ -225,33 +234,37 @@ static void gh_compute_ref_max_speed(struct Int32Vect2* ref_vector) { } /** saturate reference accelerations */ -static void gh_saturate_ref_accel(void) { +static void gh_saturate_ref_accel(void) +{ /* Saturate accelerations */ BoundAbs(gh_ref.accel.x, gh_ref.max_accel.x); BoundAbs(gh_ref.accel.y, gh_ref.max_accel.y); } /** Saturate ref speed and adjust acceleration accordingly */ -static void gh_saturate_ref_speed(void) { +static void gh_saturate_ref_speed(void) +{ if (gh_ref.speed.x < -gh_ref.max_vel.x) { gh_ref.speed.x = -gh_ref.max_vel.x; - if (gh_ref.accel.x < 0) + if (gh_ref.accel.x < 0) { gh_ref.accel.x = 0; - } - else if (gh_ref.speed.x > gh_ref.max_vel.x) { + } + } else if (gh_ref.speed.x > gh_ref.max_vel.x) { gh_ref.speed.x = gh_ref.max_vel.x; - if (gh_ref.accel.x > 0) + if (gh_ref.accel.x > 0) { gh_ref.accel.x = 0; + } } if (gh_ref.speed.y < -gh_ref.max_vel.y) { gh_ref.speed.y = -gh_ref.max_vel.y; - if (gh_ref.accel.y < 0) + if (gh_ref.accel.y < 0) { gh_ref.accel.y = 0; - } - else if (gh_ref.speed.y > gh_ref.max_vel.y) { + } + } else if (gh_ref.speed.y > gh_ref.max_vel.y) { gh_ref.speed.y = gh_ref.max_vel.y; - if (gh_ref.accel.y > 0) + if (gh_ref.accel.y > 0) { gh_ref.accel.y = 0; + } } } diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index 44e9a36912..ec8ab3082f 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -142,33 +142,36 @@ static void run_hover_loop(bool_t in_flight); #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_vert_loop(struct transport_tx *trans, struct link_device *dev) { +static void send_vert_loop(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_VERT_LOOP(trans, dev, AC_ID, - &guidance_v_z_sp, &guidance_v_zd_sp, - &(stateGetPositionNed_i()->z), - &(stateGetSpeedNed_i()->z), - &(stateGetAccelNed_i()->z), - &guidance_v_z_ref, &guidance_v_zd_ref, - &guidance_v_zdd_ref, - &gv_adapt_X, - &gv_adapt_P, - &gv_adapt_Xmeas, - &guidance_v_z_sum_err, - &guidance_v_ff_cmd, - &guidance_v_fb_cmd, - &guidance_v_delta_t); + &guidance_v_z_sp, &guidance_v_zd_sp, + &(stateGetPositionNed_i()->z), + &(stateGetSpeedNed_i()->z), + &(stateGetAccelNed_i()->z), + &guidance_v_z_ref, &guidance_v_zd_ref, + &guidance_v_zdd_ref, + &gv_adapt_X, + &gv_adapt_P, + &gv_adapt_Xmeas, + &guidance_v_z_sum_err, + &guidance_v_ff_cmd, + &guidance_v_fb_cmd, + &guidance_v_delta_t); } -static void send_tune_vert(struct transport_tx *trans, struct link_device *dev) { +static void send_tune_vert(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_TUNE_VERT(trans, dev, AC_ID, - &guidance_v_z_sp, - &(stateGetPositionNed_i()->z), - &guidance_v_z_ref, - &guidance_v_zd_ref); + &guidance_v_z_sp, + &(stateGetPositionNed_i()->z), + &guidance_v_z_ref, + &guidance_v_zd_ref); } #endif -void guidance_v_init(void) { +void guidance_v_init(void) +{ guidance_v_mode = GUIDANCE_V_MODE_KILL; @@ -190,48 +193,52 @@ void guidance_v_init(void) { } -void guidance_v_read_rc(void) { +void guidance_v_read_rc(void) +{ /* used in RC_DIRECT directly and as saturation in CLIMB and HOVER */ guidance_v_rc_delta_t = (int32_t)radio_control.values[RADIO_THROTTLE]; /* used in RC_CLIMB */ - guidance_v_rc_zd_sp = (MAX_PPRZ/2) - (int32_t)radio_control.values[RADIO_THROTTLE]; + guidance_v_rc_zd_sp = (MAX_PPRZ / 2) - (int32_t)radio_control.values[RADIO_THROTTLE]; DeadBand(guidance_v_rc_zd_sp, GUIDANCE_V_CLIMB_RC_DEADBAND); static const int32_t climb_scale = ABS(SPEED_BFP_OF_REAL(GUIDANCE_V_MAX_RC_CLIMB_SPEED) / - (MAX_PPRZ/2 - GUIDANCE_V_CLIMB_RC_DEADBAND)); + (MAX_PPRZ / 2 - GUIDANCE_V_CLIMB_RC_DEADBAND)); static const int32_t descent_scale = ABS(SPEED_BFP_OF_REAL(GUIDANCE_V_MAX_RC_DESCENT_SPEED) / - (MAX_PPRZ/2 - GUIDANCE_V_CLIMB_RC_DEADBAND)); + (MAX_PPRZ / 2 - GUIDANCE_V_CLIMB_RC_DEADBAND)); - if(guidance_v_rc_zd_sp > 0) + if (guidance_v_rc_zd_sp > 0) { guidance_v_rc_zd_sp *= descent_scale; - else + } else { guidance_v_rc_zd_sp *= climb_scale; + } } -void guidance_v_mode_changed(uint8_t new_mode) { +void guidance_v_mode_changed(uint8_t new_mode) +{ - if (new_mode == guidance_v_mode) + if (new_mode == guidance_v_mode) { return; + } switch (new_mode) { - case GUIDANCE_V_MODE_HOVER: - guidance_v_z_sp = stateGetPositionNed_i()->z; // set current altitude as setpoint - guidance_v_z_sum_err = 0; - GuidanceVSetRef(stateGetPositionNed_i()->z, 0, 0); - break; + case GUIDANCE_V_MODE_HOVER: + guidance_v_z_sp = stateGetPositionNed_i()->z; // set current altitude as setpoint + guidance_v_z_sum_err = 0; + GuidanceVSetRef(stateGetPositionNed_i()->z, 0, 0); + break; - case GUIDANCE_V_MODE_RC_CLIMB: - case GUIDANCE_V_MODE_CLIMB: - guidance_v_zd_sp = 0; - case GUIDANCE_V_MODE_NAV: - guidance_v_z_sum_err = 0; - GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0); - break; + case GUIDANCE_V_MODE_RC_CLIMB: + case GUIDANCE_V_MODE_CLIMB: + guidance_v_zd_sp = 0; + case GUIDANCE_V_MODE_NAV: + guidance_v_z_sum_err = 0; + GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0); + break; - default: - break; + default: + break; } @@ -239,14 +246,16 @@ void guidance_v_mode_changed(uint8_t new_mode) { } -void guidance_v_notify_in_flight( bool_t in_flight) { +void guidance_v_notify_in_flight(bool_t in_flight) +{ if (in_flight) { gv_adapt_init(); } } -void guidance_v_run(bool_t in_flight) { +void guidance_v_run(bool_t in_flight) +{ // FIXME... SATURATIONS NOT TAKEN INTO ACCOUNT // AKA SUPERVISION and co @@ -254,66 +263,62 @@ void guidance_v_run(bool_t in_flight) { if (in_flight) { int32_t vertical_thrust = (stabilization_cmd[COMMAND_THRUST] * guidance_v_thrust_coeff) >> INT32_TRIG_FRAC; gv_adapt_run(stateGetAccelNed_i()->z, vertical_thrust, guidance_v_zd_ref); - } - else { + } else { /* reset estimate while not in_flight */ gv_adapt_init(); } switch (guidance_v_mode) { - case GUIDANCE_V_MODE_RC_DIRECT: - guidance_v_z_sp = stateGetPositionNed_i()->z; // for display only - stabilization_cmd[COMMAND_THRUST] = guidance_v_rc_delta_t; - break; + case GUIDANCE_V_MODE_RC_DIRECT: + guidance_v_z_sp = stateGetPositionNed_i()->z; // for display only + stabilization_cmd[COMMAND_THRUST] = guidance_v_rc_delta_t; + break; - case GUIDANCE_V_MODE_RC_CLIMB: - guidance_v_zd_sp = guidance_v_rc_zd_sp; - gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z); - run_hover_loop(in_flight); - stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t; - break; - - case GUIDANCE_V_MODE_CLIMB: - gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z); - run_hover_loop(in_flight); -#if !NO_RC_THRUST_LIMIT - /* use rc limitation if available */ - if (radio_control.status == RC_OK) - stabilization_cmd[COMMAND_THRUST] = Min(guidance_v_rc_delta_t, guidance_v_delta_t); - else -#endif + case GUIDANCE_V_MODE_RC_CLIMB: + guidance_v_zd_sp = guidance_v_rc_zd_sp; + gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z); + run_hover_loop(in_flight); stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t; - break; + break; - case GUIDANCE_V_MODE_HOVER: - guidance_v_zd_sp = 0; - gv_update_ref_from_z_sp(guidance_v_z_sp); - run_hover_loop(in_flight); + case GUIDANCE_V_MODE_CLIMB: + gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z); + run_hover_loop(in_flight); #if !NO_RC_THRUST_LIMIT - /* use rc limitation if available */ - if (radio_control.status == RC_OK) - stabilization_cmd[COMMAND_THRUST] = Min(guidance_v_rc_delta_t, guidance_v_delta_t); - else + /* use rc limitation if available */ + if (radio_control.status == RC_OK) { + stabilization_cmd[COMMAND_THRUST] = Min(guidance_v_rc_delta_t, guidance_v_delta_t); + } else #endif - stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t; - break; + stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t; + break; - case GUIDANCE_V_MODE_NAV: - { + case GUIDANCE_V_MODE_HOVER: + guidance_v_zd_sp = 0; + gv_update_ref_from_z_sp(guidance_v_z_sp); + run_hover_loop(in_flight); +#if !NO_RC_THRUST_LIMIT + /* use rc limitation if available */ + if (radio_control.status == RC_OK) { + stabilization_cmd[COMMAND_THRUST] = Min(guidance_v_rc_delta_t, guidance_v_delta_t); + } else +#endif + stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t; + break; + + case GUIDANCE_V_MODE_NAV: { if (vertical_mode == VERTICAL_MODE_ALT) { guidance_v_z_sp = -nav_flight_altitude; guidance_v_zd_sp = 0; gv_update_ref_from_z_sp(guidance_v_z_sp); run_hover_loop(in_flight); - } - else if (vertical_mode == VERTICAL_MODE_CLIMB) { + } else if (vertical_mode == VERTICAL_MODE_CLIMB) { guidance_v_z_sp = stateGetPositionNed_i()->z; guidance_v_zd_sp = -nav_climb; gv_update_ref_from_zd_sp(guidance_v_zd_sp, stateGetPositionNed_i()->z); run_hover_loop(in_flight); - } - else if (vertical_mode == VERTICAL_MODE_MANUAL) { + } else if (vertical_mode == VERTICAL_MODE_MANUAL) { guidance_v_z_sp = stateGetPositionNed_i()->z; guidance_v_zd_sp = stateGetSpeedNed_i()->z; GuidanceVSetRef(guidance_v_z_sp, guidance_v_zd_sp, 0); @@ -322,23 +327,24 @@ void guidance_v_run(bool_t in_flight) { } #if !NO_RC_THRUST_LIMIT /* use rc limitation if available */ - if (radio_control.status == RC_OK) + if (radio_control.status == RC_OK) { stabilization_cmd[COMMAND_THRUST] = Min(guidance_v_rc_delta_t, guidance_v_delta_t); - else + } else #endif stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t; break; } - default: - break; + default: + break; } } /// get the cosine of the angle between thrust vector and gravity vector -static int32_t get_vertical_thrust_coeff(void) { +static int32_t get_vertical_thrust_coeff(void) +{ static const int32_t max_bank_coef = BFP_OF_REAL(RadOfDeg(30.), INT32_TRIG_FRAC); - struct Int32RMat* att = stateGetNedToBodyRMat_i(); + struct Int32RMat *att = stateGetNedToBodyRMat_i(); /* thrust vector: * int32_rmat_vmult(&thrust_vect, &att, &zaxis) * same as last colum of rmat with INT32_TRIG_FRAC @@ -354,21 +360,23 @@ static int32_t get_vertical_thrust_coeff(void) { * dot(v1, v2) = v1.z * v2.z = v2.z */ int32_t coef = att->m[8]; - if (coef < max_bank_coef) + if (coef < max_bank_coef) { coef = max_bank_coef; + } return coef; } #define FF_CMD_FRAC 18 -static void run_hover_loop(bool_t in_flight) { +static void run_hover_loop(bool_t in_flight) +{ /* convert our reference to generic representation */ - int64_t tmp = gv_z_ref>>(GV_Z_REF_FRAC - INT32_POS_FRAC); + int64_t tmp = gv_z_ref >> (GV_Z_REF_FRAC - INT32_POS_FRAC); guidance_v_z_ref = (int32_t)tmp; - guidance_v_zd_ref = gv_zd_ref<<(INT32_SPEED_FRAC - GV_ZD_REF_FRAC); - guidance_v_zdd_ref = gv_zdd_ref<<(INT32_ACCEL_FRAC - GV_ZDD_REF_FRAC); + guidance_v_zd_ref = gv_zd_ref << (INT32_SPEED_FRAC - GV_ZD_REF_FRAC); + guidance_v_zdd_ref = gv_zdd_ref << (INT32_ACCEL_FRAC - GV_ZDD_REF_FRAC); /* compute the error to our reference */ int32_t err_z = guidance_v_z_ref - stateGetPositionNed_i()->z; Bound(err_z, GUIDANCE_V_MIN_ERR_Z, GUIDANCE_V_MAX_ERR_Z); @@ -378,16 +386,15 @@ static void run_hover_loop(bool_t in_flight) { if (in_flight) { guidance_v_z_sum_err += err_z; Bound(guidance_v_z_sum_err, -GUIDANCE_V_MAX_SUM_ERR, GUIDANCE_V_MAX_SUM_ERR); - } - else + } else { guidance_v_z_sum_err = 0; + } /* our nominal command : (g + zdd)*m */ int32_t inv_m; if (guidance_v_adapt_throttle_enabled) { inv_m = gv_adapt_X >> (GV_ADAPT_X_FRAC - FF_CMD_FRAC); - } - else { + } else { /* use the fixed nominal throttle */ inv_m = BFP_OF_REAL(9.81 / (guidance_v_nominal_throttle * MAX_PPRZ), FF_CMD_FRAC); } diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h index 93e0aeaa4d..ee836ec83e 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h @@ -104,9 +104,9 @@ extern void guidance_v_mode_changed(uint8_t new_mode); extern void guidance_v_notify_in_flight(bool_t in_flight); extern void guidance_v_run(bool_t in_flight); -#define guidance_v_SetKi(_val) { \ - guidance_v_ki = _val; \ - guidance_v_z_sum_err = 0; \ +#define guidance_v_SetKi(_val) { \ + guidance_v_ki = _val; \ + guidance_v_z_sum_err = 0; \ } #endif /* GUIDANCE_V_H */ diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.c index e1cd5e4979..7a5b5f20c3 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.c @@ -110,9 +110,10 @@ int32_t gv_adapt_Xmeas; #define GV_ADAPT_P0_F 0.1 static const int32_t gv_adapt_P0 = BFP_OF_REAL(GV_ADAPT_P0_F, GV_ADAPT_P_FRAC); static const int32_t gv_adapt_X0 = BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC) / - (GUIDANCE_V_ADAPT_INITIAL_HOVER_THROTTLE * MAX_PPRZ); + (GUIDANCE_V_ADAPT_INITIAL_HOVER_THROTTLE *MAX_PPRZ); -void gv_adapt_init(void) { +void gv_adapt_init(void) +{ gv_adapt_X = gv_adapt_X0; gv_adapt_P = gv_adapt_P0; } @@ -124,7 +125,8 @@ void gv_adapt_init(void) { * @param thrust_applied controller input [0 : MAX_PPRZ] * @param zd_ref vertical speed reference in m/s with #INT32_SPEED_FRAC */ -void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref) { +void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref) +{ static const int32_t gv_adapt_min_cmd = GUIDANCE_V_ADAPT_MIN_CMD * MAX_PPRZ; static const int32_t gv_adapt_max_cmd = GUIDANCE_V_ADAPT_MAX_CMD * MAX_PPRZ; @@ -142,11 +144,12 @@ void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref) { gv_adapt_P = gv_adapt_P + GV_ADAPT_SYS_NOISE; /* Compute our measurement. If zdd_meas is in the range +/-5g, meas is less than 30 bits */ - const int32_t g_m_zdd = ((int32_t)BFP_OF_REAL(9.81, INT32_ACCEL_FRAC) - zdd_meas)<<(GV_ADAPT_X_FRAC - INT32_ACCEL_FRAC); - if ( g_m_zdd > 0) { - gv_adapt_Xmeas = (g_m_zdd + (thrust_applied>>1)) / thrust_applied; + const int32_t g_m_zdd = ((int32_t)BFP_OF_REAL(9.81, + INT32_ACCEL_FRAC) - zdd_meas) << (GV_ADAPT_X_FRAC - INT32_ACCEL_FRAC); + if (g_m_zdd > 0) { + gv_adapt_Xmeas = (g_m_zdd + (thrust_applied >> 1)) / thrust_applied; } else { - gv_adapt_Xmeas = (g_m_zdd - (thrust_applied>>1)) / thrust_applied; + gv_adapt_Xmeas = (g_m_zdd - (thrust_applied >> 1)) / thrust_applied; } /* Compute a residual */ @@ -154,21 +157,21 @@ void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref) { /* Covariance Error E = P + R */ int32_t ref = zd_ref >> (INT32_SPEED_FRAC - GV_ADAPT_P_FRAC); - if (zd_ref < 0) ref = -ref; + if (zd_ref < 0) { ref = -ref; } int32_t E = gv_adapt_P + GV_ADAPT_MEAS_NOISE_HOVER + ref * GV_ADAPT_MEAS_NOISE_OF_ZD; /* Kalman gain K = P / (P + R) = P / E */ - int32_t K = (gv_adapt_P<>K_FRAC); + gv_adapt_P = gv_adapt_P - ((K * gv_adapt_P) >> K_FRAC); /* Don't let covariance climb over initial value */ if (gv_adapt_P > gv_adapt_P0) { gv_adapt_P = gv_adapt_P0; } /* Update State */ - gv_adapt_X = gv_adapt_X + (((int64_t)(K * residual))>>K_FRAC); + gv_adapt_X = gv_adapt_X + (((int64_t)(K * residual)) >> K_FRAC); /* Output bounds. * Don't let it climb over a value that would @@ -176,8 +179,8 @@ void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref) { * or more than #GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE % throttle. */ static const int32_t max_out = BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC) / - (GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE * MAX_PPRZ); + (GUIDANCE_V_ADAPT_MIN_HOVER_THROTTLE * MAX_PPRZ); static const int32_t min_out = BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC) / - (GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE * MAX_PPRZ); + (GUIDANCE_V_ADAPT_MAX_HOVER_THROTTLE * MAX_PPRZ); Bound(gv_adapt_X, min_out, max_out); } diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.c index 9e315fe3db..0d98ccc0d6 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_ref.c @@ -84,26 +84,28 @@ int64_t gv_z_ref; #define GV_REF_INV_THAU_FRAC 16 #define GV_REF_INV_THAU BFP_OF_REAL((1./0.25), GV_REF_INV_THAU_FRAC) -void gv_set_ref(int32_t alt, int32_t speed, int32_t accel) { - int64_t new_z = ((int64_t)alt)<<(GV_Z_REF_FRAC - INT32_POS_FRAC); +void gv_set_ref(int32_t alt, int32_t speed, int32_t accel) +{ + int64_t new_z = ((int64_t)alt) << (GV_Z_REF_FRAC - INT32_POS_FRAC); gv_z_ref = new_z; - gv_zd_ref = speed>>(INT32_SPEED_FRAC - GV_ZD_REF_FRAC); - gv_zdd_ref = accel>>(INT32_ACCEL_FRAC - GV_ZDD_REF_FRAC); + gv_zd_ref = speed >> (INT32_SPEED_FRAC - GV_ZD_REF_FRAC); + gv_zdd_ref = accel >> (INT32_ACCEL_FRAC - GV_ZDD_REF_FRAC); } -void gv_update_ref_from_z_sp(int32_t z_sp) { +void gv_update_ref_from_z_sp(int32_t z_sp) +{ gv_z_ref += gv_zd_ref; gv_zd_ref += gv_zdd_ref; // compute the "speed part" of zdd = -2*zeta*omega*zd -omega^2(z_sp - z) - int32_t zd_zdd_res = gv_zd_ref>>(GV_ZD_REF_FRAC - GV_ZDD_REF_FRAC); - int32_t zdd_speed = ((int32_t)(-2*GV_ZETA_OMEGA)*zd_zdd_res)>>(GV_ZETA_OMEGA_FRAC); + int32_t zd_zdd_res = gv_zd_ref >> (GV_ZD_REF_FRAC - GV_ZDD_REF_FRAC); + int32_t zdd_speed = ((int32_t)(-2 * GV_ZETA_OMEGA) * zd_zdd_res) >> (GV_ZETA_OMEGA_FRAC); // compute z error in z_sp resolution - int32_t z_err_sp = z_sp - (int32_t)(gv_z_ref>>(GV_Z_REF_FRAC-INT32_POS_FRAC)); + int32_t z_err_sp = z_sp - (int32_t)(gv_z_ref >> (GV_Z_REF_FRAC - INT32_POS_FRAC)); // convert to accel resolution - int32_t z_err_accel = z_err_sp>>(INT32_POS_FRAC-GV_ZDD_REF_FRAC); - int32_t zdd_pos = ((int32_t)(GV_OMEGA_2)*z_err_accel)>>GV_OMEGA_2_FRAC; + int32_t z_err_accel = z_err_sp >> (INT32_POS_FRAC - GV_ZDD_REF_FRAC); + int32_t zdd_pos = ((int32_t)(GV_OMEGA_2) * z_err_accel) >> GV_OMEGA_2_FRAC; gv_zdd_ref = zdd_speed + zdd_pos; /* Saturate accelerations */ @@ -112,18 +114,20 @@ void gv_update_ref_from_z_sp(int32_t z_sp) { /* Saturate speed and adjust acceleration accordingly */ if (gv_zd_ref <= GV_MIN_ZD) { gv_zd_ref = GV_MIN_ZD; - if (gv_zdd_ref < 0) + if (gv_zdd_ref < 0) { gv_zdd_ref = 0; - } - else if (gv_zd_ref >= GV_MAX_ZD) { + } + } else if (gv_zd_ref >= GV_MAX_ZD) { gv_zd_ref = GV_MAX_ZD; - if (gv_zdd_ref > 0) + if (gv_zdd_ref > 0) { gv_zdd_ref = 0; + } } } -void gv_update_ref_from_zd_sp(int32_t zd_sp, int32_t z_pos) { +void gv_update_ref_from_zd_sp(int32_t zd_sp, int32_t z_pos) +{ gv_z_ref += gv_zd_ref; gv_zd_ref += gv_zdd_ref; @@ -132,9 +136,9 @@ void gv_update_ref_from_zd_sp(int32_t zd_sp, int32_t z_pos) { int64_t cur_z = ((int64_t)z_pos) << (GV_Z_REF_FRAC - INT32_POS_FRAC); Bound(gv_z_ref, cur_z - GV_MAX_Z_DIFF, cur_z + GV_MAX_Z_DIFF); - int32_t zd_err = gv_zd_ref - (zd_sp>>(INT32_SPEED_FRAC - GV_ZD_REF_FRAC)); - int32_t zd_err_zdd_res = zd_err>>(GV_ZD_REF_FRAC-GV_ZDD_REF_FRAC); - gv_zdd_ref = (-(int32_t)GV_REF_INV_THAU * zd_err_zdd_res)>>GV_REF_INV_THAU_FRAC; + int32_t zd_err = gv_zd_ref - (zd_sp >> (INT32_SPEED_FRAC - GV_ZD_REF_FRAC)); + int32_t zd_err_zdd_res = zd_err >> (GV_ZD_REF_FRAC - GV_ZDD_REF_FRAC); + gv_zdd_ref = (-(int32_t)GV_REF_INV_THAU * zd_err_zdd_res) >> GV_REF_INV_THAU_FRAC; /* Saturate accelerations */ Bound(gv_zdd_ref, GV_MIN_ZDD, GV_MAX_ZDD); @@ -142,13 +146,14 @@ void gv_update_ref_from_zd_sp(int32_t zd_sp, int32_t z_pos) { /* Saturate speed and adjust acceleration accordingly */ if (gv_zd_ref <= GV_MIN_ZD) { gv_zd_ref = GV_MIN_ZD; - if (gv_zdd_ref < 0) + if (gv_zdd_ref < 0) { gv_zdd_ref = 0; - } - else if (gv_zd_ref >= GV_MAX_ZD) { + } + } else if (gv_zd_ref >= GV_MAX_ZD) { gv_zd_ref = GV_MAX_ZD; - if (gv_zdd_ref > 0) + if (gv_zdd_ref > 0) { gv_zdd_ref = 0; + } } } diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c index 3a292ff228..44dee0a798 100644 --- a/sw/airborne/firmwares/rotorcraft/main.c +++ b/sw/airborne/firmwares/rotorcraft/main.c @@ -109,14 +109,14 @@ PRINT_CONFIG_VAR(BARO_PERIODIC_FREQUENCY) #if USE_AHRS && USE_IMU && (defined AHRS_PROPAGATE_FREQUENCY) #if (AHRS_PROPAGATE_FREQUENCY > PERIODIC_FREQUENCY) #warning "PERIODIC_FREQUENCY should be least equal or greater than AHRS_PROPAGATE_FREQUENCY" -INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY to at least ",AHRS_PROPAGATE_FREQUENCY) +INFO_VALUE("it is recommended to configure in your airframe PERIODIC_FREQUENCY to at least ", AHRS_PROPAGATE_FREQUENCY) #endif #endif -static inline void on_gyro_event( void ); -static inline void on_accel_event( void ); -static inline void on_gps_event( void ); -static inline void on_mag_event( void ); +static inline void on_gyro_event(void); +static inline void on_accel_event(void); +static inline void on_gps_event(void); +static inline void on_mag_event(void); tid_t main_periodic_tid; ///< id for main_periodic() timer @@ -130,10 +130,11 @@ tid_t baro_tid; ///< id for baro_periodic() timer #endif #ifndef SITL -int main( void ) { +int main(void) +{ main_init(); - while(1) { + while (1) { handle_periodic_tasks(); main_event(); } @@ -141,7 +142,8 @@ int main( void ) { } #endif /* SITL */ -STATIC_INLINE void main_init( void ) { +STATIC_INLINE void main_init(void) +{ mcu_init(); @@ -181,37 +183,46 @@ STATIC_INLINE void main_init( void ) { #endif // register the timers for the periodic functions - main_periodic_tid = sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); - modules_tid = sys_time_register_timer(1./MODULES_FREQUENCY, NULL); - radio_control_tid = sys_time_register_timer((1./60.), NULL); + main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); + modules_tid = sys_time_register_timer(1. / MODULES_FREQUENCY, NULL); + radio_control_tid = sys_time_register_timer((1. / 60.), NULL); failsafe_tid = sys_time_register_timer(0.05, NULL); electrical_tid = sys_time_register_timer(0.1, NULL); - telemetry_tid = sys_time_register_timer((1./TELEMETRY_FREQUENCY), NULL); + telemetry_tid = sys_time_register_timer((1. / TELEMETRY_FREQUENCY), NULL); #if USE_BARO_BOARD - baro_tid = sys_time_register_timer(1./BARO_PERIODIC_FREQUENCY, NULL); + baro_tid = sys_time_register_timer(1. / BARO_PERIODIC_FREQUENCY, NULL); #endif } -STATIC_INLINE void handle_periodic_tasks( void ) { - if (sys_time_check_and_ack_timer(main_periodic_tid)) +STATIC_INLINE void handle_periodic_tasks(void) +{ + if (sys_time_check_and_ack_timer(main_periodic_tid)) { main_periodic(); - if (sys_time_check_and_ack_timer(modules_tid)) + } + if (sys_time_check_and_ack_timer(modules_tid)) { modules_periodic_task(); - if (sys_time_check_and_ack_timer(radio_control_tid)) + } + if (sys_time_check_and_ack_timer(radio_control_tid)) { radio_control_periodic_task(); - if (sys_time_check_and_ack_timer(failsafe_tid)) + } + if (sys_time_check_and_ack_timer(failsafe_tid)) { failsafe_check(); - if (sys_time_check_and_ack_timer(electrical_tid)) + } + if (sys_time_check_and_ack_timer(electrical_tid)) { electrical_periodic(); - if (sys_time_check_and_ack_timer(telemetry_tid)) + } + if (sys_time_check_and_ack_timer(telemetry_tid)) { telemetry_periodic(); + } #if USE_BARO_BOARD - if (sys_time_check_and_ack_timer(baro_tid)) + if (sys_time_check_and_ack_timer(baro_tid)) { baro_periodic(); + } #endif } -STATIC_INLINE void main_periodic( void ) { +STATIC_INLINE void main_periodic(void) +{ imu_periodic(); @@ -224,15 +235,16 @@ STATIC_INLINE void main_periodic( void ) { if (autopilot_in_flight) { RunOnceEvery(PERIODIC_FREQUENCY, { autopilot_flight_time++; #if defined DATALINK || defined SITL - datalink_time++; + datalink_time++; #endif - }); + }); } RunOnceEvery(10, LED_PERIODIC()); } -STATIC_INLINE void telemetry_periodic(void) { +STATIC_INLINE void telemetry_periodic(void) +{ #if PERIODIC_TELEMETRY periodic_telemetry_send_Main(&(DefaultChannel).trans_tx, &(DefaultDevice).device); #endif @@ -243,20 +255,19 @@ STATIC_INLINE void telemetry_periodic(void) { #define RC_LOST_MODE AP_MODE_FAILSAFE #endif -STATIC_INLINE void failsafe_check( void ) { +STATIC_INLINE void failsafe_check(void) +{ if (radio_control.status == RC_REALLY_LOST && autopilot_mode != AP_MODE_KILL && autopilot_mode != AP_MODE_HOME && autopilot_mode != AP_MODE_FAILSAFE && - autopilot_mode != AP_MODE_NAV) - { + autopilot_mode != AP_MODE_NAV) { autopilot_set_mode(RC_LOST_MODE); } #if FAILSAFE_ON_BAT_CRITICAL if (autopilot_mode != AP_MODE_KILL && - electrical.bat_critical) - { + electrical.bat_critical) { autopilot_set_mode(AP_MODE_FAILSAFE); } #endif @@ -268,14 +279,12 @@ STATIC_INLINE void failsafe_check( void ) { #if NO_GPS_LOST_WITH_RC_VALID radio_control.status != RC_OK && #endif - GpsIsLost()) - { + GpsIsLost()) { autopilot_set_mode(AP_MODE_FAILSAFE); } if (autopilot_mode == AP_MODE_HOME && - autopilot_motors_on && GpsIsLost()) - { + autopilot_motors_on && GpsIsLost()) { autopilot_set_mode(AP_MODE_FAILSAFE); } #endif @@ -283,7 +292,8 @@ STATIC_INLINE void failsafe_check( void ) { autopilot_check_in_flight(autopilot_motors_on); } -STATIC_INLINE void main_event( void ) { +STATIC_INLINE void main_event(void) +{ i2c_event(); @@ -323,9 +333,10 @@ STATIC_INLINE void main_event( void ) { } -static inline void on_accel_event( void ) { +static inline void on_accel_event(void) +{ #if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY) -PRINT_CONFIG_MSG("Calculating dt for AHRS accel update.") + PRINT_CONFIG_MSG("Calculating dt for AHRS accel update.") // timestamp in usec when last callback was received static uint32_t last_ts = 0; // current timestamp @@ -334,8 +345,8 @@ PRINT_CONFIG_MSG("Calculating dt for AHRS accel update.") float dt = (float)(now_ts - last_ts) / 1e6; last_ts = now_ts; #else -PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS accel update.") -PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY) + PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS accel update.") + PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY) const float dt = 1. / (AHRS_CORRECT_FREQUENCY); #endif @@ -346,9 +357,10 @@ PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY) } } -static inline void on_gyro_event( void ) { +static inline void on_gyro_event(void) +{ #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY) -PRINT_CONFIG_MSG("Calculating dt for AHRS/INS propagation.") + PRINT_CONFIG_MSG("Calculating dt for AHRS/INS propagation.") // timestamp in usec when last callback was received static uint32_t last_ts = 0; // current timestamp @@ -357,8 +369,8 @@ PRINT_CONFIG_MSG("Calculating dt for AHRS/INS propagation.") float dt = (float)(now_ts - last_ts) / 1e6; last_ts = now_ts; #else -PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS/INS propagation.") -PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) + PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS/INS propagation.") + PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY); #endif @@ -366,13 +378,13 @@ PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) if (ahrs.status == AHRS_UNINIT) { ahrs_aligner_run(); - if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) + if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) { ahrs_align(); - } - else { + } + } else { ahrs_propagate(dt); #ifdef SITL - if (nps_bypass_ahrs) sim_overwrite_ahrs(); + if (nps_bypass_ahrs) { sim_overwrite_ahrs(); } #endif ins_propagate(dt); } @@ -381,21 +393,24 @@ PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY) #endif } -static inline void on_gps_event(void) { +static inline void on_gps_event(void) +{ ahrs_update_gps(); ins_update_gps(); #ifdef USE_VEHICLE_INTERFACE - if (gps.fix == GPS_FIX_3D) + if (gps.fix == GPS_FIX_3D) { vi_notify_gps_available(); + } #endif } -static inline void on_mag_event(void) { +static inline void on_mag_event(void) +{ imu_scale_mag(&imu); #if USE_MAGNETOMETER #if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY) -PRINT_CONFIG_MSG("Calculating dt for AHRS mag update.") + PRINT_CONFIG_MSG("Calculating dt for AHRS mag update.") // timestamp in usec when last callback was received static uint32_t last_ts = 0; // current timestamp @@ -404,8 +419,8 @@ PRINT_CONFIG_MSG("Calculating dt for AHRS mag update.") float dt = (float)(now_ts - last_ts) / 1e6; last_ts = now_ts; #else -PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS mag update.") -PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) + PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS mag update.") + PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY); #endif diff --git a/sw/airborne/firmwares/rotorcraft/main.h b/sw/airborne/firmwares/rotorcraft/main.h index 7f5486c615..2db7f0cdbd 100644 --- a/sw/airborne/firmwares/rotorcraft/main.h +++ b/sw/airborne/firmwares/rotorcraft/main.h @@ -34,13 +34,13 @@ #define STATIC_INLINE static inline #endif -STATIC_INLINE void main_init( void ); -STATIC_INLINE void main_event( void ); -STATIC_INLINE void handle_periodic_tasks( void ); +STATIC_INLINE void main_init(void); +STATIC_INLINE void main_event(void); +STATIC_INLINE void handle_periodic_tasks(void); -STATIC_INLINE void main_periodic( void ); +STATIC_INLINE void main_periodic(void); STATIC_INLINE void telemetry_periodic(void); -STATIC_INLINE void failsafe_check( void ); +STATIC_INLINE void failsafe_check(void); #endif /* MAIN_H */ diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index 2dee31333d..187f37462d 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -93,7 +93,7 @@ uint32_t nav_throttle; int32_t nav_climb, nav_altitude, nav_flight_altitude; float flight_altitude; -static inline void nav_set_altitude( void ); +static inline void nav_set_altitude(void); #define CLOSE_TO_WAYPOINT (15 << 8) #define CARROT_DIST (12 << 8) @@ -106,22 +106,22 @@ static inline void nav_set_altitude( void ); #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_nav_status(struct transport_tx *trans, struct link_device *dev) { +static void send_nav_status(struct transport_tx *trans, struct link_device *dev) +{ float dist_home = sqrtf(dist2_to_home); float dist_wp = sqrtf(dist2_to_wp); pprz_msg_send_ROTORCRAFT_NAV_STATUS(trans, dev, AC_ID, - &block_time, &stage_time, - &dist_home, &dist_wp, - &nav_block, &nav_stage, - &horizontal_mode); + &block_time, &stage_time, + &dist_home, &dist_wp, + &nav_block, &nav_stage, + &horizontal_mode); if (horizontal_mode == HORIZONTAL_MODE_ROUTE) { float sx = POS_FLOAT_OF_BFP(nav_segment_start.x); float sy = POS_FLOAT_OF_BFP(nav_segment_start.y); float ex = POS_FLOAT_OF_BFP(nav_segment_end.x); float ey = POS_FLOAT_OF_BFP(nav_segment_end.y); pprz_msg_send_SEGMENT(trans, dev, AC_ID, &sx, &sy, &ex, &ey); - } - else if (horizontal_mode == HORIZONTAL_MODE_CIRCLE) { + } else if (horizontal_mode == HORIZONTAL_MODE_CIRCLE) { float cx = POS_FLOAT_OF_BFP(nav_circle_center.x); float cy = POS_FLOAT_OF_BFP(nav_circle_center.y); float r = POS_FLOAT_OF_BFP(nav_circle_radius); @@ -129,18 +129,21 @@ static void send_nav_status(struct transport_tx *trans, struct link_device *dev) } } -static void send_wp_moved(struct transport_tx *trans, struct link_device *dev) { +static void send_wp_moved(struct transport_tx *trans, struct link_device *dev) +{ static uint8_t i; - i++; if (i >= nb_waypoint) i = 0; + i++; + if (i >= nb_waypoint) { i = 0; } pprz_msg_send_WP_MOVED_ENU(trans, dev, AC_ID, - &i, - &(waypoints[i].x), - &(waypoints[i].y), - &(waypoints[i].z)); + &i, + &(waypoints[i].x), + &(waypoints[i].y), + &(waypoints[i].z)); } #endif -void nav_init(void) { +void nav_init(void) +{ // convert to const struct EnuCoor_f wp_tmp_float[NB_WAYPOINT] = WAYPOINTS_ENU; // init int32 waypoints @@ -180,21 +183,21 @@ void nav_init(void) { #endif } -static inline void UNUSED nav_advance_carrot(void) { +static inline void UNUSED nav_advance_carrot(void) +{ struct EnuCoor_i *pos = stateGetPositionEnu_i(); /* compute a vector to the waypoint */ struct Int32Vect2 path_to_waypoint; VECT2_DIFF(path_to_waypoint, navigation_target, *pos); /* saturate it */ - VECT2_STRIM(path_to_waypoint, -(1<<15), (1<<15)); + VECT2_STRIM(path_to_waypoint, -(1 << 15), (1 << 15)); int32_t dist_to_waypoint = int32_vect2_norm(&path_to_waypoint); if (dist_to_waypoint < CLOSE_TO_WAYPOINT) { VECT2_COPY(navigation_carrot, navigation_target); - } - else { + } else { struct Int32Vect2 path_to_carrot; VECT2_SMUL(path_to_carrot, path_to_waypoint, CARROT_DIST); VECT2_SDIV(path_to_carrot, path_to_carrot, dist_to_waypoint); @@ -202,7 +205,8 @@ static inline void UNUSED nav_advance_carrot(void) { } } -void nav_run(void) { +void nav_run(void) +{ #if GUIDANCE_H_USE_REF // if GUIDANCE_H_USE_REF, CARROT_DIST is not used @@ -214,12 +218,12 @@ void nav_run(void) { nav_set_altitude(); } -void nav_circle(struct EnuCoor_i * wp_center, int32_t radius) { +void nav_circle(struct EnuCoor_i *wp_center, int32_t radius) +{ if (radius == 0) { VECT2_COPY(navigation_target, *wp_center); dist2_to_wp = get_dist2_to_point(wp_center); - } - else { + } else { struct Int32Vect2 pos_diff; VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), *wp_center); // go back to half metric precision or values are too large @@ -233,8 +237,7 @@ void nav_circle(struct EnuCoor_i * wp_center, int32_t radius) { int32_t angle_diff = nav_circle_qdr - last_qdr; INT32_ANGLE_NORMALIZE(angle_diff); nav_circle_radians += angle_diff; - } - else { + } else { // Smallest angle to increment at next step nav_circle_radians = 1; } @@ -244,7 +247,7 @@ void nav_circle(struct EnuCoor_i * wp_center, int32_t radius) { // absolute radius int32_t abs_radius = abs(radius); // carrot_angle - int32_t carrot_angle = ((CARROT_DIST<> INT32_POS_FRAC), 0); @@ -277,7 +281,7 @@ void nav_route(struct EnuCoor_i * wp_start, struct EnuCoor_i * wp_end) { int32_t prog_2 = nav_leg_length; Bound(nav_leg_progress, 0, prog_2); struct Int32Vect2 progress_pos; - VECT2_SMUL(progress_pos, wp_diff_prec, ((float)nav_leg_progress)/nav_leg_length); + VECT2_SMUL(progress_pos, wp_diff_prec, ((float)nav_leg_progress) / nav_leg_length); VECT2_SUM(navigation_target, *wp_start, progress_pos); nav_segment_start = *wp_start; @@ -287,16 +291,17 @@ void nav_route(struct EnuCoor_i * wp_start, struct EnuCoor_i * wp_end) { dist2_to_wp = get_dist2_to_point(wp_end); } -bool_t nav_approaching_from(struct EnuCoor_i * wp, struct EnuCoor_i * from, int16_t approaching_time) { +bool_t nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time) +{ int32_t dist_to_point; struct Int32Vect2 diff; - struct EnuCoor_i* pos = stateGetPositionEnu_i(); + struct EnuCoor_i *pos = stateGetPositionEnu_i(); /* if an approaching_time is given, estimate diff after approching_time secs */ if (approaching_time > 0) { struct Int32Vect2 estimated_pos; struct Int32Vect2 estimated_progress; - struct EnuCoor_i* speed = stateGetSpeedEnu_i(); + struct EnuCoor_i *speed = stateGetSpeedEnu_i(); VECT2_SMUL(estimated_progress, *speed, approaching_time); INT32_VECT2_RSHIFT(estimated_progress, estimated_progress, (INT32_SPEED_FRAC - INT32_POS_FRAC)); VECT2_SUM(estimated_pos, *pos, estimated_progress); @@ -309,26 +314,28 @@ bool_t nav_approaching_from(struct EnuCoor_i * wp, struct EnuCoor_i * from, int1 /* compute distance of estimated/current pos to target wp * distance with half metric precision (6.25 cm) */ - INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC/2); + INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC / 2); dist_to_point = int32_vect2_norm(&diff); /* return TRUE if we have arrived */ - if (dist_to_point < BFP_OF_REAL(ARRIVED_AT_WAYPOINT, INT32_POS_FRAC/2)) + if (dist_to_point < BFP_OF_REAL(ARRIVED_AT_WAYPOINT, INT32_POS_FRAC / 2)) { return TRUE; + } /* if coming from a valid waypoint */ if (from != NULL) { /* return TRUE if normal line at the end of the segment is crossed */ struct Int32Vect2 from_diff; VECT2_DIFF(from_diff, *wp, *from); - INT32_VECT2_RSHIFT(from_diff, from_diff, INT32_POS_FRAC/2); + INT32_VECT2_RSHIFT(from_diff, from_diff, INT32_POS_FRAC / 2); return (diff.x * from_diff.x + diff.y * from_diff.y < 0); } return FALSE; } -bool_t nav_check_wp_time(struct EnuCoor_i * wp, uint16_t stay_time) { +bool_t nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time) +{ uint16_t time_at_wp; uint32_t dist_to_point; static uint16_t wp_entry_time = 0; @@ -341,19 +348,17 @@ bool_t nav_check_wp_time(struct EnuCoor_i * wp, uint16_t stay_time) { wp_last = *wp; } VECT2_DIFF(diff, *wp, *stateGetPositionEnu_i()); - INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC/2); + INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC / 2); dist_to_point = int32_vect2_norm(&diff); - if (dist_to_point < BFP_OF_REAL(ARRIVED_AT_WAYPOINT, INT32_POS_FRAC/2)){ + if (dist_to_point < BFP_OF_REAL(ARRIVED_AT_WAYPOINT, INT32_POS_FRAC / 2)) { if (!wp_reached) { wp_reached = TRUE; wp_entry_time = autopilot_flight_time; time_at_wp = 0; - } - else { + } else { time_at_wp = autopilot_flight_time - wp_entry_time; } - } - else { + } else { time_at_wp = 0; wp_reached = FALSE; } @@ -364,7 +369,8 @@ bool_t nav_check_wp_time(struct EnuCoor_i * wp, uint16_t stay_time) { return FALSE; } -static inline void nav_set_altitude( void ) { +static inline void nav_set_altitude(void) +{ static int32_t last_nav_alt = 0; if (abs(nav_altitude - last_nav_alt) > (POS_BFP_OF_REAL(0.2))) { nav_flight_altitude = nav_altitude; @@ -374,17 +380,20 @@ static inline void nav_set_altitude( void ) { /** Reset the geographic reference to the current GPS fix */ -unit_t nav_reset_reference( void ) { +unit_t nav_reset_reference(void) +{ ins_reset_local_origin(); return 0; } -unit_t nav_reset_alt( void ) { +unit_t nav_reset_alt(void) +{ ins_reset_altitude_ref(); return 0; } -void nav_init_stage( void ) { +void nav_init_stage(void) +{ VECT3_COPY(nav_last_point, *stateGetPositionEnu_i()); stage_time = 0; nav_circle_radians = 0; @@ -392,7 +401,8 @@ void nav_init_stage( void ) { } #include -void nav_periodic_task(void) { +void nav_periodic_task(void) +{ RunOnceEvery(NAV_FREQ, { stage_time++; block_time++; }); dist2_to_wp = 0; @@ -404,27 +414,30 @@ void nav_periodic_task(void) { nav_run(); } -void nav_move_waypoint_lla(uint8_t wp_id, struct LlaCoor_i* new_lla_pos) { +void nav_move_waypoint_lla(uint8_t wp_id, struct LlaCoor_i *new_lla_pos) +{ if (stateIsLocalCoordinateValid()) { struct EnuCoor_i enu; enu_of_lla_point_i(&enu, &state.ned_origin_i, new_lla_pos); // convert ENU pos from cm to BFP with INT32_POS_FRAC - enu.x = POS_BFP_OF_REAL(enu.x)/100; - enu.y = POS_BFP_OF_REAL(enu.y)/100; - enu.z = POS_BFP_OF_REAL(enu.z)/100; + enu.x = POS_BFP_OF_REAL(enu.x) / 100; + enu.y = POS_BFP_OF_REAL(enu.y) / 100; + enu.z = POS_BFP_OF_REAL(enu.z) / 100; nav_move_waypoint(wp_id, &enu); } } -void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos) { +void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i *new_pos) +{ if (wp_id < nb_waypoint) { - VECT3_COPY(waypoints[wp_id],(*new_pos)); + VECT3_COPY(waypoints[wp_id], (*new_pos)); DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &(new_pos->x), &(new_pos->y), &(new_pos->z)); } } -void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int16_t heading_rate_sp ) { +void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int16_t heading_rate_sp) +{ // MY_ASSERT(wp < nb_waypoint); FIXME int32_t s_heading, c_heading; PPRZ_ITRIG_SIN(s_heading, nav_heading); @@ -432,30 +445,34 @@ void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int // FIXME : scale POS to SPEED struct Int32Vect3 delta_pos; VECT3_SDIV(delta_pos, speed_sp, NAV_FREQ); /* fixme :make sure the division is really a >> */ - INT32_VECT3_RSHIFT(delta_pos, delta_pos, (INT32_SPEED_FRAC-INT32_POS_FRAC)); + INT32_VECT3_RSHIFT(delta_pos, delta_pos, (INT32_SPEED_FRAC - INT32_POS_FRAC)); waypoints[wp].x += (s_heading * delta_pos.x + c_heading * delta_pos.y) >> INT32_TRIG_FRAC; waypoints[wp].y += (c_heading * delta_pos.x - s_heading * delta_pos.y) >> INT32_TRIG_FRAC; waypoints[wp].z += delta_pos.z; int32_t delta_heading = heading_rate_sp / NAV_FREQ; - delta_heading = delta_heading >> (INT32_SPEED_FRAC-INT32_POS_FRAC); + delta_heading = delta_heading >> (INT32_SPEED_FRAC - INT32_POS_FRAC); nav_heading += delta_heading; INT32_COURSE_NORMALIZE(nav_heading); - RunOnceEvery(10,DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp, &(waypoints[wp].x), &(waypoints[wp].y), &(waypoints[wp].z))); + RunOnceEvery(10, DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp, &(waypoints[wp].x), &(waypoints[wp].y), + &(waypoints[wp].z))); } -bool_t nav_detect_ground(void) { - if (!autopilot_ground_detected) return FALSE; +bool_t nav_detect_ground(void) +{ + if (!autopilot_ground_detected) { return FALSE; } autopilot_ground_detected = FALSE; return TRUE; } -bool_t nav_is_in_flight(void) { +bool_t nav_is_in_flight(void) +{ return autopilot_in_flight; } /** Home mode navigation */ -void nav_home(void) { +void nav_home(void) +{ horizontal_mode = HORIZONTAL_MODE_WAYPOINT; VECT3_COPY(navigation_target, waypoints[WP_HOME]); @@ -470,8 +487,9 @@ void nav_home(void) { } /** Returns squared horizontal distance to given point */ -float get_dist2_to_point(struct EnuCoor_i *p) { - struct EnuCoor_f* pos = stateGetPositionEnu_f(); +float get_dist2_to_point(struct EnuCoor_i *p) +{ + struct EnuCoor_f *pos = stateGetPositionEnu_f(); struct FloatVect2 pos_diff; pos_diff.x = POS_FLOAT_OF_BFP(p->x) - pos->x; pos_diff.y = POS_FLOAT_OF_BFP(p->y) - pos->y; @@ -479,32 +497,37 @@ float get_dist2_to_point(struct EnuCoor_i *p) { } /** Returns squared horizontal distance to given waypoint */ -float get_dist2_to_waypoint(uint8_t wp_id) { +float get_dist2_to_waypoint(uint8_t wp_id) +{ return get_dist2_to_point(&waypoints[wp_id]); } /** Computes squared distance to the HOME waypoint potentially sets * #too_far_from_home */ -void compute_dist2_to_home(void) { +void compute_dist2_to_home(void) +{ dist2_to_home = get_dist2_to_waypoint(WP_HOME); too_far_from_home = dist2_to_home > max_dist2_from_home; } /** Set nav_heading in degrees. */ -bool_t nav_set_heading_rad(float rad) { +bool_t nav_set_heading_rad(float rad) +{ nav_heading = ANGLE_BFP_OF_REAL(rad); INT32_COURSE_NORMALIZE(nav_heading); return FALSE; } /** Set nav_heading in degrees. */ -bool_t nav_set_heading_deg(float deg) { +bool_t nav_set_heading_deg(float deg) +{ return nav_set_heading_rad(RadOfDeg(deg)); } /** Set heading to point towards x,y position in local coordinates */ -bool_t nav_set_heading_towards(float x, float y) { +bool_t nav_set_heading_towards(float x, float y) +{ struct FloatVect2 target = {x, y}; struct FloatVect2 pos_diff; VECT2_DIFF(pos_diff, target, *stateGetPositionEnu_f()); @@ -519,12 +542,14 @@ bool_t nav_set_heading_towards(float x, float y) { } /** Set heading in the direction of a waypoint */ -bool_t nav_set_heading_towards_waypoint(uint8_t wp) { +bool_t nav_set_heading_towards_waypoint(uint8_t wp) +{ return nav_set_heading_towards(WaypointX(wp), WaypointY(wp)); } /** Set heading to the current yaw angle */ -bool_t nav_set_heading_current(void) { +bool_t nav_set_heading_current(void) +{ nav_heading = stateGetNedToBodyEulers_i()->psi; return FALSE; } diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h index 3712db21e9..16930a506a 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.h +++ b/sw/airborne/firmwares/rotorcraft/navigation.h @@ -45,7 +45,7 @@ extern const uint8_t nb_waypoint; extern void nav_init(void); extern void nav_run(void); -extern uint8_t last_wp __attribute__ ((unused)); +extern uint8_t last_wp __attribute__((unused)); extern uint8_t horizontal_mode; extern struct EnuCoor_i nav_segment_start, nav_segment_end; @@ -81,11 +81,11 @@ extern float get_dist2_to_point(struct EnuCoor_i *p); extern void compute_dist2_to_home(void); extern void nav_home(void); -unit_t nav_reset_reference( void ) __attribute__ ((unused)); -unit_t nav_reset_alt( void ) __attribute__ ((unused)); +unit_t nav_reset_reference(void) __attribute__((unused)); +unit_t nav_reset_alt(void) __attribute__((unused)); void nav_periodic_task(void); -void nav_move_waypoint_lla(uint8_t wp_id, struct LlaCoor_i* new_lla_pos); -void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos); +void nav_move_waypoint_lla(uint8_t wp_id, struct LlaCoor_i *new_lla_pos); +void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i *new_pos); bool_t nav_detect_ground(void); bool_t nav_is_in_flight(void); @@ -117,23 +117,23 @@ extern bool_t nav_set_heading_current(void); /** Normalize a degree angle between 0 and 359 */ #define NormCourse(x) { \ - while (x < 0) x += 360; \ - while (x >= 360) x -= 360; \ -} + while (x < 0) x += 360; \ + while (x >= 360) x -= 360; \ + } /*********** Navigation to waypoint *************************************/ #define NavGotoWaypoint(_wp) { \ - horizontal_mode = HORIZONTAL_MODE_WAYPOINT; \ - VECT3_COPY(navigation_target, waypoints[_wp]); \ - dist2_to_wp = get_dist2_to_waypoint(_wp); \ -} + horizontal_mode = HORIZONTAL_MODE_WAYPOINT; \ + VECT3_COPY(navigation_target, waypoints[_wp]); \ + dist2_to_wp = get_dist2_to_waypoint(_wp); \ + } /*********** Navigation on a circle **************************************/ -extern void nav_circle(struct EnuCoor_i * wp_center, int32_t radius); +extern void nav_circle(struct EnuCoor_i *wp_center, int32_t radius); #define NavCircleWaypoint(_center, _radius) { \ - horizontal_mode = HORIZONTAL_MODE_CIRCLE; \ - nav_circle(&waypoints[_center], POS_BFP_OF_REAL(_radius)); \ -} + horizontal_mode = HORIZONTAL_MODE_CIRCLE; \ + nav_circle(&waypoints[_center], POS_BFP_OF_REAL(_radius)); \ + } #define NavCircleCount() ((float)abs(nav_circle_radians) / INT32_ANGLE_2_PI) #define NavCircleQdr() ({ int32_t qdr = INT32_DEG_OF_RAD(INT32_ANGLE_2_PI_2 - nav_circle_qdr) >> INT32_ANGLE_FRAC; NormCourse(qdr); qdr; }) @@ -143,34 +143,34 @@ extern void nav_circle(struct EnuCoor_i * wp_center, int32_t radius); #define NavCourseCloseTo(x) {} /*********** Navigation along a line *************************************/ -extern void nav_route(struct EnuCoor_i * wp_start, struct EnuCoor_i * wp_end); +extern void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end); #define NavSegment(_start, _end) { \ - horizontal_mode = HORIZONTAL_MODE_ROUTE; \ - nav_route(&waypoints[_start], &waypoints[_end]); \ -} + horizontal_mode = HORIZONTAL_MODE_ROUTE; \ + nav_route(&waypoints[_start], &waypoints[_end]); \ + } /** Nav glide routine */ #define NavGlide(_last_wp, _wp) { \ - int32_t start_alt = waypoints[_last_wp].z; \ - int32_t diff_alt = waypoints[_wp].z - start_alt; \ - int32_t alt = start_alt + ((diff_alt * nav_leg_progress) / nav_leg_length); \ - NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(alt),0); \ -} + int32_t start_alt = waypoints[_last_wp].z; \ + int32_t diff_alt = waypoints[_wp].z - start_alt; \ + int32_t alt = start_alt + ((diff_alt * nav_leg_progress) / nav_leg_length); \ + NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(alt),0); \ + } /** Proximity tests on approaching a wp */ -bool_t nav_approaching_from(struct EnuCoor_i * wp, struct EnuCoor_i * from, int16_t approaching_time); +bool_t nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time); #define NavApproaching(wp, time) nav_approaching_from(&waypoints[wp], NULL, time) #define NavApproachingFrom(wp, from, time) nav_approaching_from(&waypoints[wp], &waypoints[from], time) /** Check the time spent in a radius of 'ARRIVED_AT_WAYPOINT' around a wp */ -bool_t nav_check_wp_time(struct EnuCoor_i * wp, uint16_t stay_time); +bool_t nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time); #define NavCheckWaypointTime(wp, time) nav_check_wp_time(&waypoints[wp], time) /** Set the climb control to auto-throttle with the specified pitch pre-command */ #define NavVerticalAutoThrottleMode(_pitch) { \ - nav_pitch = ANGLE_BFP_OF_REAL(_pitch); \ -} + nav_pitch = ANGLE_BFP_OF_REAL(_pitch); \ + } /** Set the climb control to auto-pitch with the specified throttle pre-command */ @@ -179,28 +179,28 @@ bool_t nav_check_wp_time(struct EnuCoor_i * wp, uint16_t stay_time); /** Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command. */ #define NavVerticalAltitudeMode(_alt, _pre_climb) { \ - vertical_mode = VERTICAL_MODE_ALT; \ - nav_altitude = POS_BFP_OF_REAL(_alt); \ -} + vertical_mode = VERTICAL_MODE_ALT; \ + nav_altitude = POS_BFP_OF_REAL(_alt); \ + } /** Set the vertical mode to climb control with the specified climb setpoint */ #define NavVerticalClimbMode(_climb) { \ - vertical_mode = VERTICAL_MODE_CLIMB; \ - nav_climb = SPEED_BFP_OF_REAL(_climb); \ -} + vertical_mode = VERTICAL_MODE_CLIMB; \ + nav_climb = SPEED_BFP_OF_REAL(_climb); \ + } /** Set the vertical mode to fixed throttle with the specified setpoint */ #define NavVerticalThrottleMode(_throttle) { \ - vertical_mode = VERTICAL_MODE_MANUAL; \ - nav_throttle = _throttle; \ -} + vertical_mode = VERTICAL_MODE_MANUAL; \ + nav_throttle = _throttle; \ + } #define NavHeading(_course) {} #define NavAttitude(_roll) { \ - horizontal_mode = HORIZONTAL_MODE_ATTITUDE; \ - nav_roll = ANGLE_BFP_OF_REAL(_roll); \ -} + horizontal_mode = HORIZONTAL_MODE_ATTITUDE; \ + nav_roll = ANGLE_BFP_OF_REAL(_roll); \ + } #define NavStartDetectGround() ({ autopilot_detect_ground_once = TRUE; FALSE; }) #define NavDetectGround() nav_detect_ground() @@ -211,9 +211,9 @@ bool_t nav_check_wp_time(struct EnuCoor_i * wp, uint16_t stay_time); #define navigation_SetFlightAltitude(x) { \ - flight_altitude = x; \ - nav_flight_altitude = POS_BFP_OF_REAL(flight_altitude - state.ned_origin_f.hmsl); \ -} + flight_altitude = x; \ + nav_flight_altitude = POS_BFP_OF_REAL(flight_altitude - state.ned_origin_f.hmsl); \ + } #define GetPosX() (stateGetPositionEnu_f()->x) @@ -222,6 +222,6 @@ bool_t nav_check_wp_time(struct EnuCoor_i * wp, uint16_t stay_time); #define GetAltRef() (state.ned_origin_f.hmsl) -extern void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int16_t heading_rate_sp ); +extern void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int16_t heading_rate_sp); #endif /* NAVIGATION_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization.c b/sw/airborne/firmwares/rotorcraft/stabilization.c index 0a97a72ab5..748d602cf2 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization.c @@ -27,8 +27,9 @@ int32_t stabilization_cmd[COMMANDS_NB]; -void stabilization_init(void) { - for (uint8_t i=0; ip), &(body_rate->q), &(body_rate->r), - &(att->phi), &(att->theta), &(att->psi), - &stab_att_sp_euler.phi, - &stab_att_sp_euler.theta, - &stab_att_sp_euler.psi, - &stabilization_att_sum_err.phi, - &stabilization_att_sum_err.theta, - &stabilization_att_sum_err.psi, - &stabilization_att_fb_cmd[COMMAND_ROLL], - &stabilization_att_fb_cmd[COMMAND_PITCH], - &stabilization_att_fb_cmd[COMMAND_YAW], - &stabilization_att_ff_cmd[COMMAND_ROLL], - &stabilization_att_ff_cmd[COMMAND_PITCH], - &stabilization_att_ff_cmd[COMMAND_YAW], - &stabilization_cmd[COMMAND_ROLL], - &stabilization_cmd[COMMAND_PITCH], - &stabilization_cmd[COMMAND_YAW], - &foo, &foo, &foo); + &(body_rate->p), &(body_rate->q), &(body_rate->r), + &(att->phi), &(att->theta), &(att->psi), + &stab_att_sp_euler.phi, + &stab_att_sp_euler.theta, + &stab_att_sp_euler.psi, + &stabilization_att_sum_err.phi, + &stabilization_att_sum_err.theta, + &stabilization_att_sum_err.psi, + &stabilization_att_fb_cmd[COMMAND_ROLL], + &stabilization_att_fb_cmd[COMMAND_PITCH], + &stabilization_att_fb_cmd[COMMAND_YAW], + &stabilization_att_ff_cmd[COMMAND_ROLL], + &stabilization_att_ff_cmd[COMMAND_PITCH], + &stabilization_att_ff_cmd[COMMAND_YAW], + &stabilization_cmd[COMMAND_ROLL], + &stabilization_cmd[COMMAND_PITCH], + &stabilization_cmd[COMMAND_YAW], + &foo, &foo, &foo); } -static void send_att_ref(struct transport_tx *trans, struct link_device *dev) { +static void send_att_ref(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_STAB_ATTITUDE_REF_FLOAT(trans, dev, AC_ID, - &stab_att_sp_euler.phi, - &stab_att_sp_euler.theta, - &stab_att_sp_euler.psi, - &stab_att_ref_euler.phi, - &stab_att_ref_euler.theta, - &stab_att_ref_euler.psi, - &stab_att_ref_rate.p, - &stab_att_ref_rate.q, - &stab_att_ref_rate.r, - &stab_att_ref_accel.p, - &stab_att_ref_accel.q, - &stab_att_ref_accel.r); + &stab_att_sp_euler.phi, + &stab_att_sp_euler.theta, + &stab_att_sp_euler.psi, + &stab_att_ref_euler.phi, + &stab_att_ref_euler.theta, + &stab_att_ref_euler.psi, + &stab_att_ref_rate.p, + &stab_att_ref_rate.q, + &stab_att_ref_rate.r, + &stab_att_ref_accel.p, + &stab_att_ref_accel.q, + &stab_att_ref_accel.r); } #endif -void stabilization_attitude_init(void) { +void stabilization_attitude_init(void) +{ stabilization_attitude_ref_init(); @@ -110,7 +113,7 @@ void stabilization_attitude_init(void) { STABILIZATION_ATTITUDE_THETA_DDGAIN, STABILIZATION_ATTITUDE_PSI_DDGAIN); - FLOAT_EULERS_ZERO( stabilization_att_sum_err ); + FLOAT_EULERS_ZERO(stabilization_att_sum_err); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att); @@ -118,11 +121,13 @@ void stabilization_attitude_init(void) { #endif } -void stabilization_attitude_read_rc(bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) { +void stabilization_attitude_read_rc(bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) +{ stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn); } -void stabilization_attitude_enter(void) { +void stabilization_attitude_enter(void) +{ /* reset psi setpoint to current psi angle */ stab_att_sp_euler.psi = stabilization_attitude_get_heading_f(); @@ -132,17 +137,20 @@ void stabilization_attitude_enter(void) { FLOAT_EULERS_ZERO(stabilization_att_sum_err); } -void stabilization_attitude_set_failsafe_setpoint(void) { +void stabilization_attitude_set_failsafe_setpoint(void) +{ stab_att_sp_euler.phi = 0.0; stab_att_sp_euler.theta = 0.0; stab_att_sp_euler.psi = stateGetNedToBodyEulers_f()->psi; } -void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy) { +void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy) +{ EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *rpy); } -void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) { +void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) +{ struct FloatVect2 cmd_f; cmd_f.x = ANGLE_FLOAT_OF_BFP(cmd->x); cmd_f.y = ANGLE_FLOAT_OF_BFP(cmd->y); @@ -158,7 +166,8 @@ void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t head #define MAX_SUM_ERR 200 -void stabilization_attitude_run(bool_t in_flight) { +void stabilization_attitude_run(bool_t in_flight) +{ stabilization_attitude_ref_update(); @@ -181,13 +190,12 @@ void stabilization_attitude_run(bool_t in_flight) { /* update integrator */ EULERS_ADD(stabilization_att_sum_err, att_err); EULERS_BOUND_CUBE(stabilization_att_sum_err, -MAX_SUM_ERR, MAX_SUM_ERR); - } - else { + } else { FLOAT_EULERS_ZERO(stabilization_att_sum_err); } /* rate error */ - struct FloatRates* rate_float = stateGetBodyRates_f(); + struct FloatRates *rate_float = stateGetBodyRates_f(); struct FloatRates rate_err; RATES_DIFF(rate_err, stab_att_ref_rate, *rate_float); @@ -210,11 +218,11 @@ void stabilization_attitude_run(bool_t in_flight) { stabilization_cmd[COMMAND_ROLL] = - (stabilization_att_fb_cmd[COMMAND_ROLL]+stabilization_att_ff_cmd[COMMAND_ROLL]); + (stabilization_att_fb_cmd[COMMAND_ROLL] + stabilization_att_ff_cmd[COMMAND_ROLL]); stabilization_cmd[COMMAND_PITCH] = - (stabilization_att_fb_cmd[COMMAND_PITCH]+stabilization_att_ff_cmd[COMMAND_PITCH]); + (stabilization_att_fb_cmd[COMMAND_PITCH] + stabilization_att_ff_cmd[COMMAND_PITCH]); stabilization_cmd[COMMAND_YAW] = - (stabilization_att_fb_cmd[COMMAND_YAW]+stabilization_att_ff_cmd[COMMAND_YAW]); + (stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW]); /* bound the result */ BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c index 03731bf2a9..b2a0a300d9 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c @@ -55,7 +55,8 @@ struct Int32Eulers stabilization_att_sum_err; int32_t stabilization_att_fb_cmd[COMMANDS_NB]; int32_t stabilization_att_ff_cmd[COMMANDS_NB]; -static inline void reset_psi_ref_from_body(void) { +static inline void reset_psi_ref_from_body(void) +{ //sp has been set from body using stabilization_attitude_get_yaw_i, use that value stab_att_ref_euler.psi = stab_att_sp_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); stab_att_ref_rate.r = 0; @@ -65,47 +66,50 @@ static inline void reset_psi_ref_from_body(void) { #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_att(struct transport_tx *trans, struct link_device *dev) { - struct Int32Rates* body_rate = stateGetBodyRates_i(); - struct Int32Eulers* att = stateGetNedToBodyEulers_i(); +static void send_att(struct transport_tx *trans, struct link_device *dev) +{ + struct Int32Rates *body_rate = stateGetBodyRates_i(); + struct Int32Eulers *att = stateGetNedToBodyEulers_i(); pprz_msg_send_STAB_ATTITUDE_INT(trans, dev, AC_ID, - &(body_rate->p), &(body_rate->q), &(body_rate->r), - &(att->phi), &(att->theta), &(att->psi), - &stab_att_sp_euler.phi, - &stab_att_sp_euler.theta, - &stab_att_sp_euler.psi, - &stabilization_att_sum_err.phi, - &stabilization_att_sum_err.theta, - &stabilization_att_sum_err.psi, - &stabilization_att_fb_cmd[COMMAND_ROLL], - &stabilization_att_fb_cmd[COMMAND_PITCH], - &stabilization_att_fb_cmd[COMMAND_YAW], - &stabilization_att_ff_cmd[COMMAND_ROLL], - &stabilization_att_ff_cmd[COMMAND_PITCH], - &stabilization_att_ff_cmd[COMMAND_YAW], - &stabilization_cmd[COMMAND_ROLL], - &stabilization_cmd[COMMAND_PITCH], - &stabilization_cmd[COMMAND_YAW]); + &(body_rate->p), &(body_rate->q), &(body_rate->r), + &(att->phi), &(att->theta), &(att->psi), + &stab_att_sp_euler.phi, + &stab_att_sp_euler.theta, + &stab_att_sp_euler.psi, + &stabilization_att_sum_err.phi, + &stabilization_att_sum_err.theta, + &stabilization_att_sum_err.psi, + &stabilization_att_fb_cmd[COMMAND_ROLL], + &stabilization_att_fb_cmd[COMMAND_PITCH], + &stabilization_att_fb_cmd[COMMAND_YAW], + &stabilization_att_ff_cmd[COMMAND_ROLL], + &stabilization_att_ff_cmd[COMMAND_PITCH], + &stabilization_att_ff_cmd[COMMAND_YAW], + &stabilization_cmd[COMMAND_ROLL], + &stabilization_cmd[COMMAND_PITCH], + &stabilization_cmd[COMMAND_YAW]); } -static void send_att_ref(struct transport_tx *trans, struct link_device *dev) { +static void send_att_ref(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_STAB_ATTITUDE_REF_INT(trans, dev, AC_ID, - &stab_att_sp_euler.phi, - &stab_att_sp_euler.theta, - &stab_att_sp_euler.psi, - &stab_att_ref_euler.phi, - &stab_att_ref_euler.theta, - &stab_att_ref_euler.psi, - &stab_att_ref_rate.p, - &stab_att_ref_rate.q, - &stab_att_ref_rate.r, - &stab_att_ref_accel.p, - &stab_att_ref_accel.q, - &stab_att_ref_accel.r); + &stab_att_sp_euler.phi, + &stab_att_sp_euler.theta, + &stab_att_sp_euler.psi, + &stab_att_ref_euler.phi, + &stab_att_ref_euler.theta, + &stab_att_ref_euler.psi, + &stab_att_ref_rate.p, + &stab_att_ref_rate.q, + &stab_att_ref_rate.r, + &stab_att_ref_accel.p, + &stab_att_ref_accel.q, + &stab_att_ref_accel.r); } #endif -void stabilization_attitude_init(void) { +void stabilization_attitude_init(void) +{ stabilization_attitude_ref_init(); @@ -131,7 +135,7 @@ void stabilization_attitude_init(void) { STABILIZATION_ATTITUDE_PSI_DDGAIN); - INT_EULERS_ZERO( stabilization_att_sum_err ); + INT_EULERS_ZERO(stabilization_att_sum_err); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att); @@ -139,27 +143,32 @@ void stabilization_attitude_init(void) { #endif } -void stabilization_attitude_read_rc(bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) { +void stabilization_attitude_read_rc(bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) +{ stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn); } -void stabilization_attitude_enter(void) { +void stabilization_attitude_enter(void) +{ stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi; reset_psi_ref_from_body(); - INT_EULERS_ZERO( stabilization_att_sum_err ); + INT_EULERS_ZERO(stabilization_att_sum_err); } -void stabilization_attitude_set_failsafe_setpoint(void) { +void stabilization_attitude_set_failsafe_setpoint(void) +{ stab_att_sp_euler.phi = 0; stab_att_sp_euler.theta = 0; stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi; } -void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy) { +void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy) +{ memcpy(&stab_att_sp_euler, rpy, sizeof(struct Int32Eulers)); } -void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) { +void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) +{ /* Rotate horizontal commands to body frame by psi */ int32_t psi = stateGetNedToBodyEulers_i()->psi; int32_t s_psi, c_psi; @@ -175,7 +184,8 @@ void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t head #define MAX_SUM_ERR 4000000 -void stabilization_attitude_run(bool_t in_flight) { +void stabilization_attitude_run(bool_t in_flight) +{ /* update reference */ stabilization_attitude_ref_update(); @@ -191,11 +201,12 @@ void stabilization_attitude_run(bool_t in_flight) { /* compute feedback command */ /* attitude error */ const struct Int32Eulers att_ref_scaled = { - OFFSET_AND_ROUND(stab_att_ref_euler.phi, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)), + OFFSET_AND_ROUND(stab_att_ref_euler.phi, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)), OFFSET_AND_ROUND(stab_att_ref_euler.theta, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)), - OFFSET_AND_ROUND(stab_att_ref_euler.psi, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)) }; + OFFSET_AND_ROUND(stab_att_ref_euler.psi, (REF_ANGLE_FRAC - INT32_ANGLE_FRAC)) + }; struct Int32Eulers att_err; - struct Int32Eulers* ltp_to_body_euler = stateGetNedToBodyEulers_i(); + struct Int32Eulers *ltp_to_body_euler = stateGetNedToBodyEulers_i(); EULERS_DIFF(att_err, att_ref_scaled, (*ltp_to_body_euler)); INT32_ANGLE_NORMALIZE(att_err.psi); @@ -203,8 +214,7 @@ void stabilization_attitude_run(bool_t in_flight) { /* update integrator */ EULERS_ADD(stabilization_att_sum_err, att_err); EULERS_BOUND_CUBE(stabilization_att_sum_err, -MAX_SUM_ERR, MAX_SUM_ERR); - } - else { + } else { INT_EULERS_ZERO(stabilization_att_sum_err); } @@ -212,9 +222,10 @@ void stabilization_attitude_run(bool_t in_flight) { const struct Int32Rates rate_ref_scaled = { OFFSET_AND_ROUND(stab_att_ref_rate.p, (REF_RATE_FRAC - INT32_RATE_FRAC)), OFFSET_AND_ROUND(stab_att_ref_rate.q, (REF_RATE_FRAC - INT32_RATE_FRAC)), - OFFSET_AND_ROUND(stab_att_ref_rate.r, (REF_RATE_FRAC - INT32_RATE_FRAC)) }; + OFFSET_AND_ROUND(stab_att_ref_rate.r, (REF_RATE_FRAC - INT32_RATE_FRAC)) + }; struct Int32Rates rate_err; - struct Int32Rates* body_rate = stateGetBodyRates_i(); + struct Int32Rates *body_rate = stateGetBodyRates_i(); RATES_DIFF(rate_err, rate_ref_scaled, (*body_rate)); /* PID */ @@ -242,13 +253,13 @@ void stabilization_attitude_run(bool_t in_flight) { /* sum feedforward and feedback */ stabilization_cmd[COMMAND_ROLL] = - OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_ROLL]+stabilization_att_ff_cmd[COMMAND_ROLL]), CMD_SHIFT); + OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_ROLL] + stabilization_att_ff_cmd[COMMAND_ROLL]), CMD_SHIFT); stabilization_cmd[COMMAND_PITCH] = - OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_PITCH]+stabilization_att_ff_cmd[COMMAND_PITCH]), CMD_SHIFT); + OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_PITCH] + stabilization_att_ff_cmd[COMMAND_PITCH]), CMD_SHIFT); stabilization_cmd[COMMAND_YAW] = - OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_YAW]+stabilization_att_ff_cmd[COMMAND_YAW]), CMD_SHIFT); + OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW]), CMD_SHIFT); /* bound the result */ BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c index 4d296a1f21..524de39586 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c @@ -41,30 +41,34 @@ struct Int32Eulers stab_att_sp_euler; -void stabilization_attitude_init(void) { +void stabilization_attitude_init(void) +{ INT_EULERS_ZERO(stab_att_sp_euler); } -void stabilization_attitude_read_rc(bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) { +void stabilization_attitude_read_rc(bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) +{ //Read from RC stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn); } -void stabilization_attitude_enter(void) { +void stabilization_attitude_enter(void) +{ } -void stabilization_attitude_run(bool_t in_flight __attribute__ ((unused))) { +void stabilization_attitude_run(bool_t in_flight __attribute__((unused))) +{ /* For roll and pitch we pass trough the desired angles as stabilization command */ - const int32_t angle2cmd = (MAX_PPRZ/TRAJ_MAX_BANK); + const int32_t angle2cmd = (MAX_PPRZ / TRAJ_MAX_BANK); stabilization_cmd[COMMAND_ROLL] = stab_att_sp_euler.phi * angle2cmd; stabilization_cmd[COMMAND_PITCH] = stab_att_sp_euler.theta * angle2cmd; //TODO: Fix yaw with PID controller int32_t yaw_error = stateGetNedToBodyEulers_i()->psi - stab_att_sp_euler.psi; INT32_ANGLE_NORMALIZE(yaw_error); - // stabilization_cmd[COMMAND_YAW] = yaw_error * MAX_PPRZ / INT32_ANGLE_PI; + // stabilization_cmd[COMMAND_YAW] = yaw_error * MAX_PPRZ / INT32_ANGLE_PI; /* bound the result */ BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ); @@ -72,17 +76,20 @@ void stabilization_attitude_run(bool_t in_flight __attribute__ ((unused))) { BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ); } -void stabilization_attitude_set_failsafe_setpoint(void) { +void stabilization_attitude_set_failsafe_setpoint(void) +{ stab_att_sp_euler.phi = 0; stab_att_sp_euler.theta = 0; stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi; } -void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy) { +void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy) +{ memcpy(&stab_att_sp_euler, rpy, sizeof(struct Int32Eulers)); } -void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) { +void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) +{ /* Rotate horizontal commands to body frame by psi */ int32_t psi = stateGetNedToBodyEulers_i()->psi; int32_t s_psi, c_psi; diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c index b7542fd772..b96c6cc505 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c @@ -95,9 +95,10 @@ static const float psi_ddgain_surface[] = STABILIZATION_ATTITUDE_PSI_DDGAIN_SURF #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_att(struct transport_tx *trans, struct link_device *dev) { - struct FloatRates* body_rate = stateGetBodyRates_f(); - struct FloatEulers* att = stateGetNedToBodyEulers_f(); +static void send_att(struct transport_tx *trans, struct link_device *dev) +{ + struct FloatRates *body_rate = stateGetBodyRates_f(); + struct FloatEulers *att = stateGetNedToBodyEulers_f(); pprz_msg_send_STAB_ATTITUDE_FLOAT(trans, dev, AC_ID, &(body_rate->p), &(body_rate->q), &(body_rate->r), &(att->phi), &(att->theta), &(att->psi), @@ -119,7 +120,8 @@ static void send_att(struct transport_tx *trans, struct link_device *dev) { &body_rate_d.p, &body_rate_d.q, &body_rate_d.r); } -static void send_att_ref(struct transport_tx *trans, struct link_device *dev) { +static void send_att_ref(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_STAB_ATTITUDE_REF_FLOAT(trans, dev, AC_ID, &stab_att_sp_euler.phi, &stab_att_sp_euler.theta, @@ -136,7 +138,8 @@ static void send_att_ref(struct transport_tx *trans, struct link_device *dev) { } #endif -void stabilization_attitude_init(void) { +void stabilization_attitude_init(void) +{ stabilization_attitude_ref_init(); @@ -155,8 +158,8 @@ void stabilization_attitude_init(void) { } float_quat_identity(&stabilization_att_sum_err_quat); - FLOAT_RATES_ZERO( last_body_rate ); - FLOAT_RATES_ZERO( body_rate_d ); + FLOAT_RATES_ZERO(last_body_rate); + FLOAT_RATES_ZERO(body_rate_d); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "STAB_ATTITUDE", send_att); @@ -174,7 +177,8 @@ void stabilization_attitude_gain_schedule(uint8_t idx) stabilization_attitude_ref_schedule(idx); } -void stabilization_attitude_enter(void) { +void stabilization_attitude_enter(void) +{ /* reset psi setpoint to current psi angle */ stab_att_sp_euler.psi = stabilization_attitude_get_heading_f(); @@ -184,7 +188,8 @@ void stabilization_attitude_enter(void) { float_quat_identity(&stabilization_att_sum_err_quat); } -void stabilization_attitude_set_failsafe_setpoint(void) { +void stabilization_attitude_set_failsafe_setpoint(void) +{ /* set failsafe to zero roll/pitch and current heading */ float heading2 = stabilization_attitude_get_heading_f() / 2; stab_att_sp_quat.qi = cosf(heading2); @@ -193,14 +198,16 @@ void stabilization_attitude_set_failsafe_setpoint(void) { stab_att_sp_quat.qz = sinf(heading2); } -void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy) { +void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy) +{ // copy euler setpoint for debugging EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *rpy); quat_from_rpy_cmd_f(&stab_att_sp_quat, &stab_att_sp_euler); } -void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) { +void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) +{ struct FloatVect2 cmd_f; cmd_f.x = ANGLE_FLOAT_OF_BFP(cmd->x); cmd_f.y = ANGLE_FLOAT_OF_BFP(cmd->y); @@ -237,7 +244,7 @@ static void attitude_run_ff(float ff_commands[], struct FloatAttitudeGains *gain #define GAIN_PRESCALER_I 1 #endif static void attitude_run_fb(float fb_commands[], struct FloatAttitudeGains *gains, struct FloatQuat *att_err, - struct FloatRates *rate_err, struct FloatRates *rate_err_d, struct FloatQuat *sum_err) + struct FloatRates *rate_err, struct FloatRates *rate_err_d, struct FloatQuat *sum_err) { /* PID feedback */ fb_commands[COMMAND_ROLL] = @@ -276,7 +283,8 @@ static void attitude_run_fb(float fb_commands[], struct FloatAttitudeGains *gain #endif } -void stabilization_attitude_run(bool_t enable_integrator) { +void stabilization_attitude_run(bool_t enable_integrator) +{ /* * Update reference @@ -289,14 +297,14 @@ void stabilization_attitude_run(bool_t enable_integrator) { /* attitude error */ struct FloatQuat att_err; - struct FloatQuat* att_quat = stateGetNedToBodyQuat_f(); + struct FloatQuat *att_quat = stateGetNedToBodyQuat_f(); float_quat_inv_comp(&att_err, att_quat, &stab_att_ref_quat); /* wrap it in the shortest direction */ float_quat_wrap_shortest(&att_err); /* rate error */ struct FloatRates rate_err; - struct FloatRates* body_rate = stateGetBodyRates_f(); + struct FloatRates *body_rate = stateGetBodyRates_f(); RATES_DIFF(rate_err, stab_att_ref_rate, *body_rate); /* rate_d error */ RATES_DIFF(body_rate_d, *body_rate, last_body_rate); @@ -305,12 +313,12 @@ void stabilization_attitude_run(bool_t enable_integrator) { #define INTEGRATOR_BOUND 1.0 /* integrated error */ if (enable_integrator) { - stabilization_att_sum_err_quat.qx += att_err.qx /IERROR_SCALE; - stabilization_att_sum_err_quat.qy += att_err.qy /IERROR_SCALE; - stabilization_att_sum_err_quat.qz += att_err.qz /IERROR_SCALE; - Bound(stabilization_att_sum_err_quat.qx,-INTEGRATOR_BOUND,INTEGRATOR_BOUND); - Bound(stabilization_att_sum_err_quat.qy,-INTEGRATOR_BOUND,INTEGRATOR_BOUND); - Bound(stabilization_att_sum_err_quat.qz,-INTEGRATOR_BOUND,INTEGRATOR_BOUND); + stabilization_att_sum_err_quat.qx += att_err.qx / IERROR_SCALE; + stabilization_att_sum_err_quat.qy += att_err.qy / IERROR_SCALE; + stabilization_att_sum_err_quat.qz += att_err.qz / IERROR_SCALE; + Bound(stabilization_att_sum_err_quat.qx, -INTEGRATOR_BOUND, INTEGRATOR_BOUND); + Bound(stabilization_att_sum_err_quat.qy, -INTEGRATOR_BOUND, INTEGRATOR_BOUND); + Bound(stabilization_att_sum_err_quat.qz, -INTEGRATOR_BOUND, INTEGRATOR_BOUND); } else { /* reset accumulator */ float_quat_identity(&stabilization_att_sum_err_quat); @@ -318,16 +326,20 @@ void stabilization_attitude_run(bool_t enable_integrator) { attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains[gain_idx], &stab_att_ref_accel); - attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains[gain_idx], &att_err, &rate_err, &body_rate_d, &stabilization_att_sum_err_quat); + attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains[gain_idx], &att_err, &rate_err, &body_rate_d, + &stabilization_att_sum_err_quat); stabilization_cmd[COMMAND_ROLL] = stabilization_att_fb_cmd[COMMAND_ROLL] + stabilization_att_ff_cmd[COMMAND_ROLL]; stabilization_cmd[COMMAND_PITCH] = stabilization_att_fb_cmd[COMMAND_PITCH] + stabilization_att_ff_cmd[COMMAND_PITCH]; stabilization_cmd[COMMAND_YAW] = stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW]; #ifdef HAS_SURFACE_COMMANDS - stabilization_cmd[COMMAND_ROLL_SURFACE] = stabilization_att_fb_cmd[COMMAND_ROLL_SURFACE] + stabilization_att_ff_cmd[COMMAND_ROLL_SURFACE]; - stabilization_cmd[COMMAND_PITCH_SURFACE] = stabilization_att_fb_cmd[COMMAND_PITCH_SURFACE] + stabilization_att_ff_cmd[COMMAND_PITCH_SURFACE]; - stabilization_cmd[COMMAND_YAW_SURFACE] = stabilization_att_fb_cmd[COMMAND_YAW_SURFACE] + stabilization_att_ff_cmd[COMMAND_YAW_SURFACE]; + stabilization_cmd[COMMAND_ROLL_SURFACE] = stabilization_att_fb_cmd[COMMAND_ROLL_SURFACE] + + stabilization_att_ff_cmd[COMMAND_ROLL_SURFACE]; + stabilization_cmd[COMMAND_PITCH_SURFACE] = stabilization_att_fb_cmd[COMMAND_PITCH_SURFACE] + + stabilization_att_ff_cmd[COMMAND_PITCH_SURFACE]; + stabilization_cmd[COMMAND_YAW_SURFACE] = stabilization_att_fb_cmd[COMMAND_YAW_SURFACE] + + stabilization_att_ff_cmd[COMMAND_YAW_SURFACE]; #endif /* bound the result */ @@ -336,7 +348,8 @@ void stabilization_attitude_run(bool_t enable_integrator) { BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ); } -void stabilization_attitude_read_rc(bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) { +void stabilization_attitude_read_rc(bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) +{ stabilization_attitude_read_rc_setpoint_quat_f(&stab_att_sp_quat, in_flight, in_carefree, coordinated_turn); //float_quat_wrap_shortest(&stab_att_sp_quat); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c index a8db35f28b..14798c6536 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c @@ -69,30 +69,32 @@ int32_t stabilization_att_ff_cmd[COMMANDS_NB]; #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_att(struct transport_tx *trans, struct link_device *dev) { //FIXME really use this message here ? - struct Int32Rates* body_rate = stateGetBodyRates_i(); - struct Int32Eulers* att = stateGetNedToBodyEulers_i(); +static void send_att(struct transport_tx *trans, struct link_device *dev) //FIXME really use this message here ? +{ + struct Int32Rates *body_rate = stateGetBodyRates_i(); + struct Int32Eulers *att = stateGetNedToBodyEulers_i(); pprz_msg_send_STAB_ATTITUDE_INT(trans, dev, AC_ID, - &(body_rate->p), &(body_rate->q), &(body_rate->r), - &(att->phi), &(att->theta), &(att->psi), - &stab_att_sp_euler.phi, - &stab_att_sp_euler.theta, - &stab_att_sp_euler.psi, - &stabilization_att_sum_err_quat.qx, - &stabilization_att_sum_err_quat.qy, - &stabilization_att_sum_err_quat.qz, - &stabilization_att_fb_cmd[COMMAND_ROLL], - &stabilization_att_fb_cmd[COMMAND_PITCH], - &stabilization_att_fb_cmd[COMMAND_YAW], - &stabilization_att_ff_cmd[COMMAND_ROLL], - &stabilization_att_ff_cmd[COMMAND_PITCH], - &stabilization_att_ff_cmd[COMMAND_YAW], - &stabilization_cmd[COMMAND_ROLL], - &stabilization_cmd[COMMAND_PITCH], - &stabilization_cmd[COMMAND_YAW]); + &(body_rate->p), &(body_rate->q), &(body_rate->r), + &(att->phi), &(att->theta), &(att->psi), + &stab_att_sp_euler.phi, + &stab_att_sp_euler.theta, + &stab_att_sp_euler.psi, + &stabilization_att_sum_err_quat.qx, + &stabilization_att_sum_err_quat.qy, + &stabilization_att_sum_err_quat.qz, + &stabilization_att_fb_cmd[COMMAND_ROLL], + &stabilization_att_fb_cmd[COMMAND_PITCH], + &stabilization_att_fb_cmd[COMMAND_YAW], + &stabilization_att_ff_cmd[COMMAND_ROLL], + &stabilization_att_ff_cmd[COMMAND_PITCH], + &stabilization_att_ff_cmd[COMMAND_YAW], + &stabilization_cmd[COMMAND_ROLL], + &stabilization_cmd[COMMAND_PITCH], + &stabilization_cmd[COMMAND_YAW]); } -static void send_att_ref(struct transport_tx *trans, struct link_device *dev) { +static void send_att_ref(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_STAB_ATTITUDE_REF_INT(trans, dev, AC_ID, &stab_att_sp_euler.phi, &stab_att_sp_euler.theta, @@ -108,21 +110,23 @@ static void send_att_ref(struct transport_tx *trans, struct link_device *dev) { &stab_att_ref_accel.r); } -static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev) { - struct Int32Quat* quat = stateGetNedToBodyQuat_i(); +static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev) +{ + struct Int32Quat *quat = stateGetNedToBodyQuat_i(); pprz_msg_send_AHRS_REF_QUAT(trans, dev, AC_ID, - &stab_att_ref_quat.qi, - &stab_att_ref_quat.qx, - &stab_att_ref_quat.qy, - &stab_att_ref_quat.qz, - &(quat->qi), - &(quat->qx), - &(quat->qy), - &(quat->qz)); + &stab_att_ref_quat.qi, + &stab_att_ref_quat.qx, + &stab_att_ref_quat.qy, + &stab_att_ref_quat.qz, + &(quat->qi), + &(quat->qx), + &(quat->qy), + &(quat->qz)); } #endif -void stabilization_attitude_init(void) { +void stabilization_attitude_init(void) +{ stabilization_attitude_ref_init(); @@ -135,7 +139,8 @@ void stabilization_attitude_init(void) { #endif } -void stabilization_attitude_enter(void) { +void stabilization_attitude_enter(void) +{ /* reset psi setpoint to current psi angle */ stab_att_sp_euler.psi = stabilization_attitude_get_heading_i(); @@ -146,7 +151,8 @@ void stabilization_attitude_enter(void) { } -void stabilization_attitude_set_failsafe_setpoint(void) { +void stabilization_attitude_set_failsafe_setpoint(void) +{ /* set failsafe to zero roll/pitch and current heading */ int32_t heading2 = stabilization_attitude_get_heading_i() / 2; PPRZ_ITRIG_COS(stab_att_sp_quat.qi, heading2); @@ -155,14 +161,16 @@ void stabilization_attitude_set_failsafe_setpoint(void) { PPRZ_ITRIG_SIN(stab_att_sp_quat.qz, heading2); } -void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy) { +void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy) +{ // stab_att_sp_euler.psi still used in ref.. memcpy(&stab_att_sp_euler, rpy, sizeof(struct Int32Eulers)); quat_from_rpy_cmd_i(&stab_att_sp_quat, &stab_att_sp_euler); } -void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) { +void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) +{ // stab_att_sp_euler.psi still used in ref.. stab_att_sp_euler.psi = heading; @@ -191,7 +199,7 @@ static void attitude_run_ff(int32_t ff_commands[], struct Int32AttitudeGains *ga } static void attitude_run_fb(int32_t fb_commands[], struct Int32AttitudeGains *gains, struct Int32Quat *att_err, - struct Int32Rates *rate_err, struct Int32Quat *sum_err) + struct Int32Rates *rate_err, struct Int32Quat *sum_err) { /* PID feedback */ fb_commands[COMMAND_ROLL] = @@ -211,7 +219,8 @@ static void attitude_run_fb(int32_t fb_commands[], struct Int32AttitudeGains *ga } -void stabilization_attitude_run(bool_t enable_integrator) { +void stabilization_attitude_run(bool_t enable_integrator) +{ /* * Update reference @@ -224,7 +233,7 @@ void stabilization_attitude_run(bool_t enable_integrator) { /* attitude error */ struct Int32Quat att_err; - struct Int32Quat* att_quat = stateGetNedToBodyQuat_i(); + struct Int32Quat *att_quat = stateGetNedToBodyQuat_i(); INT32_QUAT_INV_COMP(att_err, *att_quat, stab_att_ref_quat); /* wrap it in the shortest direction */ int32_quat_wrap_shortest(&att_err); @@ -234,20 +243,21 @@ void stabilization_attitude_run(bool_t enable_integrator) { const struct Int32Rates rate_ref_scaled = { OFFSET_AND_ROUND(stab_att_ref_rate.p, (REF_RATE_FRAC - INT32_RATE_FRAC)), OFFSET_AND_ROUND(stab_att_ref_rate.q, (REF_RATE_FRAC - INT32_RATE_FRAC)), - OFFSET_AND_ROUND(stab_att_ref_rate.r, (REF_RATE_FRAC - INT32_RATE_FRAC)) }; + OFFSET_AND_ROUND(stab_att_ref_rate.r, (REF_RATE_FRAC - INT32_RATE_FRAC)) + }; struct Int32Rates rate_err; - struct Int32Rates* body_rate = stateGetBodyRates_i(); + struct Int32Rates *body_rate = stateGetBodyRates_i(); RATES_DIFF(rate_err, rate_ref_scaled, (*body_rate)); #define INTEGRATOR_BOUND 100000 /* integrated error */ if (enable_integrator) { - stabilization_att_sum_err_quat.qx += att_err.qx /IERROR_SCALE; - stabilization_att_sum_err_quat.qy += att_err.qy /IERROR_SCALE; - stabilization_att_sum_err_quat.qz += att_err.qz /IERROR_SCALE; - Bound(stabilization_att_sum_err_quat.qx,-INTEGRATOR_BOUND,INTEGRATOR_BOUND); - Bound(stabilization_att_sum_err_quat.qy,-INTEGRATOR_BOUND,INTEGRATOR_BOUND); - Bound(stabilization_att_sum_err_quat.qz,-INTEGRATOR_BOUND,INTEGRATOR_BOUND); + stabilization_att_sum_err_quat.qx += att_err.qx / IERROR_SCALE; + stabilization_att_sum_err_quat.qy += att_err.qy / IERROR_SCALE; + stabilization_att_sum_err_quat.qz += att_err.qz / IERROR_SCALE; + Bound(stabilization_att_sum_err_quat.qx, -INTEGRATOR_BOUND, INTEGRATOR_BOUND); + Bound(stabilization_att_sum_err_quat.qy, -INTEGRATOR_BOUND, INTEGRATOR_BOUND); + Bound(stabilization_att_sum_err_quat.qz, -INTEGRATOR_BOUND, INTEGRATOR_BOUND); } else { /* reset accumulator */ int32_quat_identity(&stabilization_att_sum_err_quat); @@ -270,7 +280,8 @@ void stabilization_attitude_run(bool_t enable_integrator) { BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ); } -void stabilization_attitude_read_rc(bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) { +void stabilization_attitude_read_rc(bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) +{ struct FloatQuat q_sp; #if USE_EARTH_BOUND_RC_SETPOINT stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(&q_sp, in_flight, in_carefree, coordinated_turn); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.c index 83d3032a6f..dc7b439a9a 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.c @@ -25,7 +25,8 @@ #include "stabilization_attitude_quat_transformations.h" -void quat_from_rpy_cmd_i(struct Int32Quat *quat, struct Int32Eulers *rpy) { +void quat_from_rpy_cmd_i(struct Int32Quat *quat, struct Int32Eulers *rpy) +{ struct FloatEulers rpy_f; EULERS_FLOAT_OF_BFP(rpy_f, *rpy); struct FloatQuat quat_f; @@ -33,7 +34,8 @@ void quat_from_rpy_cmd_i(struct Int32Quat *quat, struct Int32Eulers *rpy) { QUAT_BFP_OF_REAL(*quat, quat_f); } -void quat_from_rpy_cmd_f(struct FloatQuat *quat, struct FloatEulers *rpy) { +void quat_from_rpy_cmd_f(struct FloatQuat *quat, struct FloatEulers *rpy) +{ // only a plug for now... doesn't apply roll/pitch wrt. current yaw angle /* orientation vector describing simultaneous rotation of roll/pitch/yaw */ @@ -43,7 +45,8 @@ void quat_from_rpy_cmd_f(struct FloatQuat *quat, struct FloatEulers *rpy) { } -void quat_from_earth_cmd_i(struct Int32Quat *quat, struct Int32Vect2 *cmd, int32_t heading) { +void quat_from_earth_cmd_i(struct Int32Quat *quat, struct Int32Vect2 *cmd, int32_t heading) +{ // use float conversion for now... struct FloatVect2 cmd_f; cmd_f.x = ANGLE_FLOAT_OF_BFP(cmd->x); @@ -57,7 +60,8 @@ void quat_from_earth_cmd_i(struct Int32Quat *quat, struct Int32Vect2 *cmd, int32 QUAT_BFP_OF_REAL(*quat, quat_f); } -void quat_from_earth_cmd_f(struct FloatQuat *quat, struct FloatVect2 *cmd, float heading) { +void quat_from_earth_cmd_f(struct FloatQuat *quat, struct FloatVect2 *cmd, float heading) +{ /* cmd_x is positive to north = negative pitch * cmd_y is positive to east = positive roll diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c index 3500b65f90..5c8b2558f9 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.c @@ -51,7 +51,8 @@ float care_free_heading = 0; -static int32_t get_rc_roll(void) { +static int32_t get_rc_roll(void) +{ const int32_t max_rc_phi = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI); int32_t roll = radio_control.values[RADIO_ROLL]; #if STABILIZATION_ATTITUDE_DEADBAND_A @@ -62,7 +63,8 @@ static int32_t get_rc_roll(void) { #endif } -static int32_t get_rc_pitch(void) { +static int32_t get_rc_pitch(void) +{ const int32_t max_rc_theta = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA); int32_t pitch = radio_control.values[RADIO_PITCH]; #if STABILIZATION_ATTITUDE_DEADBAND_E @@ -73,14 +75,16 @@ static int32_t get_rc_pitch(void) { #endif } -static int32_t get_rc_yaw(void) { +static int32_t get_rc_yaw(void) +{ const int32_t max_rc_r = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R); int32_t yaw = radio_control.values[RADIO_YAW]; DeadBand(yaw, STABILIZATION_ATTITUDE_DEADBAND_R); return yaw * max_rc_r / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_R); } -static float get_rc_roll_f(void) { +static float get_rc_roll_f(void) +{ int32_t roll = radio_control.values[RADIO_ROLL]; #if STABILIZATION_ATTITUDE_DEADBAND_A DeadBand(roll, STABILIZATION_ATTITUDE_DEADBAND_A); @@ -90,7 +94,8 @@ static float get_rc_roll_f(void) { #endif } -static float get_rc_pitch_f(void) { +static float get_rc_pitch_f(void) +{ int32_t pitch = radio_control.values[RADIO_PITCH]; #if STABILIZATION_ATTITUDE_DEADBAND_E DeadBand(pitch, STABILIZATION_ATTITUDE_DEADBAND_E); @@ -100,50 +105,54 @@ static float get_rc_pitch_f(void) { #endif } -static inline float get_rc_yaw_f(void) { +static inline float get_rc_yaw_f(void) +{ int32_t yaw = radio_control.values[RADIO_YAW]; DeadBand(yaw, STABILIZATION_ATTITUDE_DEADBAND_R); return yaw * STABILIZATION_ATTITUDE_SP_MAX_R / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_R); } /// reset the heading for care-free mode to current heading -void stabilization_attitude_reset_care_free_heading(void) { +void stabilization_attitude_reset_care_free_heading(void) +{ care_free_heading = stateGetNedToBodyEulers_f()->psi; } /* This is a different way to obtain yaw. It will not switch when going beyond 90 degrees pitch. However, when rolling more then 90 degrees in combination with pitch it switches. For a transition vehicle this is better as 90 degrees pitch will occur, but more than 90 degrees roll probably not. */ -int32_t stabilization_attitude_get_heading_i(void) { - struct Int32Eulers* att = stateGetNedToBodyEulers_i(); +int32_t stabilization_attitude_get_heading_i(void) +{ + struct Int32Eulers *att = stateGetNedToBodyEulers_i(); int32_t heading; - if(abs(att->phi) < INT32_ANGLE_PI_2) { + if (abs(att->phi) < INT32_ANGLE_PI_2) { int32_t sin_theta; PPRZ_ITRIG_SIN(sin_theta, att->theta); heading = att->psi - INT_MULT_RSHIFT(sin_theta, att->phi, INT32_TRIG_FRAC); - } - else if(ANGLE_FLOAT_OF_BFP(att->theta) > 0) + } else if (ANGLE_FLOAT_OF_BFP(att->theta) > 0) { heading = att->psi - att->phi; - else + } else { heading = att->psi + att->phi; + } return heading; } -float stabilization_attitude_get_heading_f(void) { - struct FloatEulers* att = stateGetNedToBodyEulers_f(); +float stabilization_attitude_get_heading_f(void) +{ + struct FloatEulers *att = stateGetNedToBodyEulers_f(); float heading; - if(abs(att->phi) < M_PI/2) { - heading = att->psi - sinf(att->theta)*att->phi; - } - else if(att->theta > 0) + if (abs(att->phi) < M_PI / 2) { + heading = att->psi - sinf(att->theta) * att->phi; + } else if (att->theta > 0) { heading = att->psi - att->phi; - else + } else { heading = att->psi + att->phi; + } return heading; } @@ -155,7 +164,9 @@ float stabilization_attitude_get_heading_f(void) { * @param[in] in_flight true if in flight * @param[out] sp attitude setpoint as euler angles */ -void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) { +void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight, bool_t in_carefree, + bool_t coordinated_turn) +{ sp->phi = get_rc_roll(); sp->theta = get_rc_pitch(); @@ -171,12 +182,13 @@ void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool //Take v = 9.81/1.3 m/s int32_t omega; const int32_t max_phi = ANGLE_BFP_OF_REAL(RadOfDeg(85.0)); - if (abs(sp->phi) < max_phi) - omega = ANGLE_BFP_OF_REAL(1.3*tanf(ANGLE_FLOAT_OF_BFP(sp->phi))); - else //max 60 degrees roll, then take constant omega - omega = ANGLE_BFP_OF_REAL(1.3*1.72305* ((sp->phi > 0) - (sp->phi < 0))); + if (abs(sp->phi) < max_phi) { + omega = ANGLE_BFP_OF_REAL(1.3 * tanf(ANGLE_FLOAT_OF_BFP(sp->phi))); + } else { //max 60 degrees roll, then take constant omega + omega = ANGLE_BFP_OF_REAL(1.3 * 1.72305 * ((sp->phi > 0) - (sp->phi < 0))); + } - sp->psi += omega/RC_UPDATE_FREQ; + sp->psi += omega / RC_UPDATE_FREQ; } #ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT // Make sure the yaw setpoint does not differ too much from the real yaw @@ -187,10 +199,9 @@ void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool int32_t delta_psi = sp->psi - heading; INT32_ANGLE_NORMALIZE(delta_psi); - if (delta_psi > delta_limit){ + if (delta_psi > delta_limit) { sp->psi = heading + delta_limit; - } - else if (delta_psi < -delta_limit){ + } else if (delta_psi < -delta_limit) { sp->psi = heading - delta_limit; } INT32_ANGLE_NORMALIZE(sp->psi); @@ -210,19 +221,21 @@ void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool PPRZ_ITRIG_SIN(sin_psi, care_free_delta_psi_i); PPRZ_ITRIG_COS(cos_psi, care_free_delta_psi_i); - temp_theta = INT_MULT_RSHIFT(cos_psi, sp->theta, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->phi, INT32_ANGLE_FRAC); + temp_theta = INT_MULT_RSHIFT(cos_psi, sp->theta, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->phi, + INT32_ANGLE_FRAC); sp->phi = INT_MULT_RSHIFT(cos_psi, sp->phi, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->theta, INT32_ANGLE_FRAC); sp->theta = temp_theta; } - } - else { /* if not flying, use current yaw as setpoint */ + } else { /* if not flying, use current yaw as setpoint */ sp->psi = stateGetNedToBodyEulers_i()->psi; } } -void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) { +void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool_t in_flight, bool_t in_carefree, + bool_t coordinated_turn) +{ sp->phi = get_rc_roll_f(); sp->theta = get_rc_pitch_f(); @@ -238,12 +251,13 @@ void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bo //Take v = 9.81/1.3 m/s float omega; const float max_phi = RadOfDeg(85.0); - if(abs(sp->phi) < max_phi) - omega = 1.3*tanf(sp->phi); - else //max 60 degrees roll, then take constant omega - omega = 1.3*1.72305* ((sp->phi > 0) - (sp->phi < 0)); + if (abs(sp->phi) < max_phi) { + omega = 1.3 * tanf(sp->phi); + } else { //max 60 degrees roll, then take constant omega + omega = 1.3 * 1.72305 * ((sp->phi > 0) - (sp->phi < 0)); + } - sp->psi += omega/RC_UPDATE_FREQ; + sp->psi += omega / RC_UPDATE_FREQ; } #ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT // Make sure the yaw setpoint does not differ too much from the real yaw @@ -252,10 +266,9 @@ void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bo float delta_psi = sp->psi - heading; FLOAT_ANGLE_NORMALIZE(delta_psi); - if (delta_psi > STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT){ + if (delta_psi > STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT) { sp->psi = heading + STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT; - } - else if (delta_psi < -STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT){ + } else if (delta_psi < -STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT) { sp->psi = heading - STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT; } FLOAT_ANGLE_NORMALIZE(sp->psi); @@ -274,13 +287,12 @@ void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bo sin_psi = sinf(care_free_delta_psi_f); cos_psi = cosf(care_free_delta_psi_f); - temp_theta = cos_psi*sp->theta - sin_psi*sp->phi; - sp->phi = cos_psi*sp->phi - sin_psi*sp->theta; + temp_theta = cos_psi * sp->theta - sin_psi * sp->phi; + sp->phi = cos_psi * sp->phi - sin_psi * sp->theta; sp->theta = temp_theta; } - } - else { /* if not flying, use current yaw as setpoint */ + } else { /* if not flying, use current yaw as setpoint */ sp->psi = stateGetNedToBodyEulers_f()->psi; } } @@ -290,7 +302,8 @@ void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bo * Interprets the stick positions as axes. * @param[out] q quaternion representing the RC roll/pitch input */ -void stabilization_attitude_read_rc_roll_pitch_quat_f(struct FloatQuat* q) { +void stabilization_attitude_read_rc_roll_pitch_quat_f(struct FloatQuat *q) +{ /* orientation vector describing simultaneous rotation of roll/pitch */ struct FloatVect3 ov; ov.x = get_rc_roll_f(); @@ -305,7 +318,8 @@ void stabilization_attitude_read_rc_roll_pitch_quat_f(struct FloatQuat* q) { * Both angles are are interpreted relative to to the horizontal plane (earth bound). * @param[out] q quaternion representing the RC roll/pitch input */ -void stabilization_attitude_read_rc_roll_pitch_earth_quat_f(struct FloatQuat* q) { +void stabilization_attitude_read_rc_roll_pitch_earth_quat_f(struct FloatQuat *q) +{ /* only non-zero entries for roll quaternion */ float roll2 = get_rc_roll_f() / 2.0f; float qx_roll = sinf(roll2); @@ -331,7 +345,9 @@ void stabilization_attitude_read_rc_roll_pitch_earth_quat_f(struct FloatQuat* q) * @param[in] in_flight true if in flight * @param[out] q_sp attitude setpoint as quaternion */ -void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat* q_sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) { +void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat *q_sp, bool_t in_flight, bool_t in_carefree, + bool_t coordinated_turn) +{ // FIXME: remove me, do in quaternion directly // is currently still needed, since the yaw setpoint integration is done in eulers @@ -352,8 +368,7 @@ void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat* q_sp, bool if (in_carefree) { //care_free_heading has been set to current psi when entering care free mode. float_quat_of_axis_angle(&q_yaw, &zaxis, care_free_heading); - } - else { + } else { float_quat_of_axis_angle(&q_yaw, &zaxis, stateGetNedToBodyEulers_f()->psi); } @@ -362,8 +377,7 @@ void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat* q_sp, bool float_quat_comp(&q_rp_sp, &q_yaw, &q_rp_cmd); float_quat_normalize(&q_rp_sp); - if (in_flight) - { + if (in_flight) { /* get current heading setpoint */ struct FloatQuat q_yaw_sp; #if defined STABILIZATION_ATTITUDE_TYPE_INT @@ -384,14 +398,16 @@ void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat* q_sp, bool } //Function that reads the rc setpoint in an earth bound frame -void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat* q_sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) { +void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat *q_sp, bool_t in_flight, + bool_t in_carefree, bool_t coordinated_turn) +{ // FIXME: remove me, do in quaternion directly // is currently still needed, since the yaw setpoint integration is done in eulers - #if defined STABILIZATION_ATTITUDE_TYPE_INT +#if defined STABILIZATION_ATTITUDE_TYPE_INT stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn); - #else +#else stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn); - #endif +#endif const struct FloatVect3 zaxis = {0., 0., 1.}; @@ -402,15 +418,14 @@ void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat /* get current heading setpoint */ struct FloatQuat q_yaw_sp; - #if defined STABILIZATION_ATTITUDE_TYPE_INT +#if defined STABILIZATION_ATTITUDE_TYPE_INT float_quat_of_axis_angle(&q_yaw_sp, &zaxis, ANGLE_FLOAT_OF_BFP(stab_att_sp_euler.psi)); - #else +#else float_quat_of_axis_angle(&q_yaw_sp, &zaxis, stab_att_sp_euler.psi); - #endif +#endif float_quat_comp(q_sp, &q_yaw_sp, &q_rp_cmd); - } - else { + } else { struct FloatQuat q_yaw; float_quat_of_axis_angle(&q_yaw, &zaxis, stateGetNedToBodyEulers_f()->psi); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h index 401f479772..80a5834df8 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h @@ -33,11 +33,15 @@ extern void stabilization_attitude_reset_care_free_heading(void); extern int32_t stabilization_attitude_get_heading_i(void); extern float stabilization_attitude_get_heading_f(void); -extern void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn); -extern void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn); -extern void stabilization_attitude_read_rc_roll_pitch_quat_f(struct FloatQuat* q); -extern void stabilization_attitude_read_rc_roll_pitch_earth_quat_f(struct FloatQuat* q); -extern void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat* q_sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn); -extern void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat* q_sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn); +extern void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight, bool_t in_carefree, + bool_t coordinated_turn); +extern void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool_t in_flight, + bool_t in_carefree, bool_t coordinated_turn); +extern void stabilization_attitude_read_rc_roll_pitch_quat_f(struct FloatQuat *q); +extern void stabilization_attitude_read_rc_roll_pitch_earth_quat_f(struct FloatQuat *q); +extern void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat *q_sp, bool_t in_flight, bool_t in_carefree, + bool_t coordinated_turn); +extern void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat *q_sp, bool_t in_flight, + bool_t in_carefree, bool_t coordinated_turn); #endif /* STABILIZATION_ATTITUDE_RC_SETPOINT_H */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c index 5a38eb3605..de93c7be81 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_float.c @@ -34,7 +34,8 @@ struct FloatEulers stab_att_ref_euler; struct FloatRates stab_att_ref_rate; struct FloatRates stab_att_ref_accel; -void stabilization_attitude_ref_init(void) { +void stabilization_attitude_ref_init(void) +{ FLOAT_EULERS_ZERO(stab_att_sp_euler); FLOAT_EULERS_ZERO(stab_att_ref_euler); @@ -68,7 +69,8 @@ void stabilization_attitude_ref_init(void) { #define USE_REF 1 -static inline void reset_psi_ref_from_body(void) { +static inline void reset_psi_ref_from_body(void) +{ //sp has been set from body using stabilization_attitude_get_yaw_f, use that value stab_att_ref_euler.psi = stab_att_sp_euler.psi; stab_att_ref_rate.r = 0; @@ -80,7 +82,8 @@ void stabilization_attitude_ref_enter() reset_psi_ref_from_body(); } -void stabilization_attitude_ref_update() { +void stabilization_attitude_ref_update() +{ #if USE_REF @@ -89,7 +92,7 @@ void stabilization_attitude_ref_update() { RATES_SMUL(delta_rate, stab_att_ref_rate, DT_UPDATE); struct FloatEulers delta_angle; EULERS_ASSIGN(delta_angle, delta_rate.p, delta_rate.q, delta_rate.r); - EULERS_ADD(stab_att_ref_euler, delta_angle ); + EULERS_ADD(stab_att_ref_euler, delta_angle); FLOAT_ANGLE_NORMALIZE(stab_att_ref_euler.psi); /* integrate reference rotational speeds */ @@ -104,11 +107,11 @@ void stabilization_attitude_ref_update() { FLOAT_ANGLE_NORMALIZE(ref_err.psi); /* compute reference angular accelerations */ - stab_att_ref_accel.p = -2.*ZETA_P*OMEGA_P*stab_att_ref_rate.p - OMEGA_P*OMEGA_P*ref_err.phi; - stab_att_ref_accel.q = -2.*ZETA_Q*OMEGA_P*stab_att_ref_rate.q - OMEGA_Q*OMEGA_Q*ref_err.theta; - stab_att_ref_accel.r = -2.*ZETA_R*OMEGA_P*stab_att_ref_rate.r - OMEGA_R*OMEGA_R*ref_err.psi; + stab_att_ref_accel.p = -2.*ZETA_P * OMEGA_P * stab_att_ref_rate.p - OMEGA_P * OMEGA_P * ref_err.phi; + stab_att_ref_accel.q = -2.*ZETA_Q * OMEGA_P * stab_att_ref_rate.q - OMEGA_Q * OMEGA_Q * ref_err.theta; + stab_att_ref_accel.r = -2.*ZETA_R * OMEGA_P * stab_att_ref_rate.r - OMEGA_R * OMEGA_R * ref_err.psi; - /* saturate acceleration */ + /* saturate acceleration */ const struct FloatRates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R }; const struct FloatRates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R }; RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c index 14472fbb03..fa7f6a4b20 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c @@ -33,12 +33,13 @@ struct Int32Eulers stab_att_ref_euler; ///< with #REF_ANGLE_FRAC struct Int32Rates stab_att_ref_rate; struct Int32Rates stab_att_ref_accel; -void stabilization_attitude_ref_init(void) { +void stabilization_attitude_ref_init(void) +{ INT_EULERS_ZERO(stab_att_sp_euler); INT_EULERS_ZERO(stab_att_ref_euler); - INT_RATES_ZERO( stab_att_ref_rate); - INT_RATES_ZERO( stab_att_ref_accel); + INT_RATES_ZERO(stab_att_ref_rate); + INT_RATES_ZERO(stab_att_ref_accel); } @@ -88,23 +89,26 @@ void stabilization_attitude_ref_init(void) { #define USE_ATTITUDE_REF 1 #endif -void stabilization_attitude_ref_update() { +void stabilization_attitude_ref_update() +{ #if USE_ATTITUDE_REF /* dumb integrate reference attitude */ const struct Int32Eulers d_angle = { - stab_att_ref_rate.p >> ( F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC), - stab_att_ref_rate.q >> ( F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC), - stab_att_ref_rate.r >> ( F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC)}; - EULERS_ADD(stab_att_ref_euler, d_angle ); + stab_att_ref_rate.p >> (F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC), + stab_att_ref_rate.q >> (F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC), + stab_att_ref_rate.r >> (F_UPDATE_RES + REF_RATE_FRAC - REF_ANGLE_FRAC) + }; + EULERS_ADD(stab_att_ref_euler, d_angle); ANGLE_REF_NORMALIZE(stab_att_ref_euler.psi); /* integrate reference rotational speeds */ const struct Int32Rates d_rate = { - stab_att_ref_accel.p >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC), - stab_att_ref_accel.q >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC), - stab_att_ref_accel.r >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC)}; + stab_att_ref_accel.p >> (F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC), + stab_att_ref_accel.q >> (F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC), + stab_att_ref_accel.r >> (F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC) + }; RATES_ADD(stab_att_ref_rate, d_rate); /* attitude setpoint with REF_ANGLE_FRAC */ @@ -120,20 +124,22 @@ void stabilization_attitude_ref_update() { /* compute reference angular accelerations */ const struct Int32Rates accel_rate = { ((int32_t)(-2.*ZETA_OMEGA_P) * (stab_att_ref_rate.p >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) - >> (ZETA_OMEGA_P_RES), + >> (ZETA_OMEGA_P_RES), ((int32_t)(-2.*ZETA_OMEGA_Q) * (stab_att_ref_rate.q >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) >> (ZETA_OMEGA_Q_RES), ((int32_t)(-2.*ZETA_OMEGA_R) * (stab_att_ref_rate.r >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) - >> (ZETA_OMEGA_R_RES) }; + >> (ZETA_OMEGA_R_RES) + }; const struct Int32Rates accel_angle = { - ((int32_t)(-OMEGA_2_P)* (ref_err.phi >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_P_RES), - ((int32_t)(-OMEGA_2_Q)* (ref_err.theta >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_Q_RES), - ((int32_t)(-OMEGA_2_R)* (ref_err.psi >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_R_RES) }; + ((int32_t)(-OMEGA_2_P) * (ref_err.phi >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_P_RES), + ((int32_t)(-OMEGA_2_Q) * (ref_err.theta >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_Q_RES), + ((int32_t)(-OMEGA_2_R) * (ref_err.psi >> (REF_ANGLE_FRAC - REF_ACCEL_FRAC))) >> (OMEGA_2_R_RES) + }; RATES_SUM(stab_att_ref_accel, accel_rate, accel_angle); - /* saturate acceleration */ + /* saturate acceleration */ const struct Int32Rates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R }; const struct Int32Rates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R }; RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c index 17c93cd299..ea3aab080a 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c @@ -60,70 +60,81 @@ static const float zeta_r[] = STABILIZATION_ATTITUDE_REF_ZETA_R; struct FloatRates two_omega_squared[STABILIZATION_ATTITUDE_GAIN_NB]; -static inline void reset_psi_ref_from_body(void) { +static inline void reset_psi_ref_from_body(void) +{ //sp has been set from body using stabilization_attitude_get_yaw_f, use that value stab_att_ref_euler.psi = stab_att_sp_euler.psi; stab_att_ref_rate.r = 0; stab_att_ref_accel.r = 0; } -static inline void update_ref_quat_from_eulers(void) { +static inline void update_ref_quat_from_eulers(void) +{ struct FloatRMat ref_rmat; float_rmat_of_eulers(&ref_rmat, &stab_att_ref_euler); float_quat_of_rmat(&stab_att_ref_quat, &ref_rmat); float_quat_wrap_shortest(&stab_att_ref_quat); } -void stabilization_attitude_ref_idx_set_omega_p(uint8_t idx, float omega) { +void stabilization_attitude_ref_idx_set_omega_p(uint8_t idx, float omega) +{ stab_att_ref_model[idx].omega.p = omega; two_omega_squared[idx].p = 2 * omega * omega; } -void stabilization_attitude_ref_idx_set_omega_q(uint8_t idx, float omega) { +void stabilization_attitude_ref_idx_set_omega_q(uint8_t idx, float omega) +{ stab_att_ref_model[idx].omega.q = omega; two_omega_squared[idx].q = 2 * omega * omega; } -void stabilization_attitude_ref_idx_set_omega_r(uint8_t idx, float omega) { +void stabilization_attitude_ref_idx_set_omega_r(uint8_t idx, float omega) +{ stab_att_ref_model[idx].omega.r = omega; two_omega_squared[idx].r = 2 * omega * omega; } -void stabilization_attitude_ref_set_omega_p(float omega) { +void stabilization_attitude_ref_set_omega_p(float omega) +{ stabilization_attitude_ref_idx_set_omega_p(ref_idx, omega); } -void stabilization_attitude_ref_set_omega_q(float omega) { +void stabilization_attitude_ref_set_omega_q(float omega) +{ stabilization_attitude_ref_idx_set_omega_q(ref_idx, omega); } -void stabilization_attitude_ref_set_omega_r(float omega) { +void stabilization_attitude_ref_set_omega_r(float omega) +{ stabilization_attitude_ref_idx_set_omega_r(ref_idx, omega); } -void stabilization_attitude_ref_init(void) { +void stabilization_attitude_ref_init(void) +{ FLOAT_EULERS_ZERO(stab_att_sp_euler); float_quat_identity(&stab_att_sp_quat); FLOAT_EULERS_ZERO(stab_att_ref_euler); float_quat_identity(&stab_att_ref_quat); - FLOAT_RATES_ZERO( stab_att_ref_rate); - FLOAT_RATES_ZERO( stab_att_ref_accel); + FLOAT_RATES_ZERO(stab_att_ref_rate); + FLOAT_RATES_ZERO(stab_att_ref_accel); for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) { RATES_ASSIGN(stab_att_ref_model[i].omega, omega_p[i], omega_q[i], omega_r[i]); RATES_ASSIGN(stab_att_ref_model[i].zeta, zeta_p[i], zeta_q[i], zeta_r[i]); - RATES_ASSIGN(two_omega_squared[i], 2*omega_p[i]*omega_p[i], 2*omega_q[i]*omega_q[i], 2*omega_r[i]*omega_r[i]); + RATES_ASSIGN(two_omega_squared[i], 2 * omega_p[i]*omega_p[i], 2 * omega_q[i]*omega_q[i], 2 * omega_r[i]*omega_r[i]); } } -void stabilization_attitude_ref_schedule(uint8_t idx) { +void stabilization_attitude_ref_schedule(uint8_t idx) +{ ref_idx = idx; } -void stabilization_attitude_ref_enter(void) { +void stabilization_attitude_ref_enter(void) +{ reset_psi_ref_from_body(); update_ref_quat_from_eulers(); } @@ -138,7 +149,8 @@ void stabilization_attitude_ref_enter(void) { #define STABILIZATION_ATTITUDE_REF_QUAT_INFINITESIMAL_STEP TRUE #endif -void stabilization_attitude_ref_update(void) { +void stabilization_attitude_ref_update(void) +{ /* integrate reference attitude */ #if STABILIZATION_ATTITUDE_REF_QUAT_INFINITESIMAL_STEP @@ -169,14 +181,17 @@ void stabilization_attitude_ref_update(void) { float_quat_wrap_shortest(&err); /* propagate the 2nd order linear model: xdotdot = -2*zeta*omega*xdot - omega^2*x */ /* since error quaternion contains the half-angles we get 2*omega^2*err */ - stab_att_ref_accel.p = -2.*stab_att_ref_model[ref_idx].zeta.p*stab_att_ref_model[ref_idx].omega.p*stab_att_ref_rate.p - - two_omega_squared[ref_idx].p * err.qx; - stab_att_ref_accel.q = -2.*stab_att_ref_model[ref_idx].zeta.q*stab_att_ref_model[ref_idx].omega.q*stab_att_ref_rate.q - - two_omega_squared[ref_idx].q * err.qy; - stab_att_ref_accel.r = -2.*stab_att_ref_model[ref_idx].zeta.r*stab_att_ref_model[ref_idx].omega.r*stab_att_ref_rate.r - - two_omega_squared[ref_idx].r * err.qz; + stab_att_ref_accel.p = -2.*stab_att_ref_model[ref_idx].zeta.p * stab_att_ref_model[ref_idx].omega.p * + stab_att_ref_rate.p + - two_omega_squared[ref_idx].p * err.qx; + stab_att_ref_accel.q = -2.*stab_att_ref_model[ref_idx].zeta.q * stab_att_ref_model[ref_idx].omega.q * + stab_att_ref_rate.q + - two_omega_squared[ref_idx].q * err.qy; + stab_att_ref_accel.r = -2.*stab_att_ref_model[ref_idx].zeta.r * stab_att_ref_model[ref_idx].omega.r * + stab_att_ref_rate.r + - two_omega_squared[ref_idx].r * err.qz; - /* saturate acceleration */ + /* saturate acceleration */ const struct FloatRates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R }; const struct FloatRates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R }; RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c index cb6b07a9a6..abf5dcf762 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c @@ -57,79 +57,93 @@ struct FloatRefModel stab_att_ref_model = { static struct Int32Rates two_zeta_omega; static struct Int32Rates two_omega_2; -static void update_ref_model_p(void) { - two_zeta_omega.p = BFP_OF_REAL((2*stab_att_ref_model.zeta.p * stab_att_ref_model.omega.p), TWO_ZETA_OMEGA_RES); - two_omega_2.p = BFP_OF_REAL((2*stab_att_ref_model.omega.p * stab_att_ref_model.omega.p), TWO_OMEGA_2_RES); +static void update_ref_model_p(void) +{ + two_zeta_omega.p = BFP_OF_REAL((2 * stab_att_ref_model.zeta.p * stab_att_ref_model.omega.p), TWO_ZETA_OMEGA_RES); + two_omega_2.p = BFP_OF_REAL((2 * stab_att_ref_model.omega.p * stab_att_ref_model.omega.p), TWO_OMEGA_2_RES); } -static void update_ref_model_q(void) { - two_zeta_omega.q = BFP_OF_REAL((2*stab_att_ref_model.zeta.q * stab_att_ref_model.omega.q), TWO_ZETA_OMEGA_RES); - two_omega_2.q = BFP_OF_REAL((2*stab_att_ref_model.omega.q * stab_att_ref_model.omega.q), TWO_OMEGA_2_RES); +static void update_ref_model_q(void) +{ + two_zeta_omega.q = BFP_OF_REAL((2 * stab_att_ref_model.zeta.q * stab_att_ref_model.omega.q), TWO_ZETA_OMEGA_RES); + two_omega_2.q = BFP_OF_REAL((2 * stab_att_ref_model.omega.q * stab_att_ref_model.omega.q), TWO_OMEGA_2_RES); } -static void update_ref_model_r(void) { - two_zeta_omega.r = BFP_OF_REAL((2*stab_att_ref_model.zeta.r * stab_att_ref_model.omega.r), TWO_ZETA_OMEGA_RES); - two_omega_2.r = BFP_OF_REAL((2*stab_att_ref_model.omega.r * stab_att_ref_model.omega.r), TWO_OMEGA_2_RES); +static void update_ref_model_r(void) +{ + two_zeta_omega.r = BFP_OF_REAL((2 * stab_att_ref_model.zeta.r * stab_att_ref_model.omega.r), TWO_ZETA_OMEGA_RES); + two_omega_2.r = BFP_OF_REAL((2 * stab_att_ref_model.omega.r * stab_att_ref_model.omega.r), TWO_OMEGA_2_RES); } -static void update_ref_model(void) { +static void update_ref_model(void) +{ update_ref_model_p(); update_ref_model_q(); update_ref_model_r(); } -void stabilization_attitude_ref_set_omega_p(float omega_p) { +void stabilization_attitude_ref_set_omega_p(float omega_p) +{ stab_att_ref_model.omega.p = omega_p; update_ref_model_p(); } -void stabilization_attitude_ref_set_omega_q(float omega_q) { +void stabilization_attitude_ref_set_omega_q(float omega_q) +{ stab_att_ref_model.omega.q = omega_q; update_ref_model_q(); } -void stabilization_attitude_ref_set_omega_r(float omega_r) { +void stabilization_attitude_ref_set_omega_r(float omega_r) +{ stab_att_ref_model.omega.r = omega_r; update_ref_model_r(); } -void stabilization_attitude_ref_set_omega(struct FloatRates *omega) { +void stabilization_attitude_ref_set_omega(struct FloatRates *omega) +{ stabilization_attitude_ref_set_omega_p(omega->p); stabilization_attitude_ref_set_omega_q(omega->q); stabilization_attitude_ref_set_omega_r(omega->r); } -void stabilization_attitude_ref_set_zeta_p(float zeta_p) { +void stabilization_attitude_ref_set_zeta_p(float zeta_p) +{ stab_att_ref_model.zeta.p = zeta_p; update_ref_model_p(); } -void stabilization_attitude_ref_set_zeta_q(float zeta_q) { +void stabilization_attitude_ref_set_zeta_q(float zeta_q) +{ stab_att_ref_model.zeta.q = zeta_q; update_ref_model_q(); } -void stabilization_attitude_ref_set_zeta_r(float zeta_r) { +void stabilization_attitude_ref_set_zeta_r(float zeta_r) +{ stab_att_ref_model.zeta.r = zeta_r; update_ref_model_r(); } -void stabilization_attitude_ref_set_zeta(struct FloatRates *zeta) { +void stabilization_attitude_ref_set_zeta(struct FloatRates *zeta) +{ stabilization_attitude_ref_set_zeta_p(zeta->p); stabilization_attitude_ref_set_zeta_q(zeta->q); stabilization_attitude_ref_set_zeta_r(zeta->r); } -static inline void reset_psi_ref_from_body(void) { +static inline void reset_psi_ref_from_body(void) +{ //sp has been set from body using stabilization_attitude_get_yaw_i, use that value stab_att_ref_euler.psi = stab_att_sp_euler.psi << (REF_ANGLE_FRAC - INT32_ANGLE_FRAC); stab_att_ref_rate.r = 0; stab_att_ref_accel.r = 0; } -void stabilization_attitude_ref_init(void) { +void stabilization_attitude_ref_init(void) +{ INT_EULERS_ZERO(stab_att_sp_euler); int32_quat_identity(&stab_att_sp_quat); @@ -168,13 +182,15 @@ void stabilization_attitude_ref_enter(void) #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b)) -void stabilization_attitude_ref_update(void) { +void stabilization_attitude_ref_update(void) +{ /* integrate reference attitude */ const struct Int32Rates rate_ref_scaled = { OFFSET_AND_ROUND(stab_att_ref_rate.p, (REF_RATE_FRAC - INT32_RATE_FRAC)), OFFSET_AND_ROUND(stab_att_ref_rate.q, (REF_RATE_FRAC - INT32_RATE_FRAC)), - OFFSET_AND_ROUND(stab_att_ref_rate.r, (REF_RATE_FRAC - INT32_RATE_FRAC)) }; + OFFSET_AND_ROUND(stab_att_ref_rate.r, (REF_RATE_FRAC - INT32_RATE_FRAC)) + }; struct Int32Quat qdot; int32_quat_derivative(&qdot, &rate_ref_scaled, &stab_att_ref_quat); //QUAT_SMUL(qdot, qdot, DT_UPDATE); @@ -190,9 +206,10 @@ void stabilization_attitude_ref_update(void) { * ref_rate = old_ref_rate + delta_rate */ const struct Int32Rates delta_rate = { - stab_att_ref_accel.p >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC), - stab_att_ref_accel.q >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC), - stab_att_ref_accel.r >> ( F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC)}; + stab_att_ref_accel.p >> (F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC), + stab_att_ref_accel.q >> (F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC), + stab_att_ref_accel.r >> (F_UPDATE_RES + REF_ACCEL_FRAC - REF_RATE_FRAC) + }; RATES_ADD(stab_att_ref_rate, delta_rate); /* compute reference angular accelerations */ @@ -207,13 +224,15 @@ void stabilization_attitude_ref_update(void) { const struct Int32Rates accel_rate = { (-two_zeta_omega.p * (stab_att_ref_rate.p >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) >> (TWO_ZETA_OMEGA_RES), (-two_zeta_omega.q * (stab_att_ref_rate.q >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) >> (TWO_ZETA_OMEGA_RES), - (-two_zeta_omega.r * (stab_att_ref_rate.r >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) >> (TWO_ZETA_OMEGA_RES) }; + (-two_zeta_omega.r * (stab_att_ref_rate.r >> (REF_RATE_FRAC - REF_ACCEL_FRAC))) >> (TWO_ZETA_OMEGA_RES) + }; /* since error quaternion contains the half-angles we get 2*omega^2*err */ const struct Int32Rates accel_angle = { (-two_omega_2.p * (err.qx >> (INT32_QUAT_FRAC - REF_ACCEL_FRAC))) >> (TWO_OMEGA_2_RES), (-two_omega_2.q * (err.qy >> (INT32_QUAT_FRAC - REF_ACCEL_FRAC))) >> (TWO_OMEGA_2_RES), - (-two_omega_2.r * (err.qz >> (INT32_QUAT_FRAC - REF_ACCEL_FRAC))) >> (TWO_OMEGA_2_RES) }; + (-two_omega_2.r * (err.qz >> (INT32_QUAT_FRAC - REF_ACCEL_FRAC))) >> (TWO_OMEGA_2_RES) + }; RATES_SUM(stab_att_ref_accel, accel_rate, accel_angle); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h index 5bb4efb06d..30fac212a3 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h @@ -27,32 +27,32 @@ #define STABILIZATION_ATTITUDE_REF_SATURATE_H #define SATURATE_SPEED_TRIM_ACCEL() { \ - if (stab_att_ref_rate.p >= REF_RATE_MAX_P) { \ + if (stab_att_ref_rate.p >= REF_RATE_MAX_P) { \ stab_att_ref_rate.p = REF_RATE_MAX_P; \ if (stab_att_ref_accel.p > 0) \ stab_att_ref_accel.p = 0; \ } \ - else if (stab_att_ref_rate.p <= -REF_RATE_MAX_P) { \ + else if (stab_att_ref_rate.p <= -REF_RATE_MAX_P) { \ stab_att_ref_rate.p = -REF_RATE_MAX_P; \ if (stab_att_ref_accel.p < 0) \ stab_att_ref_accel.p = 0; \ } \ - if (stab_att_ref_rate.q >= REF_RATE_MAX_Q) { \ + if (stab_att_ref_rate.q >= REF_RATE_MAX_Q) { \ stab_att_ref_rate.q = REF_RATE_MAX_Q; \ if (stab_att_ref_accel.q > 0) \ stab_att_ref_accel.q = 0; \ } \ - else if (stab_att_ref_rate.q <= -REF_RATE_MAX_Q) { \ + else if (stab_att_ref_rate.q <= -REF_RATE_MAX_Q) { \ stab_att_ref_rate.q = -REF_RATE_MAX_Q; \ if (stab_att_ref_accel.q < 0) \ stab_att_ref_accel.q = 0; \ } \ - if (stab_att_ref_rate.r >= REF_RATE_MAX_R) { \ + if (stab_att_ref_rate.r >= REF_RATE_MAX_R) { \ stab_att_ref_rate.r = REF_RATE_MAX_R; \ if (stab_att_ref_accel.r > 0) \ stab_att_ref_accel.r = 0; \ } \ - else if (stab_att_ref_rate.r <= -REF_RATE_MAX_R) { \ + else if (stab_att_ref_rate.r <= -REF_RATE_MAX_R) { \ stab_att_ref_rate.r = -REF_RATE_MAX_R; \ if (stab_att_ref_accel.r < 0) \ stab_att_ref_accel.r = 0; \ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.c index 36550c9869..a3aca85b22 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.c @@ -34,22 +34,26 @@ struct Int32Rates stabilization_none_rc_cmd; -void stabilization_none_init(void) { +void stabilization_none_init(void) +{ INT_RATES_ZERO(stabilization_none_rc_cmd); } -void stabilization_none_read_rc( void ) { +void stabilization_none_read_rc(void) +{ - stabilization_none_rc_cmd.p = (int32_t)radio_control.values[RADIO_ROLL]; - stabilization_none_rc_cmd.q = (int32_t)radio_control.values[RADIO_PITCH]; - stabilization_none_rc_cmd.r = (int32_t)radio_control.values[RADIO_YAW]; + stabilization_none_rc_cmd.p = (int32_t)radio_control.values[RADIO_ROLL]; + stabilization_none_rc_cmd.q = (int32_t)radio_control.values[RADIO_PITCH]; + stabilization_none_rc_cmd.r = (int32_t)radio_control.values[RADIO_YAW]; } -void stabilization_none_enter(void) { +void stabilization_none_enter(void) +{ INT_RATES_ZERO(stabilization_none_rc_cmd); } -void stabilization_none_run(bool_t in_flight __attribute__ ((unused))) { +void stabilization_none_run(bool_t in_flight __attribute__((unused))) +{ /* just directly pass rc commands through */ stabilization_cmd[COMMAND_ROLL] = stabilization_none_rc_cmd.p; stabilization_cmd[COMMAND_PITCH] = stabilization_none_rc_cmd.q; diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c index 1b40f8445e..8df1ade787 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c @@ -118,31 +118,33 @@ struct Int32Rates stabilization_rate_ff_cmd; #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_rate(struct transport_tx *trans, struct link_device *dev) { +static void send_rate(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_RATE_LOOP(trans, dev, AC_ID, - &stabilization_rate_sp.p, - &stabilization_rate_sp.q, - &stabilization_rate_sp.r, - &stabilization_rate_ref.p, - &stabilization_rate_ref.q, - &stabilization_rate_ref.r, - &stabilization_rate_refdot.p, - &stabilization_rate_refdot.q, - &stabilization_rate_refdot.r, - &stabilization_rate_sum_err.p, - &stabilization_rate_sum_err.q, - &stabilization_rate_sum_err.r, - &stabilization_rate_ff_cmd.p, - &stabilization_rate_ff_cmd.q, - &stabilization_rate_ff_cmd.r, - &stabilization_rate_fb_cmd.p, - &stabilization_rate_fb_cmd.q, - &stabilization_rate_fb_cmd.r, - &stabilization_cmd[COMMAND_THRUST]); + &stabilization_rate_sp.p, + &stabilization_rate_sp.q, + &stabilization_rate_sp.r, + &stabilization_rate_ref.p, + &stabilization_rate_ref.q, + &stabilization_rate_ref.r, + &stabilization_rate_refdot.p, + &stabilization_rate_refdot.q, + &stabilization_rate_refdot.r, + &stabilization_rate_sum_err.p, + &stabilization_rate_sum_err.q, + &stabilization_rate_sum_err.r, + &stabilization_rate_ff_cmd.p, + &stabilization_rate_ff_cmd.q, + &stabilization_rate_ff_cmd.r, + &stabilization_rate_fb_cmd.p, + &stabilization_rate_fb_cmd.q, + &stabilization_rate_fb_cmd.r, + &stabilization_cmd[COMMAND_THRUST]); } #endif -void stabilization_rate_init(void) { +void stabilization_rate_init(void) +{ INT_RATES_ZERO(stabilization_rate_sp); @@ -169,55 +171,65 @@ void stabilization_rate_init(void) { } -void stabilization_rate_read_rc( void ) { +void stabilization_rate_read_rc(void) +{ - if (ROLL_RATE_DEADBAND_EXCEEDED()) + if (ROLL_RATE_DEADBAND_EXCEEDED()) { stabilization_rate_sp.p = (int32_t)radio_control.values[RADIO_ROLL] * STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ; - else + } else { stabilization_rate_sp.p = 0; + } - if (PITCH_RATE_DEADBAND_EXCEEDED()) + if (PITCH_RATE_DEADBAND_EXCEEDED()) { stabilization_rate_sp.q = (int32_t)radio_control.values[RADIO_PITCH] * STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ; - else + } else { stabilization_rate_sp.q = 0; + } - if (YAW_RATE_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) + if (YAW_RATE_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) { stabilization_rate_sp.r = (int32_t)radio_control.values[RADIO_YAW] * STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ; - else + } else { stabilization_rate_sp.r = 0; + } // Setpoint at ref resolution INT_RATES_LSHIFT(stabilization_rate_sp, stabilization_rate_sp, REF_FRAC - INT32_RATE_FRAC); } //Read rc with roll and yaw sitcks switched if the default orientation is vertical but airplane sticks are desired -void stabilization_rate_read_rc_switched_sticks( void ) { +void stabilization_rate_read_rc_switched_sticks(void) +{ - if (ROLL_RATE_DEADBAND_EXCEEDED()) - stabilization_rate_sp.r = (int32_t) -radio_control.values[RADIO_ROLL] * STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ; - else + if (ROLL_RATE_DEADBAND_EXCEEDED()) { + stabilization_rate_sp.r = (int32_t) - radio_control.values[RADIO_ROLL] * STABILIZATION_RATE_SP_MAX_P / MAX_PPRZ; + } else { stabilization_rate_sp.r = 0; + } - if (PITCH_RATE_DEADBAND_EXCEEDED()) + if (PITCH_RATE_DEADBAND_EXCEEDED()) { stabilization_rate_sp.q = (int32_t)radio_control.values[RADIO_PITCH] * STABILIZATION_RATE_SP_MAX_Q / MAX_PPRZ; - else + } else { stabilization_rate_sp.q = 0; + } - if (YAW_RATE_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) + if (YAW_RATE_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) { stabilization_rate_sp.p = (int32_t)radio_control.values[RADIO_YAW] * STABILIZATION_RATE_SP_MAX_R / MAX_PPRZ; - else + } else { stabilization_rate_sp.p = 0; + } // Setpoint at ref resolution - INT_RATES_LSHIFT(stabilization_rate_sp, stabilization_rate_sp, REF_FRAC - INT32_RATE_FRAC); + INT_RATES_LSHIFT(stabilization_rate_sp, stabilization_rate_sp, REF_FRAC - INT32_RATE_FRAC); } -void stabilization_rate_enter(void) { +void stabilization_rate_enter(void) +{ RATES_COPY(stabilization_rate_ref, stabilization_rate_sp); INT_RATES_ZERO(stabilization_rate_sum_err); } -void stabilization_rate_run(bool_t in_flight) { +void stabilization_rate_run(bool_t in_flight) +{ /* reference */ struct Int32Rates _r; @@ -225,9 +237,10 @@ void stabilization_rate_run(bool_t in_flight) { RATES_SDIV(stabilization_rate_refdot, _r, STABILIZATION_RATE_REF_TAU); /* integrate ref */ const struct Int32Rates _delta_ref = { - stabilization_rate_refdot.p >> ( F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC), - stabilization_rate_refdot.q >> ( F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC), - stabilization_rate_refdot.r >> ( F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC)}; + stabilization_rate_refdot.p >> (F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC), + stabilization_rate_refdot.q >> (F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC), + stabilization_rate_refdot.r >> (F_UPDATE_RES + REF_DOT_FRAC - REF_FRAC) + }; RATES_ADD(stabilization_rate_ref, _delta_ref); /* compute feed-forward command */ @@ -239,28 +252,28 @@ void stabilization_rate_run(bool_t in_flight) { const struct Int32Rates _ref_scaled = { OFFSET_AND_ROUND(stabilization_rate_ref.p, (REF_FRAC - INT32_RATE_FRAC)), OFFSET_AND_ROUND(stabilization_rate_ref.q, (REF_FRAC - INT32_RATE_FRAC)), - OFFSET_AND_ROUND(stabilization_rate_ref.r, (REF_FRAC - INT32_RATE_FRAC)) }; + OFFSET_AND_ROUND(stabilization_rate_ref.r, (REF_FRAC - INT32_RATE_FRAC)) + }; struct Int32Rates _error; - struct Int32Rates* body_rate = stateGetBodyRates_i(); + struct Int32Rates *body_rate = stateGetBodyRates_i(); RATES_DIFF(_error, _ref_scaled, (*body_rate)); if (in_flight) { /* update integrator */ RATES_ADD(stabilization_rate_sum_err, _error); RATES_BOUND_CUBE(stabilization_rate_sum_err, -MAX_SUM_ERR, MAX_SUM_ERR); - } - else { + } else { INT_RATES_ZERO(stabilization_rate_sum_err); } /* PI */ stabilization_rate_fb_cmd.p = stabilization_rate_gain.p * _error.p + - OFFSET_AND_ROUND2((stabilization_rate_igain.p * stabilization_rate_sum_err.p), 10); + OFFSET_AND_ROUND2((stabilization_rate_igain.p * stabilization_rate_sum_err.p), 10); stabilization_rate_fb_cmd.q = stabilization_rate_gain.q * _error.q + - OFFSET_AND_ROUND2((stabilization_rate_igain.q * stabilization_rate_sum_err.q), 10); + OFFSET_AND_ROUND2((stabilization_rate_igain.q * stabilization_rate_sum_err.q), 10); stabilization_rate_fb_cmd.r = stabilization_rate_gain.r * _error.r + - OFFSET_AND_ROUND2((stabilization_rate_igain.r * stabilization_rate_sum_err.r), 10); + OFFSET_AND_ROUND2((stabilization_rate_igain.r * stabilization_rate_sum_err.r), 10); stabilization_rate_fb_cmd.p = stabilization_rate_fb_cmd.p >> 11; stabilization_rate_fb_cmd.q = stabilization_rate_fb_cmd.q >> 11; diff --git a/sw/airborne/firmwares/setup/setup_actuators.c b/sw/airborne/firmwares/setup/setup_actuators.c index eb776b61f3..6e755f3d80 100644 --- a/sw/airborne/firmwares/setup/setup_actuators.c +++ b/sw/airborne/firmwares/setup/setup_actuators.c @@ -38,54 +38,60 @@ #include "subsystems/actuators.h" -static inline void main_init( void ); -static inline void main_periodic( void ); +static inline void main_init(void); +static inline void main_periodic(void); static inline void main_event(void); -int main(void) { +int main(void) +{ main_init(); while (1) { - if (sys_time_check_and_ack_timer(0)) + if (sys_time_check_and_ack_timer(0)) { main_periodic(); + } main_event(); }; return 0; } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); actuators_init(); uint8_t i; - for(i = 0; i < ACTUATORS_NB; i++) { + for (i = 0; i < ACTUATORS_NB; i++) { //SetServo(i, 1500); } mcu_int_enable(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); } -static inline void main_periodic( void ) { +static inline void main_periodic(void) +{ // generated macro from airframe file AllActuatorsCommit(); LED_PERIODIC(); RunOnceEvery(100, {DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);}); - RunOnceEvery(300, DOWNLINK_SEND_ACTUATORS(DefaultChannel, DefaultDevice, ACTUATORS_NB, actuators )); + RunOnceEvery(300, DOWNLINK_SEND_ACTUATORS(DefaultChannel, DefaultDevice, ACTUATORS_NB, actuators)); } -static inline void main_event(void) { +static inline void main_event(void) +{ DatalinkEvent(); } #define IdOfMsg(x) (x[1]) -void dl_parse_msg( void ) { +void dl_parse_msg(void) +{ uint8_t msg_id = IdOfMsg(dl_buffer); if (msg_id == DL_SET_ACTUATOR) { uint8_t actuator_no = DL_SET_ACTUATOR_no(dl_buffer); @@ -139,34 +145,34 @@ void dl_parse_msg( void ) { LED_TOGGLE(2); #ifdef SERVO_0 - ActuatorSet(0, actuators[SERVO_0_IDX]; + ActuatorSet(0, actuators[SERVO_0_IDX]; #endif #ifdef SERVO_1 - ActuatorSet(1, actuators[SERVO_1_IDX]); + ActuatorSet(1, actuators[SERVO_1_IDX]); #endif #ifdef SERVO_2 - ActuatorSet(2, actuators[SERVO_2_IDX]); + ActuatorSet(2, actuators[SERVO_2_IDX]); #endif #ifdef SERVO_3 - ActuatorSet(3, actuators[SERVO_3_IDX]); + ActuatorSet(3, actuators[SERVO_3_IDX]); #endif #ifdef SERVO_4 - ActuatorSet(4, actuators[SERVO_4_IDX]); + ActuatorSet(4, actuators[SERVO_4_IDX]); #endif #ifdef SERVO_5 - ActuatorSet(5, actuators[SERVO_5_IDX]); + ActuatorSet(5, actuators[SERVO_5_IDX]); #endif #ifdef SERVO_6 - ActuatorSet(6, actuators[SERVO_6_IDX]); + ActuatorSet(6, actuators[SERVO_6_IDX]); #endif #ifdef SERVO_7 - ActuatorSet(7, actuators[SERVO_7_IDX]); + ActuatorSet(7, actuators[SERVO_7_IDX]); #endif #ifdef SERVO_8 - ActuatorSet(8, actuators[SERVO_8_IDX]); + ActuatorSet(8, actuators[SERVO_8_IDX]); #endif - DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val); + DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val); } else if (msg_id == DL_GET_SETTING && DL_GET_SETTING_ac_id(dl_buffer) == AC_ID) { uint8_t i = DL_GET_SETTING_index(dl_buffer); float val = settings_get_value(i); diff --git a/sw/airborne/firmwares/setup/usb_tunnel.c b/sw/airborne/firmwares/setup/usb_tunnel.c index bae982603c..901af33733 100644 --- a/sw/airborne/firmwares/setup/usb_tunnel.c +++ b/sw/airborne/firmwares/setup/usb_tunnel.c @@ -54,13 +54,13 @@ static inline void tunnel_event(void) unsigned char inc; #if LED_AVAILABLE(TUNNEL_RX_LED) - static uint32_t rx_time=0; + static uint32_t rx_time = 0; if (get_sys_time_msec() > rx_time + BLINK_MIN) { LED_OFF(TUNNEL_RX_LED); } #endif #if LED_AVAILABLE(TUNNEL_TX_LED) - static uint32_t tx_time=0; + static uint32_t tx_time = 0; if (get_sys_time_msec() > tx_time + BLINK_MIN) { LED_OFF(TUNNEL_TX_LED); } @@ -96,7 +96,7 @@ int main(void) mcu_int_enable(); - while(1) { + while (1) { VCOM_event(); tunnel_event(); } diff --git a/sw/airborne/firmwares/tutorial/main_demo1.c b/sw/airborne/firmwares/tutorial/main_demo1.c index 02ccf6727c..9c7648314a 100644 --- a/sw/airborne/firmwares/tutorial/main_demo1.c +++ b/sw/airborne/firmwares/tutorial/main_demo1.c @@ -2,8 +2,9 @@ #include "std.h" -int main( void ) { - while(1) { +int main(void) +{ + while (1) { } return 0; diff --git a/sw/airborne/firmwares/tutorial/main_demo2.c b/sw/airborne/firmwares/tutorial/main_demo2.c index 23950fb208..09eb218d4f 100644 --- a/sw/airborne/firmwares/tutorial/main_demo2.c +++ b/sw/airborne/firmwares/tutorial/main_demo2.c @@ -4,23 +4,27 @@ #include "mcu_periph/sys_time.h" #include "led.h" -static inline void main_init( void ); -static inline void main_periodic_task( void ); +static inline void main_init(void); +static inline void main_periodic_task(void); -int main( void ) { +int main(void) +{ main_init(); - while(1) { - if (sys_time_check_and_ack_timer(0)) + while (1) { + if (sys_time_check_and_ack_timer(0)) { main_periodic_task(); + } } return 0; } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); } -static inline void main_periodic_task( void ) { +static inline void main_periodic_task(void) +{ LED_TOGGLE(1); } diff --git a/sw/airborne/firmwares/tutorial/main_demo3.c b/sw/airborne/firmwares/tutorial/main_demo3.c index 4995f784e1..e19e72ef2e 100644 --- a/sw/airborne/firmwares/tutorial/main_demo3.c +++ b/sw/airborne/firmwares/tutorial/main_demo3.c @@ -6,26 +6,30 @@ #include "mcu_periph/uart.h" #include "subsystems/datalink/uart_print.h" -static inline void main_init( void ); -static inline void main_periodic_task( void ); +static inline void main_init(void); +static inline void main_periodic_task(void); -int main( void ) { +int main(void) +{ main_init(); - while(1) { - if (sys_time_check_and_ack_timer(0)) + while (1) { + if (sys_time_check_and_ack_timer(0)) { main_periodic_task(); + } } return 0; } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); uart0_init_tx(); mcu_int_enable(); } -static inline void main_periodic_task( void ) { +static inline void main_periodic_task(void) +{ LED_TOGGLE(1); UART0PrintString("demo3 running since "); UART0PrintHex32(sys_time.nb_sec); diff --git a/sw/airborne/firmwares/tutorial/main_demo4.c b/sw/airborne/firmwares/tutorial/main_demo4.c index 6a7d285954..cc8af43436 100644 --- a/sw/airborne/firmwares/tutorial/main_demo4.c +++ b/sw/airborne/firmwares/tutorial/main_demo4.c @@ -7,26 +7,30 @@ #include "messages.h" #include "subsystems/datalink/downlink.h" -static inline void main_init( void ); -static inline void main_periodic_task( void ); +static inline void main_init(void); +static inline void main_periodic_task(void); -int main( void ) { +int main(void) +{ main_init(); - while(1) { - if (sys_time_check_and_ack_timer(0)) + while (1) { + if (sys_time_check_and_ack_timer(0)) { main_periodic_task(); + } } return 0; } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); uart0_init_tx(); mcu_int_enable(); } -static inline void main_periodic_task( void ) { +static inline void main_periodic_task(void) +{ LED_TOGGLE(1); uint16_t time_sec = sys_time.nb_sec; DOWNLINK_SEND_TAKEOFF(&time_sec); diff --git a/sw/airborne/firmwares/tutorial/main_demo5.c b/sw/airborne/firmwares/tutorial/main_demo5.c index 59a982965f..cb020add05 100644 --- a/sw/airborne/firmwares/tutorial/main_demo5.c +++ b/sw/airborne/firmwares/tutorial/main_demo5.c @@ -7,36 +7,41 @@ #include "messages.h" #include "subsystems/datalink/downlink.h" -static inline void main_init( void ); -static inline void main_periodic_task( void ); -static inline void main_event_task( void ); +static inline void main_init(void); +static inline void main_periodic_task(void); +static inline void main_event_task(void); -static inline void main_dl_parse_msg( void ); +static inline void main_dl_parse_msg(void); -int main( void ) { +int main(void) +{ main_init(); - while(1) { - if (sys_time_check_and_ack_timer(0)) + while (1) { + if (sys_time_check_and_ack_timer(0)) { main_periodic_task(); + } main_event_task(); } return 0; } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); uart0_init_tx(); mcu_int_enable(); } -static inline void main_periodic_task( void ) { +static inline void main_periodic_task(void) +{ // LED_TOGGLE(1); uint16_t time_sec = sys_time.nb_sec; DOWNLINK_SEND_TAKEOFF(&time_sec); } -static inline void main_event_task( void ) { +static inline void main_event_task(void) +{ if (PprzBuffer()) { ReadPprzBuffer(); if (pprz_msg_received) { @@ -56,13 +61,14 @@ uint16_t foo; bool_t dl_msg_available; #define MSG_SIZE 128 -uint8_t dl_buffer[MSG_SIZE] __attribute__ ((aligned)); +uint8_t dl_buffer[MSG_SIZE] __attribute__((aligned)); #include "generated/settings.h" #define IdOfMsg(x) (x[1]) -static inline void main_dl_parse_msg(void) { +static inline void main_dl_parse_msg(void) +{ uint8_t msg_id = IdOfMsg(dl_buffer); if (msg_id == DL_SETTING) { uint8_t i = DL_SETTING_index(dl_buffer); diff --git a/sw/airborne/firmwares/tutorial/main_demo6.c b/sw/airborne/firmwares/tutorial/main_demo6.c index a34882882f..bd80e1ea6d 100644 --- a/sw/airborne/firmwares/tutorial/main_demo6.c +++ b/sw/airborne/firmwares/tutorial/main_demo6.c @@ -8,30 +8,34 @@ #include "messages.h" #include "subsystems/datalink/downlink.h" -static inline void main_init( void ); -static inline void main_periodic_task( void ); +static inline void main_init(void); +static inline void main_periodic_task(void); -int main( void ) { +int main(void) +{ main_init(); - while(1) { - if (sys_time_check_and_ack_timer(0)) + while (1) { + if (sys_time_check_and_ack_timer(0)) { main_periodic_task(); + } } return 0; } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); usb_serial_init(); mcu_int_enable(); } -static inline void main_periodic_task( void ) { +static inline void main_periodic_task(void) +{ LED_TOGGLE(1); //uint16_t time_sec = sys_time.nb_sec; // DOWNLINK_SEND_TAKEOFF(&time_sec); - usb_serial_transmit( 'A' ); - usb_serial_transmit( '\n' ); + usb_serial_transmit('A'); + usb_serial_transmit('\n'); } diff --git a/sw/airborne/firmwares/wind_tunnel/main.c b/sw/airborne/firmwares/wind_tunnel/main.c index 30bd9f0c19..21ad7b2952 100644 --- a/sw/airborne/firmwares/wind_tunnel/main.c +++ b/sw/airborne/firmwares/wind_tunnel/main.c @@ -17,29 +17,32 @@ #include "spi.h" #include "wt_baro.h" -static inline void main_init( void ); -static inline void main_periodic_task( void ); -static inline void main_event_task( void ); +static inline void main_init(void); +static inline void main_periodic_task(void); +static inline void main_event_task(void); uint16_t motor_power; -uint8_t dl_buffer[MSG_SIZE] __attribute__ ((aligned)); +uint8_t dl_buffer[MSG_SIZE] __attribute__((aligned)); bool_t dl_msg_available; uint16_t datalink_time; -int main( void ) { +int main(void) +{ main_init(); - while(1) { - if (sys_time_check_and_ack_timer(0)) + while (1) { + if (sys_time_check_and_ack_timer(0)) { main_periodic_task(); - main_event_task( ); + } + main_event_task(); } return 0; } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); led_init(); uart0_init(); @@ -53,14 +56,16 @@ static inline void main_init( void ) { mcu_int_enable(); } -static inline void main_periodic_task( void ) { +static inline void main_periodic_task(void) +{ LED_TOGGLE(1); DOWNLINK_SEND_TAKEOFF(&motor_power); wt_baro_periodic(); - DOWNLINK_SEND_DEBUG(3,buf_input); + DOWNLINK_SEND_DEBUG(3, buf_input); } -static inline void main_event_task( void ) { +static inline void main_event_task(void) +{ DatalinkEvent(); // spi baro @@ -70,7 +75,7 @@ static inline void main_event_task( void ) { wt_baro_event(); uint16_t temp = 0; float alt = 0.; - DOWNLINK_SEND_BARO_MS5534A(&wt_baro_pressure,&temp,&alt); + DOWNLINK_SEND_BARO_MS5534A(&wt_baro_pressure, &temp, &alt); } } @@ -78,25 +83,26 @@ static inline void main_event_task( void ) { #define IdOfMsg(x) (x[1]) -void dl_parse_msg(void) { +void dl_parse_msg(void) +{ LED_TOGGLE(1); uint8_t msg_id = IdOfMsg(dl_buffer); switch (msg_id) { - case DL_PING: { - DOWNLINK_SEND_PONG(); - break; - } + case DL_PING: { + DOWNLINK_SEND_PONG(); + break; + } - case DL_SETTING : { - uint8_t i = DL_SETTING_index(dl_buffer); - float var = DL_SETTING_value(dl_buffer); - DlSetting(i, var); - DOWNLINK_SEND_DL_VALUE(&i, &var); - break; - } + case DL_SETTING : { + uint8_t i = DL_SETTING_index(dl_buffer); + float var = DL_SETTING_value(dl_buffer); + DlSetting(i, var); + DOWNLINK_SEND_DL_VALUE(&i, &var); + break; + } } } diff --git a/sw/airborne/firmwares/wind_tunnel/main_mb.c b/sw/airborne/firmwares/wind_tunnel/main_mb.c index 75354cc7b0..cbc1269827 100644 --- a/sw/airborne/firmwares/wind_tunnel/main_mb.c +++ b/sw/airborne/firmwares/wind_tunnel/main_mb.c @@ -16,29 +16,32 @@ #include "mb_twi_controller_mkk.h" #include "mb_tacho.h" -static inline void main_init( void ); -static inline void main_periodic_task( void ); -static inline void main_event_task( void ); +static inline void main_init(void); +static inline void main_periodic_task(void); +static inline void main_event_task(void); //uint16_t motor_power; -uint8_t dl_buffer[MSG_SIZE] __attribute__ ((aligned)); +uint8_t dl_buffer[MSG_SIZE] __attribute__((aligned)); bool_t dl_msg_available; uint16_t datalink_time; -int main( void ) { +int main(void) +{ main_init(); - while(1) { - if (sys_time_check_and_ack_timer(0)) + while (1) { + if (sys_time_check_and_ack_timer(0)) { main_periodic_task(); - main_event_task( ); + } + main_event_task(); } return 0; } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); led_init(); uart0_init(); @@ -52,7 +55,8 @@ static inline void main_init( void ) { mcu_int_enable(); } -static inline void main_periodic_task( void ) { +static inline void main_periodic_task(void) +{ LED_TOGGLE(1); // DOWNLINK_SEND_TAKEOFF(&wt_servo_motor_power); // DOWNLINK_SEND_DEBUG(3,buf_input); @@ -66,7 +70,8 @@ static inline void main_periodic_task( void ) { } -static inline void main_event_task( void ) { +static inline void main_event_task(void) +{ DatalinkEvent(); @@ -75,25 +80,26 @@ static inline void main_event_task( void ) { #define IdOfMsg(x) (x[1]) -void dl_parse_msg(void) { +void dl_parse_msg(void) +{ LED_TOGGLE(1); uint8_t msg_id = IdOfMsg(dl_buffer); switch (msg_id) { - case DL_PING: { - DOWNLINK_SEND_PONG(); - break; - } + case DL_PING: { + DOWNLINK_SEND_PONG(); + break; + } - case DL_SETTING : { - uint8_t i = DL_SETTING_index(dl_buffer); - float var = DL_SETTING_value(dl_buffer); - DlSetting(i, var); - DOWNLINK_SEND_DL_VALUE(&i, &var); - break; - } + case DL_SETTING : { + uint8_t i = DL_SETTING_index(dl_buffer); + float var = DL_SETTING_value(dl_buffer); + DlSetting(i, var); + DOWNLINK_SEND_DL_VALUE(&i, &var); + break; + } } } diff --git a/sw/airborne/firmwares/wind_tunnel/wt_baro.c b/sw/airborne/firmwares/wind_tunnel/wt_baro.c index dbaaa67cf1..92d5d2ed91 100644 --- a/sw/airborne/firmwares/wind_tunnel/wt_baro.c +++ b/sw/airborne/firmwares/wind_tunnel/wt_baro.c @@ -48,17 +48,19 @@ uint8_t buf_output[3]; #define Uint24(buf_input) (((uint32_t)buf_input[0]) << 16 |((uint16_t)buf_input[1]) << 8 | buf_input[2]) -static void send1_on_spi(uint8_t d) { +static void send1_on_spi(uint8_t d) +{ buf_output[0] = d; spi_buffer_length = 1; - spi_buffer_input = (uint8_t*)&buf_input; - spi_buffer_output = (uint8_t*)&buf_output; + spi_buffer_input = (uint8_t *)&buf_input; + spi_buffer_output = (uint8_t *)&buf_output; SpiStart(); } -void wt_baro_init( void ) { +void wt_baro_init(void) +{ wt_baro_pressure = 0; @@ -74,7 +76,8 @@ void wt_baro_init( void ) { } -void wt_baro_periodic(void) { +void wt_baro_periodic(void) +{ if (!SpiCheckAvailable()) { SpiOverRun(); return; @@ -83,14 +86,13 @@ void wt_baro_periodic(void) { if (status_read_data) { buf_output[0] = buf_output[1] = buf_output[2] = 0; spi_buffer_length = 3; - } - else { + } else { buf_output[0] = CMD_MEASUREMENT; spi_buffer_length = 1; } - spi_buffer_input = (uint8_t*)&buf_input; - spi_buffer_output = (uint8_t*)&buf_output; + spi_buffer_input = (uint8_t *)&buf_input; + spi_buffer_output = (uint8_t *)&buf_output; //if (status_read_data) // SpiSetCPHA(); //else @@ -101,7 +103,8 @@ void wt_baro_periodic(void) { static uint32_t data; /* Handle the SPI message, i.e. store the received values in variables */ -void wt_baro_event( void ) { +void wt_baro_event(void) +{ if (status_read_data) { data = Uint24(buf_input); diff --git a/sw/airborne/firmwares/wind_tunnel/wt_servo.c b/sw/airborne/firmwares/wind_tunnel/wt_servo.c index 4955adc2f0..67d51249fd 100644 --- a/sw/airborne/firmwares/wind_tunnel/wt_servo.c +++ b/sw/airborne/firmwares/wind_tunnel/wt_servo.c @@ -10,7 +10,8 @@ uint16_t wt_servo_motor_power; void mb_servo_set_ns(uint32_t duration_ns); -void wt_servo_init(void) { +void wt_servo_init(void) +{ /* set P0.21 as PWM5 output */ PINSEL1 |= (0X01 << 10); @@ -28,16 +29,18 @@ void wt_servo_init(void) { #define MB_SERVO_MIN_PULSE_NS 1000000 #define MB_SERVO_RANGE_PULSE_NS 1000 -void wt_servo_set(uint16_t val) { +void wt_servo_set(uint16_t val) +{ - uint32_t pulse_ns = MB_SERVO_MIN_PULSE_NS + val*MB_SERVO_RANGE_PULSE_NS; + uint32_t pulse_ns = MB_SERVO_MIN_PULSE_NS + val * MB_SERVO_RANGE_PULSE_NS; mb_servo_set_ns(pulse_ns); } -void mb_servo_set_ns(uint32_t duration_ns) { +void mb_servo_set_ns(uint32_t duration_ns) +{ /* set Match5 value (pulse duration )*/ PWMMR5 = cpu_ticks_of_nsec(duration_ns); - /* commit PWMMRx changes */ + /* commit PWMMRx changes */ PWMLER = PWMLER_LATCH5; } diff --git a/sw/airborne/firmwares/wind_tunnel/wt_servo.h b/sw/airborne/firmwares/wind_tunnel/wt_servo.h index 832b2e2587..cd92c9a459 100644 --- a/sw/airborne/firmwares/wind_tunnel/wt_servo.h +++ b/sw/airborne/firmwares/wind_tunnel/wt_servo.h @@ -8,9 +8,9 @@ extern void wt_servo_set(uint16_t val); extern uint16_t wt_servo_motor_power; -#define wt_servo_SetPower(_val) { \ - wt_servo_motor_power = _val; \ - wt_servo_set(wt_servo_motor_power); \ +#define wt_servo_SetPower(_val) { \ + wt_servo_motor_power = _val; \ + wt_servo_set(wt_servo_motor_power); \ } diff --git a/sw/airborne/fms/fms_autopilot_msg.h b/sw/airborne/fms/fms_autopilot_msg.h index 37a94c06a3..50b9e61d05 100644 --- a/sw/airborne/fms/fms_autopilot_msg.h +++ b/sw/airborne/fms/fms_autopilot_msg.h @@ -16,8 +16,7 @@ * Testing */ -struct __attribute__ ((packed)) AutopilotMessageFoo -{ +struct __attribute__((packed)) AutopilotMessageFoo { uint32_t foo; uint32_t bar; uint32_t bla; @@ -31,8 +30,7 @@ struct __attribute__ ((packed)) AutopilotMessageFoo /* * BETH */ -struct __attribute__ ((packed)) AutopilotMessageBethUp -{ +struct __attribute__((packed)) AutopilotMessageBethUp { struct Int16Rates gyro; struct Int16Vect3 accel; struct Int16Vect3 bench_sensor; @@ -43,8 +41,7 @@ struct __attribute__ ((packed)) AutopilotMessageBethUp int8_t pitch_out; }; -struct __attribute__ ((packed)) AutopilotMessageBethDown -{ +struct __attribute__((packed)) AutopilotMessageBethDown { uint8_t thrust; uint8_t pitch; uint32_t errors; @@ -55,14 +52,12 @@ struct __attribute__ ((packed)) AutopilotMessageBethDown * STM Telemetry through wifi */ #define TW_BUF_LEN 63 -struct __attribute__ ((packed)) AutopilotMessageTWUp -{ +struct __attribute__((packed)) AutopilotMessageTWUp { uint8_t tw_len; uint8_t data[TW_BUF_LEN]; }; -struct __attribute__ ((packed)) AutopilotMessageTWDown -{ +struct __attribute__((packed)) AutopilotMessageTWDown { uint8_t tw_len; uint8_t data[TW_BUF_LEN]; }; @@ -71,23 +66,21 @@ struct __attribute__ ((packed)) AutopilotMessageTWDown * Passthrough, aka biplan */ -struct __attribute__ ((packed)) ADCMessage { +struct __attribute__((packed)) ADCMessage { uint16_t channels[NB_ADC]; }; /* used to indicate parts of the message which actually represent a new measurement */ -struct __attribute__ ((packed)) PTUpValidFlags -{ - unsigned rc:1; - unsigned pressure_absolute:1; - unsigned pressure_differential:1; - unsigned vane:1; - unsigned imu:1; - unsigned adc:1; +struct __attribute__((packed)) PTUpValidFlags { + unsigned rc: 1; + unsigned pressure_absolute: 1; + unsigned pressure_differential: 1; + unsigned vane: 1; + unsigned imu: 1; + unsigned adc: 1; }; -struct __attribute__ ((packed)) AutopilotMessagePTUp -{ +struct __attribute__((packed)) AutopilotMessagePTUp { struct Int32Rates gyro; struct Int32Vect3 accel; struct Int32Vect3 mag; @@ -113,8 +106,7 @@ struct __attribute__ ((packed)) AutopilotMessagePTUp uint32_t stm_crc_err_cnt; }; -struct __attribute__ ((packed)) AutopilotMessagePTDown -{ +struct __attribute__((packed)) AutopilotMessagePTDown { uint16_t pwm_outputs_usecs[LISA_PWM_OUTPUT_NB]; }; @@ -125,8 +117,7 @@ struct __attribute__ ((packed)) AutopilotMessagePTDown #define VI_GPS_DATA_VALID 2 #define VI_BARO_ABS_DATA_VALID 3 -struct __attribute__ ((packed)) AutopilotMessageVIUp -{ +struct __attribute__((packed)) AutopilotMessageVIUp { struct Int16Rates gyro; struct Int16Vect3 accel; struct Int16Vect3 mag; @@ -136,8 +127,7 @@ struct __attribute__ ((packed)) AutopilotMessageVIUp uint8_t valid_sensors; }; -struct __attribute__ ((packed)) AutopilotMessageVIDown -{ +struct __attribute__((packed)) AutopilotMessageVIDown { }; @@ -149,7 +139,7 @@ struct __attribute__ ((packed)) AutopilotMessageVIDown * is recomposed to a raw byte array on application * level. * Advantage: Interleaving message exchange, constant - * latency + * latency * Disadvantage: Overhead of message / package counters * * If there is no message to be transferred, an empty @@ -192,8 +182,7 @@ struct __attribute__ ((packed)) AutopilotMessageVIDown #ifndef SPISTREAM_PACKAGE_SIZE #define SPISTREAM_PACKAGE_SIZE 32 #endif -struct __attribute__ ((packed)) AutopilotMessagePTStream -{ +struct __attribute__((packed)) AutopilotMessagePTStream { uint8_t message_cnt; int8_t package_cntd; uint8_t pkg_data[SPISTREAM_PACKAGE_SIZE]; @@ -205,8 +194,7 @@ union AutopilotMessage { struct OVERO_LINK_MSG_DOWN msg_down; }; -struct __attribute__ ((packed)) AutopilotMessageCRCFrame -{ +struct __attribute__((packed)) AutopilotMessageCRCFrame { union AutopilotMessage payload; uint8_t crc; }; diff --git a/sw/airborne/fms/fms_gs_com.c b/sw/airborne/fms/fms_gs_com.c index 6475222a9e..94f94eb39f 100644 --- a/sw/airborne/fms/fms_gs_com.c +++ b/sw/airborne/fms/fms_gs_com.c @@ -23,19 +23,21 @@ uint8_t telemetry_mode_Main_DefaultChannel; static void on_datalink_event(int fd, short event __attribute__((unused)), void *arg); 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) { +uint8_t fms_gs_com_init(const char *gs_host, uint16_t gs_port, + 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; } -void fms_gs_com_periodic(void) { +void fms_gs_com_periodic(void) +{ PeriodicSendMain(fms_gs_com.udp_transport); @@ -44,12 +46,13 @@ void fms_gs_com_periodic(void) { } -static void on_datalink_event(int fd, short event __attribute__((unused)), void *arg) { +static void on_datalink_event(int fd, short event __attribute__((unused)), void *arg) +{ char buf[512]; int bytes_read = read(fd, buf, 512); uint16_t i = 0; struct udp_transport *tp = fms_gs_com.udp_transport->impl; - while (iudp_dl_msg_received) { on_datalink_message(); @@ -60,25 +63,26 @@ static void on_datalink_event(int fd, short event __attribute__((unused)), void } -static void on_datalink_message(void) { +static void on_datalink_message(void) +{ struct udp_transport *tp = fms_gs_com.udp_transport->impl; uint8_t msg_id = tp->udp_dl_payload[1]; switch (msg_id) { - case DL_PING: - DOWNLINK_SEND_PONG(fms_gs_com.udp_transport); - break; - case DL_SETTING : { - uint8_t i = DL_SETTING_index(tp->udp_dl_payload); - float var = DL_SETTING_value(tp->udp_dl_payload); - DlSetting(i, var); - DOWNLINK_SEND_DL_VALUE(fms_gs_com.udp_transport, &i, &var); - } + case DL_PING: + DOWNLINK_SEND_PONG(fms_gs_com.udp_transport); + break; + case DL_SETTING : { + uint8_t i = DL_SETTING_index(tp->udp_dl_payload); + float var = DL_SETTING_value(tp->udp_dl_payload); + DlSetting(i, var); + DOWNLINK_SEND_DL_VALUE(fms_gs_com.udp_transport, &i, &var); + } break; - default : - break; + default : + break; } } diff --git a/sw/airborne/fms/fms_gs_com.h b/sw/airborne/fms/fms_gs_com.h index 1732220b44..168afa3abe 100644 --- a/sw/airborne/fms/fms_gs_com.h +++ b/sw/airborne/fms/fms_gs_com.h @@ -16,7 +16,7 @@ struct FmsGsCom { - struct FmsNetwork* network; + struct FmsNetwork *network; struct DownlinkTransport *udp_transport; struct event datalink_event; @@ -27,8 +27,8 @@ extern struct FmsGsCom fms_gs_com; /* remove me */ 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); +extern uint8_t fms_gs_com_init(const char *gs_host, uint16_t gs_port, + 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_network.c b/sw/airborne/fms/fms_network.c index 9d9d611b85..2f6a361aa0 100644 --- a/sw/airborne/fms/fms_network.c +++ b/sw/airborne/fms/fms_network.c @@ -7,24 +7,28 @@ #include "fms_debug.h" -static inline void create_socket(int *my_socket, const int protocol, const int reuse_addr, const int broadcast) { +static inline void create_socket(int *my_socket, const int protocol, const int reuse_addr, const int broadcast) +{ // Create the socket with the correct protocl *my_socket = socket(PF_INET, SOCK_DGRAM, protocol); // Enable reusing of addres (must be exactly 1) - if(reuse_addr == 1) + if (reuse_addr == 1) { setsockopt(*my_socket, SOL_SOCKET, SO_REUSEADDR, &reuse_addr, sizeof(reuse_addr)); + } // Enable broadcasting - if(broadcast == 1) + if (broadcast == 1) { setsockopt(*my_socket, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof(broadcast)); + } } -struct FmsNetwork* network_new(const char* str_ip_out, const int port_out, const int port_in, const int broadcast) { +struct FmsNetwork *network_new(const char *str_ip_out, const int port_out, const int port_in, const int broadcast) +{ struct FmsNetwork *me = malloc(sizeof(struct FmsNetwork)); - if(port_out >= 0) { + if (port_out >= 0) { // Create the output socket (enable reuse of the address, and broadcast if necessary) create_socket(&me->socket_out, 0, 1, broadcast); @@ -34,7 +38,7 @@ struct FmsNetwork* network_new(const char* str_ip_out, const int port_out, const me->addr_out.sin_addr.s_addr = inet_addr(str_ip_out); } - if(port_in >= 0) { + if (port_in >= 0) { // Creat the input socket (enable reuse of the address, and disable broadcast) create_socket(&me->socket_in, 0, 1, 0); @@ -49,7 +53,8 @@ struct FmsNetwork* network_new(const char* str_ip_out, const int port_out, const return me; } -int network_subscribe_multicast(struct FmsNetwork* me, const char* multicast_addr) { +int network_subscribe_multicast(struct FmsNetwork *me, const char *multicast_addr) +{ // Create the request struct ip_mreq mreq; mreq.imr_multiaddr.s_addr = inet_addr(multicast_addr); @@ -59,27 +64,31 @@ int network_subscribe_multicast(struct FmsNetwork* me, const char* multicast_add return setsockopt(me->socket_in, IPPROTO_IP, IP_ADD_MEMBERSHIP, (char *)&mreq, sizeof(mreq)); } -int network_set_recvbuf(struct FmsNetwork* me, int buf_size) { +int network_set_recvbuf(struct FmsNetwork *me, int buf_size) +{ // Set and check unsigned int optval_size = 4; int buf_ret; setsockopt(me->socket_in, SOL_SOCKET, SO_RCVBUF, (char *)&buf_size, optval_size); getsockopt(me->socket_in, SOL_SOCKET, SO_RCVBUF, (char *)&buf_ret, &optval_size); - if(buf_size != buf_ret) + if (buf_size != buf_ret) { return -1; + } return 0; } -int network_write(struct FmsNetwork* me, char* buf, int len) { +int network_write(struct FmsNetwork *me, char *buf, int len) +{ // Check if the output address is set - if(!me->socket_out) + if (!me->socket_out) { return -1; + } ssize_t byte_written = sendto(me->socket_out, buf, len, MSG_DONTWAIT, - (struct sockaddr*)&me->addr_out, sizeof(me->addr_out)); - if ( byte_written != len) { + (struct sockaddr *)&me->addr_out, sizeof(me->addr_out)); + if (byte_written != len) { TRACE(TRACE_ERROR, "error sending to network %d (%d)\n", (int)byte_written, errno); } return len; @@ -87,22 +96,23 @@ int network_write(struct FmsNetwork* me, char* buf, int len) { ///< returns: -1 = error, 0 = no data, >0 = nrofbytesread -int network_read(struct FmsNetwork* me, unsigned char* buf, int len) { +int network_read(struct FmsNetwork *me, unsigned char *buf, int len) +{ // Check if the input address is set - if(!me->socket_in) + if (!me->socket_in) { return -1; + } socklen_t slen = sizeof(struct sockaddr_in); // MSG_DONTWAIT => nonblocking flag ssize_t byte_read = recvfrom(me->socket_in, buf, len, MSG_DONTWAIT, - (struct sockaddr*)&me->addr_in, &slen); + (struct sockaddr *)&me->addr_in, &slen); if (byte_read == -1) { if (errno == EWOULDBLOCK) { // If not data is available, simply return zero bytes read, no error return 0; - } - else { - TRACE(TRACE_ERROR, "error reading from network error %d \n",errno); + } else { + TRACE(TRACE_ERROR, "error reading from network error %d \n", errno); } } diff --git a/sw/airborne/fms/fms_network.h b/sw/airborne/fms/fms_network.h index df28d8371c..ce4c8d5a40 100644 --- a/sw/airborne/fms/fms_network.h +++ b/sw/airborne/fms/fms_network.h @@ -15,10 +15,11 @@ struct FmsNetwork { }; -extern struct FmsNetwork* network_new(const char* str_ip_out, const int port_out, const int port_in, const int broadcast); -extern int network_subscribe_multicast(struct FmsNetwork* me, const char* multicast_addr); -extern int network_set_recvbuf(struct FmsNetwork* me, int buf_size); -extern int network_write(struct FmsNetwork* me, char* buf, int len); -extern int network_read(struct FmsNetwork* me, unsigned char* buf, int len); +extern struct FmsNetwork *network_new(const char *str_ip_out, const int port_out, const int port_in, + const int broadcast); +extern int network_subscribe_multicast(struct FmsNetwork *me, const char *multicast_addr); +extern int network_set_recvbuf(struct FmsNetwork *me, int buf_size); +extern int network_write(struct FmsNetwork *me, char *buf, int len); +extern int network_read(struct FmsNetwork *me, unsigned char *buf, int len); #endif /* FMS_NETWORK_H */ diff --git a/sw/airborne/fms/fms_periodic.c b/sw/airborne/fms/fms_periodic.c index 044fefbf4b..39be84a805 100644 --- a/sw/airborne/fms/fms_periodic.c +++ b/sw/airborne/fms/fms_periodic.c @@ -42,15 +42,16 @@ struct FmsPeriodic { }; -int fms_periodic_init(void(*periodic_handler)(int) ) { +int fms_periodic_init(void(*periodic_handler)(int)) +{ pid_t my_pid = fork(); if (my_pid == -1) { - TRACE(TRACE_ERROR,"fms_periodic : unable to fork : %s (%d)\n", strerror(errno), errno); + TRACE(TRACE_ERROR, "fms_periodic : unable to fork : %s (%d)\n", strerror(errno), errno); return -1; } /* child process */ - else if (my_pid == 0) { + else if (my_pid == 0) { fms_periodic_run(); } /* succesful fork parent process */ @@ -58,7 +59,7 @@ int fms_periodic_init(void(*periodic_handler)(int) ) { /* install signal handler */ struct sigaction my_sigaction = {.sa_handler = periodic_handler }; if (sigaction(SIGUSR1, &my_sigaction, NULL)) { - TRACE(TRACE_ERROR,"fms_periodic : unable to install signal handler : %s (%d)\n", strerror(errno), errno); + TRACE(TRACE_ERROR, "fms_periodic : unable to install signal handler : %s (%d)\n", strerror(errno), errno); return -1; } @@ -66,7 +67,7 @@ int fms_periodic_init(void(*periodic_handler)(int) ) { struct sched_param param; param.sched_priority = 49; if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) { - TRACE(TRACE_ERROR,"fms_periodic : hs sched_setscheduler failed : %s (%d)\n", strerror(errno), errno); + TRACE(TRACE_ERROR, "fms_periodic : hs sched_setscheduler failed : %s (%d)\n", strerror(errno), errno); } return 0; @@ -80,12 +81,13 @@ int fms_periodic_init(void(*periodic_handler)(int) ) { #define NS_PER_SEC 1000000000 #define PERIODIC_DT_NSEC (NS_PER_SEC/(FMS_PERIODIC_FREQ)) -static void fms_periodic_run(void) { +static void fms_periodic_run(void) +{ struct sched_param param; param.sched_priority = 95; if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) { - TRACE(TRACE_ERROR,"fms_periodic : hs sched_setscheduler failed : %s (%d)\n", strerror(errno), errno); + TRACE(TRACE_ERROR, "fms_periodic : hs sched_setscheduler failed : %s (%d)\n", strerror(errno), errno); } pid_t father_pid = getppid(); diff --git a/sw/airborne/fms/fms_periodic.h b/sw/airborne/fms/fms_periodic.h index 618ee01655..f2ae59dfb3 100644 --- a/sw/airborne/fms/fms_periodic.h +++ b/sw/airborne/fms/fms_periodic.h @@ -24,6 +24,6 @@ #ifndef FMS_PERIODIC_H #define FMS_PERIODIC_H -extern int fms_periodic_init( void(*periodic_handler)(int) ); +extern int fms_periodic_init(void(*periodic_handler)(int)); #endif /* FMS_PERIODIC_H */ diff --git a/sw/airborne/fms/fms_spi_autopilot_msg.c b/sw/airborne/fms/fms_spi_autopilot_msg.c index c0d2f41625..c2c26622db 100644 --- a/sw/airborne/fms/fms_spi_autopilot_msg.c +++ b/sw/airborne/fms/fms_spi_autopilot_msg.c @@ -52,14 +52,14 @@ static void (* vane_callback)(uint8_t vane_id, float alpha, float beta) = NULL; static void (* pressure_absolute_callback)(uint8_t pressure_id, uint32_t pressure) = NULL; static void (* pressure_differential_callback)(uint8_t pressure_id, uint32_t pressure) = NULL; static void (* radio_control_callback)(void) = NULL; -static void (* adc_callback)(uint16_t * adc_channels) = NULL; +static void (* adc_callback)(uint16_t *adc_channels) = NULL; 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)) @@ -72,7 +72,8 @@ void spi_ap_link_set_pressure_absolute_callback(void (* pressure_absolute_cb)(ui pressure_absolute_callback = pressure_absolute_cb; } -void spi_ap_link_set_pressure_differential_callback(void (* pressure_differential_cb)(uint8_t pressure_id, uint32_t pressure)) +void spi_ap_link_set_pressure_differential_callback(void (* pressure_differential_cb)(uint8_t pressure_id, + uint32_t pressure)) { pressure_differential_callback = pressure_differential_cb; } @@ -82,7 +83,7 @@ void spi_ap_link_set_radio_control_callback(void (* radio_control_cb)(void)) radio_control_callback = radio_control_cb; } -void spi_ap_link_set_adc_callback(void (* adc_callback_fun)(uint16_t * adc_channels)) +void spi_ap_link_set_adc_callback(void (* adc_callback_fun)(uint16_t *adc_channels)) { adc_callback = adc_callback_fun; } @@ -117,18 +118,21 @@ int spi_ap_link_init() static void passthrough_up_parse(struct AutopilotMessagePTUp *msg_up) { - if (msg_up->valid.vane && vane_callback) + if (msg_up->valid.vane && vane_callback) { vane_callback(0, msg_up->vane_angle1, msg_up->vane_angle2); + } // Fill pressure data - if (msg_up->valid.pressure_absolute && pressure_absolute_callback) + if (msg_up->valid.pressure_absolute && pressure_absolute_callback) { pressure_absolute_callback(0, msg_up->pressure_absolute); + } - if (msg_up->valid.pressure_differential && pressure_differential_callback) + if (msg_up->valid.pressure_differential && pressure_differential_callback) { pressure_differential_callback(0, (32768 + msg_up->pressure_differential)); + } if (msg_up->valid.adc) { - if(adc_callback) { + if (adc_callback) { adc_callback(msg_up->adc.channels); } } @@ -164,8 +168,9 @@ static void passthrough_up_parse(struct AutopilotMessagePTUp *msg_up) imuFloat.sample_count = msg_up->imu_tick; - if (msg_up->valid.imu) + if (msg_up->valid.imu) { rdyb_booz_imu_update(&imuFloat); + } } static void passthrough_down_fill(struct AutopilotMessagePTDown *msg_down) @@ -186,7 +191,7 @@ void spi_ap_link_periodic() // SPI transcieve spi_link_send(&msg_down, sizeof(struct AutopilotMessageCRCFrame), &msg_up, &crc_valid); - if(crc_valid) { + if (crc_valid) { passthrough_up_parse(&msg_up.payload.msg_up); } } diff --git a/sw/airborne/fms/fms_spi_autopilot_msg.h b/sw/airborne/fms/fms_spi_autopilot_msg.h index 0253e5aedd..6ca32b82a2 100644 --- a/sw/airborne/fms/fms_spi_autopilot_msg.h +++ b/sw/airborne/fms/fms_spi_autopilot_msg.h @@ -28,7 +28,7 @@ void spi_ap_link_set_vane_callback(void (* vane_cb)(uint8_t vane_id, float alpha void spi_ap_link_set_pressure_absolute_callback(void (* pressure_cb)(uint8_t pressure_id, uint32_t pressure)); void spi_ap_link_set_pressure_differential_callback(void (* pressure_cb)(uint8_t pressure_id, uint32_t pressure)); void spi_ap_link_set_radio_control_callback(void (* radio_control_cb)(void)); -void spi_ap_link_set_adc_callback(void (* adc_callback_fun)(uint16_t * adc_channels)); +void spi_ap_link_set_adc_callback(void (* adc_callback_fun)(uint16_t *adc_channels)); void spi_ap_link_periodic(void); void spi_ap_link_downlink_send(struct DownlinkTransport *tp); diff --git a/sw/airborne/fms/fms_spi_link.c b/sw/airborne/fms/fms_spi_link.c index bae7177635..e0c20a7935 100644 --- a/sw/airborne/fms/fms_spi_link.c +++ b/sw/airborne/fms/fms_spi_link.c @@ -10,7 +10,8 @@ #include -int spi_link_init(void) { +int spi_link_init(void) +{ spi_link.device = "/dev/spidev1.1"; spi_link.mode = SPI_CPHA; @@ -22,26 +23,31 @@ int spi_link_init(void) { spi_link.crc_err_cnt = 0; spi_link.fd = open(spi_link.device, O_RDWR); - if (spi_link.fd < 0) + if (spi_link.fd < 0) { return -1; + } int ret = 0; ret = ioctl(spi_link.fd, SPI_IOC_WR_MODE, &spi_link.mode); - if (ret == -1) + if (ret == -1) { return -2; + } ret = ioctl(spi_link.fd, SPI_IOC_WR_BITS_PER_WORD, &spi_link.bits); - if (ret == -1) + if (ret == -1) { return -3; + } ret = ioctl(spi_link.fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_link.speed); - if (ret == -1) + if (ret == -1) { return -4; + } return 0; } -int spi_link_send(void *buf_out, size_t count, void *buf_in, uint8_t* crc_valid) { +int spi_link_send(void *buf_out, size_t count, void *buf_in, uint8_t *crc_valid) +{ int ret; @@ -54,14 +60,14 @@ int spi_link_send(void *buf_out, size_t count, void *buf_in, uint8_t* crc_valid) .bits_per_word = spi_link.bits, }; - ((uint8_t*)buf_out)[count-1] = crc_calc_block_crc8(buf_out, count-1); + ((uint8_t *)buf_out)[count - 1] = crc_calc_block_crc8(buf_out, count - 1); ret = ioctl(spi_link.fd, SPI_IOC_MESSAGE(1), &tr); 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]) + 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 { + } else { *crc_valid = 0; spi_link.crc_err_cnt++; } @@ -74,15 +80,17 @@ int spi_link_send(void *buf_out, size_t count, void *buf_in, uint8_t* crc_valid) #define POLYNOMIAL 0x31 #define WIDTH (8 * sizeof(uint8_t)) #define TOPBIT (1 << (WIDTH - 1)) -uint8_t crc_calc_block_crc8(const uint8_t buf[], uint32_t len) { +uint8_t crc_calc_block_crc8(const uint8_t buf[], uint32_t len) +{ uint8_t _remainder = 0; for (uint32_t byte = 0; byte < len; ++byte) { _remainder ^= (buf[byte] << (WIDTH - 8)); for (uint8_t bit = 8; bit > 0; --bit) { - if (_remainder & TOPBIT) + if (_remainder & TOPBIT) { _remainder = (_remainder << 1) ^ POLYNOMIAL; - else + } else { _remainder = (_remainder << 1); + } } } return (_remainder); @@ -93,18 +101,18 @@ uint8_t crc_calc_block_crc8(const uint8_t buf[], uint32_t len) { /* for reference: need to write a more efficient crc computation */ crc_t crc__table[256]; -void crc__init(uint32_t polynomial) { +void crc__init(uint32_t polynomial) +{ crc_t crc_remainder; uint32_t crc_dividend; crc_t top_bit = (1 << (CRC__WIDTH - 1)); uint8_t bit; - for(crc_dividend = 0; crc_dividend < 256; crc_dividend++) { + for (crc_dividend = 0; crc_dividend < 256; crc_dividend++) { crc_remainder = crc_dividend << (CRC__WIDTH - 8); - for(bit = 8; bit > 0; bit--) { - if(crc_remainder & top_bit) { + for (bit = 8; bit > 0; bit--) { + if (crc_remainder & top_bit) { crc_remainder = (crc_remainder << 1) ^ polynomial; - } - else { + } else { crc_remainder = (crc_remainder << 1); } } @@ -112,21 +120,22 @@ void crc__init(uint32_t polynomial) { } #if 0 - int i=0; - while (i<256) { - printf("%03d ",crc__table[i]); - if ((i%8)==7) printf("\n"); + int i = 0; + while (i < 256) { + printf("%03d ", crc__table[i]); + if ((i % 8) == 7) { printf("\n"); } i++; } #endif } -uint8_t crc__calc_block_crc8(const uint8_t buffer[], uint32_t buffer_length) { +uint8_t crc__calc_block_crc8(const uint8_t buffer[], uint32_t buffer_length) +{ int counter; uint16_t crc = 0; - for(counter = 0; counter < buffer_length; counter++) { - crc = crc ^ crc__table[ ( crc ^ *(char *)(buffer)++ ) & 0x00FF ]; + for (counter = 0; counter < buffer_length; counter++) { + crc = crc ^ crc__table[(crc ^ * (char *)(buffer)++) & 0x00FF ]; } return crc; } diff --git a/sw/airborne/fms/fms_spi_link.h b/sw/airborne/fms/fms_spi_link.h index 73c67401e1..eab2a3d234 100644 --- a/sw/airborne/fms/fms_spi_link.h +++ b/sw/airborne/fms/fms_spi_link.h @@ -28,7 +28,7 @@ struct SpiLink { int fd; - char* device; + char *device; uint8_t mode; uint8_t bits; uint32_t speed; @@ -53,7 +53,7 @@ extern int spi_link_init(void); * count is the size of buf_out and buf_in, that is * the count of data to exchange+1 */ -extern int spi_link_send(void *buf_out, size_t count, void* buf_in, uint8_t* crc_valid); +extern int spi_link_send(void *buf_out, size_t count, void *buf_in, uint8_t *crc_valid); /* * just for debuging purposes diff --git a/sw/airborne/fms/fms_spistream.h b/sw/airborne/fms/fms_spistream.h index 8c038bbeba..22a98964ee 100644 --- a/sw/airborne/fms/fms_spistream.h +++ b/sw/airborne/fms/fms_spistream.h @@ -6,29 +6,30 @@ #define min(a,b) ((a>b)? (b) : (a)) -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]; -*/ +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]; + */ uint8_t cnt; 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); + if (log_bytes > 16) { log_bytes = 16; } + /* + 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]); -*/ + printf("%s %s bytes: %3d | id: %3d | UART%d | ", + prefix, time_str, num_bytes, msg_id, data[0]); + */ printf("%s bytes: %3d | id: %3d | UART%d | ", prefix, num_bytes, msg_id, data[0]); - for(cnt = 1; cnt < log_bytes; cnt++) { + for (cnt = 1; cnt < log_bytes; cnt++) { printf("%02X ", data[cnt]); - if(cnt >= 24 && cnt % 24 == 0 && cnt+1 < log_bytes) { + if (cnt >= 24 && cnt % 24 == 0 && cnt + 1 < log_bytes) { printf("\n "); } } diff --git a/sw/airborne/fms/fms_spistream_client.c b/sw/airborne/fms/fms_spistream_client.c index 099f391473..5b572981fc 100644 --- a/sw/airborne/fms/fms_spistream_client.c +++ b/sw/airborne/fms/fms_spistream_client.c @@ -51,7 +51,7 @@ #include "fms_spistream.h" -static void parse_command_line(int argc, char** argv); +static void parse_command_line(int argc, char **argv); static void main_init(void); static void main_exit(void); static void main_periodic(int my_sig_num); @@ -67,7 +67,8 @@ static char cfifo_files[4][40]; -int main(int argc, char *argv[]) { +int main(int argc, char *argv[]) +{ parse_command_line(argc, argv); @@ -76,7 +77,7 @@ int main(int argc, char *argv[]) { /* Enter our mainloop */ event_dispatch(); - while(1) { + while (1) { sleep(100); } @@ -87,39 +88,38 @@ int main(int argc, char *argv[]) { return 0; } -static void main_periodic(int my_sig_num) { +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]; + static uint8_t buf[SPISTREAM_MAX_MESSAGE_LENGTH * 10]; - for(fifo_idx = 0; fifo_idx < 4; fifo_idx++) { + for (fifo_idx = 0; fifo_idx < 4; fifo_idx++) { // The periodic is triggered before fifo // connections have been initialized, so // check for a valid fd first: - if(dfifo[fifo_idx] > 0) { + if (dfifo[fifo_idx] > 0) { ret = read(dfifo[fifo_idx], (uint8_t *)(&num_bytes), 2); ret = read(dfifo[fifo_idx], (uint8_t *)(&msg_id), 1); memset(&buf, 0, SPISTREAM_MAX_MESSAGE_LENGTH); - if(num_bytes > SPISTREAM_MAX_MESSAGE_LENGTH) { + if (num_bytes > SPISTREAM_MAX_MESSAGE_LENGTH) { fprintf(stderr, "Warning: Message has length %d, but limit " - "is %d\n", + "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) { + if (ret > 0 && ret == num_bytes) { // Message received print_message(">> Client", msg_id, buf, num_bytes); - } - else if(ret > 0 && ret < num_bytes) { + } else if (ret > 0 && ret < num_bytes) { fprintf(stderr, "Tried to read %d bytes, but only got %d\n", num_bytes, ret); } - } - else { + } else { // FIFO file descriptor is invalid, // retry to open it: dfifo[fifo_idx] = open(dfifo_files[fifo_idx], O_RDONLY | O_NONBLOCK); @@ -128,7 +128,8 @@ static void main_periodic(int my_sig_num) { } -static void main_init(void) { +static void main_init(void) +{ TRACE(TRACE_DEBUG, "%s", "Starting initialization\n"); @@ -149,7 +150,7 @@ static void main_init(void) { signal(SIGTERM, on_kill); signal(SIGSEGV, on_kill); - if(!open_stream()) { + if (!open_stream()) { fprintf(stderr, "Could not open stream, sorry\n"); exit(1); } @@ -176,7 +177,8 @@ static void main_init(void) { * trigger on them, like select() or libevent. * */ -static int open_stream(void) { +static int open_stream(void) +{ uint8_t fifo_idx; strcpy(dfifo_files[0], "/tmp/spistream_d0.fifo"); // FIFOs for data @@ -188,7 +190,7 @@ static int open_stream(void) { 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++) { + 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"); @@ -196,10 +198,10 @@ static int open_stream(void) { return 1; - for(fifo_idx = 0; fifo_idx < 3; fifo_idx++) { + for (fifo_idx = 0; fifo_idx < 3; fifo_idx++) { fprintf(stderr, "Open command stream %s ... \n", cfifo_files[fifo_idx]); cfifo[fifo_idx] = open(cfifo_files[fifo_idx], O_WRONLY); - if(cfifo[fifo_idx] < 0) { + if (cfifo[fifo_idx] < 0) { fprintf(stderr, " failed\n"); return 0; } @@ -212,7 +214,8 @@ static void main_exit(void) fprintf(stderr, "Bye!\n"); } -static void parse_command_line(int argc, char** argv) { +static void parse_command_line(int argc, char **argv) +{ } static void on_kill(int signum) diff --git a/sw/airborne/fms/fms_spistream_daemon.c b/sw/airborne/fms/fms_spistream_daemon.c index 567fc02d07..f4792efc99 100644 --- a/sw/airborne/fms/fms_spistream_daemon.c +++ b/sw/airborne/fms/fms_spistream_daemon.c @@ -55,7 +55,7 @@ #define LOG_OUT stdout -static void parse_command_line(int argc, char** argv); +static void parse_command_line(int argc, char **argv); static void main_init(void); static void main_exit(void); static void main_periodic(int my_sig_num); @@ -68,10 +68,10 @@ static void on_timeout(int signum); static void on_kill(int signum); static void on_dead_pipe(int signum); -static void on_spistream_msg_received(uint8_t msg_id, uint8_t * data, uint16_t num_bytes); +static void on_spistream_msg_received(uint8_t msg_id, uint8_t *data, uint16_t num_bytes); static void on_spistream_msg_sent(uint8_t msg_id); -static void send_to_client(uint8_t * data, uint16_t num_bytes, uint8_t fifo_idx); +static void send_to_client(uint8_t *data, uint16_t num_bytes, uint8_t fifo_idx); static uint8_t spistream_msg[123]; @@ -82,7 +82,8 @@ static char cfifo_files[4][40]; -int main(int argc, char *argv[]) { +int main(int argc, char *argv[]) +{ parse_command_line(argc, argv); @@ -91,7 +92,7 @@ int main(int argc, char *argv[]) { /* Enter our mainloop */ event_dispatch(); - while(1) { + while (1) { sleep(100); } @@ -102,10 +103,11 @@ int main(int argc, char *argv[]) { return 0; } -static void main_periodic(int my_sig_num) { +static void main_periodic(int my_sig_num) +{ static int32_t every_100 = 1000; - if(every_100-- == 0) { + if (every_100-- == 0) { every_100 = 1000; spistream_send_msg(spistream_msg, 21, SPISTREAM_NO_WAIT); } @@ -113,90 +115,93 @@ static void main_periodic(int my_sig_num) { spistream_event(); } -static void spistream_event() { +static void spistream_event() +{ static struct AutopilotMessagePTStream msg_in; static struct AutopilotMessagePTStream msg_out; static uint8_t crc_valid; spistream_read_pkg(&msg_in); -/* - 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]); + /* + 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"); } - 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, - pkg_size, msg_in.message_cnt, msg_in.package_cntd); - for(cnt = 0; cnt < pkg_size; cnt++) { - printf("%02X ", msg_in.pkg_data[cnt]); + /* + 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"); } - printf("\n"); - } -*/ + */ spistream_write_pkg(&msg_out); } static void on_spistream_msg_received(uint8_t msg_id, - uint8_t * data, - uint16_t num_bytes) { + uint8_t *data, + uint16_t num_bytes) +{ uint8_t uart; - uint8_t buf[SPISTREAM_MAX_MESSAGE_LENGTH+3]; + uint8_t buf[SPISTREAM_MAX_MESSAGE_LENGTH + 3]; print_message("<< Daemon", msg_id, data, num_bytes); uart = data[0]; // Check for valid uart ID - if(uart >= 0 && uart <= 3) { - if(msg_id > 0) { + 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) { + if (num_bytes > SPISTREAM_MAX_MESSAGE_LENGTH) { fprintf(LOG_OUT, "Warning: Message has length %d, but limit " - "is %d - truncating message\n", + "is %d - truncating message\n", num_bytes, SPISTREAM_MAX_MESSAGE_LENGTH); num_bytes = SPISTREAM_MAX_MESSAGE_LENGTH; } - memcpy(buf+3, data, num_bytes); - send_to_client(buf, num_bytes+3, uart); + 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) +static void send_to_client(uint8_t *data, uint16_t num_bytes, uint8_t fifo_idx) { - if(dfifo[fifo_idx] <= 0) { + if (dfifo[fifo_idx] <= 0) { // No client connected to this fifo, yet dfifo[fifo_idx] = open(dfifo_files[fifo_idx], O_WRONLY | O_NONBLOCK); - if(dfifo[fifo_idx] <= 0) { + if (dfifo[fifo_idx] <= 0) { fprintf(LOG_OUT, "No client for data fifo %d (%s)\n", fifo_idx, dfifo_files[fifo_idx]); return; } - } - else { + } else { // Client connected to this fifo - if(write(dfifo[fifo_idx], data, num_bytes) == -1) { + if (write(dfifo[fifo_idx], data, num_bytes) == -1) { fprintf(LOG_OUT, "Write error on data fifo %d\n", fifo_idx); } } } -static void on_spistream_msg_sent(uint8_t msg_id) { +static void on_spistream_msg_sent(uint8_t msg_id) +{ // TRACE(TRACE_DEBUG, "%s", "SPI message sent \n"); } -static void main_init(void) { +static void main_init(void) +{ uint8_t byte_idx; TRACE(TRACE_DEBUG, "%s", "Starting initialization\n"); @@ -210,7 +215,7 @@ static void main_init(void) { spistream_init(&on_spistream_msg_received, &on_spistream_msg_sent); - for(byte_idx=1; byte_idx < 123; byte_idx++) { + for (byte_idx = 1; byte_idx < 123; byte_idx++) { spistream_msg[byte_idx] = byte_idx; } /* Initalize the event library */ @@ -231,7 +236,7 @@ static void main_init(void) { signal(SIGSEGV, on_kill); signal(SIGPIPE, on_dead_pipe); - if(!open_stream()) { + if (!open_stream()) { fprintf(LOG_OUT, "Could not open stream, sorry\n"); exit(1); } @@ -245,19 +250,21 @@ static void main_exit(void) 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; +static void parse_command_line(int argc, char **argv) +{ + /* + while ((ch = getopt(argc, argv, "d:")) != -1) { + switch (ch) { + case 'd': + daemon_mode = 1; + break; + } } - } -*/ + */ } -static int open_stream(void) { +static int open_stream(void) +{ uint8_t fifo_idx; int ret; @@ -270,31 +277,29 @@ static int open_stream(void) { 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++) { + 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) { + 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]); + fifo_idx, dfifo_files[fifo_idx]); close_stream(); return 0; - } - else { + } 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++) { + 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) { + 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]); + fifo_idx, cfifo_files[fifo_idx]); close_stream(); return 0; - } - else { + } else { fprintf(LOG_OUT, " ok\n"); cfifo[fifo_idx] = open(cfifo_files[fifo_idx], O_RDONLY | O_NONBLOCK); } @@ -302,16 +307,16 @@ static int open_stream(void) { return 1; } -static void close_stream(void) { +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) { + 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) { + if (cfifo[fifo_idx] >= 0) { close(cfifo[fifo_idx]); } unlink(cfifo_files[fifo_idx]); @@ -337,7 +342,7 @@ static void on_dead_pipe(int signum) 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++) { + for (fifo_idx = 0; fifo_idx < 4; fifo_idx++) { close(dfifo[fifo_idx]); dfifo[fifo_idx] = -1; } diff --git a/sw/airborne/fms/fms_test_datalink.c b/sw/airborne/fms/fms_test_datalink.c index d93fe1956e..eccbf2ff5d 100644 --- a/sw/airborne/fms/fms_test_datalink.c +++ b/sw/airborne/fms/fms_test_datalink.c @@ -21,37 +21,38 @@ static struct event periodic_event; static struct event datalink_event; static struct timespec periodic_date; -static struct FmsNetwork* network; +static struct FmsNetwork *network; -#define PERIODIC_START() { \ - clock_gettime(CLOCK_MONOTONIC, &periodic_date); \ - struct timeval tv; \ - evutil_timerclear(&tv); \ - tv.tv_sec = PERIODIC_SEC; \ - tv.tv_usec = PERIODIC_USEC; \ - event_add(&periodic_event, &tv); \ +#define PERIODIC_START() { \ + clock_gettime(CLOCK_MONOTONIC, &periodic_date); \ + struct timeval tv; \ + evutil_timerclear(&tv); \ + tv.tv_sec = PERIODIC_SEC; \ + tv.tv_usec = PERIODIC_USEC; \ + event_add(&periodic_event, &tv); \ } -#define PERIODIC_RESCHEDULE() { \ - periodic_date.tv_nsec += PERIODIC_NSEC; \ - if (periodic_date.tv_nsec>= ONE_SEC_NS) { \ - periodic_date.tv_nsec -= ONE_SEC_NS; \ - periodic_date.tv_sec++; \ - } \ - struct timespec time_now; \ - clock_gettime(CLOCK_MONOTONIC, &time_now); \ - int32_t dt_ns = (int32_t)periodic_date.tv_nsec - (int32_t)time_now.tv_nsec; \ - if (time_now.tv_sec != periodic_date.tv_sec) \ - dt_ns += ONE_SEC_NS; \ - struct timeval tv; \ - evutil_timerclear(&tv); \ - tv.tv_sec = PERIODIC_SEC; \ - tv.tv_usec = dt_ns / 1000; \ - event_add(&periodic_event, &tv); \ +#define PERIODIC_RESCHEDULE() { \ + periodic_date.tv_nsec += PERIODIC_NSEC; \ + if (periodic_date.tv_nsec>= ONE_SEC_NS) { \ + periodic_date.tv_nsec -= ONE_SEC_NS; \ + periodic_date.tv_sec++; \ + } \ + struct timespec time_now; \ + clock_gettime(CLOCK_MONOTONIC, &time_now); \ + int32_t dt_ns = (int32_t)periodic_date.tv_nsec - (int32_t)time_now.tv_nsec; \ + if (time_now.tv_sec != periodic_date.tv_sec) \ + dt_ns += ONE_SEC_NS; \ + struct timeval tv; \ + evutil_timerclear(&tv); \ + tv.tv_sec = PERIODIC_SEC; \ + tv.tv_usec = dt_ns / 1000; \ + event_add(&periodic_event, &tv); \ } -static void periodic_task(int fd, short event, void *arg) { +static void periodic_task(int fd, short event, void *arg) +{ DOWNLINK_SEND_ALIVE(16, MD5SUM); @@ -64,7 +65,8 @@ static void periodic_task(int fd, short event, void *arg) { } -static void on_datalink_event(int fd, short event, void *arg) { +static void on_datalink_event(int fd, short event, void *arg) +{ char buf[255]; int bytes_read; bytes_read = read(fd, buf, sizeof(buf) - 1); @@ -82,7 +84,8 @@ static void on_datalink_event(int fd, short event, void *arg) { } -int main(int argc , char** argv) { +int main(int argc , char **argv) +{ /* Set real time priority */ struct sched_param param; diff --git a/sw/airborne/fms/libeknav/estimate_attitude.c b/sw/airborne/fms/libeknav/estimate_attitude.c index b23f49e9c2..2abff54507 100644 --- a/sw/airborne/fms/libeknav/estimate_attitude.c +++ b/sw/airborne/fms/libeknav/estimate_attitude.c @@ -2,13 +2,15 @@ #include -struct DoubleMat44 square_skaled(struct DoubleMat44 A){ - double _1_max = 1/INFTY_NORM16(A); +struct DoubleMat44 square_skaled(struct DoubleMat44 A) +{ + double _1_max = 1 / INFTY_NORM16(A); struct DoubleMat44 A2; - int row,col; - for(row=0; row<4; row++){ - for(col=0; col<4; col++){ - M4(A2, row, col) = M4(A, row,0)*M4(A,0,col) + M4(A, row,1)*M4(A,1,col) + M4(A, row,2)*M4(A,2,col) + M4(A, row,3)*M4(A,3,col); + int row, col; + for (row = 0; row < 4; row++) { + for (col = 0; col < 4; col++) { + M4(A2, row, col) = M4(A, row, 0) * M4(A, 0, col) + M4(A, row, 1) * M4(A, 1, col) + M4(A, row, 2) * M4(A, 2, col) + M4(A, + row, 3) * M4(A, 3, col); M4(A2, row, col) *= _1_max; // pays attention that the values don't grow too far } } @@ -34,7 +36,9 @@ struct DoubleMat44 square_skaled(struct DoubleMat44 A){ * It ends if the steps are getting too close to each other. * */ -DoubleVect4 dominant_Eigenvector(struct DoubleMat44 A, unsigned int maximum_iterations, double precision, struct DoubleMat44 sigma_A, DoubleVect4 *sigma_x){ +DoubleVect4 dominant_Eigenvector(struct DoubleMat44 A, unsigned int maximum_iterations, double precision, + struct DoubleMat44 sigma_A, DoubleVect4 *sigma_x) +{ unsigned int k; DoubleVect4 x_k, x_kp1; @@ -44,13 +48,13 @@ DoubleVect4 dominant_Eigenvector(struct DoubleMat44 A, unsigned int maximum_iter FLOAT_QUAT_ZERO(x_k); //for(k=0; (kprecision); k++){ - for(k=0; k(b)?(a):(b)) #define MAXABS(a,b) MAX(ABS(a),ABS(b)) -#define MAT33_TRANSP(_to,_from) { \ - MAT33_ELMT((_to),0,0) = MAT33_ELMT((_from),0,0); \ - MAT33_ELMT((_to),0,1) = MAT33_ELMT((_from),1,0); \ - MAT33_ELMT((_to),0,2) = MAT33_ELMT((_from),2,0); \ - MAT33_ELMT((_to),1,0) = MAT33_ELMT((_from),0,1); \ - MAT33_ELMT((_to),1,1) = MAT33_ELMT((_from),1,1); \ - MAT33_ELMT((_to),1,2) = MAT33_ELMT((_from),2,1); \ - MAT33_ELMT((_to),2,0) = MAT33_ELMT((_from),0,2); \ - MAT33_ELMT((_to),2,1) = MAT33_ELMT((_from),1,2); \ - MAT33_ELMT((_to),2,2) = MAT33_ELMT((_from),2,2); \ -} -#define SWAP(a, b){ \ - typeof (a) temp = a; \ - a = b; \ - b = temp; \ -} +#define MAT33_TRANSP(_to,_from) { \ + MAT33_ELMT((_to),0,0) = MAT33_ELMT((_from),0,0); \ + MAT33_ELMT((_to),0,1) = MAT33_ELMT((_from),1,0); \ + MAT33_ELMT((_to),0,2) = MAT33_ELMT((_from),2,0); \ + MAT33_ELMT((_to),1,0) = MAT33_ELMT((_from),0,1); \ + MAT33_ELMT((_to),1,1) = MAT33_ELMT((_from),1,1); \ + MAT33_ELMT((_to),1,2) = MAT33_ELMT((_from),2,1); \ + MAT33_ELMT((_to),2,0) = MAT33_ELMT((_from),0,2); \ + MAT33_ELMT((_to),2,1) = MAT33_ELMT((_from),1,2); \ + MAT33_ELMT((_to),2,2) = MAT33_ELMT((_from),2,2); \ + } +#define SWAP(a, b){ \ + 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; \ -} +#define MAT33_ROW(mat, row, value0, value1, 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]; \ -} +#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]; \ + } #define DOUBLE_VECT3_NORM(_v) (sqrt((_v).x*(_v).x + (_v).y*(_v).y + (_v).z*(_v).z)) -#define DOUBLE_QUAT_OF_RMAT(_q, _r) { \ - const double tr = RMAT_TRACE(_r); \ - if (tr > 0) { \ - const double two_qi = sqrt(1.+tr); \ - const double four_qi = 2. * two_qi; \ - _q.qi = 0.5 * two_qi; \ - _q.qx = (RMAT_ELMT(_r, 1, 2)-RMAT_ELMT(_r, 2, 1))/four_qi; \ - _q.qy = (RMAT_ELMT(_r, 2, 0)-RMAT_ELMT(_r, 0, 2))/four_qi; \ - _q.qz = (RMAT_ELMT(_r, 0, 1)-RMAT_ELMT(_r, 1, 0))/four_qi; \ - /*printf("tr > 0\n");*/ \ - } \ - 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");*/ \ - } \ - 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");*/ \ - } \ - 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");*/ \ - } \ - } \ +#define DOUBLE_QUAT_OF_RMAT(_q, _r) { \ + const double tr = RMAT_TRACE(_r); \ + if (tr > 0) { \ + const double two_qi = sqrt(1.+tr); \ + const double four_qi = 2. * two_qi; \ + _q.qi = 0.5 * two_qi; \ + _q.qx = (RMAT_ELMT(_r, 1, 2)-RMAT_ELMT(_r, 2, 1))/four_qi; \ + _q.qy = (RMAT_ELMT(_r, 2, 0)-RMAT_ELMT(_r, 0, 2))/four_qi; \ + _q.qz = (RMAT_ELMT(_r, 0, 1)-RMAT_ELMT(_r, 1, 0))/four_qi; \ + /*printf("tr > 0\n");*/ \ + } \ + 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");*/ \ + } \ + 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");*/ \ + } \ + 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");*/ \ + } \ + } \ } -#define QUAT_ENU_FROM_TO_NED(from, to){ \ - to.qi = - from.qx - from.qy; \ - to.qy = + from.qi + from.qz; \ - to.qx = + from.qi - from.qz; \ - to.qz = - from.qx + from.qy; \ - QUAT_SMUL(to, to, M_SQRT1_2); \ -} +#define QUAT_ENU_FROM_TO_NED(from, to){ \ + to.qi = - from.qx - from.qy; \ + to.qy = + from.qi + from.qz; \ + to.qx = + from.qi - from.qz; \ + to.qz = - from.qx + from.qy; \ + QUAT_SMUL(to, to, M_SQRT1_2); \ + } -#define QUAT_IMAGINARY_PART(quat, vector) VECT3_ASSIGN(vector, (quat).qx, (quat).qy, (quat).qz) +#define QUAT_IMAGINARY_PART(quat, vector) VECT3_ASSIGN(vector, (quat).qx, (quat).qy, (quat).qz) -#define VECT3_TO_EULERS(vector, eulers){ \ - (eulers).phi = (vector).x; \ - (eulers).theta = (vector).y; \ - (eulers).psi = (vector).z; \ -} +#define VECT3_TO_EULERS(vector, eulers){ \ + (eulers).phi = (vector).x; \ + (eulers).theta = (vector).y; \ + (eulers).psi = (vector).z; \ + } /* @@ -149,7 +149,7 @@ #define RATES_BFP_AS_VECTOR3D(rates) Vector3d(RATE_FLOAT_OF_BFP(rates.p),RATE_FLOAT_OF_BFP(rates.q),RATE_FLOAT_OF_BFP(rates.r)) #define DOUBLEQUAT_AS_QUATERNIOND(quat) Quaterniond(quat.qi, quat.qx, quat.qy, quat.qz) -#define RMAT_TO_EIGENMATRIX(Eigen,c) Eigen << (c)[0],(c)[1],(c)[2],(c)[3],(c)[4],(c)[5],(c)[6],(c)[7],(c)[8] +#define RMAT_TO_EIGENMATRIX(Eigen,c) Eigen << (c)[0],(c)[1],(c)[2],(c)[3],(c)[4],(c)[5],(c)[6],(c)[7],(c)[8] /* @@ -158,8 +158,8 @@ #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); \ -} +#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); \ + } diff --git a/sw/airborne/fms/libeknav/raw_log.h b/sw/airborne/fms/libeknav/raw_log.h index 55cecbf8e5..1b5aabb6cf 100644 --- a/sw/airborne/fms/libeknav/raw_log.h +++ b/sw/airborne/fms/libeknav/raw_log.h @@ -10,13 +10,13 @@ struct __attribute__ ((packed)) raw_log_entry{ struct FloatRates gyro; struct FloatVect3 accel; struct FloatVect3 mag; - struct FloatVect3 ecef_pos; - struct FloatVect3 ecef_vel; + struct FloatVect3 ecef_pos; + struct FloatVect3 ecef_vel; int16_t pressure_absolute; uint8_t data_valid; }; */ -struct __attribute__ ((packed)) raw_log_entry{ +struct __attribute__((packed)) raw_log_entry { float time; struct AutopilotMessageVIUp message; }; diff --git a/sw/airborne/fms/libeknav/raw_log_to_ascii.c b/sw/airborne/fms/libeknav/raw_log_to_ascii.c index e776993b16..a16cc7f19f 100644 --- a/sw/airborne/fms/libeknav/raw_log_to_ascii.c +++ b/sw/airborne/fms/libeknav/raw_log_to_ascii.c @@ -13,7 +13,7 @@ #include "fms/fms_autopilot_msg.h" #include "fms/libeknav/raw_log.h" -void print_raw_log_entry(struct raw_log_entry*); +void print_raw_log_entry(struct raw_log_entry *); //void build_fake_log(void); #define PRT(a) printf("%f ", a); @@ -28,7 +28,8 @@ struct raw_log_entry __attribute__ ((packed)){ */ -int main(int argc, char** argv) { +int main(int argc, char **argv) +{ // build_fake_log(); @@ -44,7 +45,7 @@ int main(int argc, char** argv) { while (1) { struct raw_log_entry entry; ssize_t nb_read = read(raw_log_fd, &entry, sizeof(entry)); - if (nb_read != sizeof(entry)) break; + if (nb_read != sizeof(entry)) { break; } print_raw_log_entry(&entry); printf("\n"); //printf("%f %f %f %f", e.time, e.gyro.p, e.gyro.q, e.gyro.r); @@ -55,7 +56,8 @@ int main(int argc, char** argv) { -void print_raw_log_entry(struct raw_log_entry* e){ +void print_raw_log_entry(struct raw_log_entry *e) +{ struct DoubleVect3 tempvect; struct DoubleRates temprates; printf("%f\t", e->time); @@ -68,7 +70,7 @@ void print_raw_log_entry(struct raw_log_entry* e){ 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; + double baro_scaled = (double)(e->message.pressure_absolute) / 256; printf("%f", baro_scaled); } diff --git a/sw/airborne/fms/lpc_test_spi.c b/sw/airborne/fms/lpc_test_spi.c index 3af04aa144..39861cbe10 100644 --- a/sw/airborne/fms/lpc_test_spi.c +++ b/sw/airborne/fms/lpc_test_spi.c @@ -28,37 +28,42 @@ #include "armVIC.h" #include "LPC21xx.h" -static inline void main_init( void ); -static inline void main_periodic_task( void ); -static inline void main_event_task( void ); +static inline void main_init(void); +static inline void main_periodic_task(void); +static inline void main_event_task(void); static inline void main_spi_init(void); static void SPI0_ISR(void) __attribute__((naked)); -int main( void ) { +int main(void) +{ main_init(); - while(1) { - if (sys_time_check_and_ack_timer(0)) + while (1) { + if (sys_time_check_and_ack_timer(0)) { main_periodic_task(); + } main_event_task(); } return 0; } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); main_spi_init(); mcu_int_enable(); } -static inline void main_periodic_task( void ) { +static inline void main_periodic_task(void) +{ } -static inline void main_event_task( void ) { +static inline void main_event_task(void) +{ } @@ -75,8 +80,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 @@ -88,7 +93,8 @@ static inline void main_event_task( void ) { #define SPIE 7 -static inline void main_spi_init(void) { +static inline void main_spi_init(void) +{ /* setup pins for SPI0 (SCK, MISO, MOSI, SS) */ PINSEL0 |= PINSEL0_SCK | PINSEL0_MISO | PINSEL0_MOSI | PINSEL0_SSEL; @@ -106,22 +112,23 @@ static inline void main_spi_init(void) { } -static void SPI0_ISR(void) { - ISR_ENTRY(); +static void SPI0_ISR(void) +{ + ISR_ENTRY(); - static uint8_t cnt = 0; - LED_TOGGLE(1); + static uint8_t cnt = 0; + LED_TOGGLE(1); - if ( bit_is_set(S0SPSR, SPIF)) { /* transfer complete */ - uint8_t foo = S0SPDR; - S0SPDR = cnt; - cnt++; - } + if (bit_is_set(S0SPSR, SPIF)) { /* transfer complete */ + uint8_t foo = S0SPDR; + S0SPDR = cnt; + cnt++; + } - /* clear_it */ - S0SPINT = 1<tv_sec - start_secs, (unsigned) ts->tv_usec); - printf("%d ", (uint32_t) buf[1]); // paparazzi timestamp; see udp_transport.h - printf("%i %i ", buf[5], buf[6]); // AC_ID MSG_ID - for(i = 6; i < length - 3; i++) - { + printf("%d ", (uint32_t) buf[1]); // paparazzi timestamp; see udp_transport.h + printf("%i %i ", buf[5], buf[6]); // AC_ID MSG_ID + for (i = 6; i < length - 3; i++) { printf("%02x ", buf[i]); } printf("\n"); } -static void got_packet (u_char *args, const struct pcap_pkthdr *header, - const u_char *packet) +static void got_packet(u_char *args, const struct pcap_pkthdr *header, + const u_char *packet) { const u_char *payload; const struct ethernet_header *ethernet; @@ -57,16 +55,16 @@ static void got_packet (u_char *args, const struct pcap_pkthdr *header, int i; ethernet = (struct ethernet_header *) packet; - ip = (struct ip_header *) (packet + ETHERNET_HEADER_LENGTH); + ip = (struct ip_header *)(packet + ETHERNET_HEADER_LENGTH); size_ip = IP_HL_WORDS(ip) * 4; if (size_ip < 20) { printf("invalid IP hdr length: %u bytes\n", size_ip); return; } - udp = (struct udp_header *) ((u_char *)ip + size_ip); + udp = (struct udp_header *)((u_char *)ip + size_ip); - payload = (u_char *) ((u_char *)udp + sizeof(struct udp_header)); + payload = (u_char *)((u_char *)udp + sizeof(struct udp_header)); udp_length = htons(udp->uh_len); //printf ("Got udp packet length %i\n", udp_length); diff --git a/sw/airborne/fms/onboard_transport.c b/sw/airborne/fms/onboard_transport.c index 5065dbd2e8..c49d1b1a34 100644 --- a/sw/airborne/fms/onboard_transport.c +++ b/sw/airborne/fms/onboard_transport.c @@ -18,7 +18,8 @@ #define FILENAME_LEN 64 #define TIMESTAMP_SCALE 10000 -static void put_bytes(void *impl, enum DownlinkDataType data_type, uint8_t len __attribute__((unused)), const void *bytes) +static void put_bytes(void *impl, enum DownlinkDataType data_type, uint8_t len __attribute__((unused)), + const void *bytes) { struct onboard_transport *onboard = (struct onboard_transport *) impl; uint32_t length = 0; @@ -35,57 +36,69 @@ static void put_bytes(void *impl, enum DownlinkDataType data_type, uint8_t len _ onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, " "); } switch (data_type) { - case DL_TYPE_UINT8: - onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%hhu", * (const uint8_t *)bytes); - bytes = (const uint8_t *) bytes + 1; - break; - case DL_TYPE_UINT16: - onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%hu", * (const uint16_t *)bytes); - bytes = (const uint16_t *) bytes + 2; - break; - case DL_TYPE_UINT32: - onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%u", * (const uint32_t *)bytes); - bytes = (const uint32_t *) bytes + 4; - break; - case DL_TYPE_UINT64: - onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%llu", *(const uint64_t *)bytes); - bytes = (const uint64_t *) bytes + 8; - break; - case DL_TYPE_INT8: - onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%hhi", * (const int8_t *)bytes); - bytes = (const int8_t *) bytes + 1; - break; - case DL_TYPE_INT16: - onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%hi", * (const int16_t *)bytes); - bytes = (const int16_t *) bytes + 2; - break; - case DL_TYPE_INT32: - onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%i", * (const int32_t *)bytes); - bytes = (const int32_t *) bytes + 4; - break; - case DL_TYPE_INT64: - onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%lli", *(const int64_t *)bytes); - bytes = (const int64_t *) bytes + 8; - break; - case DL_TYPE_FLOAT: - onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%#f", *(const float *)bytes); - bytes = (const float *) bytes + 4; - break; - case DL_TYPE_DOUBLE: - onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%#f", *(const double *)bytes); - bytes = (const double *) bytes + 8; - break; - case DL_TYPE_TIMESTAMP: - onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%u.%04u", (*(const uint32_t *)bytes) / TIMESTAMP_SCALE,(*(const uint32_t *)bytes) % TIMESTAMP_SCALE); - bytes = (const uint32_t *) bytes + 4; - break; - case DL_TYPE_ARRAY_LENGTH: - break; + case DL_TYPE_UINT8: + onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%hhu", + * (const uint8_t *)bytes); + bytes = (const uint8_t *) bytes + 1; + break; + case DL_TYPE_UINT16: + onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%hu", + * (const uint16_t *)bytes); + bytes = (const uint16_t *) bytes + 2; + break; + case DL_TYPE_UINT32: + onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%u", + * (const uint32_t *)bytes); + bytes = (const uint32_t *) bytes + 4; + break; + case DL_TYPE_UINT64: + onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%llu", + *(const uint64_t *)bytes); + bytes = (const uint64_t *) bytes + 8; + break; + case DL_TYPE_INT8: + onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%hhi", + * (const int8_t *)bytes); + bytes = (const int8_t *) bytes + 1; + break; + case DL_TYPE_INT16: + onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%hi", + * (const int16_t *)bytes); + bytes = (const int16_t *) bytes + 2; + break; + case DL_TYPE_INT32: + onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%i", + * (const int32_t *)bytes); + bytes = (const int32_t *) bytes + 4; + break; + case DL_TYPE_INT64: + onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%lli", + *(const int64_t *)bytes); + bytes = (const int64_t *) bytes + 8; + break; + case DL_TYPE_FLOAT: + onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%#f", + *(const float *)bytes); + bytes = (const float *) bytes + 4; + break; + case DL_TYPE_DOUBLE: + onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, "%#f", + *(const double *)bytes); + bytes = (const double *) bytes + 8; + break; + case DL_TYPE_TIMESTAMP: + onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, + "%u.%04u", (*(const uint32_t *)bytes) / TIMESTAMP_SCALE, (*(const uint32_t *)bytes) % TIMESTAMP_SCALE); + bytes = (const uint32_t *) bytes + 4; + break; + case DL_TYPE_ARRAY_LENGTH: + break; } } } -static void start_message(void *impl, char *name, uint8_t msg_id __attribute__((unused)), uint8_t payload_len __attribute__((unused))) +static void start_message(void *impl, char *name, uint8_t msg_id __attribute__((unused)), + uint8_t payload_len __attribute__((unused))) { uint8_t ac_id = AC_ID; struct onboard_transport *onboard = (struct onboard_transport *) impl; @@ -94,7 +107,8 @@ static void start_message(void *impl, char *name, uint8_t msg_id __attribute__(( put_bytes(onboard, DL_TYPE_TIMESTAMP, 4, (uint8_t *) onboard->timestamp); put_bytes(onboard, DL_TYPE_UINT8, 1, (uint8_t *) &ac_id); - onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, " %s", name); + onboard->buffer_idx += snprintf(onboard->buffer + onboard->buffer_idx, ONBOARD_BUFFER_LEN - onboard->buffer_idx, " %s", + name); } static void end_message(void *impl) @@ -150,8 +164,9 @@ static int open_piped(char *filepath) int retval; /* Lower our priority -- logging is not that important */ - if (setpriority(PRIO_PROCESS, 0, 10) < 0) + if (setpriority(PRIO_PROCESS, 0, 10) < 0) { fprintf(stderr, "Couldn't renice logger for some reason!\n"); + } // copy from the read side of the pipe to the log while (1) { @@ -170,7 +185,7 @@ static int open_piped(char *filepath) close(fd); // set non blocking on the write side of the pipe - flags = fcntl( pipe_fd[1], F_GETFL ); + flags = fcntl(pipe_fd[1], F_GETFL); fcntl(pipe_fd[1], F_SETFL, flags | O_NONBLOCK); // return the write side of the pipe diff --git a/sw/airborne/fms/overo_blmc_calibrate.c b/sw/airborne/fms/overo_blmc_calibrate.c index 72e021cd87..f1e86c5684 100644 --- a/sw/airborne/fms/overo_blmc_calibrate.c +++ b/sw/airborne/fms/overo_blmc_calibrate.c @@ -42,7 +42,8 @@ struct OveroBLMCCalibrate blmc_calibrate; static void main_init(void); static void dialog_with_io_proc(void); -int main(int argc, char *argv[]) { +int main(int argc, char *argv[]) +{ main_init(); @@ -50,19 +51,21 @@ int main(int argc, char *argv[]) { } -static void dialog_with_io_proc() { +static void dialog_with_io_proc() +{ struct AutopilotMessageCRCFrame msg_in; struct AutopilotMessageCRCFrame msg_out; uint8_t crc_valid; - for (uint8_t i=0; i 48) { log_bytes = 48; } + 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++) { + for (cnt = 1; cnt < log_bytes; cnt++) { printf("%02X ", data[cnt]); } printf("\n"); } -static void on_spistream_msg_sent(void) { +static void on_spistream_msg_sent(void) +{ // TRACE(TRACE_DEBUG, "%s", "SPI message sent \n"); } -static void main_init(void) { +static void main_init(void) +{ uint8_t byte_idx; TRACE(TRACE_DEBUG, "%s", "Starting initialization\n"); @@ -162,16 +168,16 @@ static void main_init(void) { spistream_init(&on_spistream_msg_received, &on_spistream_msg_sent); -/* - 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; - spistream_msg[byte_idx+2] = 0xBE; - spistream_msg[byte_idx+3] = 0xEF; - } -*/ - for(byte_idx=1; byte_idx < 123; byte_idx++) { + /* + 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; + spistream_msg[byte_idx+2] = 0xBE; + spistream_msg[byte_idx+3] = 0xEF; + } + */ + for (byte_idx = 1; byte_idx < 123; byte_idx++) { spistream_msg[byte_idx] = byte_idx; } /* Initalize the event library */ @@ -186,5 +192,6 @@ static void main_init(void) { TRACE(TRACE_DEBUG, "%s", "Initialization completed\n"); } -static void parse_command_line(int argc, char** argv) { +static void parse_command_line(int argc, char **argv) +{ } diff --git a/sw/airborne/fms/overo_test_passthrough.c b/sw/airborne/fms/overo_test_passthrough.c index 34416964e1..7e80dd58f6 100644 --- a/sw/airborne/fms/overo_test_passthrough.c +++ b/sw/airborne/fms/overo_test_passthrough.c @@ -43,13 +43,14 @@ struct OveroTestPassthrough otp; -static void parse_command_line(int argc, char** argv); +static void parse_command_line(int argc, char **argv); static void main_init(void); static void main_periodic(int my_sig_num); static void dialog_with_io_proc(void); -int main(int argc, char *argv[]) { +int main(int argc, char *argv[]) +{ parse_command_line(argc, argv); @@ -65,7 +66,8 @@ int main(int argc, char *argv[]) { } -static void main_periodic(int my_sig_num) { +static void main_periodic(int my_sig_num) +{ dialog_with_io_proc(); @@ -75,13 +77,14 @@ static void main_periodic(int my_sig_num) { -static void dialog_with_io_proc() { +static void dialog_with_io_proc() +{ struct AutopilotMessageCRCFrame msg_in; struct AutopilotMessageCRCFrame msg_out; uint8_t crc_valid; - for (uint8_t i=0; i<6; i++) msg_out.payload.msg_down.pwm_outputs_usecs[i] = otp.servos_outputs_usecs[i]; + for (uint8_t i = 0; i < 6; i++) { msg_out.payload.msg_down.pwm_outputs_usecs[i] = otp.servos_outputs_usecs[i]; } // for (uint8_t i=0; i<4; i++) msg_out.payload.msg_down.csc_servo_cmd[i] = otp.csc_servo_outputs[i]; spi_link_send(&msg_out, sizeof(struct AutopilotMessageCRCFrame), &msg_in, &crc_valid); @@ -102,7 +105,8 @@ static void dialog_with_io_proc() { } -static void main_init(void) { +static void main_init(void) +{ TRACE(TRACE_DEBUG, "%s", "Starting initialization\n"); @@ -125,20 +129,22 @@ static void main_init(void) { fms_gs_com_init(otp.gs_gw, 4242, 4243, FALSE); /* Initialize blaaa */ - for (uint8_t i=0; i<6; i++) otp.servos_outputs_usecs[i] = 1500; - for (uint8_t i=0; i<4; i++) otp.csc_servo_outputs[i] = 1500; + for (uint8_t i = 0; i < 6; i++) { otp.servos_outputs_usecs[i] = 1500; } + for (uint8_t i = 0; i < 4; i++) { otp.csc_servo_outputs[i] = 1500; } TRACE(TRACE_DEBUG, "%s", "Initialization completed\n"); } -static void parse_command_line(int argc, char** argv) { +static void parse_command_line(int argc, char **argv) +{ - if (argc > 1) + if (argc > 1) { otp.gs_gw = strdup(argv[1]); - else + } else { otp.gs_gw = strdup("10.31.4.7"); + } TRACE(TRACE_DEBUG, "%s", "Parsing command line:\n"); TRACE(TRACE_DEBUG, " gw: %s\n", otp.gs_gw); diff --git a/sw/airborne/fms/overo_test_passthrough.h b/sw/airborne/fms/overo_test_passthrough.h index 9d5bd66fca..bbe605e9b5 100644 --- a/sw/airborne/fms/overo_test_passthrough.h +++ b/sw/airborne/fms/overo_test_passthrough.h @@ -6,7 +6,7 @@ struct OveroTestPassthrough { /* our network connection */ - char* gs_gw; + char *gs_gw; /* our sensors */ struct ImuFloat imu; @@ -21,7 +21,7 @@ struct OveroTestPassthrough { /* the io proc status */ uint32_t io_proc_msg_cnt; uint32_t io_proc_err_cnt; - }; +}; extern struct OveroTestPassthrough otp; diff --git a/sw/airborne/fms/overo_test_passthrough_telemetry.h b/sw/airborne/fms/overo_test_passthrough_telemetry.h index fc15027513..4aec58feba 100644 --- a/sw/airborne/fms/overo_test_passthrough_telemetry.h +++ b/sw/airborne/fms/overo_test_passthrough_telemetry.h @@ -6,25 +6,25 @@ #include "fms/overo_test_passthrough.h" #include "fms/fms_spi_link.h" #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) +#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) -#define PERIODIC_SEND_IMU_GYRO(_transport) \ +#define PERIODIC_SEND_IMU_GYRO(_transport) \ DOWNLINK_SEND_IMU_GYRO(_transport, &otp.imu.gyro.p, &otp.imu.gyro.q, &otp.imu.gyro.r) -#define PERIODIC_SEND_IMU_ACCEL(_transport) \ +#define PERIODIC_SEND_IMU_ACCEL(_transport) \ DOWNLINK_SEND_IMU_ACCEL(_transport, &otp.imu.accel.x, &otp.imu.accel.y, &otp.imu.accel.z) -#define PERIODIC_SEND_IMU_MAG(_transport) \ +#define PERIODIC_SEND_IMU_MAG(_transport) \ DOWNLINK_SEND_IMU_MAG(_transport, &otp.imu.mag.x, &otp.imu.mag.y, &otp.imu.mag.z) -#define PERIODIC_SEND_BARO_RAW(_transport) \ +#define PERIODIC_SEND_BARO_RAW(_transport) \ DOWNLINK_SEND_BARO_RAW(_transport, &otp.baro_abs, &otp.baro_diff) diff --git a/sw/airborne/fms/overo_test_periodic.c b/sw/airborne/fms/overo_test_periodic.c index e3c5028992..6cbd499c11 100644 --- a/sw/airborne/fms/overo_test_periodic.c +++ b/sw/airborne/fms/overo_test_periodic.c @@ -25,9 +25,10 @@ static void main_periodic(int); static void main_send_to_stm(void); static void on_datalink_event(int fd, short event, void *arg); -static struct FmsNetwork* network; +static struct FmsNetwork *network; -int main(int argc, char** argv) { +int main(int argc, char **argv) +{ /* Initalize event library */ event_init(); @@ -54,7 +55,8 @@ int main(int argc, char** argv) { } -static void main_periodic(int my_sig_num) { +static void main_periodic(int my_sig_num) +{ DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM); @@ -65,7 +67,8 @@ static void main_periodic(int my_sig_num) { } -static void main_send_to_stm(void) { +static void main_send_to_stm(void) +{ struct OVERO_LINK_MSG_UP msg_in; struct OVERO_LINK_MSG_DOWN msg_out; @@ -78,7 +81,8 @@ static void main_send_to_stm(void) { } -static void on_datalink_event(int fd, short event, void *arg) { +static void on_datalink_event(int fd, short event, void *arg) +{ char buf[255]; int bytes_read; bytes_read = read(fd, buf, sizeof(buf) - 1); diff --git a/sw/airborne/fms/overo_test_spi_link.c b/sw/airborne/fms/overo_test_spi_link.c index 52640f9fb7..bf19ebc5d7 100644 --- a/sw/airborne/fms/overo_test_spi_link.c +++ b/sw/airborne/fms/overo_test_spi_link.c @@ -31,21 +31,21 @@ #include "fms/fms_autopilot_msg.h" #define fill_msg fill_msg_random -static void fill_msg_counter(struct AutopilotMessageCRCFrame * msg); -static void fill_msg_cst(struct AutopilotMessageCRCFrame * msg); -static void fill_msg_random(struct AutopilotMessageCRCFrame * msg); -static void print_up_msg(struct AutopilotMessageCRCFrame * msg); -static void print_down_msg(struct AutopilotMessageCRCFrame * msg); +static void fill_msg_counter(struct AutopilotMessageCRCFrame *msg); +static void fill_msg_cst(struct AutopilotMessageCRCFrame *msg); +static void fill_msg_random(struct AutopilotMessageCRCFrame *msg); +static void print_up_msg(struct AutopilotMessageCRCFrame *msg); +static void print_down_msg(struct AutopilotMessageCRCFrame *msg); -int main(int argc, char *argv[]) { +int main(int argc, char *argv[]) +{ uint32_t us_delay; - if(argc > 1) { + if (argc > 1) { us_delay = atoi(argv[1]); - } - else { + } else { us_delay = 1953; } @@ -87,7 +87,7 @@ int main(int argc, char *argv[]) { 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_calc_block_crc8((uint8_t *)&crc_msg_in.payload, sizeof(struct OVERO_LINK_MSG_DOWN))); } /* report message count */ if (!(spi_link.msg_cnt % 1000)) @@ -95,15 +95,17 @@ int main(int argc, char *argv[]) { buf_check_errors, spi_link.crc_err_cnt); /* give it some rest */ - if(us_delay > 0) + if (us_delay > 0) { usleep(us_delay); + } } return 0; } -static void print_up_msg(struct AutopilotMessageCRCFrame * msg) { +static void print_up_msg(struct AutopilotMessageCRCFrame *msg) +{ printf("UP: %08X %08X %08X %08X %08X %08X %08X %08X CRC: %08X\n", msg->payload.msg_up.foo, msg->payload.msg_up.bar, @@ -115,7 +117,8 @@ static void print_up_msg(struct AutopilotMessageCRCFrame * msg) { msg->payload.msg_up.bly, msg->crc); } -static void print_down_msg(struct AutopilotMessageCRCFrame * msg) { +static void print_down_msg(struct AutopilotMessageCRCFrame *msg) +{ printf("DW: %08X %08X %08X %08X %08X %08X %08X %08X CRC: %08X\n", msg->payload.msg_down.foo, msg->payload.msg_down.bar, @@ -129,7 +132,8 @@ static void print_down_msg(struct AutopilotMessageCRCFrame * msg) { } -static void fill_msg_counter(struct AutopilotMessageCRCFrame * msg) { +static void fill_msg_counter(struct AutopilotMessageCRCFrame *msg) +{ static uint32_t foo = 5000; msg->payload.msg_up.foo = 0x55; msg->payload.msg_up.bar = 1; @@ -141,12 +145,13 @@ static void fill_msg_counter(struct AutopilotMessageCRCFrame * msg) { msg->payload.msg_up.bly = 0; foo--; - if(foo == 0) { + if (foo == 0) { foo = 5000; } } -static void fill_msg_cst(struct AutopilotMessageCRCFrame * msg) { +static void fill_msg_cst(struct AutopilotMessageCRCFrame *msg) +{ msg->payload.msg_up.foo = 0; msg->payload.msg_up.bar = 0; msg->payload.msg_up.bla = 0; @@ -157,7 +162,8 @@ static void fill_msg_cst(struct AutopilotMessageCRCFrame * msg) { msg->payload.msg_up.bly = 0x01; } -static void fill_msg_random(struct AutopilotMessageCRCFrame * msg) { +static void fill_msg_random(struct AutopilotMessageCRCFrame *msg) +{ msg->payload.msg_up.foo = random(); msg->payload.msg_up.bar = random(); msg->payload.msg_up.bla = random(); diff --git a/sw/airborne/fms/overo_test_telemetry.c b/sw/airborne/fms/overo_test_telemetry.c index 482a21bce2..5642e5b890 100644 --- a/sw/airborne/fms/overo_test_telemetry.c +++ b/sw/airborne/fms/overo_test_telemetry.c @@ -19,20 +19,21 @@ #define TIMEOUT_DT_USEC 50000 -#define ADD_TIMEOUT() { \ - struct timeval tv; \ - evutil_timerclear(&tv); \ - tv.tv_sec = TIMEOUT_DT_SEC; \ - tv.tv_usec = TIMEOUT_DT_USEC; \ - event_add(&timeout, &tv); \ +#define ADD_TIMEOUT() { \ + struct timeval tv; \ + evutil_timerclear(&tv); \ + tv.tv_sec = TIMEOUT_DT_SEC; \ + tv.tv_usec = TIMEOUT_DT_USEC; \ + event_add(&timeout, &tv); \ } static void timeout_cb(int fd, short event, void *arg); static struct event timeout; -static struct FmsNetwork* network; +static struct FmsNetwork *network; -void timeout_cb(int fd, short event, void *arg) { +void timeout_cb(int fd, short event, void *arg) +{ // printf("in timeout_cb\n"); @@ -49,7 +50,8 @@ void timeout_cb(int fd, short event, void *arg) { } -int main(int argc, char** argv) { +int main(int argc, char **argv) +{ network = network_new(GCS_HOST, GCS_PORT, DATALINK_PORT, FALSE); diff --git a/sw/airborne/fms/overo_test_telemetry2.c b/sw/airborne/fms/overo_test_telemetry2.c index a4a8cfb7a3..eab1e05dfe 100644 --- a/sw/airborne/fms/overo_test_telemetry2.c +++ b/sw/airborne/fms/overo_test_telemetry2.c @@ -25,22 +25,23 @@ #define DL_MSG_SIZE 128 -#define ADD_TIMEOUT() { \ - struct timeval tv; \ - evutil_timerclear(&tv); \ - tv.tv_sec = TIMEOUT_DT_SEC; \ - tv.tv_usec = TIMEOUT_DT_USEC; \ - event_add(&timeout, &tv); \ +#define ADD_TIMEOUT() { \ + struct timeval tv; \ + evutil_timerclear(&tv); \ + tv.tv_sec = TIMEOUT_DT_SEC; \ + tv.tv_usec = TIMEOUT_DT_USEC; \ + event_add(&timeout, &tv); \ } static void timeout_cb(int fd, short event, void *arg); static struct event timeout; static struct event read_event; -static struct FmsNetwork* network; +static struct FmsNetwork *network; static struct DownlinkTransport *udp_transport; -void timeout_cb(int fd, short event, void *arg) { +void timeout_cb(int fd, short event, void *arg) +{ // printf("in timeout_cb\n"); @@ -72,22 +73,21 @@ static inline int checked_read(int fd, char *buf, size_t len) } bool_t my_dl_msg_available; -uint8_t my_dl_buffer[DL_MSG_SIZE] __attribute__ ((aligned)); +uint8_t my_dl_buffer[DL_MSG_SIZE] __attribute__((aligned)); #define IdOfMsg(x) (x[1]) -static void dl_handle_msg(struct DownlinkTransport *tp) { +static void dl_handle_msg(struct DownlinkTransport *tp) +{ uint8_t msg_id = IdOfMsg(my_dl_buffer); switch (msg_id) { - case DL_PING: - { + case DL_PING: { DOWNLINK_SEND_PONG(tp); } break; - case DL_SETTING : - { + case DL_SETTING : { uint8_t i = DL_SETTING_index(my_dl_buffer); float var = DL_SETTING_value(my_dl_buffer); // DlSetting(i, var); @@ -107,7 +107,7 @@ static void on_datalink_event(int fd, short event __attribute__((unused)), void struct udp_transport *udp_impl = tp->impl; int i = 0; - while (iudp_dl_msg_received) { @@ -118,7 +118,8 @@ static void on_datalink_event(int fd, short event __attribute__((unused)), void } } -int main(int argc, char** argv) { +int main(int argc, char **argv) +{ network = network_new(GCS_HOST, GCS_PORT, DATALINK_PORT, FALSE); udp_transport = udp_transport_new(network); diff --git a/sw/airborne/fms/packet_header.h b/sw/airborne/fms/packet_header.h index f1e162dfa8..8b4e7336f4 100644 --- a/sw/airborne/fms/packet_header.h +++ b/sw/airborne/fms/packet_header.h @@ -1,5 +1,5 @@ /* Ethernet addresses are 6 bytes */ -#define ETHER_ADDR_LEN 6 +#define ETHER_ADDR_LEN 6 #define ETHERNET_HEADER_LENGTH 14 @@ -8,7 +8,7 @@ struct ethernet_header { u_char ether_dhost[ETHER_ADDR_LEN]; /* Destination host address */ u_char ether_shost[ETHER_ADDR_LEN]; /* Source host address */ u_short ether_type; /* IP? ARP? RARP? etc */ -}__attribute__((packed));; +} __attribute__((packed));; /* IP header */ struct ip_header { @@ -17,15 +17,15 @@ struct ip_header { u_short ip_len; /* total length */ u_short ip_id; /* identification */ u_short ip_off; /* fragment offset field */ -#define IP_RF 0x8000 /* reserved fragment flag */ -#define IP_DF 0x4000 /* dont fragment flag */ -#define IP_MF 0x2000 /* more fragments flag */ +#define IP_RF 0x8000 /* reserved fragment flag */ +#define IP_DF 0x4000 /* dont fragment flag */ +#define IP_MF 0x2000 /* more fragments flag */ #define IP_OFFMASK 0x1fff /* mask for fragmenting bits */ u_char ip_ttl; /* time to live */ u_char ip_p; /* protocol */ u_short ip_sum; /* checksum */ - struct in_addr ip_src,ip_dst; /* source and dest address */ -}__attribute__((packed)); + struct in_addr ip_src, ip_dst; /* source and dest address */ +} __attribute__((packed)); #define IP_HL_WORDS(ip) (((ip)->ip_vhl) & 0x0f) #define IP_V(ip) (((ip)->ip_vhl) >> 4) diff --git a/sw/airborne/fms/test_telemetry.c b/sw/airborne/fms/test_telemetry.c index dca294c49f..92a025b770 100644 --- a/sw/airborne/fms/test_telemetry.c +++ b/sw/airborne/fms/test_telemetry.c @@ -2,10 +2,11 @@ #include #include -int main(int argc, char** argv) { +int main(int argc, char **argv) +{ - GMainLoop* ml = g_main_loop_new(NULL, FALSE); + GMainLoop *ml = g_main_loop_new(NULL, FALSE); g_main_loop_run(ml); return 0; diff --git a/sw/airborne/fms/test_telemetry_2.c b/sw/airborne/fms/test_telemetry_2.c index 2bed7c5ec3..e890282756 100644 --- a/sw/airborne/fms/test_telemetry_2.c +++ b/sw/airborne/fms/test_telemetry_2.c @@ -20,9 +20,10 @@ static void timeout_cb(int fd, short event, void *arg); static struct event timeout; -static struct FmsNetwork* network; +static struct FmsNetwork *network; -void timeout_cb(int fd, short event, void *arg) { +void timeout_cb(int fd, short event, void *arg) +{ printf("in timeout_cb\n"); @@ -37,7 +38,8 @@ void timeout_cb(int fd, short event, void *arg) { } -int main(int argc, char** argv) { +int main(int argc, char **argv) +{ printf("hello world\n"); diff --git a/sw/airborne/fms/udp_transport.h b/sw/airborne/fms/udp_transport.h index 1d73e771e8..123e9e546b 100644 --- a/sw/airborne/fms/udp_transport.h +++ b/sw/airborne/fms/udp_transport.h @@ -31,72 +31,72 @@ extern uint8_t udpt_ck_a, udpt_ck_b; #define UdpTransportPutSTX() UdpTransportPut1Byte(STX_UDP_TX) -#define UdpTransportPeriodic() { \ - if (udpt_tx_buf_idx) { \ - int len; \ - len = network_write(network, updt_tx_buf, udpt_tx_buf_idx); \ - downlink.nb_bytes += udpt_tx_buf_idx; \ - downlink.nb_msgs++; \ - if (len != udpt_tx_buf_idx) \ - downlink.nb_ovrn++; \ - udpt_tx_buf_idx = 0; \ - } \ +#define UdpTransportPeriodic() { \ + if (udpt_tx_buf_idx) { \ + int len; \ + len = network_write(network, updt_tx_buf, udpt_tx_buf_idx); \ + downlink.nb_bytes += udpt_tx_buf_idx; \ + downlink.nb_msgs++; \ + if (len != udpt_tx_buf_idx) \ + downlink.nb_ovrn++; \ + udpt_tx_buf_idx = 0; \ + } \ } #define UdpTransportCheckFreeSpace(_x) (TRUE) -#define UdpTransportHeader(payload_len) { \ - uint32_t msg_timestamp = MSG_TIMESTAMP; \ - /*udpt_tx_buf_idx = 0;*/ \ - UdpTransportPutSTX(); \ - uint8_t msg_len = UdpTransportSizeOf(payload_len); \ - udpt_ck_a = udpt_ck_b = 0; \ - UdpTransportPutUint8(msg_len); \ - UdpTransportPutTimestamp(&msg_timestamp); \ -} - - -#define UdpTransportTrailer() { \ - UdpTransportPut1Byte(udpt_ck_a); \ - UdpTransportPut1Byte(udpt_ck_b); \ - if (udpt_tx_buf_idx > UDPT_TX_BUF_WATERMARK) { \ - network_write(network, updt_tx_buf, udpt_tx_buf_idx); \ - udpt_tx_buf_idx = 0; \ - } \ +#define UdpTransportHeader(payload_len) { \ + uint32_t msg_timestamp = MSG_TIMESTAMP; \ + /*udpt_tx_buf_idx = 0;*/ \ + UdpTransportPutSTX(); \ + uint8_t msg_len = UdpTransportSizeOf(payload_len); \ + udpt_ck_a = udpt_ck_b = 0; \ + UdpTransportPutUint8(msg_len); \ + UdpTransportPutTimestamp(&msg_timestamp); \ } -#define UdpTransportPut1Byte(_x) { \ - updt_tx_buf[udpt_tx_buf_idx] = (_x); \ - udpt_tx_buf_idx++; \ + +#define UdpTransportTrailer() { \ + UdpTransportPut1Byte(udpt_ck_a); \ + UdpTransportPut1Byte(udpt_ck_b); \ + if (udpt_tx_buf_idx > UDPT_TX_BUF_WATERMARK) { \ + network_write(network, updt_tx_buf, udpt_tx_buf_idx); \ + udpt_tx_buf_idx = 0; \ + } \ } -#define UdpTransportPutUint8(_byte) { \ - udpt_ck_a += _byte; \ - udpt_ck_b += udpt_ck_a; \ - UdpTransportPut1Byte(_byte); \ +#define UdpTransportPut1Byte(_x) { \ + updt_tx_buf[udpt_tx_buf_idx] = (_x); \ + udpt_tx_buf_idx++; \ + } + +#define UdpTransportPutUint8(_byte) { \ + udpt_ck_a += _byte; \ + udpt_ck_b += udpt_ck_a; \ + UdpTransportPut1Byte(_byte); \ } #define UdpTransportPutNamedUint8(_name, _byte) UdpTransportPutUint8(_byte) -#define UdpTransportPut1ByteByAddr(_byte) { \ - uint8_t _x = *(_byte); \ - UdpTransportPutUint8(_x); \ +#define UdpTransportPut1ByteByAddr(_byte) { \ + uint8_t _x = *(_byte); \ + UdpTransportPutUint8(_x); \ } -#define UdpTransportPut2ByteByAddr(_byte) { \ - UdpTransportPut1ByteByAddr(_byte); \ - UdpTransportPut1ByteByAddr((const uint8_t*)_byte+1); \ +#define UdpTransportPut2ByteByAddr(_byte) { \ + UdpTransportPut1ByteByAddr(_byte); \ + UdpTransportPut1ByteByAddr((const uint8_t*)_byte+1); \ } -#define UdpTransportPut4ByteByAddr(_byte) { \ - UdpTransportPut2ByteByAddr(_byte); \ - UdpTransportPut2ByteByAddr((const uint8_t*)_byte+2); \ +#define UdpTransportPut4ByteByAddr(_byte) { \ + UdpTransportPut2ByteByAddr(_byte); \ + UdpTransportPut2ByteByAddr((const uint8_t*)_byte+2); \ } -#define UdpTransportPut8ByteByAddr(_byte) { \ - UdpTransportPut4ByteByAddr(_byte); \ - UdpTransportPut4ByteByAddr((const uint8_t*)_byte+4); \ +#define UdpTransportPut8ByteByAddr(_byte) { \ + UdpTransportPut4ByteByAddr(_byte); \ + UdpTransportPut4ByteByAddr((const uint8_t*)_byte+4); \ } @@ -111,12 +111,12 @@ extern uint8_t udpt_ck_a, udpt_ck_b; #define UdpTransportPutDoubleByAddr(_x) UdpTransportPut8ByteByAddr((const uint8_t*)_x) #define UdpTransportPutArray(_put, _n, _x) { \ - uint8_t _i; \ - UdpTransportPutUint8(_n); \ - for(_i = 0; _i < _n; _i++) { \ - _put(&_x[_i]); \ - } \ -} + uint8_t _i; \ + UdpTransportPutUint8(_n); \ + for(_i = 0; _i < _n; _i++) { \ + _put(&_x[_i]); \ + } \ + } #define UdpTransportPutInt16Array(_n, _x) UdpTransportPutArray(UdpTransportPutInt16ByAddr, _n, _x) @@ -140,47 +140,52 @@ extern volatile uint8_t udp_dl_payload_len; extern volatile bool_t udp_dl_msg_received; extern uint8_t udp_dl_ovrn, udp_dl_nb_err; -static inline void parse_udp_dl( uint8_t c ) { +static inline void parse_udp_dl(uint8_t c) +{ static uint8_t udp_dl_status = UNINIT; static uint8_t _ck_a, _ck_b, payload_idx; switch (udp_dl_status) { - case UNINIT: - if (c == STX_UDP_RX) + case UNINIT: + if (c == STX_UDP_RX) { + udp_dl_status++; + } + break; + case GOT_STX: + if (udp_dl_msg_received) { + udp_dl_ovrn++; + goto error; + } + udp_dl_payload_len = c - 4; /* Counting STX, LENGTH and CRC1 and CRC2 */ + _ck_a = _ck_b = c; udp_dl_status++; - break; - case GOT_STX: - if (udp_dl_msg_received) { - udp_dl_ovrn++; - goto error; - } - udp_dl_payload_len = c-4; /* Counting STX, LENGTH and CRC1 and CRC2 */ - _ck_a = _ck_b = c; - udp_dl_status++; - payload_idx = 0; - break; - case GOT_LENGTH: - udp_dl_payload[payload_idx] = c; - _ck_a += c; _ck_b += _ck_a; - payload_idx++; - if (payload_idx == udp_dl_payload_len) + payload_idx = 0; + break; + case GOT_LENGTH: + udp_dl_payload[payload_idx] = c; + _ck_a += c; _ck_b += _ck_a; + payload_idx++; + if (payload_idx == udp_dl_payload_len) { + udp_dl_status++; + } + break; + case GOT_PAYLOAD: + if (c != _ck_a) { + goto error; + } udp_dl_status++; - break; - case GOT_PAYLOAD: - if (c != _ck_a) - goto error; - udp_dl_status++; - break; - case GOT_CRC1: - if (c != _ck_b) - goto error; - udp_dl_msg_received = TRUE; - goto restart; + break; + case GOT_CRC1: + if (c != _ck_b) { + goto error; + } + udp_dl_msg_received = TRUE; + goto restart; } return; - error: +error: udp_dl_nb_err++; - restart: +restart: udp_dl_status = UNINIT; return; } diff --git a/sw/airborne/fms/udp_transport2.h b/sw/airborne/fms/udp_transport2.h index d3501e2aa4..2014a586db 100644 --- a/sw/airborne/fms/udp_transport2.h +++ b/sw/airborne/fms/udp_transport2.h @@ -51,45 +51,50 @@ struct udp_transport { #define GOT_PAYLOAD 3 #define GOT_CRC1 4 -static inline void parse_udp_dl( struct udp_transport *tp, uint8_t c ) { +static inline void parse_udp_dl(struct udp_transport *tp, uint8_t c) +{ switch (tp->udp_dl_status) { - case UNINIT: - if (c == STX_UDP_RX) + case UNINIT: + if (c == STX_UDP_RX) { + tp->udp_dl_status++; + } + break; + case GOT_STX: + if (tp->udp_dl_msg_received) { + tp->udp_dl_ovrn++; + goto error; + } + tp->udp_dl_payload_len = c - 4; /* Counting STX, LENGTH and CRC1 and CRC2 */ + tp->_ck_a = tp->_ck_b = c; tp->udp_dl_status++; - break; - case GOT_STX: - if (tp->udp_dl_msg_received) { - tp->udp_dl_ovrn++; - goto error; - } - tp->udp_dl_payload_len = c-4; /* Counting STX, LENGTH and CRC1 and CRC2 */ - tp->_ck_a = tp->_ck_b = c; - tp->udp_dl_status++; - tp->payload_idx = 0; - break; - case GOT_LENGTH: - tp->udp_dl_payload[tp->payload_idx] = c; - tp->_ck_a += c; tp->_ck_b += tp->_ck_a; - tp->payload_idx++; - if (tp->payload_idx == tp->udp_dl_payload_len) + tp->payload_idx = 0; + break; + case GOT_LENGTH: + tp->udp_dl_payload[tp->payload_idx] = c; + tp->_ck_a += c; tp->_ck_b += tp->_ck_a; + tp->payload_idx++; + if (tp->payload_idx == tp->udp_dl_payload_len) { + tp->udp_dl_status++; + } + break; + case GOT_PAYLOAD: + if (c != tp->_ck_a) { + goto error; + } tp->udp_dl_status++; - break; - case GOT_PAYLOAD: - if (c != tp->_ck_a) - goto error; - tp->udp_dl_status++; - break; - case GOT_CRC1: - if (c != tp->_ck_b) - goto error; - tp->udp_dl_msg_received = TRUE; - goto restart; + break; + case GOT_CRC1: + if (c != tp->_ck_b) { + goto error; + } + tp->udp_dl_msg_received = TRUE; + goto restart; } return; - error: +error: tp->udp_dl_nb_err++; - restart: +restart: tp->udp_dl_status = UNINIT; return; } diff --git a/sw/airborne/inter_mcu.c b/sw/airborne/inter_mcu.c index 0c62ae1e5d..09b463da87 100644 --- a/sw/airborne/inter_mcu.c +++ b/sw/airborne/inter_mcu.c @@ -29,12 +29,12 @@ #if defined SINGLE_MCU static struct fbw_state _fbw_state; static struct ap_state _ap_state; -struct fbw_state* fbw_state = &_fbw_state; -struct ap_state* ap_state = &_ap_state; +struct fbw_state *fbw_state = &_fbw_state; +struct ap_state *ap_state = &_ap_state; #else /* SINGLE_MCU */ #include "link_mcu.h" -struct fbw_state* fbw_state = &link_mcu_from_fbw_msg.payload.from_fbw; -struct ap_state* ap_state = &link_mcu_from_ap_msg.payload.from_ap; +struct fbw_state *fbw_state = &link_mcu_from_fbw_msg.payload.from_fbw; +struct ap_state *ap_state = &link_mcu_from_ap_msg.payload.from_ap; #endif /* ! SINGLE_MCU */ volatile bool_t inter_mcu_received_fbw = FALSE; diff --git a/sw/airborne/inter_mcu.h b/sw/airborne/inter_mcu.h index 660ca33f54..25a7e32ebf 100644 --- a/sw/airborne/inter_mcu.h +++ b/sw/airborne/inter_mcu.h @@ -75,8 +75,8 @@ struct ap_state { #define MASK_FBW_CHANGED 0xf -extern struct fbw_state* fbw_state; -extern struct ap_state* ap_state; +extern struct fbw_state *fbw_state; +extern struct ap_state *ap_state; extern volatile bool_t inter_mcu_received_fbw; extern volatile bool_t inter_mcu_received_ap; @@ -90,7 +90,8 @@ extern bool_t ap_ok; #define AP_STALLED_TIME 30 // 500ms with a 60Hz timer -static inline void inter_mcu_init(void) { +static inline void inter_mcu_init(void) +{ fbw_state->status = 0; fbw_state->nb_err = 0; @@ -99,19 +100,22 @@ static inline void inter_mcu_init(void) { /* Prepare data to be sent to mcu0 */ -static inline void inter_mcu_fill_fbw_state (void) { +static inline void inter_mcu_fill_fbw_state(void) +{ uint8_t status = 0; #ifdef RADIO_CONTROL uint8_t i; - for(i = 0; i < RADIO_CONTROL_NB_CHANNEL; i++) + for (i = 0; i < RADIO_CONTROL_NB_CHANNEL; i++) { fbw_state->channels[i] = radio_control.values[i]; + } fbw_state->ppm_cpt = radio_control.frame_rate; status = (radio_control.status == RC_OK ? _BV(STATUS_RADIO_OK) : 0); status |= (radio_control.status == RC_REALLY_LOST ? _BV(STATUS_RADIO_REALLY_LOST) : 0); - status |= (radio_control.status == RC_OK ? _BV(AVERAGED_CHANNELS_SENT) : 0); // Any valid frame contains averaged channels + status |= (radio_control.status == RC_OK ? _BV(AVERAGED_CHANNELS_SENT) : + 0); // Any valid frame contains averaged channels #endif // RADIO_CONTROL status |= (fbw_mode == FBW_MODE_AUTO ? _BV(STATUS_MODE_AUTO) : 0); @@ -128,13 +132,15 @@ static inline void inter_mcu_fill_fbw_state (void) { } /** Prepares date for next comm with AP. Set ::ap_ok to TRUE */ -static inline void inter_mcu_event_task( void) { +static inline void inter_mcu_event_task(void) +{ time_since_last_ap = 0; ap_ok = TRUE; } /** Monitors AP. Set ::ap_ok to false if AP is down for a long time. */ -static inline void inter_mcu_periodic_task(void) { +static inline void inter_mcu_periodic_task(void) +{ if (time_since_last_ap >= AP_STALLED_TIME) { ap_ok = FALSE; #ifdef SINGLE_MCU @@ -142,8 +148,9 @@ static inline void inter_mcu_periodic_task(void) { inter_mcu_fill_fbw_state(); #endif - } else + } else { time_since_last_ap++; + } } #endif /* FBW */ diff --git a/sw/airborne/led.h b/sw/airborne/led.h index 640117a7ac..b5ed7e0fd0 100644 --- a/sw/airborne/led.h +++ b/sw/airborne/led.h @@ -33,7 +33,8 @@ #include "led_hw.h" -static inline void led_init ( void ) { +static inline void led_init(void) +{ #if USE_LED_1 LED_INIT(1); LED_OFF(1); @@ -99,7 +100,7 @@ static inline void led_init ( void ) { #define LED_AVAILABLE(i) _LED_AVAILABLE(i) #else /* USE_LED */ -static inline void led_init ( void ) {} +static inline void led_init(void) {} #define LED_ON(i) {} #define LED_OFF(i) {} #define LED_TOGGLE(i) {} diff --git a/sw/airborne/link_mcu_can.c b/sw/airborne/link_mcu_can.c index a457f6c704..053bc1b443 100644 --- a/sw/airborne/link_mcu_can.c +++ b/sw/airborne/link_mcu_can.c @@ -53,8 +53,8 @@ union { // READ MESSAGES -void link_mcu_on_can_msg(uint32_t id, uint8_t* data, int len); -void link_mcu_on_can_msg(uint32_t id, uint8_t* data, int len) +void link_mcu_on_can_msg(uint32_t id, uint8_t *data, int len); +void link_mcu_on_can_msg(uint32_t id, uint8_t *data, int len) { #if COMMANDS_NB > 8 #error "INTERMCU_CAN CAN ONLY SEND 4 OR 8 COMMANDS (packets of 8 bytes)" diff --git a/sw/airborne/link_mcu_spi.c b/sw/airborne/link_mcu_spi.c index 87efc22a5e..3c89ab1bc7 100644 --- a/sw/airborne/link_mcu_spi.c +++ b/sw/airborne/link_mcu_spi.c @@ -39,29 +39,31 @@ static uint16_t crc = 0; #define LINK_MCU_FRAME_LENGTH sizeof(struct link_mcu_msg) #define ComputeChecksum(_buf) { \ - uint8_t i; \ - crc = CRC_INIT; \ - for(i = 0; i < PAYLOAD_LENGTH; i++) { \ - uint8_t _byte = ((uint8_t*)&_buf)[i]; \ - crc = CrcUpdate(crc, _byte); \ - } \ -} + uint8_t i; \ + crc = CRC_INIT; \ + for(i = 0; i < PAYLOAD_LENGTH; i++) { \ + uint8_t _byte = ((uint8_t*)&_buf)[i]; \ + crc = CrcUpdate(crc, _byte); \ + } \ + } #ifdef FBW -void link_mcu_init(void) { +void link_mcu_init(void) +{ link_mcu_trans.cpol = SPICpolIdleLow; link_mcu_trans.cpha = SPICphaEdge2; link_mcu_trans.dss = SPIDss8bit; - link_mcu_trans.input_buf = (uint8_t*)&link_mcu_from_ap_msg; - link_mcu_trans.output_buf = (uint8_t*)&link_mcu_from_fbw_msg; + link_mcu_trans.input_buf = (uint8_t *)&link_mcu_from_ap_msg; + link_mcu_trans.output_buf = (uint8_t *)&link_mcu_from_fbw_msg; link_mcu_trans.input_length = LINK_MCU_FRAME_LENGTH; link_mcu_trans.output_length = LINK_MCU_FRAME_LENGTH; spi_slave_register(&(LINK_MCU_SPI_DEV), &link_mcu_trans); } -void link_mcu_restart(void) { +void link_mcu_restart(void) +{ ComputeChecksum(link_mcu_from_fbw_msg); link_mcu_from_fbw_msg.checksum = crc; @@ -69,7 +71,8 @@ void link_mcu_restart(void) { spi_slave_wait(&(LINK_MCU_SPI_DEV)); } -void link_mcu_event_task( void ) { +void link_mcu_event_task(void) +{ if (link_mcu_trans.status == SPITransSuccess) { /* Got a message on SPI. */ link_mcu_trans.status = SPITransDone; @@ -77,10 +80,11 @@ void link_mcu_event_task( void ) { /* A message has been received */ ComputeChecksum(link_mcu_from_ap_msg); link_mcu_received = TRUE; - if (link_mcu_from_ap_msg.checksum == crc) + if (link_mcu_from_ap_msg.checksum == crc) { inter_mcu_received_ap = TRUE; - else + } else { fbw_state->nb_err++; + } } if (link_mcu_trans.status == SPITransFailed) { link_mcu_trans.status = SPITransDone; @@ -106,14 +110,16 @@ uint8_t link_mcu_fbw_nb_err; #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_debug_link(struct transport_tx *trans, struct link_device *dev) { +static void send_debug_link(struct transport_tx *trans, struct link_device *dev) +{ uint8_t mcu1_ppm_cpt_foo = 0; //FIXME pprz_msg_send_DEBUG_MCU_LINK(trans, dev, AC_ID, - &link_mcu_nb_err, &link_mcu_fbw_nb_err, &mcu1_ppm_cpt_foo); + &link_mcu_nb_err, &link_mcu_fbw_nb_err, &mcu1_ppm_cpt_foo); } #endif -void link_mcu_init(void) { +void link_mcu_init(void) +{ link_mcu_nb_err = 0; link_mcu_trans.cpol = SPICpolIdleLow; @@ -121,8 +127,8 @@ void link_mcu_init(void) { link_mcu_trans.dss = SPIDss8bit; link_mcu_trans.select = SPISelectUnselect; link_mcu_trans.slave_idx = LINK_MCU_SLAVE_IDX; - link_mcu_trans.input_buf = (uint8_t*)&link_mcu_from_fbw_msg; - link_mcu_trans.output_buf = (uint8_t*)&link_mcu_from_ap_msg; + link_mcu_trans.input_buf = (uint8_t *)&link_mcu_from_fbw_msg; + link_mcu_trans.output_buf = (uint8_t *)&link_mcu_from_ap_msg; link_mcu_trans.input_length = LINK_MCU_FRAME_LENGTH; link_mcu_trans.output_length = LINK_MCU_FRAME_LENGTH; @@ -131,23 +137,26 @@ void link_mcu_init(void) { #endif } -void link_mcu_send(void) { +void link_mcu_send(void) +{ ComputeChecksum(link_mcu_from_ap_msg); link_mcu_from_ap_msg.checksum = crc; spi_submit(&(LINK_MCU_SPI_DEV), &link_mcu_trans); } -void link_mcu_event_task( void ) { +void link_mcu_event_task(void) +{ if (link_mcu_trans.status == SPITransSuccess) { /* Got a message on SPI. */ link_mcu_trans.status = SPITransDone; /* A message has been received */ ComputeChecksum(link_mcu_from_fbw_msg); - if (link_mcu_from_fbw_msg.checksum == crc) + if (link_mcu_from_fbw_msg.checksum == crc) { inter_mcu_received_fbw = TRUE; - else + } else { link_mcu_nb_err++; + } } if (link_mcu_trans.status == SPITransFailed) { link_mcu_trans.status = SPITransDone; diff --git a/sw/airborne/link_mcu_spi.h b/sw/airborne/link_mcu_spi.h index 4b61e493a0..9967bc5f71 100644 --- a/sw/airborne/link_mcu_spi.h +++ b/sw/airborne/link_mcu_spi.h @@ -51,7 +51,7 @@ extern struct spi_transaction link_mcu_trans; extern bool_t link_mcu_received; extern void link_mcu_init(void); -extern void link_mcu_event_task( void ); +extern void link_mcu_event_task(void); #ifdef FBW extern void link_mcu_restart(void); diff --git a/sw/airborne/link_mcu_usart.c b/sw/airborne/link_mcu_usart.c index c28308abd1..d4a212be14 100644 --- a/sw/airborne/link_mcu_usart.c +++ b/sw/airborne/link_mcu_usart.c @@ -77,26 +77,26 @@ #define MSG_INTERMCU_COMMAND(_intermcu_payload, nr) (uint16_t)(*((uint8_t*)_intermcu_payload+0+(2*(nr)))|*((uint8_t*)_intermcu_payload+1+(2*(nr)))<<8) #define InterMcuSend_INTERMCU_COMMAND(cmd) { \ - InterMcuHeader(MSG_INTERMCU_ID, MSG_INTERMCU_COMMAND_ID, MSG_INTERMCU_COMMAND_LENGTH);\ - for (int i=0;i INTERMCU_MAX_PAYLOAD) { - goto error; - } - intermcu_data.msg_idx = 0; - intermcu_data.status++; - break; - case LINK_MCU_GOT_LEN2: - intermcu_data.msg_buf[intermcu_data.msg_idx] = c; - intermcu_data.msg_idx++; - if (intermcu_data.msg_idx >= intermcu_data.len) { + break; + case LINK_MCU_GOT_SYNC2: + if (intermcu_data.msg_available) { + /* Previous message has not yet been parsed: discard this one */ + goto error; + } + intermcu_data.msg_class = c; intermcu_data.status++; - } - break; - case LINK_MCU_GOT_PAYLOAD: - if (c != intermcu_data.ck_a) { + break; + case LINK_MCU_GOT_CLASS: + intermcu_data.msg_id = c; + intermcu_data.status++; + break; + case LINK_MCU_GOT_ID: + intermcu_data.len = c; + intermcu_data.status++; + break; + case LINK_MCU_GOT_LEN1: + intermcu_data.len |= (c << 8); + if (intermcu_data.len > INTERMCU_MAX_PAYLOAD) { + goto error; + } + intermcu_data.msg_idx = 0; + intermcu_data.status++; + break; + case LINK_MCU_GOT_LEN2: + intermcu_data.msg_buf[intermcu_data.msg_idx] = c; + intermcu_data.msg_idx++; + if (intermcu_data.msg_idx >= intermcu_data.len) { + intermcu_data.status++; + } + break; + case LINK_MCU_GOT_PAYLOAD: + if (c != intermcu_data.ck_a) { + goto error; + } + intermcu_data.status++; + break; + case LINK_MCU_GOT_CHECKSUM1: + if (c != intermcu_data.ck_b) { + goto error; + } + intermcu_data.msg_available = TRUE; + goto restart; + break; + default: goto error; - } - intermcu_data.status++; - break; - case LINK_MCU_GOT_CHECKSUM1: - if (c != intermcu_data.ck_b) { - goto error; - } - intermcu_data.msg_available = TRUE; - goto restart; - break; - default: - goto error; } return; - error: +error: intermcu_data.error_cnt++; - restart: +restart: intermcu_data.status = LINK_MCU_UNINIT; return; } @@ -243,7 +245,7 @@ void intermcu_parse( uint8_t c ) { struct link_mcu_msg link_mcu_from_ap_msg; struct link_mcu_msg link_mcu_from_fbw_msg; -inline void parse_mavpilot_msg( void ); +inline void parse_mavpilot_msg(void); #ifdef AP @@ -254,55 +256,57 @@ inline void parse_mavpilot_msg( void ); #define RC_REALLY_LOST 2 -static void send_commands(struct transport_tx *trans, struct link_device *dev) { +static void send_commands(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_COMMANDS(trans, dev, AC_ID, COMMANDS_NB, ap_state->commands); } -static void send_fbw_status(struct transport_tx *trans, struct link_device *dev) { +static void send_fbw_status(struct transport_tx *trans, struct link_device *dev) +{ uint8_t rc_status = 0; uint8_t fbw_status = 0; - if (bit_is_set(fbw_state->status, STATUS_MODE_AUTO)) + if (bit_is_set(fbw_state->status, STATUS_MODE_AUTO)) { fbw_status = FBW_MODE_AUTO; - if (bit_is_set(fbw_state->status, STATUS_MODE_FAILSAFE)) + } + if (bit_is_set(fbw_state->status, STATUS_MODE_FAILSAFE)) { fbw_status = FBW_MODE_FAILSAFE; - if (bit_is_set(fbw_state->status, STATUS_RADIO_REALLY_LOST)) + } + if (bit_is_set(fbw_state->status, STATUS_RADIO_REALLY_LOST)) { rc_status = RC_REALLY_LOST; - else if (bit_is_set(fbw_state->status, RC_OK)) + } else if (bit_is_set(fbw_state->status, RC_OK)) { rc_status = RC_OK; - else + } else { rc_status = RC_LOST; + } pprz_msg_send_FBW_STATUS(trans, dev, AC_ID, - &(rc_status), &(fbw_state->ppm_cpt), &(fbw_status), &(fbw_state->vsupply), &(fbw_state->current)); + &(rc_status), &(fbw_state->ppm_cpt), &(fbw_status), &(fbw_state->vsupply), &(fbw_state->current)); } #endif -void link_mcu_init( void ) +void link_mcu_init(void) { - intermcu_data.status = LINK_MCU_UNINIT; - intermcu_data.msg_available = FALSE; - intermcu_data.error_cnt = 0; + intermcu_data.status = LINK_MCU_UNINIT; + intermcu_data.msg_available = FALSE; + intermcu_data.error_cnt = 0; #ifdef AP - #if PERIODIC_TELEMETRY - // If FBW has not telemetry, then AP can send some of the info - register_periodic_telemetry(DefaultPeriodic, "COMMANDS", send_commands); - register_periodic_telemetry(DefaultPeriodic, "FBW_STATUS", send_fbw_status); +#if PERIODIC_TELEMETRY + // If FBW has not telemetry, then AP can send some of the info + register_periodic_telemetry(DefaultPeriodic, "COMMANDS", send_commands); + register_periodic_telemetry(DefaultPeriodic, "FBW_STATUS", send_fbw_status); #endif #endif } -void parse_mavpilot_msg( void ) +void parse_mavpilot_msg(void) { - if (intermcu_data.msg_class == MSG_INTERMCU_ID) - { - if (intermcu_data.msg_id == MSG_INTERMCU_COMMAND_ID) - { + if (intermcu_data.msg_class == MSG_INTERMCU_ID) { + if (intermcu_data.msg_id == MSG_INTERMCU_COMMAND_ID) { #if COMMANDS_NB > 8 #error "INTERMCU UART CAN ONLY SEND 8 COMMANDS OR THE UART WILL BE OVERFILLED" #endif - for (int i=0; i< COMMANDS_NB; i++) - { + for (int i = 0; i < COMMANDS_NB; i++) { ap_state->commands[i] = ((pprz_t)MSG_INTERMCU_COMMAND(intermcu_data.msg_buf, i)); } @@ -310,25 +314,18 @@ void parse_mavpilot_msg( void ) LED_TOGGLE(LINK_MCU_LED); #endif inter_mcu_received_ap = TRUE; - } - else if (intermcu_data.msg_id == MSG_INTERMCU_RADIO_ID) - { + } else if (intermcu_data.msg_id == MSG_INTERMCU_RADIO_ID) { #if RADIO_CONTROL_NB_CHANNEL > 10 #error "INTERMCU UART CAN ONLY SEND 10 RADIO CHANNELS OR THE UART WILL BE OVERFILLED" #endif - for (int i=0; i< RADIO_CONTROL_NB_CHANNEL; i++) - { + for (int i = 0; i < RADIO_CONTROL_NB_CHANNEL; i++) { fbw_state->channels[i] = ((pprz_t)MSG_INTERMCU_RADIO(intermcu_data.msg_buf, i)); } - } - else if (intermcu_data.msg_id == MSG_INTERMCU_TRIM_ID) - { + } else if (intermcu_data.msg_id == MSG_INTERMCU_TRIM_ID) { ap_state->command_roll_trim = ((pprz_t) MSG_INTERMCU_TRIM_ROLL(intermcu_data.msg_buf)); ap_state->command_pitch_trim = ((pprz_t) MSG_INTERMCU_TRIM_PITCH(intermcu_data.msg_buf)); - } - else if (intermcu_data.msg_id == MSG_INTERMCU_FBW_ID) - { + } else if (intermcu_data.msg_id == MSG_INTERMCU_FBW_ID) { fbw_state->ppm_cpt = MSG_INTERMCU_FBW_MOD(intermcu_data.msg_buf); fbw_state->status = MSG_INTERMCU_FBW_STAT(intermcu_data.msg_buf); fbw_state->nb_err = MSG_INTERMCU_FBW_ERR(intermcu_data.msg_buf); @@ -345,10 +342,10 @@ void parse_mavpilot_msg( void ) #ifdef AP -void link_mcu_send( void ) +void link_mcu_send(void) { - InterMcuSend_INTERMCU_COMMAND( ap_state->commands ); - InterMcuSend_INTERMCU_TRIM( ap_state->command_roll_trim, ap_state->command_pitch_trim ); + InterMcuSend_INTERMCU_COMMAND(ap_state->commands); + InterMcuSend_INTERMCU_TRIM(ap_state->command_roll_trim, ap_state->command_pitch_trim); } #endif @@ -356,34 +353,33 @@ void link_mcu_send( void ) // 60 Hz static uint8_t SixtyHzCounter = 0; -void link_mcu_periodic_task( void ) +void link_mcu_periodic_task(void) { SixtyHzCounter++; - if (SixtyHzCounter >= 3) - { + if (SixtyHzCounter >= 3) { // 20 Hz SixtyHzCounter = 0; 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 ); + InterMcuSend_INTERMCU_RADIO(fbw_state->channels); #endif } } #endif -void link_mcu_event_task( void ) { +void link_mcu_event_task(void) +{ /* A message has been received */ if (InterMcuBuffer()) { - while (InterMcuLink(ChAvailable())) - { + while (InterMcuLink(ChAvailable())) { intermcu_parse(InterMcuLink(Getch())); if (intermcu_data.msg_available) { parse_mavpilot_msg(); diff --git a/sw/airborne/link_mcu_usart.h b/sw/airborne/link_mcu_usart.h index 0140c4146a..375717d2f1 100644 --- a/sw/airborne/link_mcu_usart.h +++ b/sw/airborne/link_mcu_usart.h @@ -42,10 +42,10 @@ extern struct link_mcu_msg link_mcu_from_fbw_msg; extern bool_t link_mcu_received; -extern void link_mcu_send( void ); -extern void link_mcu_init( void ); -extern void link_mcu_event_task( void ); -extern void link_mcu_periodic_task( void ); +extern void link_mcu_send(void); +extern void link_mcu_init(void); +extern void link_mcu_event_task(void); +extern void link_mcu_periodic_task(void); #endif /* LINK_MCU_USART_H */ diff --git a/sw/airborne/math/pprz_algebra_double.c b/sw/airborne/math/pprz_algebra_double.c index 5532931a51..9e83a314f5 100644 --- a/sw/airborne/math/pprz_algebra_double.c +++ b/sw/airborne/math/pprz_algebra_double.c @@ -26,7 +26,7 @@ #include "pprz_algebra_double.h" -void double_rmat_of_eulers_321(struct DoubleRMat* rm, struct DoubleEulers* e) +void double_rmat_of_eulers_321(struct DoubleRMat *rm, struct DoubleEulers *e) { const double sphi = sin(e->phi); const double cphi = cos(e->phi); @@ -46,7 +46,7 @@ void double_rmat_of_eulers_321(struct DoubleRMat* rm, struct DoubleEulers* e) RMAT_ELMT(*rm, 2, 2) = cphi * ctheta; } -void double_quat_of_eulers(struct DoubleQuat* q, struct DoubleEulers* e) +void double_quat_of_eulers(struct DoubleQuat *q, struct DoubleEulers *e) { const double phi2 = e->phi / 2.0; const double theta2 = e->theta / 2.0; @@ -65,7 +65,7 @@ void double_quat_of_eulers(struct DoubleQuat* q, struct DoubleEulers* e) q->qz = c_phi2 * c_theta2 * s_psi2 - s_phi2 * s_theta2 * c_psi2; } -void double_eulers_of_quat(struct DoubleEulers* e, struct DoubleQuat* q) +void double_eulers_of_quat(struct DoubleEulers *e, struct DoubleQuat *q) { const double qx2 = q->qx * q->qx; const double qy2 = q->qy * q->qy; @@ -87,7 +87,7 @@ void double_eulers_of_quat(struct DoubleEulers* e, struct DoubleQuat* q) e->psi = atan2(dcm01, dcm00); } -void double_quat_vmult(struct DoubleVect3* v_out, struct DoubleQuat* q, struct DoubleVect3* v_in) +void double_quat_vmult(struct DoubleVect3 *v_out, struct DoubleQuat *q, struct DoubleVect3 *v_in) { const double qi2_M1_2 = q->qi * q->qi - 0.5; const double qiqx = q->qi * q->qx; diff --git a/sw/airborne/math/pprz_algebra_double.h b/sw/airborne/math/pprz_algebra_double.h index 5b7c8752ec..bb8f1c9b55 100644 --- a/sw/airborne/math/pprz_algebra_double.h +++ b/sw/airborne/math/pprz_algebra_double.h @@ -93,13 +93,13 @@ struct DoubleRates { (_vout).z = rint((_vin).z); \ } -static inline double double_vect3_norm(struct DoubleVect3* v) +static inline double double_vect3_norm(struct DoubleVect3 *v) { return sqrt(VECT3_NORM2(*v)); } /** normalize 3D vector in place */ -static inline void double_vect3_normalize(struct DoubleVect3* v) +static inline void double_vect3_normalize(struct DoubleVect3 *v) { const double n = double_vect3_norm(v); if (n > 0) { @@ -111,7 +111,7 @@ static inline void double_vect3_normalize(struct DoubleVect3* v) /** initialises a quaternion to identity */ -static inline void double_quat_identity(struct DoubleQuat* q) +static inline void double_quat_identity(struct DoubleQuat *q) { q->qi = 1.0; q->qx = 0; @@ -119,13 +119,13 @@ static inline void double_quat_identity(struct DoubleQuat* q) q->qz = 0; } -static inline double double_quat_norm(struct DoubleQuat* q) +static inline double double_quat_norm(struct DoubleQuat *q) { return sqrt(SQUARE(q->qi) + SQUARE(q->qx) + SQUARE(q->qy) + SQUARE(q->qz)); } -static inline void double_quat_normalize(struct DoubleQuat* q) +static inline void double_quat_normalize(struct DoubleQuat *q) { double qnorm = double_quat_norm(q); if (qnorm > FLT_MIN) { @@ -148,12 +148,12 @@ static inline void double_quat_normalize(struct DoubleQuat* q) * @param[out] rm pointer to rotation matrix * @param[in] e pointer to Euler angles */ -extern void double_rmat_of_eulers_321(struct DoubleRMat* rm, struct DoubleEulers* e); -extern void double_quat_of_eulers(struct DoubleQuat* q, struct DoubleEulers* e); -extern void double_eulers_of_quat(struct DoubleEulers* e, struct DoubleQuat* q); -extern void double_quat_vmult(struct DoubleVect3* v_out, struct DoubleQuat* q, struct DoubleVect3* v_in); +extern void double_rmat_of_eulers_321(struct DoubleRMat *rm, struct DoubleEulers *e); +extern void double_quat_of_eulers(struct DoubleQuat *q, struct DoubleEulers *e); +extern void double_eulers_of_quat(struct DoubleEulers *e, struct DoubleQuat *q); +extern void double_quat_vmult(struct DoubleVect3 *v_out, struct DoubleQuat *q, struct DoubleVect3 *v_in); -static inline void double_rmat_of_eulers(struct DoubleRMat* rm, struct DoubleEulers* e) +static inline void double_rmat_of_eulers(struct DoubleRMat *rm, struct DoubleEulers *e) { double_rmat_of_eulers_321(rm, e); } diff --git a/sw/airborne/math/pprz_algebra_float.c b/sw/airborne/math/pprz_algebra_float.c index 4f3a066a19..ff978ca0a2 100644 --- a/sw/airborne/math/pprz_algebra_float.c +++ b/sw/airborne/math/pprz_algebra_float.c @@ -27,7 +27,7 @@ #include "pprz_algebra_float.h" /** in place first order integration of a 3D-vector */ -void float_vect3_integrate_fi(struct FloatVect3* vec, struct FloatVect3* dv, float dt) +void float_vect3_integrate_fi(struct FloatVect3 *vec, struct FloatVect3 *dv, float dt) { vec->x += dv->x * dt; vec->y += dv->y * dt; @@ -35,14 +35,14 @@ void float_vect3_integrate_fi(struct FloatVect3* vec, struct FloatVect3* dv, flo } /** in place first order integration of angular rates */ -void float_rates_integrate_fi(struct FloatRates* r, struct FloatRates* dr, float dt) +void float_rates_integrate_fi(struct FloatRates *r, struct FloatRates *dr, float dt) { r->p += dr->p * dt; r->q += dr->q * dt; r->r += dr->r * dt; } -void float_rates_of_euler_dot(struct FloatRates* r, struct FloatEulers* e, struct FloatEulers* edot) +void float_rates_of_euler_dot(struct FloatRates *r, struct FloatEulers *e, struct FloatEulers *edot) { r->p = edot->phi - sinf(e->theta) * edot->psi; r->q = cosf(e->phi) * edot->theta + sinf(e->phi) * cosf(e->theta) * edot->psi; @@ -52,7 +52,7 @@ void float_rates_of_euler_dot(struct FloatRates* r, struct FloatEulers* e, struc -void float_rmat_inv(struct FloatRMat* m_b2a, struct FloatRMat* m_a2b) +void float_rmat_inv(struct FloatRMat *m_b2a, struct FloatRMat *m_a2b) { RMAT_ELMT(*m_b2a, 0, 0) = RMAT_ELMT(*m_a2b, 0, 0); RMAT_ELMT(*m_b2a, 0, 1) = RMAT_ELMT(*m_a2b, 1, 0); @@ -65,17 +65,17 @@ void float_rmat_inv(struct FloatRMat* m_b2a, struct FloatRMat* m_a2b) RMAT_ELMT(*m_b2a, 2, 2) = RMAT_ELMT(*m_a2b, 2, 2); } -float float_rmat_norm(struct FloatRMat* rm) +float float_rmat_norm(struct FloatRMat *rm) { - return sqrtf(SQUARE(rm->m[0])+ SQUARE(rm->m[1])+ SQUARE(rm->m[2])+ - SQUARE(rm->m[3])+ SQUARE(rm->m[4])+ SQUARE(rm->m[5])+ - SQUARE(rm->m[6])+ SQUARE(rm->m[7])+ SQUARE(rm->m[8])); + return sqrtf(SQUARE(rm->m[0]) + SQUARE(rm->m[1]) + SQUARE(rm->m[2]) + + SQUARE(rm->m[3]) + SQUARE(rm->m[4]) + SQUARE(rm->m[5]) + + SQUARE(rm->m[6]) + SQUARE(rm->m[7]) + SQUARE(rm->m[8])); } /** Composition (multiplication) of two rotation matrices. * m_a2c = m_a2b comp m_b2c , aka m_a2c = m_b2c * m_a2b */ -void float_rmat_comp(struct FloatRMat* m_a2c, struct FloatRMat* m_a2b, struct FloatRMat* m_b2c) +void float_rmat_comp(struct FloatRMat *m_a2c, struct FloatRMat *m_a2b, struct FloatRMat *m_b2c) { m_a2c->m[0] = m_b2c->m[0] * m_a2b->m[0] + m_b2c->m[1] * m_a2b->m[3] + m_b2c->m[2] * m_a2b->m[6]; m_a2c->m[1] = m_b2c->m[0] * m_a2b->m[1] + m_b2c->m[1] * m_a2b->m[4] + m_b2c->m[2] * m_a2b->m[7]; @@ -91,7 +91,7 @@ void float_rmat_comp(struct FloatRMat* m_a2c, struct FloatRMat* m_a2b, struct Fl /** Composition (multiplication) of two rotation matrices. * m_a2b = m_a2c comp_inv m_b2c , aka m_a2b = inv(_m_b2c) * m_a2c */ -void float_rmat_comp_inv(struct FloatRMat* m_a2b, struct FloatRMat* m_a2c, struct FloatRMat* m_b2c) +void float_rmat_comp_inv(struct FloatRMat *m_a2b, struct FloatRMat *m_a2c, struct FloatRMat *m_b2c) { m_a2b->m[0] = m_b2c->m[0] * m_a2c->m[0] + m_b2c->m[3] * m_a2c->m[3] + m_b2c->m[6] * m_a2c->m[6]; m_a2b->m[1] = m_b2c->m[0] * m_a2c->m[1] + m_b2c->m[3] * m_a2c->m[4] + m_b2c->m[6] * m_a2c->m[7]; @@ -107,7 +107,7 @@ void float_rmat_comp_inv(struct FloatRMat* m_a2b, struct FloatRMat* m_a2c, struc /** rotate 3D vector by rotation matrix. * vb = m_a2b * va */ -void float_rmat_vmult(struct FloatVect3* vb, struct FloatRMat* m_a2b, struct FloatVect3* va) +void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b, struct FloatVect3 *va) { vb->x = m_a2b->m[0] * va->x + m_a2b->m[1] * va->y + m_a2b->m[2] * va->z; vb->y = m_a2b->m[3] * va->x + m_a2b->m[4] * va->y + m_a2b->m[5] * va->z; @@ -117,7 +117,7 @@ void float_rmat_vmult(struct FloatVect3* vb, struct FloatRMat* m_a2b, struct Flo /** rotate 3D vector by transposed rotation matrix. * vb = m_b2a^T * va */ -void float_rmat_transp_vmult(struct FloatVect3* vb, struct FloatRMat* m_b2a, struct FloatVect3* va) +void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va) { vb->x = m_b2a->m[0] * va->x + m_b2a->m[3] * va->y + m_b2a->m[6] * va->z; vb->y = m_b2a->m[1] * va->x + m_b2a->m[4] * va->y + m_b2a->m[7] * va->z; @@ -127,7 +127,7 @@ void float_rmat_transp_vmult(struct FloatVect3* vb, struct FloatRMat* m_b2a, str /** rotate anglular rates by rotation matrix. * rb = m_a2b * ra */ -void float_rmat_ratemult(struct FloatRates* rb, struct FloatRMat* m_a2b, struct FloatRates* ra) +void float_rmat_ratemult(struct FloatRates *rb, struct FloatRMat *m_a2b, struct FloatRates *ra) { rb->p = m_a2b->m[0] * ra->p + m_a2b->m[1] * ra->q + m_a2b->m[2] * ra->r; rb->q = m_a2b->m[3] * ra->p + m_a2b->m[4] * ra->q + m_a2b->m[5] * ra->r; @@ -137,7 +137,7 @@ void float_rmat_ratemult(struct FloatRates* rb, struct FloatRMat* m_a2b, struct /** rotate anglular rates by transposed rotation matrix. * rb = m_b2a^T * ra */ -void float_rmat_transp_ratemult(struct FloatRates* rb, struct FloatRMat* m_b2a, struct FloatRates* ra) +void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a, struct FloatRates *ra) { rb->p = m_b2a->m[0] * ra->p + m_b2a->m[3] * ra->q + m_b2a->m[6] * ra->r; rb->q = m_b2a->m[1] * ra->p + m_b2a->m[4] * ra->q + m_b2a->m[7] * ra->r; @@ -146,7 +146,7 @@ void float_rmat_transp_ratemult(struct FloatRates* rb, struct FloatRMat* m_b2a, /** initialises a rotation matrix from unit vector axis and angle */ -void float_rmat_of_axis_angle(struct FloatRMat* rm, struct FloatVect3* uv, float angle) +void float_rmat_of_axis_angle(struct FloatRMat *rm, struct FloatVect3 *uv, float angle) { const float ux2 = uv->x * uv->x; const float uy2 = uv->y * uv->y; @@ -171,7 +171,7 @@ void float_rmat_of_axis_angle(struct FloatRMat* rm, struct FloatVect3* uv, float /* C n->b rotation matrix */ -void float_rmat_of_eulers_321(struct FloatRMat* rm, struct FloatEulers* e) +void float_rmat_of_eulers_321(struct FloatRMat *rm, struct FloatEulers *e) { const float sphi = sinf(e->phi); const float cphi = cosf(e->phi); @@ -191,7 +191,7 @@ void float_rmat_of_eulers_321(struct FloatRMat* rm, struct FloatEulers* e) RMAT_ELMT(*rm, 2, 2) = cphi * ctheta; } -void float_rmat_of_eulers_312(struct FloatRMat* rm, struct FloatEulers* e) +void float_rmat_of_eulers_312(struct FloatRMat *rm, struct FloatEulers *e) { const float sphi = sinf(e->phi); const float cphi = cosf(e->phi); @@ -213,7 +213,7 @@ void float_rmat_of_eulers_312(struct FloatRMat* rm, struct FloatEulers* e) /* C n->b rotation matrix */ -void float_rmat_of_quat(struct FloatRMat* rm, struct FloatQuat* q) +void float_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q) { const float _a = M_SQRT2 * q->qi; const float _b = M_SQRT2 * q->qx; @@ -238,13 +238,13 @@ void float_rmat_of_quat(struct FloatRMat* rm, struct FloatQuat* q) } /** in place first order integration of a rotation matrix */ -void float_rmat_integrate_fi(struct FloatRMat* rm, struct FloatRates* omega, float dt) +void float_rmat_integrate_fi(struct FloatRMat *rm, struct FloatRates *omega, float dt) { struct FloatRMat exp_omega_dt = { { - 1. , dt* omega->r, -dt* omega->q, - -dt* omega->r, 1. , dt* omega->p, - dt* omega->q, -dt* omega->p, 1. + 1. , dt *omega->r, -dt *omega->q, + -dt *omega->r, 1. , dt *omega->p, + dt *omega->q, -dt *omega->p, 1. } }; struct FloatRMat R_tdt; @@ -263,7 +263,7 @@ static inline float renorm_factor(float n) } } -float float_rmat_reorthogonalize(struct FloatRMat* rm) +float float_rmat_reorthogonalize(struct FloatRMat *rm) { const struct FloatVect3 r0 = {RMAT_ELMT(*rm, 0, 0), RMAT_ELMT(*rm, 0, 1), @@ -297,7 +297,7 @@ float float_rmat_reorthogonalize(struct FloatRMat* rm) * */ -void float_quat_comp(struct FloatQuat* a2c, struct FloatQuat* a2b, struct FloatQuat* b2c) +void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c) { a2c->qi = a2b->qi * b2c->qi - a2b->qx * b2c->qx - a2b->qy * b2c->qy - a2b->qz * b2c->qz; a2c->qx = a2b->qi * b2c->qx + a2b->qx * b2c->qi + a2b->qy * b2c->qz - a2b->qz * b2c->qy; @@ -305,7 +305,7 @@ void float_quat_comp(struct FloatQuat* a2c, struct FloatQuat* a2b, struct FloatQ a2c->qz = a2b->qi * b2c->qz + a2b->qx * b2c->qy - a2b->qy * b2c->qx + a2b->qz * b2c->qi; } -void float_quat_comp_inv(struct FloatQuat* a2b, struct FloatQuat* a2c, struct FloatQuat* b2c) +void float_quat_comp_inv(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c) { a2b->qi = a2c->qi * b2c->qi + a2c->qx * b2c->qx + a2c->qy * b2c->qy + a2c->qz * b2c->qz; a2b->qx = -a2c->qi * b2c->qx + a2c->qx * b2c->qi - a2c->qy * b2c->qz + a2c->qz * b2c->qy; @@ -313,7 +313,7 @@ void float_quat_comp_inv(struct FloatQuat* a2b, struct FloatQuat* a2c, struct Fl a2b->qz = -a2c->qi * b2c->qz - a2c->qx * b2c->qy + a2c->qy * b2c->qx + a2c->qz * b2c->qi; } -void float_quat_inv_comp(struct FloatQuat* b2c, struct FloatQuat* a2b, struct FloatQuat* a2c) +void float_quat_inv_comp(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c) { b2c->qi = a2b->qi * a2c->qi + a2b->qx * a2c->qx + a2b->qy * a2c->qy + a2b->qz * a2c->qz; b2c->qx = a2b->qi * a2c->qx - a2b->qx * a2c->qi - a2b->qy * a2c->qz + a2b->qz * a2c->qy; @@ -321,28 +321,28 @@ void float_quat_inv_comp(struct FloatQuat* b2c, struct FloatQuat* a2b, struct Fl b2c->qz = a2b->qi * a2c->qz - a2b->qx * a2c->qy + a2b->qy * a2c->qx - a2b->qz * a2c->qi; } -void float_quat_comp_norm_shortest(struct FloatQuat* a2c, struct FloatQuat* a2b, struct FloatQuat* b2c) +void float_quat_comp_norm_shortest(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c) { float_quat_comp(a2c, a2b, b2c); float_quat_wrap_shortest(a2c); float_quat_normalize(a2c); } -void float_quat_comp_inv_norm_shortest(struct FloatQuat* a2b, struct FloatQuat* a2c, struct FloatQuat* b2c) +void float_quat_comp_inv_norm_shortest(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c) { float_quat_comp_inv(a2b, a2c, b2c); float_quat_wrap_shortest(a2b); float_quat_normalize(a2b); } -void float_quat_inv_comp_norm_shortest(struct FloatQuat* b2c, struct FloatQuat* a2b, struct FloatQuat* a2c) +void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c) { float_quat_inv_comp(b2c, a2b, a2c); float_quat_wrap_shortest(b2c); float_quat_normalize(b2c); } -void float_quat_differential(struct FloatQuat* q_out, struct FloatRates* w, float dt) +void float_quat_differential(struct FloatQuat *q_out, struct FloatRates *w, float dt) { const float v_norm = sqrtf(w->p * w->p + w->q * w->q + w->r * w->r); const float c2 = cos(dt * v_norm / 2.0); @@ -361,7 +361,7 @@ void float_quat_differential(struct FloatQuat* q_out, struct FloatRates* w, floa } /** in place first order quaternion integration with constant rotational velocity */ -void float_quat_integrate_fi(struct FloatQuat* q, struct FloatRates* omega, float dt) +void float_quat_integrate_fi(struct FloatQuat *q, struct FloatRates *omega, float dt) { const float qi = q->qi; const float qx = q->qx; @@ -377,7 +377,7 @@ void float_quat_integrate_fi(struct FloatQuat* q, struct FloatRates* omega, floa } /** in place quaternion integration with constant rotational velocity */ -void float_quat_integrate(struct FloatQuat* q, struct FloatRates* omega, float dt) +void float_quat_integrate(struct FloatQuat *q, struct FloatRates *omega, float dt) { const float no = FLOAT_RATES_NORM(*omega); if (no > FLT_MIN) { @@ -398,7 +398,7 @@ void float_quat_integrate(struct FloatQuat* q, struct FloatRates* omega, float d } } -void float_quat_vmult(struct FloatVect3* v_out, struct FloatQuat* q, struct FloatVect3* v_in) +void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, struct FloatVect3 *v_in) { const float qi2_M1_2 = q->qi * q->qi - 0.5; const float qiqx = q->qi * q->qx; @@ -427,7 +427,7 @@ void float_quat_vmult(struct FloatVect3* v_out, struct FloatQuat* q, struct Floa * or equally: * qd = 0.5 * q * omega(r) */ -void float_quat_derivative(struct FloatQuat* qd, struct FloatRates* r, struct FloatQuat* q) +void float_quat_derivative(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q) { qd->qi = -0.5 * (r->p * q->qx + r->q * q->qy + r->r * q->qz); qd->qx = -0.5 * (-r->p * q->qi - r->r * q->qy + r->q * q->qz); @@ -438,7 +438,7 @@ void float_quat_derivative(struct FloatQuat* qd, struct FloatRates* r, struct Fl /** Quaternion derivative from rotational velocity. * qd = -0.5*omega(r) * q */ -void float_quat_derivative_lagrange(struct FloatQuat* qd, struct FloatRates* r, struct FloatQuat* q) +void float_quat_derivative_lagrange(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q) { const float K_LAGRANGE = 1.; const float c = K_LAGRANGE * (1 - float_quat_norm(q)) / -0.5; @@ -448,7 +448,7 @@ void float_quat_derivative_lagrange(struct FloatQuat* qd, struct FloatRates* r, qd->qz = -0.5 * (-r->r * q->qi - r->q * q->qx + r->p * q->qy + c * q->qz); } -void float_quat_of_eulers(struct FloatQuat* q, struct FloatEulers* e) +void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e) { const float phi2 = e->phi / 2.0; @@ -468,7 +468,7 @@ void float_quat_of_eulers(struct FloatQuat* q, struct FloatEulers* e) q->qz = c_phi2 * c_theta2 * s_psi2 - s_phi2 * s_theta2 * c_psi2; } -void float_quat_of_axis_angle(struct FloatQuat* q, const struct FloatVect3* uv, float angle) +void float_quat_of_axis_angle(struct FloatQuat *q, const struct FloatVect3 *uv, float angle) { const float san = sinf(angle / 2.); q->qi = cosf(angle / 2.); @@ -477,7 +477,7 @@ void float_quat_of_axis_angle(struct FloatQuat* q, const struct FloatVect3* uv, q->qz = san * uv->z; } -void float_quat_of_orientation_vect(struct FloatQuat* q, const struct FloatVect3* ov) +void float_quat_of_orientation_vect(struct FloatQuat *q, const struct FloatVect3 *ov) { const float ov_norm = sqrtf(ov->x * ov->x + ov->y * ov->y + ov->z * ov->z); if (ov_norm < 1e-8) { @@ -494,7 +494,7 @@ void float_quat_of_orientation_vect(struct FloatQuat* q, const struct FloatVect3 } } -void float_quat_of_rmat(struct FloatQuat* q, struct FloatRMat* rm) +void float_quat_of_rmat(struct FloatQuat *q, struct FloatRMat *rm) { const float tr = RMAT_TRACE(*rm); if (tr > 0) { @@ -545,7 +545,7 @@ void float_quat_of_rmat(struct FloatQuat* q, struct FloatRMat* rm) * */ -void float_eulers_of_rmat(struct FloatEulers* e, struct FloatRMat* rm) +void float_eulers_of_rmat(struct FloatEulers *e, struct FloatRMat *rm) { const float dcm00 = rm->m[0]; const float dcm01 = rm->m[1]; @@ -557,7 +557,7 @@ void float_eulers_of_rmat(struct FloatEulers* e, struct FloatRMat* rm) e->psi = atan2f(dcm01, dcm00); } -void float_eulers_of_quat(struct FloatEulers* e, struct FloatQuat* q) +void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q) { const float qx2 = q->qx * q->qx; const float qy2 = q->qy * q->qy; diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h index 1728064e99..0b619fb3d9 100644 --- a/sw/airborne/math/pprz_algebra_float.h +++ b/sw/airborne/math/pprz_algebra_float.h @@ -112,18 +112,18 @@ struct FloatRates { /* macros also usable if _v is not a FloatVect2, but a different struct with x,y members */ #define FLOAT_VECT2_NORM(_v) sqrtf(VECT2_NORM2(_v)) -static inline float float_vect2_norm2(struct FloatVect2* v) +static inline float float_vect2_norm2(struct FloatVect2 *v) { return v->x * v->x + v->y * v->y; } -static inline float float_vect2_norm(struct FloatVect2* v) +static inline float float_vect2_norm(struct FloatVect2 *v) { return sqrtf(float_vect2_norm2(v)); } /** normalize 2D vector in place */ -static inline void float_vect2_normalize(struct FloatVect2* v) +static inline void float_vect2_normalize(struct FloatVect2 *v) { const float n = float_vect2_norm(v); if (n > 0) { @@ -144,18 +144,18 @@ static inline void float_vect2_normalize(struct FloatVect2* v) /* macros also usable if _v is not a FloatVect3, but a different struct with x,y,z members */ #define FLOAT_VECT3_NORM(_v) sqrtf(VECT3_NORM2(_v)) -static inline float float_vect3_norm2(struct FloatVect3* v) +static inline float float_vect3_norm2(struct FloatVect3 *v) { return v->x * v->x + v->y * v->y + v->z * v->z; } -static inline float float_vect3_norm(struct FloatVect3* v) +static inline float float_vect3_norm(struct FloatVect3 *v) { return sqrtf(float_vect3_norm2(v)); } /** normalize 3D vector in place */ -static inline void float_vect3_normalize(struct FloatVect3* v) +static inline void float_vect3_normalize(struct FloatVect3 *v) { const float n = float_vect3_norm(v); if (n > 0) { @@ -182,14 +182,14 @@ static inline void float_vect3_normalize(struct FloatVect3* v) } -extern void float_vect3_integrate_fi(struct FloatVect3* vec, struct FloatVect3* dv, +extern void float_vect3_integrate_fi(struct FloatVect3 *vec, struct FloatVect3 *dv, float dt); -extern void float_rates_integrate_fi(struct FloatRates* r, struct FloatRates* dr, +extern void float_rates_integrate_fi(struct FloatRates *r, struct FloatRates *dr, float dt); -extern void float_rates_of_euler_dot(struct FloatRates* r, struct FloatEulers* e, - struct FloatEulers* edot); +extern void float_rates_of_euler_dot(struct FloatRates *r, struct FloatEulers *e, + struct FloatEulers *edot); /* defines for backwards compatibility */ #define FLOAT_VECT3_INTEGRATE_FI(_vo, _dv, _dt) float_vect3_integrate_fi(&(_vo), &(_dv), _dt) @@ -233,7 +233,7 @@ extern void float_rates_of_euler_dot(struct FloatRates* r, struct FloatEulers* e /** initialises a rotation matrix to identity */ -static inline void float_rmat_identity(struct FloatRMat* rm) +static inline void float_rmat_identity(struct FloatRMat *rm) { FLOAT_MAT33_DIAG(*rm, 1., 1., 1.); } @@ -241,49 +241,49 @@ static inline void float_rmat_identity(struct FloatRMat* rm) /** Inverse/transpose of a rotation matrix. * m_b2a = inv(_m_a2b) = transp(_m_a2b) */ -extern void float_rmat_inv(struct FloatRMat* m_b2a, struct FloatRMat* m_a2b); +extern void float_rmat_inv(struct FloatRMat *m_b2a, struct FloatRMat *m_a2b); /** Composition (multiplication) of two rotation matrices. * m_a2c = m_a2b comp m_b2c , aka m_a2c = m_b2c * m_a2b */ -extern void float_rmat_comp(struct FloatRMat* m_a2c, struct FloatRMat* m_a2b, - struct FloatRMat* m_b2c); +extern void float_rmat_comp(struct FloatRMat *m_a2c, struct FloatRMat *m_a2b, + struct FloatRMat *m_b2c); /** Composition (multiplication) of two rotation matrices. * m_a2b = m_a2c comp_inv m_b2c , aka m_a2b = inv(_m_b2c) * m_a2c */ -extern void float_rmat_comp_inv(struct FloatRMat* m_a2b, struct FloatRMat* m_a2c, - struct FloatRMat* m_b2c); +extern void float_rmat_comp_inv(struct FloatRMat *m_a2b, struct FloatRMat *m_a2c, + struct FloatRMat *m_b2c); /// Norm of a rotation matrix. -extern float float_rmat_norm(struct FloatRMat* rm); +extern float float_rmat_norm(struct FloatRMat *rm); /** rotate 3D vector by rotation matrix. * vb = m_a2b * va */ -extern void float_rmat_vmult(struct FloatVect3* vb, struct FloatRMat* m_a2b, - struct FloatVect3* va); +extern void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b, + struct FloatVect3 *va); /** rotate 3D vector by transposed rotation matrix. * vb = m_b2a^T * va */ -extern void float_rmat_transp_vmult(struct FloatVect3* vb, struct FloatRMat* m_b2a, - struct FloatVect3* va); +extern void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, + struct FloatVect3 *va); /** rotate anglular rates by rotation matrix. * rb = m_a2b * ra */ -extern void float_rmat_ratemult(struct FloatRates* rb, struct FloatRMat* m_a2b, - struct FloatRates* ra); +extern void float_rmat_ratemult(struct FloatRates *rb, struct FloatRMat *m_a2b, + struct FloatRates *ra); /** rotate anglular rates by transposed rotation matrix. * rb = m_b2a^T * ra */ -extern void float_rmat_transp_ratemult(struct FloatRates* rb, struct FloatRMat* m_b2a, - struct FloatRates* ra); +extern void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a, + struct FloatRates *ra); /** initialises a rotation matrix from unit vector axis and angle */ -extern void float_rmat_of_axis_angle(struct FloatRMat* rm, struct FloatVect3* uv, float angle); +extern void float_rmat_of_axis_angle(struct FloatRMat *rm, struct FloatVect3 *uv, float angle); /** Rotation matrix from 321 Euler angles (float). * The Euler angles are interpreted as zy'x'' (intrinsic) rotation. @@ -297,14 +297,14 @@ extern void float_rmat_of_axis_angle(struct FloatRMat* rm, struct FloatVect3* uv * @param[out] rm pointer to rotation matrix * @param[in] e pointer to Euler angles */ -extern void float_rmat_of_eulers_321(struct FloatRMat* rm, struct FloatEulers* e); -extern void float_rmat_of_eulers_312(struct FloatRMat* rm, struct FloatEulers* e); +extern void float_rmat_of_eulers_321(struct FloatRMat *rm, struct FloatEulers *e); +extern void float_rmat_of_eulers_312(struct FloatRMat *rm, struct FloatEulers *e); #define float_rmat_of_eulers float_rmat_of_eulers_321 -extern void float_rmat_of_quat(struct FloatRMat* rm, struct FloatQuat* q); +extern void float_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q); /** in place first order integration of a rotation matrix */ -extern void float_rmat_integrate_fi(struct FloatRMat* rm, struct FloatRates* omega, float dt); -extern float float_rmat_reorthogonalize(struct FloatRMat* rm); +extern void float_rmat_integrate_fi(struct FloatRMat *rm, struct FloatRates *omega, float dt); +extern float float_rmat_reorthogonalize(struct FloatRMat *rm); /* defines for backwards compatibility */ #define FLOAT_RMAT_INV(_m_b2a, _m_a2b) float_rmat_inv(&(_m_b2a), &(_m_a2b)) @@ -331,7 +331,7 @@ extern float float_rmat_reorthogonalize(struct FloatRMat* rm); // /** initialises a quaternion to identity */ -static inline void float_quat_identity(struct FloatQuat* q) +static inline void float_quat_identity(struct FloatQuat *q) { q->qi = 1.0; q->qx = 0; @@ -341,12 +341,12 @@ static inline void float_quat_identity(struct FloatQuat* q) #define FLOAT_QUAT_NORM2(_q) (SQUARE((_q).qi) + SQUARE((_q).qx) + SQUARE((_q).qy) + SQUARE((_q).qz)) -static inline float float_quat_norm(struct FloatQuat* q) +static inline float float_quat_norm(struct FloatQuat *q) { return sqrtf(SQUARE(q->qi) + SQUARE(q->qx) + SQUARE(q->qy) + SQUARE(q->qz)); } -static inline void float_quat_normalize(struct FloatQuat* q) +static inline void float_quat_normalize(struct FloatQuat *q) { float qnorm = float_quat_norm(q); if (qnorm > FLT_MIN) { @@ -357,12 +357,12 @@ static inline void float_quat_normalize(struct FloatQuat* q) } } -static inline void float_quat_invert(struct FloatQuat* qo, struct FloatQuat* qi) +static inline void float_quat_invert(struct FloatQuat *qo, struct FloatQuat *qi) { QUAT_INVERT(*qo, *qi); } -static inline void float_quat_wrap_shortest(struct FloatQuat* q) +static inline void float_quat_wrap_shortest(struct FloatQuat *q) { if (q->qi < 0.) { QUAT_EXPLEMENTARY(*q, *q); @@ -375,75 +375,75 @@ static inline void float_quat_wrap_shortest(struct FloatQuat* q) /** Composition (multiplication) of two quaternions. * a2c = a2b comp b2c , aka a2c = a2b * b2c */ -extern void float_quat_comp(struct FloatQuat* a2c, struct FloatQuat* a2b, struct FloatQuat* b2c); +extern void float_quat_comp(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c); /** Composition (multiplication) of two quaternions. * a2b = a2c comp_inv b2c , aka a2b = a2c * inv(b2c) */ -extern void float_quat_comp_inv(struct FloatQuat* a2b, struct FloatQuat* a2c, struct FloatQuat* b2c); +extern void float_quat_comp_inv(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c); /** Composition (multiplication) of two quaternions. * b2c = a2b inv_comp a2c , aka b2c = inv(_a2b) * a2c */ -extern void float_quat_inv_comp(struct FloatQuat* b2c, struct FloatQuat* a2b, struct FloatQuat* a2c); +extern void float_quat_inv_comp(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c); /** Composition (multiplication) of two quaternions with normalization. * a2c = a2b comp b2c , aka a2c = a2b * b2c */ -extern void float_quat_comp_norm_shortest(struct FloatQuat* a2c, struct FloatQuat* a2b, struct FloatQuat* b2c); +extern void float_quat_comp_norm_shortest(struct FloatQuat *a2c, struct FloatQuat *a2b, struct FloatQuat *b2c); /** Composition (multiplication) of two quaternions with normalization. * a2b = a2c comp_inv b2c , aka a2b = a2c * inv(b2c) */ -extern void float_quat_comp_inv_norm_shortest(struct FloatQuat* a2b, struct FloatQuat* a2c, struct FloatQuat* b2c); +extern void float_quat_comp_inv_norm_shortest(struct FloatQuat *a2b, struct FloatQuat *a2c, struct FloatQuat *b2c); /** Composition (multiplication) of two quaternions with normalization. * b2c = a2b inv_comp a2c , aka b2c = inv(_a2b) * a2c */ -extern void float_quat_inv_comp_norm_shortest(struct FloatQuat* b2c, struct FloatQuat* a2b, struct FloatQuat* a2c); +extern void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c); /** Quaternion derivative from rotational velocity. * qd = -0.5*omega(r) * q * or equally: * qd = 0.5 * q * omega(r) */ -extern void float_quat_derivative(struct FloatQuat* qd, struct FloatRates* r, struct FloatQuat* q); +extern void float_quat_derivative(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q); /** Quaternion derivative from rotational velocity with Lagrange multiplier. * qd = -0.5*omega(r) * q * or equally: * qd = 0.5 * q * omega(r) */ -extern void float_quat_derivative_lagrange(struct FloatQuat* qd, struct FloatRates* r, struct FloatQuat* q); +extern void float_quat_derivative_lagrange(struct FloatQuat *qd, struct FloatRates *r, struct FloatQuat *q); /** Delta rotation quaternion with constant angular rates. */ -extern void float_quat_differential(struct FloatQuat* q_out, struct FloatRates* w, float dt); +extern void float_quat_differential(struct FloatQuat *q_out, struct FloatRates *w, float dt); /** in place first order quaternion integration with constant rotational velocity */ -extern void float_quat_integrate_fi(struct FloatQuat* q, struct FloatRates* omega, float dt); +extern void float_quat_integrate_fi(struct FloatQuat *q, struct FloatRates *omega, float dt); /** in place quaternion integration with constant rotational velocity */ -extern void float_quat_integrate(struct FloatQuat* q, struct FloatRates* omega, float dt); +extern void float_quat_integrate(struct FloatQuat *q, struct FloatRates *omega, float dt); /** rotate 3D vector by quaternion. * vb = q_a2b * va * q_a2b^-1 */ -extern void float_quat_vmult(struct FloatVect3* v_out, struct FloatQuat* q, struct FloatVect3* v_in); +extern void float_quat_vmult(struct FloatVect3 *v_out, struct FloatQuat *q, struct FloatVect3 *v_in); /// Quaternion from Euler angles. -extern void float_quat_of_eulers(struct FloatQuat* q, struct FloatEulers* e); +extern void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e); /// Quaternion from unit vector and angle. -extern void float_quat_of_axis_angle(struct FloatQuat* q, const struct FloatVect3* uv, float angle); +extern void float_quat_of_axis_angle(struct FloatQuat *q, const struct FloatVect3 *uv, float angle); /** Quaternion from orientation vector. * Length/norm of the vector is the angle. */ -extern void float_quat_of_orientation_vect(struct FloatQuat* q, const struct FloatVect3* ov); +extern void float_quat_of_orientation_vect(struct FloatQuat *q, const struct FloatVect3 *ov); /// Quaternion from rotation matrix. -extern void float_quat_of_rmat(struct FloatQuat* q, struct FloatRMat* rm); +extern void float_quat_of_rmat(struct FloatQuat *q, struct FloatRMat *rm); /* defines for backwards compatibility */ @@ -479,12 +479,12 @@ extern void float_quat_of_rmat(struct FloatQuat* q, struct FloatRMat* rm); #define FLOAT_EULERS_ZERO(_e) EULERS_ASSIGN(_e, 0., 0., 0.); -static inline float float_eulers_norm(struct FloatEulers* e) +static inline float float_eulers_norm(struct FloatEulers *e) { return sqrtf(SQUARE(e->phi) + SQUARE(e->theta) + SQUARE(e->psi)); } -extern void float_eulers_of_rmat(struct FloatEulers* e, struct FloatRMat* rm); -extern void float_eulers_of_quat(struct FloatEulers* e, struct FloatQuat* q); +extern void float_eulers_of_rmat(struct FloatEulers *e, struct FloatRMat *rm); +extern void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q); /* defines for backwards compatibility */ #define FLOAT_EULERS_OF_RMAT(_e, _rm) float_eulers_of_rmat(&(_e), &(_rm)) @@ -498,63 +498,63 @@ extern void float_eulers_of_quat(struct FloatEulers* e, struct FloatQuat* q); // /** a = 0 */ -static inline void float_vect_zero(float* a, const int n) +static inline void float_vect_zero(float *a, const int n) { int i; for (i = 0; i < n; i++) { a[i] = 0.; } } /** a = b */ -static inline void float_vect_copy(float* a, const float* b, const int n) +static inline void float_vect_copy(float *a, const float *b, const int n) { int i; for (i = 0; i < n; i++) { a[i] = b[i]; } } /** o = a + b */ -static inline void float_vect_sum(float* o, const float* a, const float* b, const int n) +static inline void float_vect_sum(float *o, const float *a, const float *b, const int n) { int i; for (i = 0; i < n; i++) { o[i] = a[i] + b[i]; } } /** o = a - b */ -static inline void float_vect_diff(float* o, const float* a, const float* b, const int n) +static inline void float_vect_diff(float *o, const float *a, const float *b, const int n) { int i; for (i = 0; i < n; i++) { o[i] = a[i] - b[i]; } } /** o = a * b (element wise) */ -static inline void float_vect_mul(float* o, const float* a, const float* b, const int n) +static inline void float_vect_mul(float *o, const float *a, const float *b, const int n) { int i; for (i = 0; i < n; i++) { o[i] = a[i] * b[i]; } } /** a += b */ -static inline void float_vect_add(float* a, const float* b, const int n) +static inline void float_vect_add(float *a, const float *b, const int n) { int i; for (i = 0; i < n; i++) { a[i] += b[i]; } } /** a -= b */ -static inline void float_vect_sub(float* a, const float* b, const int n) +static inline void float_vect_sub(float *a, const float *b, const int n) { int i; for (i = 0; i < n; i++) { a[i] -= b[i]; } } /** o = a * s */ -static inline void float_vect_smul(float* o, const float* a, const float s, const int n) +static inline void float_vect_smul(float *o, const float *a, const float s, const int n) { int i; for (i = 0; i < n; i++) { o[i] = a[i] * s; } } /** o = a / s */ -static inline void float_vect_sdiv(float* o, const float* a, const float s, const int n) +static inline void float_vect_sdiv(float *o, const float *a, const float s, const int n) { int i; if (fabs(s) > 1e-5) { @@ -563,7 +563,7 @@ static inline void float_vect_sdiv(float* o, const float* a, const float s, cons } /** ||a|| */ -static inline float float_vect_norm(const float* a, const int n) +static inline float float_vect_norm(const float *a, const int n) { int i; float sum = 0; @@ -586,7 +586,7 @@ static inline float float_vect_norm(const float* a, const int n) } /** a = 0 */ -static inline void float_mat_zero(float** a, int m, int n) +static inline void float_mat_zero(float **a, int m, int n) { int i, j; for (i = 0; i < m; i++) { @@ -595,7 +595,7 @@ static inline void float_mat_zero(float** a, int m, int n) } /** a = b */ -static inline void float_mat_copy(float** a, float** b, int m, int n) +static inline void float_mat_copy(float **a, float **b, int m, int n) { int i, j; for (i = 0; i < m; i++) { @@ -604,7 +604,7 @@ static inline void float_mat_copy(float** a, float** b, int m, int n) } /** o = a + b */ -static inline void float_mat_sum(float** o, float** a, float** b, int m, int n) +static inline void float_mat_sum(float **o, float **a, float **b, int m, int n) { int i, j; for (i = 0; i < m; i++) { @@ -613,7 +613,7 @@ static inline void float_mat_sum(float** o, float** a, float** b, int m, int n) } /** o = a - b */ -static inline void float_mat_diff(float** o, float** a, float** b, int m, int n) +static inline void float_mat_diff(float **o, float **a, float **b, int m, int n) { int i, j; for (i = 0; i < m; i++) { @@ -622,7 +622,7 @@ static inline void float_mat_diff(float** o, float** a, float** b, int m, int n) } /** transpose square matrix */ -static inline void float_mat_transpose(float** a, int n) +static inline void float_mat_transpose(float **a, int n) { int i, j; for (i = 0; i < n; i++) { @@ -640,7 +640,7 @@ static inline void float_mat_transpose(float** a, int n) * b: [n x l] * o: [m x l] */ -static inline void float_mat_mul(float** o, float** a, float** b, int m, int n, int l) +static inline void float_mat_mul(float **o, float **a, float **b, int m, int n, int l) { int i, j, k; for (i = 0; i < m; i++) { @@ -659,7 +659,7 @@ static inline void float_mat_mul(float** o, float** a, float** b, int m, int n, * o: [I(d,d) 0 ] * [ 0 a(d,m:d,n)] */ -static inline void float_mat_minor(float** o, float** a, int m, int n, int d) +static inline void float_mat_minor(float **o, float **a, int m, int n, int d) { int i, j; float_mat_zero(o, m, n); @@ -672,7 +672,7 @@ static inline void float_mat_minor(float** o, float** a, int m, int n, int d) } /** o = I - v v^T */ -static inline void float_mat_vmul(float** o, float* v, int n) +static inline void float_mat_vmul(float **o, float *v, int n) { int i, j; for (i = 0; i < n; i++) { @@ -686,7 +686,7 @@ static inline void float_mat_vmul(float** o, float* v, int n) } /** o = c-th column of matrix a[m x n] */ -static inline void float_mat_col(float* o, float** a, int m, int c) +static inline void float_mat_col(float *o, float **a, int m, int c) { int i; for (i = 0; i < m; i++) { diff --git a/sw/airborne/math/pprz_algebra_int.c b/sw/airborne/math/pprz_algebra_int.c index abe8830def..215cbda659 100644 --- a/sw/airborne/math/pprz_algebra_int.c +++ b/sw/airborne/math/pprz_algebra_int.c @@ -56,7 +56,7 @@ uint32_t int32_sqrt(uint32_t in) /** Composition (multiplication) of two rotation matrices. * _m_a2c = _m_a2b comp _m_b2c , aka _m_a2c = _m_b2c * _m_a2b */ -void int32_rmat_comp(struct Int32RMat* m_a2c, struct Int32RMat* m_a2b, struct Int32RMat* m_b2c) +void int32_rmat_comp(struct Int32RMat *m_a2c, struct Int32RMat *m_a2b, struct Int32RMat *m_b2c) { m_a2c->m[0] = (m_b2c->m[0] * m_a2b->m[0] + m_b2c->m[1] * m_a2b->m[3] + m_b2c->m[2] * m_a2b->m[6]) >> INT32_TRIG_FRAC; m_a2c->m[1] = (m_b2c->m[0] * m_a2b->m[1] + m_b2c->m[1] * m_a2b->m[4] + m_b2c->m[2] * m_a2b->m[7]) >> INT32_TRIG_FRAC; @@ -72,7 +72,7 @@ void int32_rmat_comp(struct Int32RMat* m_a2c, struct Int32RMat* m_a2b, struct In /** Composition (multiplication) of two rotation matrices. * _m_a2b = _m_a2c comp_inv _m_b2c , aka _m_a2b = inv(_m_b2c) * _m_a2c */ -void int32_rmat_comp_inv(struct Int32RMat* m_a2b, struct Int32RMat* m_a2c, struct Int32RMat* m_b2c) +void int32_rmat_comp_inv(struct Int32RMat *m_a2b, struct Int32RMat *m_a2c, struct Int32RMat *m_b2c) { m_a2b->m[0] = (m_b2c->m[0] * m_a2c->m[0] + m_b2c->m[3] * m_a2c->m[3] + m_b2c->m[6] * m_a2c->m[6]) >> INT32_TRIG_FRAC; m_a2b->m[1] = (m_b2c->m[0] * m_a2c->m[1] + m_b2c->m[3] * m_a2c->m[4] + m_b2c->m[6] * m_a2c->m[7]) >> INT32_TRIG_FRAC; @@ -88,48 +88,48 @@ void int32_rmat_comp_inv(struct Int32RMat* m_a2b, struct Int32RMat* m_a2c, struc /** rotate 3D vector by rotation matrix. * vb = m_a2b * va */ -void int32_rmat_vmult(struct Int32Vect3* vb, struct Int32RMat* m_a2b, struct Int32Vect3* va) +void int32_rmat_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_a2b, struct Int32Vect3 *va) { - vb->x = (m_a2b->m[0] * va->x + m_a2b->m[1] * va->y + m_a2b->m[2] * va->z)>>INT32_TRIG_FRAC; - vb->y = (m_a2b->m[3] * va->x + m_a2b->m[4] * va->y + m_a2b->m[5] * va->z)>>INT32_TRIG_FRAC; - vb->z = (m_a2b->m[6] * va->x + m_a2b->m[7] * va->y + m_a2b->m[8] * va->z)>>INT32_TRIG_FRAC; + vb->x = (m_a2b->m[0] * va->x + m_a2b->m[1] * va->y + m_a2b->m[2] * va->z) >> INT32_TRIG_FRAC; + vb->y = (m_a2b->m[3] * va->x + m_a2b->m[4] * va->y + m_a2b->m[5] * va->z) >> INT32_TRIG_FRAC; + vb->z = (m_a2b->m[6] * va->x + m_a2b->m[7] * va->y + m_a2b->m[8] * va->z) >> INT32_TRIG_FRAC; } /** rotate 3D vector by transposed rotation matrix. * vb = m_b2a^T * va */ -void int32_rmat_transp_vmult(struct Int32Vect3* vb, struct Int32RMat* m_b2a, struct Int32Vect3* va) +void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a, struct Int32Vect3 *va) { - vb->x = (m_b2a->m[0] * va->x + m_b2a->m[3] * va->y + m_b2a->m[6] * va->z)>>INT32_TRIG_FRAC; - vb->y = (m_b2a->m[1] * va->x + m_b2a->m[4] * va->y + m_b2a->m[7] * va->z)>>INT32_TRIG_FRAC; - vb->z = (m_b2a->m[2] * va->x + m_b2a->m[5] * va->y + m_b2a->m[8] * va->z)>>INT32_TRIG_FRAC; + vb->x = (m_b2a->m[0] * va->x + m_b2a->m[3] * va->y + m_b2a->m[6] * va->z) >> INT32_TRIG_FRAC; + vb->y = (m_b2a->m[1] * va->x + m_b2a->m[4] * va->y + m_b2a->m[7] * va->z) >> INT32_TRIG_FRAC; + vb->z = (m_b2a->m[2] * va->x + m_b2a->m[5] * va->y + m_b2a->m[8] * va->z) >> INT32_TRIG_FRAC; } /** rotate anglular rates by rotation matrix. * rb = m_a2b * ra */ -void int32_rmat_ratemult(struct Int32Rates* rb, struct Int32RMat* m_a2b, struct Int32Rates* ra) +void int32_rmat_ratemult(struct Int32Rates *rb, struct Int32RMat *m_a2b, struct Int32Rates *ra) { - rb->p = (m_a2b->m[0] * ra->p + m_a2b->m[1] * ra->q + m_a2b->m[2] * ra->r)>>INT32_TRIG_FRAC; - rb->q = (m_a2b->m[3] * ra->p + m_a2b->m[4] * ra->q + m_a2b->m[5] * ra->r)>>INT32_TRIG_FRAC; - rb->r = (m_a2b->m[6] * ra->p + m_a2b->m[7] * ra->q + m_a2b->m[8] * ra->r)>>INT32_TRIG_FRAC; + rb->p = (m_a2b->m[0] * ra->p + m_a2b->m[1] * ra->q + m_a2b->m[2] * ra->r) >> INT32_TRIG_FRAC; + rb->q = (m_a2b->m[3] * ra->p + m_a2b->m[4] * ra->q + m_a2b->m[5] * ra->r) >> INT32_TRIG_FRAC; + rb->r = (m_a2b->m[6] * ra->p + m_a2b->m[7] * ra->q + m_a2b->m[8] * ra->r) >> INT32_TRIG_FRAC; } /** rotate anglular rates by transposed rotation matrix. * rb = m_b2a^T * ra */ -void int32_rmat_transp_ratemult(struct Int32Rates* rb, struct Int32RMat* m_b2a, struct Int32Rates* ra) +void int32_rmat_transp_ratemult(struct Int32Rates *rb, struct Int32RMat *m_b2a, struct Int32Rates *ra) { - rb->p = (m_b2a->m[0] * ra->p + m_b2a->m[3] * ra->q + m_b2a->m[6] * ra->r)>>INT32_TRIG_FRAC; - rb->q = (m_b2a->m[1] * ra->p + m_b2a->m[4] * ra->q + m_b2a->m[7] * ra->r)>>INT32_TRIG_FRAC; - rb->r = (m_b2a->m[2] * ra->p + m_b2a->m[5] * ra->q + m_b2a->m[8] * ra->r)>>INT32_TRIG_FRAC; + rb->p = (m_b2a->m[0] * ra->p + m_b2a->m[3] * ra->q + m_b2a->m[6] * ra->r) >> INT32_TRIG_FRAC; + rb->q = (m_b2a->m[1] * ra->p + m_b2a->m[4] * ra->q + m_b2a->m[7] * ra->r) >> INT32_TRIG_FRAC; + rb->r = (m_b2a->m[2] * ra->p + m_b2a->m[5] * ra->q + m_b2a->m[8] * ra->r) >> INT32_TRIG_FRAC; } /** Convert unit quaternion to rotation matrix. * http://www.mathworks.com/access/helpdesk_r13/help/toolbox/aeroblks/quaternionstodirectioncosinematrix.html */ -void int32_rmat_of_quat(struct Int32RMat* rm, struct Int32Quat* q) +void int32_rmat_of_quat(struct Int32RMat *rm, struct Int32Quat *q) { const int32_t _2qi2_m1 = INT_MULT_RSHIFT(q->qi, q->qi, INT32_QUAT_FRAC + INT32_QUAT_FRAC - INT32_TRIG_FRAC - 1) - TRIG_BFP_OF_REAL(1); @@ -158,7 +158,7 @@ void int32_rmat_of_quat(struct Int32RMat* rm, struct Int32Quat* q) /** Rotation matrix from 321 Euler angles. * http://www.mathworks.com/access/helpdesk_r13/help/toolbox/aeroblks/euleranglestodirectioncosinematrix.html */ -void int32_rmat_of_eulers_321(struct Int32RMat* rm, struct Int32Eulers* e) +void int32_rmat_of_eulers_321(struct Int32RMat *rm, struct Int32Eulers *e) { int32_t sphi; PPRZ_ITRIG_SIN(sphi, e->phi); @@ -201,7 +201,7 @@ void int32_rmat_of_eulers_321(struct Int32RMat* rm, struct Int32Eulers* e) } -void int32_rmat_of_eulers_312(struct Int32RMat* rm, struct Int32Eulers* e) +void int32_rmat_of_eulers_312(struct Int32RMat *rm, struct Int32Eulers *e) { int32_t sphi; PPRZ_ITRIG_SIN(sphi, e->phi); @@ -250,7 +250,7 @@ void int32_rmat_of_eulers_312(struct Int32RMat* rm, struct Int32Eulers* e) * */ -void int32_quat_comp(struct Int32Quat* a2c, struct Int32Quat* a2b, struct Int32Quat* b2c) +void int32_quat_comp(struct Int32Quat *a2c, struct Int32Quat *a2b, struct Int32Quat *b2c) { a2c->qi = (a2b->qi * b2c->qi - a2b->qx * b2c->qx - a2b->qy * b2c->qy - a2b->qz * b2c->qz) >> INT32_QUAT_FRAC; a2c->qx = (a2b->qi * b2c->qx + a2b->qx * b2c->qi + a2b->qy * b2c->qz - a2b->qz * b2c->qy) >> INT32_QUAT_FRAC; @@ -258,7 +258,7 @@ void int32_quat_comp(struct Int32Quat* a2c, struct Int32Quat* a2b, struct Int32Q a2c->qz = (a2b->qi * b2c->qz + a2b->qx * b2c->qy - a2b->qy * b2c->qx + a2b->qz * b2c->qi) >> INT32_QUAT_FRAC; } -void int32_quat_comp_inv(struct Int32Quat* a2b, struct Int32Quat* a2c, struct Int32Quat* b2c) +void int32_quat_comp_inv(struct Int32Quat *a2b, struct Int32Quat *a2c, struct Int32Quat *b2c) { a2b->qi = (a2c->qi * b2c->qi + a2c->qx * b2c->qx + a2c->qy * b2c->qy + a2c->qz * b2c->qz) >> INT32_QUAT_FRAC; a2b->qx = (-a2c->qi * b2c->qx + a2c->qx * b2c->qi - a2c->qy * b2c->qz + a2c->qz * b2c->qy) >> INT32_QUAT_FRAC; @@ -266,7 +266,7 @@ void int32_quat_comp_inv(struct Int32Quat* a2b, struct Int32Quat* a2c, struct In a2b->qz = (-a2c->qi * b2c->qz - a2c->qx * b2c->qy + a2c->qy * b2c->qx + a2c->qz * b2c->qi) >> INT32_QUAT_FRAC; } -void int32_quat_inv_comp(struct Int32Quat* b2c, struct Int32Quat* a2b, struct Int32Quat* a2c) +void int32_quat_inv_comp(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c) { b2c->qi = (a2b->qi * a2c->qi + a2b->qx * a2c->qx + a2b->qy * a2c->qy + a2b->qz * a2c->qz) >> INT32_QUAT_FRAC; b2c->qx = (a2b->qi * a2c->qx - a2b->qx * a2c->qi - a2b->qy * a2c->qz + a2b->qz * a2c->qy) >> INT32_QUAT_FRAC; @@ -274,21 +274,21 @@ void int32_quat_inv_comp(struct Int32Quat* b2c, struct Int32Quat* a2b, struct In b2c->qz = (a2b->qi * a2c->qz - a2b->qx * a2c->qy + a2b->qy * a2c->qx - a2b->qz * a2c->qi) >> INT32_QUAT_FRAC; } -void int32_quat_comp_norm_shortest(struct Int32Quat* a2c, struct Int32Quat* a2b, struct Int32Quat* b2c) +void int32_quat_comp_norm_shortest(struct Int32Quat *a2c, struct Int32Quat *a2b, struct Int32Quat *b2c) { int32_quat_comp(a2c, a2b, b2c); int32_quat_wrap_shortest(a2c); int32_quat_normalize(a2c); } -void int32_quat_comp_inv_norm_shortest(struct Int32Quat* a2b, struct Int32Quat* a2c, struct Int32Quat* b2c) +void int32_quat_comp_inv_norm_shortest(struct Int32Quat *a2b, struct Int32Quat *a2c, struct Int32Quat *b2c) { int32_quat_comp_inv(a2b, a2c, b2c); int32_quat_wrap_shortest(a2b); int32_quat_normalize(a2b); } -void int32_quat_inv_comp_norm_shortest(struct Int32Quat* b2c, struct Int32Quat* a2b, struct Int32Quat* a2c) +void int32_quat_inv_comp_norm_shortest(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c) { int32_quat_inv_comp(b2c, a2b, a2c); int32_quat_wrap_shortest(b2c); @@ -301,7 +301,7 @@ void int32_quat_inv_comp_norm_shortest(struct Int32Quat* b2c, struct Int32Quat* * qd = 0.5 * q * omega(r) * Multiplication with 0.5 is done by shifting one more bit to the right. */ -void int32_quat_derivative(struct Int32Quat* qd, const struct Int32Rates* r, struct Int32Quat* q) +void int32_quat_derivative(struct Int32Quat *qd, const struct Int32Rates *r, struct Int32Quat *q) { qd->qi = (-(r->p * q->qx + r->q * q->qy + r->r * q->qz)) >> (INT32_RATE_FRAC + 1); qd->qx = (-(-r->p * q->qi - r->r * q->qy + r->q * q->qz)) >> (INT32_RATE_FRAC + 1); @@ -310,7 +310,7 @@ void int32_quat_derivative(struct Int32Quat* qd, const struct Int32Rates* r, str } /** in place quaternion first order integration with constant rotational velocity. */ -void int32_quat_integrate_fi(struct Int32Quat* q, struct Int64Quat* hr, struct Int32Rates* omega, int freq) +void int32_quat_integrate_fi(struct Int32Quat *q, struct Int64Quat *hr, struct Int32Rates *omega, int freq) { hr->qi += - ((int64_t) omega->p) * q->qx - ((int64_t) omega->q) * q->qy - ((int64_t) omega->r) * q->qz; hr->qx += ((int64_t) omega->p) * q->qi + ((int64_t) omega->r) * q->qy - ((int64_t) omega->q) * q->qz; @@ -334,7 +334,7 @@ void int32_quat_integrate_fi(struct Int32Quat* q, struct Int64Quat* hr, struct I hr->qz = _div.rem; } -void int32_quat_vmult(struct Int32Vect3* v_out, struct Int32Quat* q, struct Int32Vect3* v_in) +void int32_quat_vmult(struct Int32Vect3 *v_out, struct Int32Quat *q, struct Int32Vect3 *v_in) { const int32_t _2qi2_m1 = ((q->qi * q->qi) >> (INT32_QUAT_FRAC - 1)) - QUAT1_BFP_OF_REAL(1); const int32_t _2qx2 = (q->qx * q->qx) >> (INT32_QUAT_FRAC - 1); @@ -356,7 +356,7 @@ void int32_quat_vmult(struct Int32Vect3* v_out, struct Int32Quat* q, struct Int3 /* * http://www.mathworks.com/access/helpdesk_r13/help/toolbox/aeroblks/euleranglestoquaternions.html */ -void int32_quat_of_eulers(struct Int32Quat* q, struct Int32Eulers* e) +void int32_quat_of_eulers(struct Int32Quat *q, struct Int32Eulers *e) { const int32_t phi2 = e->phi / 2; const int32_t theta2 = e->theta / 2; @@ -390,7 +390,7 @@ void int32_quat_of_eulers(struct Int32Quat* q, struct Int32Eulers* e) INT_MULT_RSHIFT(-s_phi2, s_th_c_ps, INT32_TRIG_FRAC + INT32_TRIG_FRAC - INT32_QUAT_FRAC); } -void int32_quat_of_axis_angle(struct Int32Quat* q, struct Int32Vect3* uv, int32_t angle) +void int32_quat_of_axis_angle(struct Int32Quat *q, struct Int32Vect3 *uv, int32_t angle) { int32_t san2; PPRZ_ITRIG_SIN(san2, (angle / 2)); @@ -402,7 +402,7 @@ void int32_quat_of_axis_angle(struct Int32Quat* q, struct Int32Vect3* uv, int32_ q->qz = san2 * uv->z; } -void int32_quat_of_rmat(struct Int32Quat* q, struct Int32RMat* r) +void int32_quat_of_rmat(struct Int32Quat *q, struct Int32RMat *r) { const int32_t tr = RMAT_TRACE(*r); if (tr > 0) { @@ -477,7 +477,7 @@ void int32_quat_of_rmat(struct Int32Quat* q, struct Int32RMat* r) * */ -void int32_eulers_of_rmat(struct Int32Eulers* e, struct Int32RMat* rm) +void int32_eulers_of_rmat(struct Int32Eulers *e, struct Int32RMat *rm) { const float dcm00 = TRIG_FLOAT_OF_BFP(rm->m[0]); const float dcm01 = TRIG_FLOAT_OF_BFP(rm->m[1]); @@ -492,7 +492,7 @@ void int32_eulers_of_rmat(struct Int32Eulers* e, struct Int32RMat* rm) e->psi = ANGLE_BFP_OF_REAL(psi); } -void int32_eulers_of_quat(struct Int32Eulers* e, struct Int32Quat* q) +void int32_eulers_of_quat(struct Int32Eulers *e, struct Int32Quat *q) { const int32_t qx2 = INT_MULT_RSHIFT(q->qx, q->qx, INT32_QUAT_FRAC); const int32_t qy2 = INT_MULT_RSHIFT(q->qy, q->qy, INT32_QUAT_FRAC); @@ -542,7 +542,7 @@ void int32_eulers_of_quat(struct Int32Eulers* e, struct Int32Quat* q) * */ -void int32_rates_of_eulers_dot_321(struct Int32Rates* r, struct Int32Eulers* e, struct Int32Eulers* ed) +void int32_rates_of_eulers_dot_321(struct Int32Rates *r, struct Int32Eulers *e, struct Int32Eulers *ed) { int32_t sphi; PPRZ_ITRIG_SIN(sphi, e->phi); @@ -561,7 +561,7 @@ void int32_rates_of_eulers_dot_321(struct Int32Rates* r, struct Int32Eulers* e, r->r = INT_MULT_RSHIFT(cphi_ctheta, ed->psi, INT32_TRIG_FRAC) - INT_MULT_RSHIFT(sphi, ed->theta, INT32_TRIG_FRAC); } -void int32_eulers_dot_321_of_rates(struct Int32Eulers* ed, struct Int32Eulers* e, struct Int32Rates* r) +void int32_eulers_dot_321_of_rates(struct Int32Eulers *ed, struct Int32Eulers *e, struct Int32Rates *r) { int32_t sphi; PPRZ_ITRIG_SIN(sphi, e->phi); diff --git a/sw/airborne/math/pprz_algebra_int.h b/sw/airborne/math/pprz_algebra_int.h index f24c55c1bc..a75dd1cfe3 100644 --- a/sw/airborne/math/pprz_algebra_int.h +++ b/sw/airborne/math/pprz_algebra_int.h @@ -235,19 +235,19 @@ extern uint32_t int32_sqrt(uint32_t in); #define INT32_VECT2_NORM(_v) int32_sqrt(VECT2_NORM2(_v)) /** return squared norm of 2D vector */ -static inline uint32_t int32_vect2_norm2(struct Int32Vect2* v) +static inline uint32_t int32_vect2_norm2(struct Int32Vect2 *v) { return v->x * v->x + v->y * v->y; } /** return norm of 2D vector */ -static inline uint32_t int32_vect2_norm(struct Int32Vect2* v) +static inline uint32_t int32_vect2_norm(struct Int32Vect2 *v) { return int32_sqrt(int32_vect2_norm2(v)); } /** normalize 2D vector inplace */ -static inline void int32_vect2_normalize(struct Int32Vect2* v, uint8_t frac) +static inline void int32_vect2_normalize(struct Int32Vect2 *v, uint8_t frac) { const uint32_t n = int32_vect2_norm(v); if (n > 0) { @@ -338,7 +338,7 @@ static inline void int32_vect2_normalize(struct Int32Vect2* v, uint8_t frac) */ /** initialises a rotation matrix to identity */ -static inline void int32_rmat_identity(struct Int32RMat* rm) +static inline void int32_rmat_identity(struct Int32RMat *rm) { INT32_MAT33_DIAG(*rm, TRIG_BFP_OF_REAL(1.), TRIG_BFP_OF_REAL(1.), TRIG_BFP_OF_REAL(1.)); } @@ -346,41 +346,41 @@ static inline void int32_rmat_identity(struct Int32RMat* rm) /** Composition (multiplication) of two rotation matrices. * m_a2c = m_a2b comp m_b2c , aka m_a2c = m_b2c * m_a2b */ -extern void int32_rmat_comp(struct Int32RMat* m_a2c, struct Int32RMat* m_a2b, - struct Int32RMat* m_b2c); +extern void int32_rmat_comp(struct Int32RMat *m_a2c, struct Int32RMat *m_a2b, + struct Int32RMat *m_b2c); /** Composition (multiplication) of two rotation matrices. * m_a2b = m_a2c comp_inv m_b2c , aka m_a2b = inv(_m_b2c) * m_a2c */ -extern void int32_rmat_comp_inv(struct Int32RMat* m_a2b, struct Int32RMat* m_a2c, - struct Int32RMat* m_b2c); +extern void int32_rmat_comp_inv(struct Int32RMat *m_a2b, struct Int32RMat *m_a2c, + struct Int32RMat *m_b2c); /** rotate 3D vector by rotation matrix. * vb = m_a2b * va */ -extern void int32_rmat_vmult(struct Int32Vect3* vb, struct Int32RMat* m_a2b, - struct Int32Vect3* va); +extern void int32_rmat_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_a2b, + struct Int32Vect3 *va); /** rotate 3D vector by transposed rotation matrix. * vb = m_b2a^T * va */ -extern void int32_rmat_transp_vmult(struct Int32Vect3* vb, struct Int32RMat* m_b2a, - struct Int32Vect3* va); +extern void int32_rmat_transp_vmult(struct Int32Vect3 *vb, struct Int32RMat *m_b2a, + struct Int32Vect3 *va); /** rotate anglular rates by rotation matrix. * rb = m_a2b * ra */ -extern void int32_rmat_ratemult(struct Int32Rates* rb, struct Int32RMat* m_a2b, - struct Int32Rates* ra); +extern void int32_rmat_ratemult(struct Int32Rates *rb, struct Int32RMat *m_a2b, + struct Int32Rates *ra); /** rotate anglular rates by transposed rotation matrix. * rb = m_b2a^T * ra */ -extern void int32_rmat_transp_ratemult(struct Int32Rates* rb, struct Int32RMat* m_b2a, - struct Int32Rates* ra); +extern void int32_rmat_transp_ratemult(struct Int32Rates *rb, struct Int32RMat *m_b2a, + struct Int32Rates *ra); /// Convert unit quaternion to rotation matrix. -extern void int32_rmat_of_quat(struct Int32RMat* rm, struct Int32Quat* q); +extern void int32_rmat_of_quat(struct Int32RMat *rm, struct Int32Quat *q); /** Rotation matrix from 321 Euler angles (int). * The Euler angles are interpreted as zy'x'' (intrinsic) rotation. @@ -394,10 +394,10 @@ extern void int32_rmat_of_quat(struct Int32RMat* rm, struct Int32Quat* q); * @param[out] rm pointer to rotation matrix * @param[in] e pointer to Euler angles */ -extern void int32_rmat_of_eulers_321(struct Int32RMat* rm, struct Int32Eulers* e); +extern void int32_rmat_of_eulers_321(struct Int32RMat *rm, struct Int32Eulers *e); /// Rotation matrix from 312 Euler angles. -extern void int32_rmat_of_eulers_312(struct Int32RMat* rm, struct Int32Eulers* e); +extern void int32_rmat_of_eulers_312(struct Int32RMat *rm, struct Int32Eulers *e); /// Rotation matrix from Euler angles. #define int32_rmat_of_eulers int32_rmat_of_eulers_321 @@ -423,7 +423,7 @@ extern void int32_rmat_of_eulers_312(struct Int32RMat* rm, struct Int32Eulers* e */ /** initialises a quaternion to identity */ -static inline void int32_quat_identity(struct Int32Quat* q) +static inline void int32_quat_identity(struct Int32Quat *q) { q->qi = QUAT1_BFP_OF_REAL(1); q->qx = 0; @@ -433,13 +433,13 @@ static inline void int32_quat_identity(struct Int32Quat* q) /** Norm of a quaternion. */ -static inline uint32_t int32_quat_norm(struct Int32Quat* q) +static inline uint32_t int32_quat_norm(struct Int32Quat *q) { uint32_t n2 = q->qi * q->qi + q->qx * q->qx + q->qy * q->qy + q->qz * q->qz; return int32_sqrt(n2); } -static inline void int32_quat_wrap_shortest(struct Int32Quat* q) +static inline void int32_quat_wrap_shortest(struct Int32Quat *q) { if (q->qi < 0) { QUAT_EXPLEMENTARY(*q, *q); @@ -447,7 +447,7 @@ static inline void int32_quat_wrap_shortest(struct Int32Quat* q) } /** normalize a quaternion inplace */ -static inline void int32_quat_normalize(struct Int32Quat* q) +static inline void int32_quat_normalize(struct Int32Quat *q) { int32_t n = int32_quat_norm(q); if (n > 0) { @@ -461,56 +461,56 @@ static inline void int32_quat_normalize(struct Int32Quat* q) /** Composition (multiplication) of two quaternions. * a2c = a2b comp b2c , aka a2c = a2b * b2c */ -extern void int32_quat_comp(struct Int32Quat* a2c, struct Int32Quat* a2b, struct Int32Quat* b2c); +extern void int32_quat_comp(struct Int32Quat *a2c, struct Int32Quat *a2b, struct Int32Quat *b2c); /** Composition (multiplication) of two quaternions. * a2b = a2c comp_inv b2c , aka a2b = a2c * inv(b2c) */ -extern void int32_quat_comp_inv(struct Int32Quat* a2b, struct Int32Quat* a2c, struct Int32Quat* b2c); +extern void int32_quat_comp_inv(struct Int32Quat *a2b, struct Int32Quat *a2c, struct Int32Quat *b2c); /** Composition (multiplication) of two quaternions. * b2c = a2b inv_comp a2c , aka b2c = inv(_a2b) * a2c */ -extern void int32_quat_inv_comp(struct Int32Quat* b2c, struct Int32Quat* a2b, struct Int32Quat* a2c); +extern void int32_quat_inv_comp(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c); /** Composition (multiplication) of two quaternions with normalization. * a2c = a2b comp b2c , aka a2c = a2b * b2c */ -extern void int32_quat_comp_norm_shortest(struct Int32Quat* a2c, struct Int32Quat* a2b, struct Int32Quat* b2c); +extern void int32_quat_comp_norm_shortest(struct Int32Quat *a2c, struct Int32Quat *a2b, struct Int32Quat *b2c); /** Composition (multiplication) of two quaternions with normalization. * a2b = a2c comp_inv b2c , aka a2b = a2c * inv(b2c) */ -extern void int32_quat_comp_inv_norm_shortest(struct Int32Quat* a2b, struct Int32Quat* a2c, struct Int32Quat* b2c); +extern void int32_quat_comp_inv_norm_shortest(struct Int32Quat *a2b, struct Int32Quat *a2c, struct Int32Quat *b2c); /** Composition (multiplication) of two quaternions with normalization. * b2c = a2b inv_comp a2c , aka b2c = inv(_a2b) * a2c */ -extern void int32_quat_inv_comp_norm_shortest(struct Int32Quat* b2c, struct Int32Quat* a2b, struct Int32Quat* a2c); +extern void int32_quat_inv_comp_norm_shortest(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c); /** Quaternion derivative from rotational velocity. * qd = -0.5*omega(r) * q * or equally: * qd = 0.5 * q * omega(r) */ -extern void int32_quat_derivative(struct Int32Quat* qd, const struct Int32Rates* r, struct Int32Quat* q); +extern void int32_quat_derivative(struct Int32Quat *qd, const struct Int32Rates *r, struct Int32Quat *q); /** in place quaternion first order integration with constant rotational velocity. */ -extern void int32_quat_integrate_fi(struct Int32Quat* q, struct Int64Quat* hr, struct Int32Rates* omega, int freq); +extern void int32_quat_integrate_fi(struct Int32Quat *q, struct Int64Quat *hr, struct Int32Rates *omega, int freq); /** rotate 3D vector by quaternion. * vb = q_a2b * va * q_a2b^-1 */ -extern void int32_quat_vmult(struct Int32Vect3* v_out, struct Int32Quat* q, struct Int32Vect3* v_in); +extern void int32_quat_vmult(struct Int32Vect3 *v_out, struct Int32Quat *q, struct Int32Vect3 *v_in); /// Quaternion from Euler angles. -extern void int32_quat_of_eulers(struct Int32Quat* q, struct Int32Eulers* e); +extern void int32_quat_of_eulers(struct Int32Quat *q, struct Int32Eulers *e); /// Quaternion from unit vector and angle. -extern void int32_quat_of_axis_angle(struct Int32Quat* q, struct Int32Vect3* uv, int32_t angle); +extern void int32_quat_of_axis_angle(struct Int32Quat *q, struct Int32Vect3 *uv, int32_t angle); /// Quaternion from rotation matrix. -extern void int32_quat_of_rmat(struct Int32Quat* q, struct Int32RMat* r); +extern void int32_quat_of_rmat(struct Int32Quat *q, struct Int32RMat *r); /* defines for backwards compatibility */ #define INT32_QUAT_ZERO(_q) int32_quat_identity(&(_q)) @@ -538,8 +538,8 @@ extern void int32_quat_of_rmat(struct Int32Quat* q, struct Int32RMat* r); #define INT_EULERS_ZERO(_e) EULERS_ASSIGN(_e, 0, 0, 0) -extern void int32_eulers_of_rmat(struct Int32Eulers* e, struct Int32RMat* rm); -extern void int32_eulers_of_quat(struct Int32Eulers* e, struct Int32Quat* q); +extern void int32_eulers_of_rmat(struct Int32Eulers *e, struct Int32RMat *rm); +extern void int32_eulers_of_quat(struct Int32Eulers *e, struct Int32Quat *q); /* defines for backwards compatibility */ #define INT32_EULERS_OF_RMAT(_e, _rm) int32_eulers_of_rmat(&(_e), &(_rm)) @@ -577,8 +577,8 @@ extern void int32_eulers_of_quat(struct Int32Eulers* e, struct Int32Quat* q); } -extern void int32_rates_of_eulers_dot_321(struct Int32Rates* r, struct Int32Eulers* e, struct Int32Eulers* ed); -extern void int32_eulers_dot_321_of_rates(struct Int32Eulers* ed, struct Int32Eulers* e, struct Int32Rates* r); +extern void int32_rates_of_eulers_dot_321(struct Int32Rates *r, struct Int32Eulers *e, struct Int32Eulers *ed); +extern void int32_eulers_dot_321_of_rates(struct Int32Eulers *ed, struct Int32Eulers *e, struct Int32Rates *r); #define int32_eulers_dot_of_rates int32_eulers_dot_321_of_rates diff --git a/sw/airborne/math/pprz_geodetic_double.c b/sw/airborne/math/pprz_geodetic_double.c index 9961b5e3bc..69da99dcef 100644 --- a/sw/airborne/math/pprz_geodetic_double.c +++ b/sw/airborne/math/pprz_geodetic_double.c @@ -31,7 +31,7 @@ #include "std.h" /* for RadOfDeg */ -void ltp_def_from_ecef_d(struct LtpDef_d* def, struct EcefCoor_d* ecef) +void ltp_def_from_ecef_d(struct LtpDef_d *def, struct EcefCoor_d *ecef) { /* store the origin of the tangeant plane */ @@ -56,7 +56,7 @@ void ltp_def_from_ecef_d(struct LtpDef_d* def, struct EcefCoor_d* ecef) } /* http://en.wikipedia.org/wiki/Geodetic_system */ -void lla_of_ecef_d(struct LlaCoor_d* lla, struct EcefCoor_d* ecef) +void lla_of_ecef_d(struct LlaCoor_d *lla, struct EcefCoor_d *ecef) { // FIXME : make an ellipsoid struct @@ -93,7 +93,7 @@ void lla_of_ecef_d(struct LlaCoor_d* lla, struct EcefCoor_d* ecef) } -void ecef_of_lla_d(struct EcefCoor_d* ecef, struct LlaCoor_d* lla) +void ecef_of_lla_d(struct EcefCoor_d *ecef, struct LlaCoor_d *lla) { // FIXME : make an ellipsoid struct @@ -113,26 +113,26 @@ void ecef_of_lla_d(struct EcefCoor_d* ecef, struct LlaCoor_d* lla) ecef->z = (a_chi * (1. - e2) + lla->alt) * sin_lat; } -void enu_of_ecef_point_d(struct EnuCoor_d* enu, struct LtpDef_d* def, struct EcefCoor_d* ecef) +void enu_of_ecef_point_d(struct EnuCoor_d *enu, struct LtpDef_d *def, struct EcefCoor_d *ecef) { struct EcefCoor_d delta; VECT3_DIFF(delta, *ecef, def->ecef); MAT33_VECT3_MUL(*enu, def->ltp_of_ecef, delta); } -void ned_of_ecef_point_d(struct NedCoor_d* ned, struct LtpDef_d* def, struct EcefCoor_d* ecef) +void ned_of_ecef_point_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef) { struct EnuCoor_d enu; enu_of_ecef_point_d(&enu, def, ecef); ENU_OF_TO_NED(*ned, enu); } -void enu_of_ecef_vect_d(struct EnuCoor_d* enu, struct LtpDef_d* def, struct EcefCoor_d* ecef) +void enu_of_ecef_vect_d(struct EnuCoor_d *enu, struct LtpDef_d *def, struct EcefCoor_d *ecef) { MAT33_VECT3_MUL(*enu, def->ltp_of_ecef, *ecef); } -void ned_of_ecef_vect_d(struct NedCoor_d* ned, struct LtpDef_d* def, struct EcefCoor_d* ecef) +void ned_of_ecef_vect_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef) { struct EnuCoor_d enu; enu_of_ecef_vect_d(&enu, def, ecef); @@ -141,25 +141,25 @@ void ned_of_ecef_vect_d(struct NedCoor_d* ned, struct LtpDef_d* def, struct Ecef -void ecef_of_enu_point_d(struct EcefCoor_d* ecef, struct LtpDef_d* def, struct EnuCoor_d* enu) +void ecef_of_enu_point_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct EnuCoor_d *enu) { MAT33_VECT3_TRANSP_MUL(*ecef, def->ltp_of_ecef, *enu); VECT3_ADD(*ecef, def->ecef); } -void ecef_of_ned_point_d(struct EcefCoor_d* ecef, struct LtpDef_d* def, struct NedCoor_d* ned) +void ecef_of_ned_point_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct NedCoor_d *ned) { struct EnuCoor_d enu; ENU_OF_TO_NED(enu, *ned); ecef_of_enu_point_d(ecef, def, &enu); } -void ecef_of_enu_vect_d(struct EcefCoor_d* ecef, struct LtpDef_d* def, struct EnuCoor_d* enu) +void ecef_of_enu_vect_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct EnuCoor_d *enu) { MAT33_VECT3_TRANSP_MUL(*ecef, def->ltp_of_ecef, *enu); } -void ecef_of_ned_vect_d(struct EcefCoor_d* ecef, struct LtpDef_d* def, struct NedCoor_d* ned) +void ecef_of_ned_vect_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct NedCoor_d *ned) { struct EnuCoor_d enu; ENU_OF_TO_NED(enu, *ned); @@ -167,14 +167,14 @@ void ecef_of_ned_vect_d(struct EcefCoor_d* ecef, struct LtpDef_d* def, struct Ne } -void enu_of_lla_point_d(struct EnuCoor_d* enu, struct LtpDef_d* def, struct LlaCoor_d* lla) +void enu_of_lla_point_d(struct EnuCoor_d *enu, struct LtpDef_d *def, struct LlaCoor_d *lla) { struct EcefCoor_d ecef; ecef_of_lla_d(&ecef, lla); enu_of_ecef_point_d(enu, def, &ecef); } -void ned_of_lla_point_d(struct NedCoor_d* ned, struct LtpDef_d* def, struct LlaCoor_d* lla) +void ned_of_lla_point_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct LlaCoor_d *lla) { struct EcefCoor_d ecef; ecef_of_lla_d(&ecef, lla); @@ -245,7 +245,7 @@ static inline double inverse_isometric_latitude_d(double lat, double e, double e CI(v); \ } -void lla_of_utm_d(struct LlaCoor_d* lla, struct UtmCoor_d* utm) +void lla_of_utm_d(struct LlaCoor_d *lla, struct UtmCoor_d *utm) { struct DoubleVect2 v = {utm->north - DELTA_NORTH, utm->east - DELTA_EAST}; diff --git a/sw/airborne/math/pprz_geodetic_double.h b/sw/airborne/math/pprz_geodetic_double.h index dde6c7b8d7..2c62e31d74 100644 --- a/sw/airborne/math/pprz_geodetic_double.h +++ b/sw/airborne/math/pprz_geodetic_double.h @@ -97,25 +97,25 @@ struct LtpDef_d { double hmsl; ///< height in meters above mean sea level }; -extern void lla_of_utm_d(struct LlaCoor_d* out, struct UtmCoor_d* in); -extern void ltp_def_from_ecef_d(struct LtpDef_d* def, struct EcefCoor_d* ecef); -extern void lla_of_ecef_d(struct LlaCoor_d* out, struct EcefCoor_d* in); -extern void ecef_of_lla_d(struct EcefCoor_d* out, struct LlaCoor_d* in); +extern void lla_of_utm_d(struct LlaCoor_d *out, struct UtmCoor_d *in); +extern void ltp_def_from_ecef_d(struct LtpDef_d *def, struct EcefCoor_d *ecef); +extern void lla_of_ecef_d(struct LlaCoor_d *out, struct EcefCoor_d *in); +extern void ecef_of_lla_d(struct EcefCoor_d *out, struct LlaCoor_d *in); -extern void enu_of_ecef_point_d(struct EnuCoor_d* ned, struct LtpDef_d* def, struct EcefCoor_d* ecef); -extern void ned_of_ecef_point_d(struct NedCoor_d* ned, struct LtpDef_d* def, struct EcefCoor_d* ecef); +extern void enu_of_ecef_point_d(struct EnuCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef); +extern void ned_of_ecef_point_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef); -extern void enu_of_ecef_vect_d(struct EnuCoor_d* ned, struct LtpDef_d* def, struct EcefCoor_d* ecef); -extern void ned_of_ecef_vect_d(struct NedCoor_d* ned, struct LtpDef_d* def, struct EcefCoor_d* ecef); +extern void enu_of_ecef_vect_d(struct EnuCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef); +extern void ned_of_ecef_vect_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef); -extern void ecef_of_enu_point_d(struct EcefCoor_d* ecef, struct LtpDef_d* def, struct EnuCoor_d* enu); -extern void ecef_of_ned_point_d(struct EcefCoor_d* ecef, struct LtpDef_d* def, struct NedCoor_d* ned); +extern void ecef_of_enu_point_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct EnuCoor_d *enu); +extern void ecef_of_ned_point_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct NedCoor_d *ned); -extern void ecef_of_enu_vect_d(struct EcefCoor_d* ecef, struct LtpDef_d* def, struct EnuCoor_d* enu); -extern void ecef_of_ned_vect_d(struct EcefCoor_d* ecef, struct LtpDef_d* def, struct NedCoor_d* ned); +extern void ecef_of_enu_vect_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct EnuCoor_d *enu); +extern void ecef_of_ned_vect_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct NedCoor_d *ned); -extern void enu_of_lla_point_d(struct EnuCoor_d* enu, struct LtpDef_d* def, struct LlaCoor_d* lla); -extern void ned_of_lla_point_d(struct NedCoor_d* ned, struct LtpDef_d* def, struct LlaCoor_d* lla); +extern void enu_of_lla_point_d(struct EnuCoor_d *enu, struct LtpDef_d *def, struct LlaCoor_d *lla); +extern void ned_of_lla_point_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct LlaCoor_d *lla); extern double gc_of_gd_lat_d(double gd_lat, double hmsl); diff --git a/sw/airborne/math/pprz_geodetic_float.c b/sw/airborne/math/pprz_geodetic_float.c index 350ee527d9..6c254f82e1 100644 --- a/sw/airborne/math/pprz_geodetic_float.c +++ b/sw/airborne/math/pprz_geodetic_float.c @@ -33,7 +33,7 @@ /* for ecef_of_XX functions the double versions are needed */ #include "pprz_geodetic_double.h" -void ltp_def_from_ecef_f(struct LtpDef_f* def, struct EcefCoor_f* ecef) +void ltp_def_from_ecef_f(struct LtpDef_f *def, struct EcefCoor_f *ecef) { /* store the origin of the tangeant plane */ @@ -58,7 +58,7 @@ void ltp_def_from_ecef_f(struct LtpDef_f* def, struct EcefCoor_f* ecef) } -void ltp_def_from_lla_f(struct LtpDef_f* def, struct LlaCoor_f* lla) +void ltp_def_from_lla_f(struct LtpDef_f *def, struct LlaCoor_f *lla) { /* store the origin of the tangeant plane */ LLA_COPY(def->lla, *lla); @@ -83,14 +83,14 @@ void ltp_def_from_lla_f(struct LtpDef_f* def, struct LlaCoor_f* lla) def->ltp_of_ecef.m[8] = sin_lat; } -void enu_of_ecef_point_f(struct EnuCoor_f* enu, struct LtpDef_f* def, struct EcefCoor_f* ecef) +void enu_of_ecef_point_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct EcefCoor_f *ecef) { struct EcefCoor_f delta; VECT3_DIFF(delta, *ecef, def->ecef); MAT33_VECT3_MUL(*enu, def->ltp_of_ecef, delta); } -void ned_of_ecef_point_f(struct NedCoor_f* ned, struct LtpDef_f* def, struct EcefCoor_f* ecef) +void ned_of_ecef_point_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef) { struct EnuCoor_f enu; enu_of_ecef_point_f(&enu, def, ecef); @@ -98,26 +98,26 @@ void ned_of_ecef_point_f(struct NedCoor_f* ned, struct LtpDef_f* def, struct Ece } -void enu_of_ecef_vect_f(struct EnuCoor_f* enu, struct LtpDef_f* def, struct EcefCoor_f* ecef) +void enu_of_ecef_vect_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct EcefCoor_f *ecef) { MAT33_VECT3_MUL(*enu, def->ltp_of_ecef, *ecef); } -void ned_of_ecef_vect_f(struct NedCoor_f* ned, struct LtpDef_f* def, struct EcefCoor_f* ecef) +void ned_of_ecef_vect_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef) { struct EnuCoor_f enu; enu_of_ecef_vect_f(&enu, def, ecef); ENU_OF_TO_NED(*ned, enu); } -void enu_of_lla_point_f(struct EnuCoor_f* enu, struct LtpDef_f* def, struct LlaCoor_f* lla) +void enu_of_lla_point_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct LlaCoor_f *lla) { struct EcefCoor_f ecef; ecef_of_lla_f(&ecef, lla); enu_of_ecef_point_f(enu, def, &ecef); } -void ned_of_lla_point_f(struct NedCoor_f* ned, struct LtpDef_f* def, struct LlaCoor_f* lla) +void ned_of_lla_point_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct LlaCoor_f *lla) { struct EcefCoor_f ecef; ecef_of_lla_f(&ecef, lla); @@ -127,7 +127,7 @@ void ned_of_lla_point_f(struct NedCoor_f* ned, struct LtpDef_f* def, struct LlaC /* * not enought precision with float - use double */ -void ecef_of_enu_point_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct EnuCoor_f* enu) +void ecef_of_enu_point_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct EnuCoor_f *enu) { /* convert used floats to double */ struct DoubleRMat ltp_of_ecef_d; @@ -155,14 +155,14 @@ void ecef_of_enu_point_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct E ecef->z = (float) ecef_d.z + def->ecef.z; } -void ecef_of_ned_point_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct NedCoor_f* ned) +void ecef_of_ned_point_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct NedCoor_f *ned) { struct EnuCoor_f enu; ENU_OF_TO_NED(enu, *ned); ecef_of_enu_point_f(ecef, def, &enu); } -void ecef_of_enu_vect_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct EnuCoor_f* enu) +void ecef_of_enu_vect_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct EnuCoor_f *enu) { /* convert used floats to double */ struct DoubleRMat ltp_of_ecef_d; @@ -190,7 +190,7 @@ void ecef_of_enu_vect_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct En ecef->z = (float) ecef_d.z; } -void ecef_of_ned_vect_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct NedCoor_f* ned) +void ecef_of_ned_vect_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct NedCoor_f *ned) { struct EnuCoor_f enu; ENU_OF_TO_NED(enu, *ned); @@ -202,7 +202,7 @@ void ecef_of_ned_vect_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct Ne /* http://en.wikipedia.org/wiki/Geodetic_system */ -void lla_of_ecef_f(struct LlaCoor_f* out, struct EcefCoor_f* in) +void lla_of_ecef_f(struct LlaCoor_f *out, struct EcefCoor_f *in) { // FIXME : make an ellipsoid struct @@ -239,7 +239,7 @@ void lla_of_ecef_f(struct LlaCoor_f* out, struct EcefCoor_f* in) } -void ecef_of_lla_f(struct EcefCoor_f* out, struct LlaCoor_f* in) +void ecef_of_lla_f(struct EcefCoor_f *out, struct LlaCoor_f *in) { // FIXME : make an ellipsoid struct @@ -301,7 +301,7 @@ static inline float inverse_isometric_latitude_f(float lat, float e, float epsil return phi0; } -void utm_of_lla_f(struct UtmCoor_f* utm, struct LlaCoor_f* lla) +void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla) { float lambda_c = LambdaOfUtmZone(utm->zone); float ll = isometric_latitude_f(lla->lat , E); @@ -327,7 +327,7 @@ void utm_of_lla_f(struct UtmCoor_f* utm, struct LlaCoor_f* lla) utm->alt = lla->alt; } -void lla_of_utm_f(struct LlaCoor_f* lla, struct UtmCoor_f* utm) +void lla_of_utm_f(struct LlaCoor_f *lla, struct UtmCoor_f *utm) { float scale = 1 / N / serie_coeff_proj_mercator[0]; float real = (utm->north - DELTA_NORTH) * scale; diff --git a/sw/airborne/math/pprz_geodetic_float.h b/sw/airborne/math/pprz_geodetic_float.h index 2f74221f24..1ebb61214a 100644 --- a/sw/airborne/math/pprz_geodetic_float.h +++ b/sw/airborne/math/pprz_geodetic_float.h @@ -97,24 +97,24 @@ struct LtpDef_f { float hmsl; ///< Height above mean sea level in meters }; -extern void lla_of_utm_f(struct LlaCoor_f* lla, struct UtmCoor_f* utm); -extern void utm_of_lla_f(struct UtmCoor_f* utm, struct LlaCoor_f* lla); -extern void ltp_def_from_ecef_f(struct LtpDef_f* def, struct EcefCoor_f* ecef); -extern void ltp_def_from_lla_f(struct LtpDef_f* def, struct LlaCoor_f* lla); -extern void lla_of_ecef_f(struct LlaCoor_f* out, struct EcefCoor_f* in); -extern void ecef_of_lla_f(struct EcefCoor_f* out, struct LlaCoor_f* in); -extern void enu_of_ecef_point_f(struct EnuCoor_f* enu, struct LtpDef_f* def, struct EcefCoor_f* ecef); -extern void ned_of_ecef_point_f(struct NedCoor_f* ned, struct LtpDef_f* def, struct EcefCoor_f* ecef); -extern void enu_of_ecef_vect_f(struct EnuCoor_f* enu, struct LtpDef_f* def, struct EcefCoor_f* ecef); -extern void ned_of_ecef_vect_f(struct NedCoor_f* ned, struct LtpDef_f* def, struct EcefCoor_f* ecef); -extern void enu_of_lla_point_f(struct EnuCoor_f* enu, struct LtpDef_f* def, struct LlaCoor_f* lla); -extern void ned_of_lla_point_f(struct NedCoor_f* ned, struct LtpDef_f* def, struct LlaCoor_f* lla); +extern void lla_of_utm_f(struct LlaCoor_f *lla, struct UtmCoor_f *utm); +extern void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla); +extern void ltp_def_from_ecef_f(struct LtpDef_f *def, struct EcefCoor_f *ecef); +extern void ltp_def_from_lla_f(struct LtpDef_f *def, struct LlaCoor_f *lla); +extern void lla_of_ecef_f(struct LlaCoor_f *out, struct EcefCoor_f *in); +extern void ecef_of_lla_f(struct EcefCoor_f *out, struct LlaCoor_f *in); +extern void enu_of_ecef_point_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct EcefCoor_f *ecef); +extern void ned_of_ecef_point_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef); +extern void enu_of_ecef_vect_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct EcefCoor_f *ecef); +extern void ned_of_ecef_vect_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef); +extern void enu_of_lla_point_f(struct EnuCoor_f *enu, struct LtpDef_f *def, struct LlaCoor_f *lla); +extern void ned_of_lla_point_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct LlaCoor_f *lla); /* not enought precision with floats - used the double version */ -extern void ecef_of_enu_point_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct EnuCoor_f* enu); -extern void ecef_of_ned_point_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct NedCoor_f* ned); -extern void ecef_of_enu_vect_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct EnuCoor_f* enu); -extern void ecef_of_ned_vect_f(struct EcefCoor_f* ecef, struct LtpDef_f* def, struct NedCoor_f* ned); +extern void ecef_of_enu_point_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct EnuCoor_f *enu); +extern void ecef_of_ned_point_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct NedCoor_f *ned); +extern void ecef_of_enu_vect_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct EnuCoor_f *enu); +extern void ecef_of_ned_vect_f(struct EcefCoor_f *ecef, struct LtpDef_f *def, struct NedCoor_f *ned); /* end use double versions */ #ifdef __cplusplus diff --git a/sw/airborne/math/pprz_geodetic_int.c b/sw/airborne/math/pprz_geodetic_int.c index e8566d1372..4c898aec02 100644 --- a/sw/airborne/math/pprz_geodetic_int.c +++ b/sw/airborne/math/pprz_geodetic_int.c @@ -31,7 +31,7 @@ #include "pprz_algebra_int.h" -void ltp_of_ecef_rmat_from_lla_i(struct Int32RMat* ltp_of_ecef, struct LlaCoor_i* lla) +void ltp_of_ecef_rmat_from_lla_i(struct Int32RMat *ltp_of_ecef, struct LlaCoor_i *lla) { #if USE_DOUBLE_PRECISION_TRIG @@ -57,7 +57,7 @@ void ltp_of_ecef_rmat_from_lla_i(struct Int32RMat* ltp_of_ecef, struct LlaCoor_i ltp_of_ecef->m[8] = sin_lat; } -void ltp_def_from_ecef_i(struct LtpDef_i* def, struct EcefCoor_i* ecef) +void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef) { /* store the origin of the tangeant plane */ @@ -69,7 +69,7 @@ void ltp_def_from_ecef_i(struct LtpDef_i* def, struct EcefCoor_i* ecef) } -void ltp_def_from_lla_i(struct LtpDef_i* def, struct LlaCoor_i* lla) +void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla) { /* store the origin of the tangeant plane */ @@ -87,7 +87,7 @@ void ltp_def_from_lla_i(struct LtpDef_i* def, struct LlaCoor_i* lla) * @param[in] def local coordinate system definition * @param[in] ecef ECEF point in cm */ -void enu_of_ecef_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef) +void enu_of_ecef_point_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct EcefCoor_i *ecef) { struct EcefCoor_i delta; @@ -113,7 +113,7 @@ void enu_of_ecef_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct Ece * @param[in] def local coordinate system definition * @param[in] ecef ECEF point in cm */ -void ned_of_ecef_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef) +void ned_of_ecef_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef) { struct EnuCoor_i enu; enu_of_ecef_point_i(&enu, def, ecef); @@ -126,7 +126,7 @@ void ned_of_ecef_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct Ece * @param[in] def local coordinate system definition * @param[in] ecef ECEF position in cm */ -void enu_of_ecef_pos_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef) +void enu_of_ecef_pos_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct EcefCoor_i *ecef) { struct EnuCoor_i enu_cm; enu_of_ecef_point_i(&enu_cm, def, ecef); @@ -146,7 +146,7 @@ void enu_of_ecef_pos_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefC * @param[in] def local coordinate system definition * @param[in] ecef ECEF position in cm */ -void ned_of_ecef_pos_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef) +void ned_of_ecef_pos_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef) { struct EnuCoor_i enu; enu_of_ecef_pos_i(&enu, def, ecef); @@ -159,7 +159,7 @@ void ned_of_ecef_pos_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefC * @param[in] def local coordinate system definition * @param[in] ecef vector in ECEF coordinate system */ -void enu_of_ecef_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef) +void enu_of_ecef_vect_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct EcefCoor_i *ecef) { const int64_t tmpx = (int64_t)def->ltp_of_ecef.m[0] * ecef->x + @@ -183,7 +183,7 @@ void enu_of_ecef_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct Ecef * @param[in] def local coordinate system definition * @param[in] ecef vector in ECEF coordinate system */ -void ned_of_ecef_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef) +void ned_of_ecef_vect_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef) { struct EnuCoor_i enu; enu_of_ecef_vect_i(&enu, def, ecef); @@ -196,7 +196,7 @@ void ned_of_ecef_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct Ecef * @param[in] def local coordinate system definition * @param[in] enu vector in ENU coordinate system */ -void ecef_of_enu_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) +void ecef_of_enu_vect_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu) { const int64_t tmpx = (int64_t)def->ltp_of_ecef.m[0] * enu->x + @@ -222,7 +222,7 @@ void ecef_of_enu_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct En * @param[in] def local coordinate system definition * @param[in] ned vector in NED coordinate system */ -void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned) +void ecef_of_ned_vect_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct NedCoor_i *ned) { struct EnuCoor_i enu; ENU_OF_TO_NED(enu, *ned); @@ -235,7 +235,7 @@ void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct Ne * @param[in] def local coordinate system definition * @param[in] enu ENU point in cm */ -void ecef_of_enu_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) +void ecef_of_enu_point_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu) { ecef_of_enu_vect_i(ecef, def, enu); VECT3_ADD(*ecef, def->ecef); @@ -247,7 +247,7 @@ void ecef_of_enu_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct E * @param[in] def local coordinate system definition * @param[in] ned NED point in cm */ -void ecef_of_ned_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned) +void ecef_of_ned_point_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct NedCoor_i *ned) { struct EnuCoor_i enu; ENU_OF_TO_NED(enu, *ned); @@ -260,7 +260,7 @@ void ecef_of_ned_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct N * @param[in] def local coordinate system definition * @param[in] enu ENU position in meter << #INT32_POS_FRAC */ -void ecef_of_enu_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu) +void ecef_of_enu_pos_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu) { /* enu_cm = (enu * 100) >> INT32_POS_FRAC * to loose less range: @@ -280,7 +280,7 @@ void ecef_of_enu_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct Enu * @param[in] def local coordinate system definition * @param[in] ned NED position in meter << #INT32_POS_FRAC */ -void ecef_of_ned_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned) +void ecef_of_ned_pos_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct NedCoor_i *ned) { struct EnuCoor_i enu; ENU_OF_TO_NED(enu, *ned); @@ -288,28 +288,28 @@ void ecef_of_ned_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct Ned } -void enu_of_lla_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla) +void enu_of_lla_point_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla) { struct EcefCoor_i ecef; ecef_of_lla_i(&ecef, lla); enu_of_ecef_point_i(enu, def, &ecef); } -void ned_of_lla_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct LlaCoor_i* lla) +void ned_of_lla_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaCoor_i *lla) { struct EcefCoor_i ecef; ecef_of_lla_i(&ecef, lla); ned_of_ecef_point_i(ned, def, &ecef); } -void enu_of_lla_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla) +void enu_of_lla_vect_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla) { struct EcefCoor_i ecef; ecef_of_lla_i(&ecef, lla); enu_of_ecef_vect_i(enu, def, &ecef); } -void ned_of_lla_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct LlaCoor_i* lla) +void ned_of_lla_vect_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaCoor_i *lla) { struct EcefCoor_i ecef; ecef_of_lla_i(&ecef, lla); @@ -323,7 +323,7 @@ void ned_of_lla_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct LlaCo #include "pprz_geodetic_float.h" #include "pprz_geodetic_double.h" -void lla_of_ecef_i(struct LlaCoor_i* out, struct EcefCoor_i* in) +void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in) { /* convert our input to floating point */ @@ -341,7 +341,7 @@ void lla_of_ecef_i(struct LlaCoor_i* out, struct EcefCoor_i* in) } -void ecef_of_lla_i(struct EcefCoor_i* out, struct LlaCoor_i* in) +void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in) { /* convert our input to floating point */ diff --git a/sw/airborne/math/pprz_geodetic_int.h b/sw/airborne/math/pprz_geodetic_int.h index 552907e83a..92f3c0d76f 100644 --- a/sw/airborne/math/pprz_geodetic_int.h +++ b/sw/airborne/math/pprz_geodetic_int.h @@ -99,27 +99,27 @@ struct LtpDef_i { int32_t hmsl; ///< Height above mean sea level in mm }; -extern void ltp_of_ecef_rmat_from_lla_i(struct Int32RMat* ltp_of_ecef, struct LlaCoor_i* lla); -extern void ltp_def_from_ecef_i(struct LtpDef_i* def, struct EcefCoor_i* ecef); -extern void ltp_def_from_lla_i(struct LtpDef_i* def, struct LlaCoor_i* lla); -extern void lla_of_ecef_i(struct LlaCoor_i* out, struct EcefCoor_i* in); -extern void ecef_of_lla_i(struct EcefCoor_i* out, struct LlaCoor_i* in); -extern void enu_of_ecef_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef); -extern void ned_of_ecef_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef); -extern void enu_of_ecef_pos_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef); -extern void ned_of_ecef_pos_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef); -extern void enu_of_ecef_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct EcefCoor_i* ecef); -extern void ned_of_ecef_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct EcefCoor_i* ecef); -extern void enu_of_lla_point_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla); -extern void ned_of_lla_point_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct LlaCoor_i* lla); -extern void enu_of_lla_vect_i(struct EnuCoor_i* enu, struct LtpDef_i* def, struct LlaCoor_i* lla); -extern void ned_of_lla_vect_i(struct NedCoor_i* ned, struct LtpDef_i* def, struct LlaCoor_i* lla); -extern void ecef_of_enu_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu); -extern void ecef_of_ned_point_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned); -extern void ecef_of_enu_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu); -extern void ecef_of_ned_pos_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned); -extern void ecef_of_enu_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct EnuCoor_i* enu); -extern void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, struct NedCoor_i* ned); +extern void ltp_of_ecef_rmat_from_lla_i(struct Int32RMat *ltp_of_ecef, struct LlaCoor_i *lla); +extern void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef); +extern void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla); +extern void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in); +extern void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in); +extern void enu_of_ecef_point_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct EcefCoor_i *ecef); +extern void ned_of_ecef_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef); +extern void enu_of_ecef_pos_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct EcefCoor_i *ecef); +extern void ned_of_ecef_pos_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef); +extern void enu_of_ecef_vect_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct EcefCoor_i *ecef); +extern void ned_of_ecef_vect_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef); +extern void enu_of_lla_point_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla); +extern void ned_of_lla_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaCoor_i *lla); +extern void enu_of_lla_vect_i(struct EnuCoor_i *enu, struct LtpDef_i *def, struct LlaCoor_i *lla); +extern void ned_of_lla_vect_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaCoor_i *lla); +extern void ecef_of_enu_point_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu); +extern void ecef_of_ned_point_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct NedCoor_i *ned); +extern void ecef_of_enu_pos_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu); +extern void ecef_of_ned_pos_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct NedCoor_i *ned); +extern void ecef_of_enu_vect_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu); +extern void ecef_of_ned_vect_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct NedCoor_i *ned); #define CM_OF_M(_m) ((_m)*1e2) #define M_OF_CM(_cm) ((_cm)/1e2) diff --git a/sw/airborne/math/pprz_geodetic_wmm2010.c b/sw/airborne/math/pprz_geodetic_wmm2010.c index 51af306917..35354dec32 100644 --- a/sw/airborne/math/pprz_geodetic_wmm2010.c +++ b/sw/airborne/math/pprz_geodetic_wmm2010.c @@ -58,7 +58,7 @@ const double gh2[MAXCOEFF] = { 0.0, 0.0, 0.0, 0.1, 0.0, 0.1, 0.0, -0.1, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.1, 0.0, 0.1, 0.0 }; -int16_t extrapsh(double date, double dte1, int16_t nmax1, int16_t nmax2, double* gh) +int16_t extrapsh(double date, double dte1, int16_t nmax1, int16_t nmax2, double *gh) { int16_t nmax; @@ -99,8 +99,8 @@ int16_t extrapsh(double date, double dte1, int16_t nmax1, int16_t nmax2, double* return (nmax); } -int16_t mag_calc(int16_t igdgc, double flat, double flon, double elev, int16_t nmax, double* gh, double* geo_mag_x, - double* geo_mag_y, double* geo_mag_z, int16_t iext, double ext1, double ext2, double ext3) +int16_t mag_calc(int16_t igdgc, double flat, double flon, double elev, int16_t nmax, double *gh, double *geo_mag_x, + double *geo_mag_y, double *geo_mag_z, int16_t iext, double ext1, double ext2, double ext3) { double earths_radius = 6371.2; diff --git a/sw/airborne/math/pprz_geodetic_wmm2010.h b/sw/airborne/math/pprz_geodetic_wmm2010.h index 7383475da3..89493612cd 100644 --- a/sw/airborne/math/pprz_geodetic_wmm2010.h +++ b/sw/airborne/math/pprz_geodetic_wmm2010.h @@ -62,9 +62,9 @@ extern "C" { extern const double gh1[]; extern const double gh2[]; -int16_t extrapsh(double date, double dte1, int16_t nmax1, int16_t nmax2, double* gh); +int16_t extrapsh(double date, double dte1, int16_t nmax1, int16_t nmax2, double *gh); int16_t mag_calc(int16_t igdgc, double flat, double flon, double elev, int16_t nmax, - double* gh, double* geo_mag_x, double* geo_mag_y, double* geo_mag_z, + double *gh, double *geo_mag_x, double *geo_mag_y, double *geo_mag_z, int16_t iext, double ext1, double ext2, double ext3); #ifdef __cplusplus diff --git a/sw/airborne/math/pprz_isa.h b/sw/airborne/math/pprz_isa.h index 03ede60c4f..3d2085dcb9 100644 --- a/sw/airborne/math/pprz_isa.h +++ b/sw/airborne/math/pprz_isa.h @@ -59,7 +59,7 @@ extern "C" { /** standard air density in kg/m^3 */ #define PPRZ_ISA_AIR_DENSITY 1.225 -static const float PPRZ_ISA_M_OF_P_CONST = (PPRZ_ISA_AIR_GAS_CONSTANT * PPRZ_ISA_SEA_LEVEL_TEMP / PPRZ_ISA_GRAVITY); +static const float PPRZ_ISA_M_OF_P_CONST = (PPRZ_ISA_AIR_GAS_CONSTANT *PPRZ_ISA_SEA_LEVEL_TEMP / PPRZ_ISA_GRAVITY); /** * Get absolute altitude from pressure (using simplified equation). @@ -137,7 +137,7 @@ static inline float pprz_isa_height_of_pressure_full(float pressure, float ref_p if (ref_p > 0.) { const float prel = pressure / ref_p; const float inv_expo = PPRZ_ISA_GAS_CONSTANT * PPRZ_ISA_TEMP_LAPS_RATE / - PPRZ_ISA_GRAVITY / PPRZ_ISA_MOLAR_MASS; + PPRZ_ISA_GRAVITY / PPRZ_ISA_MOLAR_MASS; return (1 - powf(prel, inv_expo)) * PPRZ_ISA_SEA_LEVEL_TEMP / PPRZ_ISA_TEMP_LAPS_RATE; } else { return 0.; @@ -157,7 +157,7 @@ static inline float pprz_isa_ref_pressure_of_height_full(float pressure, float h // Trel = 1 - L*h/T0; const float Trel = 1.0 - PPRZ_ISA_TEMP_LAPS_RATE * height / PPRZ_ISA_SEA_LEVEL_TEMP; const float expo = PPRZ_ISA_GRAVITY * PPRZ_ISA_MOLAR_MASS / PPRZ_ISA_GAS_CONSTANT / - PPRZ_ISA_TEMP_LAPS_RATE; + PPRZ_ISA_TEMP_LAPS_RATE; return pressure / pow(Trel, expo); } diff --git a/sw/airborne/math/pprz_matrix_decomp_float.c b/sw/airborne/math/pprz_matrix_decomp_float.c index 2ecc8b1e73..1642ccc59a 100644 --- a/sw/airborne/math/pprz_matrix_decomp_float.c +++ b/sw/airborne/math/pprz_matrix_decomp_float.c @@ -37,7 +37,7 @@ * @param in pointer to the input array [n x n] * @param n dimension of the matrix */ -void pprz_cholesky_float(float** out, float** in, int n) +void pprz_cholesky_float(float **out, float **in, int n) { int i, j, k; float _o[n][n]; @@ -70,11 +70,11 @@ void pprz_cholesky_float(float** out, float** in, int n) * @param m number of rows of the input matrix * @param n number of column of the input matrix */ -void pprz_qr_float(float** Q, float** R, float** in, int m, int n) +void pprz_qr_float(float **Q, float **R, float **in, int m, int n) { int i, k; float _q[m][m][m]; - float* q[m][m]; + float *q[m][m]; float _z[m][n], _z1[m][n], _z2[m][m]; MAKE_MATRIX_PTR(z, _z, m); MAKE_MATRIX_PTR(z1, _z1, m); @@ -144,7 +144,7 @@ static inline float pythag(float a, float b) * @param n number of columns of the input matrix * @return 0 (false) if convergence failed, 1 (true) if decomposition succed */ -int pprz_svd_float(float** a, float* w, float** v, int m, int n) +int pprz_svd_float(float **a, float *w, float **v, int m, int n) { /* Householder reduction to bidiagonal form. */ int flag, i, its, j, jj, k, l, NM; @@ -443,7 +443,7 @@ int pprz_svd_float(float** a, float* w, float** v, int m, int n) * @param n number of columns of the matrix A * @param l number of columns of the matrix B */ -void pprz_svd_solve_float(float** x, float** u, float* w, float** v, float** b, int m, int n, int l) +void pprz_svd_solve_float(float **x, float **u, float *w, float **v, float **b, int m, int n, int l) { int i, j, jj, k; float s; diff --git a/sw/airborne/math/pprz_matrix_decomp_float.h b/sw/airborne/math/pprz_matrix_decomp_float.h index b629eed0b5..3af63c73b3 100644 --- a/sw/airborne/math/pprz_matrix_decomp_float.h +++ b/sw/airborne/math/pprz_matrix_decomp_float.h @@ -41,7 +41,7 @@ extern "C" { * @param in pointer to the input array [n x n] * @param n dimension of the matrix */ -void pprz_cholesky_float(float** out, float** in, int n); +void pprz_cholesky_float(float **out, float **in, int n); /** QR decomposition * @@ -55,7 +55,7 @@ void pprz_cholesky_float(float** out, float** in, int n); * @param m number of rows of the input matrix * @param n number of columns of the input matrix */ -void pprz_qr_float(float** Q, float** R, float** in, int m, int n); +void pprz_qr_float(float **Q, float **R, float **in, int m, int n); /** SVD decomposition * @@ -81,7 +81,7 @@ void pprz_qr_float(float** Q, float** R, float** in, int m, int n); * @param n number of columns of the input matrix * @return 0 (false) if convergence failed, 1 (true) if decomposition succed */ -int pprz_svd_float(float** a, float* w, float** v, int m, int n); +int pprz_svd_float(float **a, float *w, float **v, int m, int n); /** SVD based linear solver * @@ -101,7 +101,7 @@ int pprz_svd_float(float** a, float* w, float** v, int m, int n); * @param n number of columns of the matrix A * @param l number of columns of the matrix B */ -void pprz_svd_solve_float(float** x, float** u, float* w, float** v, float** b, int m, int n, int l); +void pprz_svd_solve_float(float **x, float **u, float *w, float **v, float **b, int m, int n, int l); #ifdef __cplusplus } /* extern "C" */ diff --git a/sw/airborne/math/pprz_orientation_conversion.c b/sw/airborne/math/pprz_orientation_conversion.c index 2a5f38fbee..e825130ff3 100644 --- a/sw/airborne/math/pprz_orientation_conversion.c +++ b/sw/airborne/math/pprz_orientation_conversion.c @@ -45,7 +45,7 @@ * * *****************************************************************************/ -void orientationCalcQuat_i(struct OrientationReps* orientation) +void orientationCalcQuat_i(struct OrientationReps *orientation) { if (bit_is_set(orientation->status, ORREP_QUAT_I)) { return; @@ -70,7 +70,7 @@ void orientationCalcQuat_i(struct OrientationReps* orientation) SetBit(orientation->status, ORREP_QUAT_I); } -void orientationCalcRMat_i(struct OrientationReps* orientation) +void orientationCalcRMat_i(struct OrientationReps *orientation) { if (bit_is_set(orientation->status, ORREP_RMAT_I)) { return; @@ -95,7 +95,7 @@ void orientationCalcRMat_i(struct OrientationReps* orientation) SetBit(orientation->status, ORREP_RMAT_I); } -void orientationCalcEulers_i(struct OrientationReps* orientation) +void orientationCalcEulers_i(struct OrientationReps *orientation) { if (bit_is_set(orientation->status, ORREP_EULER_I)) { return; @@ -120,7 +120,7 @@ void orientationCalcEulers_i(struct OrientationReps* orientation) SetBit(orientation->status, ORREP_EULER_I); } -void orientationCalcQuat_f(struct OrientationReps* orientation) +void orientationCalcQuat_f(struct OrientationReps *orientation) { if (bit_is_set(orientation->status, ORREP_QUAT_F)) { return; @@ -145,7 +145,7 @@ void orientationCalcQuat_f(struct OrientationReps* orientation) SetBit(orientation->status, ORREP_QUAT_F); } -void orientationCalcRMat_f(struct OrientationReps* orientation) +void orientationCalcRMat_f(struct OrientationReps *orientation) { if (bit_is_set(orientation->status, ORREP_RMAT_F)) { return; @@ -170,7 +170,7 @@ void orientationCalcRMat_f(struct OrientationReps* orientation) SetBit(orientation->status, ORREP_RMAT_F); } -void orientationCalcEulers_f(struct OrientationReps* orientation) +void orientationCalcEulers_f(struct OrientationReps *orientation) { if (bit_is_set(orientation->status, ORREP_EULER_F)) { return; diff --git a/sw/airborne/math/pprz_orientation_conversion.h b/sw/airborne/math/pprz_orientation_conversion.h index 0c209f4545..941480d1d0 100644 --- a/sw/airborne/math/pprz_orientation_conversion.h +++ b/sw/airborne/math/pprz_orientation_conversion.h @@ -122,23 +122,23 @@ struct OrientationReps { }; /************* declaration of transformation functions ************/ -extern void orientationCalcQuat_i(struct OrientationReps* orientation); -extern void orientationCalcRMat_i(struct OrientationReps* orientation); -extern void orientationCalcEulers_i(struct OrientationReps* orientation); -extern void orientationCalcQuat_f(struct OrientationReps* orientation); -extern void orientationCalcRMat_f(struct OrientationReps* orientation); -extern void orientationCalcEulers_f(struct OrientationReps* orientation); +extern void orientationCalcQuat_i(struct OrientationReps *orientation); +extern void orientationCalcRMat_i(struct OrientationReps *orientation); +extern void orientationCalcEulers_i(struct OrientationReps *orientation); +extern void orientationCalcQuat_f(struct OrientationReps *orientation); +extern void orientationCalcRMat_f(struct OrientationReps *orientation); +extern void orientationCalcEulers_f(struct OrientationReps *orientation); /*********************** validity test functions ******************/ /// Test if orientations are valid. -static inline bool_t orienationCheckValid(struct OrientationReps* orientation) +static inline bool_t orienationCheckValid(struct OrientationReps *orientation) { return (orientation->status); } /// Set vehicle body attitude from quaternion (int). -static inline void orientationSetQuat_i(struct OrientationReps* orientation, struct Int32Quat* quat) +static inline void orientationSetQuat_i(struct OrientationReps *orientation, struct Int32Quat *quat) { QUAT_COPY(orientation->quat_i, *quat); /* clear bits for all attitude representations and only set the new one */ @@ -146,7 +146,7 @@ static inline void orientationSetQuat_i(struct OrientationReps* orientation, str } /// Set vehicle body attitude from rotation matrix (int). -static inline void orientationSetRMat_i(struct OrientationReps* orientation, struct Int32RMat* rmat) +static inline void orientationSetRMat_i(struct OrientationReps *orientation, struct Int32RMat *rmat) { RMAT_COPY(orientation->rmat_i, *rmat); /* clear bits for all attitude representations and only set the new one */ @@ -154,7 +154,7 @@ static inline void orientationSetRMat_i(struct OrientationReps* orientation, str } /// Set vehicle body attitude from euler angles (int). -static inline void orientationSetEulers_i(struct OrientationReps* orientation, struct Int32Eulers* eulers) +static inline void orientationSetEulers_i(struct OrientationReps *orientation, struct Int32Eulers *eulers) { EULERS_COPY(orientation->eulers_i, *eulers); /* clear bits for all attitude representations and only set the new one */ @@ -162,7 +162,7 @@ static inline void orientationSetEulers_i(struct OrientationReps* orientation, s } /// Set vehicle body attitude from quaternion (float). -static inline void orientationSetQuat_f(struct OrientationReps* orientation, struct FloatQuat* quat) +static inline void orientationSetQuat_f(struct OrientationReps *orientation, struct FloatQuat *quat) { QUAT_COPY(orientation->quat_f, *quat); /* clear bits for all attitude representations and only set the new one */ @@ -170,7 +170,7 @@ static inline void orientationSetQuat_f(struct OrientationReps* orientation, str } /// Set vehicle body attitude from rotation matrix (float). -static inline void orientationSetRMat_f(struct OrientationReps* orientation, struct FloatRMat* rmat) +static inline void orientationSetRMat_f(struct OrientationReps *orientation, struct FloatRMat *rmat) { RMAT_COPY(orientation->rmat_f, *rmat); /* clear bits for all attitude representations and only set the new one */ @@ -178,7 +178,7 @@ static inline void orientationSetRMat_f(struct OrientationReps* orientation, str } /// Set vehicle body attitude from euler angles (float). -static inline void orientationSetEulers_f(struct OrientationReps* orientation, struct FloatEulers* eulers) +static inline void orientationSetEulers_f(struct OrientationReps *orientation, struct FloatEulers *eulers) { EULERS_COPY(orientation->eulers_f, *eulers); /* clear bits for all attitude representations and only set the new one */ @@ -187,7 +187,7 @@ static inline void orientationSetEulers_f(struct OrientationReps* orientation, s /// Get vehicle body attitude quaternion (int). -static inline struct Int32Quat* orientationGetQuat_i(struct OrientationReps* orientation) +static inline struct Int32Quat *orientationGetQuat_i(struct OrientationReps *orientation) { if (!bit_is_set(orientation->status, ORREP_QUAT_I)) { orientationCalcQuat_i(orientation); @@ -196,7 +196,7 @@ static inline struct Int32Quat* orientationGetQuat_i(struct OrientationReps* ori } /// Get vehicle body attitude rotation matrix (int). -static inline struct Int32RMat* orientationGetRMat_i(struct OrientationReps* orientation) +static inline struct Int32RMat *orientationGetRMat_i(struct OrientationReps *orientation) { if (!bit_is_set(orientation->status, ORREP_RMAT_I)) { orientationCalcRMat_i(orientation); @@ -205,7 +205,7 @@ static inline struct Int32RMat* orientationGetRMat_i(struct OrientationReps* ori } /// Get vehicle body attitude euler angles (int). -static inline struct Int32Eulers* orientationGetEulers_i(struct OrientationReps* orientation) +static inline struct Int32Eulers *orientationGetEulers_i(struct OrientationReps *orientation) { if (!bit_is_set(orientation->status, ORREP_EULER_I)) { orientationCalcEulers_i(orientation); @@ -214,7 +214,7 @@ static inline struct Int32Eulers* orientationGetEulers_i(struct OrientationReps* } /// Get vehicle body attitude quaternion (float). -static inline struct FloatQuat* orientationGetQuat_f(struct OrientationReps* orientation) +static inline struct FloatQuat *orientationGetQuat_f(struct OrientationReps *orientation) { if (!bit_is_set(orientation->status, ORREP_QUAT_F)) { orientationCalcQuat_f(orientation); @@ -223,7 +223,7 @@ static inline struct FloatQuat* orientationGetQuat_f(struct OrientationReps* ori } /// Get vehicle body attitude rotation matrix (float). -static inline struct FloatRMat* orientationGetRMat_f(struct OrientationReps* orientation) +static inline struct FloatRMat *orientationGetRMat_f(struct OrientationReps *orientation) { if (!bit_is_set(orientation->status, ORREP_RMAT_F)) { orientationCalcRMat_f(orientation); @@ -232,7 +232,7 @@ static inline struct FloatRMat* orientationGetRMat_f(struct OrientationReps* ori } /// Get vehicle body attitude euler angles (float). -static inline struct FloatEulers* orientationGetEulers_f(struct OrientationReps* orientation) +static inline struct FloatEulers *orientationGetEulers_f(struct OrientationReps *orientation) { if (!bit_is_set(orientation->status, ORREP_EULER_F)) { orientationCalcEulers_f(orientation); diff --git a/sw/airborne/math/pprz_polyfit_float.c b/sw/airborne/math/pprz_polyfit_float.c index f663ddd693..3b1639d78b 100644 --- a/sw/airborne/math/pprz_polyfit_float.c +++ b/sw/airborne/math/pprz_polyfit_float.c @@ -60,9 +60,9 @@ * @param[in] p degree of the output polynomial * @param[out] c pointer to the output array of polynomial coefficients [p+1] */ -void pprz_polyfit_float(float* x, float* y, int n, int p, float* c) +void pprz_polyfit_float(float *x, float *y, int n, int p, float *c) { - int i,j,k; + int i, j, k; // Instead of solving directly (X'X)^-1 X' y // let's build the matrices (X'X) and (X'y) @@ -71,8 +71,8 @@ void pprz_polyfit_float(float* x, float* y, int n, int p, float* c) // Finally we can solve the linear system (X'X).c = (X'y) using SVD decomposition // First build a table of element S_i = sum_{k=0,n-1} x_k^i of dimension 2*p+1 - float S[2*p + 1]; - float_vect_zero(S, 2*p + 1); + float S[2 * p + 1]; + float_vect_zero(S, 2 * p + 1); // and a table of element T_i = sum_{k=0,n-1} x_k^i*y_k of dimension p+1 // make it a matrix for later use float _T[p + 1][1]; @@ -82,10 +82,11 @@ void pprz_polyfit_float(float* x, float* y, int n, int p, float* c) for (k = 0; k < n; k++) { float x_tmp = x[k]; T[0][0] += y[k]; - for (i = 1; i < 2*p + 1; i++) { + for (i = 1; i < 2 * p + 1; i++) { S[i] += x_tmp; // add element to S_i - if (i < p + 1) - T[i][0] += x_tmp*y[k]; // add element to T_i if i < p+1 + if (i < p + 1) { + T[i][0] += x_tmp * y[k]; // add element to T_i if i < p+1 + } x_tmp *= x[k]; // multiply x_tmp by current value of x } } @@ -95,7 +96,7 @@ void pprz_polyfit_float(float* x, float* y, int n, int p, float* c) MAKE_MATRIX_PTR(XtX, _XtX, p + 1); for (i = 0; i < p + 1; i++) { for (j = 0; j < p + 1; j++) { - XtX[i][j] = S[i+j]; + XtX[i][j] = S[i + j]; } } // Solve linear system XtX.c = T after performing a SVD decomposition of XtX @@ -107,8 +108,9 @@ void pprz_polyfit_float(float* x, float* y, int n, int p, float* c) MAKE_MATRIX_PTR(c_tmp, _c, p + 1); pprz_svd_solve_float(c_tmp, XtX, w, v, T, p + 1, p + 1, 1); // set output vector - for (i = 0; i < p + 1; i++) + for (i = 0; i < p + 1; i++) { c[i] = c_tmp[i][0]; + } } diff --git a/sw/airborne/math/pprz_polyfit_float.h b/sw/airborne/math/pprz_polyfit_float.h index 80a507669b..bdec9ce981 100644 --- a/sw/airborne/math/pprz_polyfit_float.h +++ b/sw/airborne/math/pprz_polyfit_float.h @@ -63,7 +63,7 @@ extern "C" { * @param[in] p degree of the output polynomial * @param[out] c pointer to the output array of polynomial coefficients [p] */ -void pprz_polyfit_float(float* x, float* y, int n, int p, float* c); +void pprz_polyfit_float(float *x, float *y, int n, int p, float *c); #ifdef __cplusplus diff --git a/sw/airborne/math/pprz_rk_float.h b/sw/airborne/math/pprz_rk_float.h index 4ca3559a61..efca3353ab 100644 --- a/sw/airborne/math/pprz_rk_float.h +++ b/sw/airborne/math/pprz_rk_float.h @@ -53,10 +53,10 @@ extern "C" { * @param dt integration step */ static inline void runge_kutta_1_float( - float* xo, - const float* x, const int n, - const float* u, const int m, - void (*f)(float* o, const float* x, const int n, const float* u, const int m), + float *xo, + const float *x, const int n, + const float *u, const int m, + void (*f)(float *o, const float *x, const int n, const float *u, const int m), const float dt) { float dx[n]; @@ -87,10 +87,10 @@ static inline void runge_kutta_1_float( * @param dt integration step */ static inline void runge_kutta_2_float( - float* xo, - const float* x, const int n, - const float* u, const int m, - void (*f)(float* o, const float* x, const int n, const float* u, const int m), + float *xo, + const float *x, const int n, + const float *u, const int m, + void (*f)(float *o, const float *x, const int n, const float *u, const int m), const float dt) { float mid_point[n]; @@ -130,10 +130,10 @@ static inline void runge_kutta_2_float( * @param dt integration step */ static inline void runge_kutta_4_float( - float* xo, - const float* x, const int n, - const float* u, const int m, - void (*f)(float* o, const float* x, const int n, const float* u, const int m), + float *xo, + const float *x, const int n, + const float *u, const int m, + void (*f)(float *o, const float *x, const int n, const float *u, const int m), const float dt) { float k1[n], k2[n], k3[n], k4[n], ktmp[n]; diff --git a/sw/airborne/math/pprz_stat.h b/sw/airborne/math/pprz_stat.h index f889eaaba0..e5437b7444 100644 --- a/sw/airborne/math/pprz_stat.h +++ b/sw/airborne/math/pprz_stat.h @@ -42,7 +42,7 @@ extern "C" { * @param nb numbre of values in the array, must be >0 * @return variance */ -static inline float variance_float(float* array, int nb) +static inline float variance_float(float *array, int nb) { float me = 0.; float see = 0.; @@ -63,7 +63,7 @@ static inline float variance_float(float* array, int nb) * @param nb numbre of values in the array, must be >0 * @return variance */ -static inline int32_t variance_int(int32_t* array, int nb) +static inline int32_t variance_int(int32_t *array, int nb) { float me = 0; float see = 0; diff --git a/sw/airborne/mcu.c b/sw/airborne/mcu.c index ca87015b52..bb30748348 100644 --- a/sw/airborne/mcu.c +++ b/sw/airborne/mcu.c @@ -61,7 +61,8 @@ #endif #endif /* PERIPHERALS_AUTO_INIT */ -void mcu_init(void) { +void mcu_init(void) +{ mcu_arch_init(); @@ -167,7 +168,7 @@ void mcu_init(void) { #endif #else -INFO("PERIPHERALS_AUTO_INIT not enabled! Peripherals (including sys_time) need explicit initialization.") + INFO("PERIPHERALS_AUTO_INIT not enabled! Peripherals (including sys_time) need explicit initialization.") #endif /* PERIPHERALS_AUTO_INIT */ } diff --git a/sw/airborne/mcu_periph/adc.h b/sw/airborne/mcu_periph/adc.h index 465ab7f5d9..5ac865ab13 100644 --- a/sw/airborne/mcu_periph/adc.h +++ b/sw/airborne/mcu_periph/adc.h @@ -67,9 +67,9 @@ struct adc_buf { Registers channel_buf as buffer for ADC channel 1, with max index 12 (12 samples). */ -void adc_buf_channel(uint8_t adc_channel, struct adc_buf* s, uint8_t av_nb_sample); +void adc_buf_channel(uint8_t adc_channel, struct adc_buf *s, uint8_t av_nb_sample); /** Starts conversions */ -void adc_init( void ); +void adc_init(void); #endif diff --git a/sw/airborne/mcu_periph/can.c b/sw/airborne/mcu_periph/can.c index df04e8c3fc..40e8db34af 100644 --- a/sw/airborne/mcu_periph/can.c +++ b/sw/airborne/mcu_periph/can.c @@ -42,6 +42,7 @@ int ppz_can_transmit(uint32_t id, const uint8_t *buf, uint8_t len) void _can_run_rx_callback(uint32_t id, uint8_t *buf, uint8_t len) { - if(can_rx_callback) + if (can_rx_callback) { can_rx_callback(id, buf, len); + } } diff --git a/sw/airborne/mcu_periph/i2c.c b/sw/airborne/mcu_periph/i2c.c index ef2c6db820..cc6736c83f 100644 --- a/sw/airborne/mcu_periph/i2c.c +++ b/sw/airborne/mcu_periph/i2c.c @@ -36,7 +36,8 @@ struct i2c_periph i2c0; #if PERIODIC_TELEMETRY -static void send_i2c0_err(struct transport_tx *trans, struct link_device *dev) { +static void send_i2c0_err(struct transport_tx *trans, struct link_device *dev) +{ uint16_t i2c0_queue_full_cnt = i2c0.errors->queue_full_cnt; uint16_t i2c0_ack_fail_cnt = i2c0.errors->ack_fail_cnt; uint16_t i2c0_miss_start_stop_cnt = i2c0.errors->miss_start_stop_cnt; @@ -49,21 +50,22 @@ static void send_i2c0_err(struct transport_tx *trans, struct link_device *dev) { uint32_t i2c0_last_unexpected_event = i2c0.errors->last_unexpected_event; uint8_t _bus0 = 0; pprz_msg_send_I2C_ERRORS(trans, dev, AC_ID, - &i2c0_queue_full_cnt, - &i2c0_ack_fail_cnt, - &i2c0_miss_start_stop_cnt, - &i2c0_arb_lost_cnt, - &i2c0_over_under_cnt, - &i2c0_pec_recep_cnt, - &i2c0_timeout_tlow_cnt, - &i2c0_smbus_alert_cnt, - &i2c0_unexpected_event_cnt, - &i2c0_last_unexpected_event, - &_bus0); + &i2c0_queue_full_cnt, + &i2c0_ack_fail_cnt, + &i2c0_miss_start_stop_cnt, + &i2c0_arb_lost_cnt, + &i2c0_over_under_cnt, + &i2c0_pec_recep_cnt, + &i2c0_timeout_tlow_cnt, + &i2c0_smbus_alert_cnt, + &i2c0_unexpected_event_cnt, + &i2c0_last_unexpected_event, + &_bus0); } #endif -void i2c0_init(void) { +void i2c0_init(void) +{ i2c_init(&i2c0); i2c0_hw_init(); } @@ -76,7 +78,8 @@ void i2c0_init(void) { struct i2c_periph i2c1; #if PERIODIC_TELEMETRY -static void send_i2c1_err(struct transport_tx *trans, struct link_device *dev) { +static void send_i2c1_err(struct transport_tx *trans, struct link_device *dev) +{ uint16_t i2c1_queue_full_cnt = i2c1.errors->queue_full_cnt; uint16_t i2c1_ack_fail_cnt = i2c1.errors->ack_fail_cnt; uint16_t i2c1_miss_start_stop_cnt = i2c1.errors->miss_start_stop_cnt; @@ -89,21 +92,22 @@ static void send_i2c1_err(struct transport_tx *trans, struct link_device *dev) { uint32_t i2c1_last_unexpected_event = i2c1.errors->last_unexpected_event; uint8_t _bus1 = 1; pprz_msg_send_I2C_ERRORS(trans, dev, AC_ID, - &i2c1_queue_full_cnt, - &i2c1_ack_fail_cnt, - &i2c1_miss_start_stop_cnt, - &i2c1_arb_lost_cnt, - &i2c1_over_under_cnt, - &i2c1_pec_recep_cnt, - &i2c1_timeout_tlow_cnt, - &i2c1_smbus_alert_cnt, - &i2c1_unexpected_event_cnt, - &i2c1_last_unexpected_event, - &_bus1); + &i2c1_queue_full_cnt, + &i2c1_ack_fail_cnt, + &i2c1_miss_start_stop_cnt, + &i2c1_arb_lost_cnt, + &i2c1_over_under_cnt, + &i2c1_pec_recep_cnt, + &i2c1_timeout_tlow_cnt, + &i2c1_smbus_alert_cnt, + &i2c1_unexpected_event_cnt, + &i2c1_last_unexpected_event, + &_bus1); } #endif -void i2c1_init(void) { +void i2c1_init(void) +{ i2c_init(&i2c1); i2c1_hw_init(); } @@ -116,7 +120,8 @@ void i2c1_init(void) { struct i2c_periph i2c2; #if PERIODIC_TELEMETRY -static void send_i2c2_err(struct transport_tx *trans, struct link_device *dev) { +static void send_i2c2_err(struct transport_tx *trans, struct link_device *dev) +{ uint16_t i2c2_queue_full_cnt = i2c2.errors->queue_full_cnt; uint16_t i2c2_ack_fail_cnt = i2c2.errors->ack_fail_cnt; uint16_t i2c2_miss_start_stop_cnt = i2c2.errors->miss_start_stop_cnt; @@ -129,21 +134,22 @@ static void send_i2c2_err(struct transport_tx *trans, struct link_device *dev) { uint32_t i2c2_last_unexpected_event = i2c2.errors->last_unexpected_event; uint8_t _bus2 = 2; pprz_msg_send_I2C_ERRORS(trans, dev, AC_ID, - &i2c2_queue_full_cnt, - &i2c2_ack_fail_cnt, - &i2c2_miss_start_stop_cnt, - &i2c2_arb_lost_cnt, - &i2c2_over_under_cnt, - &i2c2_pec_recep_cnt, - &i2c2_timeout_tlow_cnt, - &i2c2_smbus_alert_cnt, - &i2c2_unexpected_event_cnt, - &i2c2_last_unexpected_event, - &_bus2); + &i2c2_queue_full_cnt, + &i2c2_ack_fail_cnt, + &i2c2_miss_start_stop_cnt, + &i2c2_arb_lost_cnt, + &i2c2_over_under_cnt, + &i2c2_pec_recep_cnt, + &i2c2_timeout_tlow_cnt, + &i2c2_smbus_alert_cnt, + &i2c2_unexpected_event_cnt, + &i2c2_last_unexpected_event, + &_bus2); } #endif -void i2c2_init(void) { +void i2c2_init(void) +{ i2c_init(&i2c2); i2c2_hw_init(); } @@ -154,13 +160,15 @@ void i2c2_init(void) { struct i2c_periph i2c3; -void i2c3_init(void) { +void i2c3_init(void) +{ i2c_init(&i2c3); i2c3_hw_init(); } #if PERIODIC_TELEMETRY -static void send_i2c3_err(struct transport_tx *trans, struct link_device *dev) { +static void send_i2c3_err(struct transport_tx *trans, struct link_device *dev) +{ uint16_t i2c3_queue_full_cnt = i2c3.errors->queue_full_cnt; uint16_t i2c3_ack_fail_cnt = i2c3.errors->ack_fail_cnt; uint16_t i2c3_miss_start_stop_cnt = i2c3.errors->miss_start_stop_cnt; @@ -173,24 +181,26 @@ static void send_i2c3_err(struct transport_tx *trans, struct link_device *dev) { uint32_t i2c3_last_unexpected_event = i2c3.errors->last_unexpected_event; uint8_t _bus3 = 3; pprz_msg_send_I2C_ERRORS(trans, dev, AC_ID, - &i2c3_queue_full_cnt, - &i2c3_ack_fail_cnt, - &i2c3_miss_start_stop_cnt, - &i2c3_arb_lost_cnt, - &i2c3_over_under_cnt, - &i2c3_pec_recep_cnt, - &i2c3_timeout_tlow_cnt, - &i2c3_smbus_alert_cnt, - &i2c3_unexpected_event_cnt, - &i2c3_last_unexpected_event, - &_bus3); + &i2c3_queue_full_cnt, + &i2c3_ack_fail_cnt, + &i2c3_miss_start_stop_cnt, + &i2c3_arb_lost_cnt, + &i2c3_over_under_cnt, + &i2c3_pec_recep_cnt, + &i2c3_timeout_tlow_cnt, + &i2c3_smbus_alert_cnt, + &i2c3_unexpected_event_cnt, + &i2c3_last_unexpected_event, + &_bus3); } #endif #endif /* USE_I2C3 */ #if PERIODIC_TELEMETRY -static void send_i2c_err(struct transport_tx *trans __attribute__((unused)), struct link_device *dev __attribute__((unused))) { +static void send_i2c_err(struct transport_tx *trans __attribute__((unused)), + struct link_device *dev __attribute__((unused))) +{ static uint8_t _i2c_nb_cnt = 0; switch (_i2c_nb_cnt) { case 0: @@ -217,13 +227,15 @@ static void send_i2c_err(struct transport_tx *trans __attribute__((unused)), str break; } _i2c_nb_cnt++; - if (_i2c_nb_cnt == 4) + if (_i2c_nb_cnt == 4) { _i2c_nb_cnt = 0; + } } #endif -void i2c_init(struct i2c_periph* p) { +void i2c_init(struct i2c_periph *p) +{ p->trans_insert_idx = 0; p->trans_extract_idx = 0; p->status = I2CIdle; @@ -235,8 +247,8 @@ void i2c_init(struct i2c_periph* p) { } -bool_t i2c_transmit(struct i2c_periph* p, struct i2c_transaction* t, - uint8_t s_addr, uint8_t len) +bool_t i2c_transmit(struct i2c_periph *p, struct i2c_transaction *t, + uint8_t s_addr, uint8_t len) { t->type = I2CTransTx; t->slave_addr = s_addr; @@ -245,8 +257,8 @@ bool_t i2c_transmit(struct i2c_periph* p, struct i2c_transaction* t, return i2c_submit(p, t); } -bool_t i2c_receive(struct i2c_periph* p, struct i2c_transaction* t, - uint8_t s_addr, uint16_t len) +bool_t i2c_receive(struct i2c_periph *p, struct i2c_transaction *t, + uint8_t s_addr, uint16_t len) { t->type = I2CTransRx; t->slave_addr = s_addr; @@ -255,8 +267,8 @@ bool_t i2c_receive(struct i2c_periph* p, struct i2c_transaction* t, return i2c_submit(p, t); } -bool_t i2c_transceive(struct i2c_periph* p, struct i2c_transaction* t, - uint8_t s_addr, uint8_t len_w, uint16_t len_r) +bool_t i2c_transceive(struct i2c_periph *p, struct i2c_transaction *t, + uint8_t s_addr, uint8_t len_w, uint16_t len_r) { t->type = I2CTransTxRx; t->slave_addr = s_addr; diff --git a/sw/airborne/mcu_periph/i2c.h b/sw/airborne/mcu_periph/i2c.h index 30c3e2bf6d..9fb5174986 100644 --- a/sw/airborne/mcu_periph/i2c.h +++ b/sw/airborne/mcu_periph/i2c.h @@ -137,13 +137,13 @@ struct i2c_transaction { */ struct i2c_periph { /* circular buffer holding transactions */ - struct i2c_transaction* trans[I2C_TRANSACTION_QUEUE_LEN]; + struct i2c_transaction *trans[I2C_TRANSACTION_QUEUE_LEN]; uint8_t trans_insert_idx; uint8_t trans_extract_idx; /* internal state of the peripheral */ volatile enum I2CStatus status; volatile uint8_t idx_buf; - void* reg_addr; + void *reg_addr; void *init_struct; struct i2c_errors *errors; volatile int16_t watchdog; @@ -176,18 +176,18 @@ struct i2c_errors { memset((void*)_err.status_chain, 0, sizeof(_err.status_chain)); \ } -#define ZEROS_ERR_COUNTER(_i2c_err) { \ +#define ZEROS_ERR_COUNTER(_i2c_err) { \ _i2c_err.queue_full_cnt = 0; \ - _i2c_err.ack_fail_cnt = 0; \ - _i2c_err.miss_start_stop_cnt = 0; \ - _i2c_err.arb_lost_cnt = 0; \ - _i2c_err.over_under_cnt = 0; \ - _i2c_err.pec_recep_cnt = 0; \ - _i2c_err.timeout_tlow_cnt = 0; \ - _i2c_err.smbus_alert_cnt = 0; \ - _i2c_err.unexpected_event_cnt = 0; \ - _i2c_err.last_unexpected_event = 0; \ - _i2c_err.er_irq_cnt = 0; \ + _i2c_err.ack_fail_cnt = 0; \ + _i2c_err.miss_start_stop_cnt = 0; \ + _i2c_err.arb_lost_cnt = 0; \ + _i2c_err.over_under_cnt = 0; \ + _i2c_err.pec_recep_cnt = 0; \ + _i2c_err.timeout_tlow_cnt = 0; \ + _i2c_err.smbus_alert_cnt = 0; \ + _i2c_err.unexpected_event_cnt = 0; \ + _i2c_err.last_unexpected_event = 0; \ + _i2c_err.er_irq_cnt = 0; \ } @@ -224,13 +224,13 @@ extern void i2c3_init(void); /** Initialize I2C peripheral */ -extern void i2c_init(struct i2c_periph* p); +extern void i2c_init(struct i2c_periph *p); /** Check if I2C bus is idle. * @param p i2c peripheral to be used * @return TRUE if idle */ -extern bool_t i2c_idle(struct i2c_periph* p); +extern bool_t i2c_idle(struct i2c_periph *p); /** Submit a I2C transaction. * Must be implemented by the underlying architecture @@ -238,13 +238,13 @@ extern bool_t i2c_idle(struct i2c_periph* p); * @param t i2c transaction * @return TRUE if insertion to the transaction queue succeded */ -extern bool_t i2c_submit(struct i2c_periph* p, struct i2c_transaction* t); +extern bool_t i2c_submit(struct i2c_periph *p, struct i2c_transaction *t); /** Set I2C bitrate. * @param p i2c peripheral to be used * @param bitrate bitrate */ -extern void i2c_setbitrate(struct i2c_periph* p, int bitrate); +extern void i2c_setbitrate(struct i2c_periph *p, int bitrate); extern void i2c_event(void); /* @@ -263,7 +263,7 @@ extern void i2c_event(void); * @param len number of bytes to transmit * @return TRUE if insertion to the transaction queue succeded */ -extern bool_t i2c_transmit(struct i2c_periph* p, struct i2c_transaction* t, +extern bool_t i2c_transmit(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint8_t len); /** Submit a read only transaction. @@ -275,7 +275,7 @@ extern bool_t i2c_transmit(struct i2c_periph* p, struct i2c_transaction* t, * @param len number of bytes to receive * @return TRUE if insertion to the transaction queue succeded */ -extern bool_t i2c_receive(struct i2c_periph* p, struct i2c_transaction* t, +extern bool_t i2c_receive(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint16_t len); /** Submit a write/read transaction. @@ -288,7 +288,7 @@ extern bool_t i2c_receive(struct i2c_periph* p, struct i2c_transaction* t, * @param len_r number of bytes to receive * @return TRUE if insertion to the transaction queue succeded */ -extern bool_t i2c_transceive(struct i2c_periph* p, struct i2c_transaction* t, +extern bool_t i2c_transceive(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint8_t len_w, uint16_t len_r); /** @}*/ diff --git a/sw/airborne/mcu_periph/spi.c b/sw/airborne/mcu_periph/spi.c index a8b40a54c1..3402ad748b 100644 --- a/sw/airborne/mcu_periph/spi.c +++ b/sw/airborne/mcu_periph/spi.c @@ -34,7 +34,8 @@ #if USE_SPI0 struct spi_periph spi0; -void spi0_init(void) { +void spi0_init(void) +{ spi_init(&spi0); spi0_arch_init(); } @@ -44,7 +45,8 @@ void spi0_init(void) { #if USE_SPI1 struct spi_periph spi1; -void spi1_init(void) { +void spi1_init(void) +{ spi_init(&spi1); spi1_arch_init(); } @@ -54,7 +56,8 @@ void spi1_init(void) { #if USE_SPI2 struct spi_periph spi2; -void spi2_init(void) { +void spi2_init(void) +{ spi_init(&spi2); spi2_arch_init(); } @@ -64,14 +67,16 @@ void spi2_init(void) { #if USE_SPI3 struct spi_periph spi3; -void spi3_init(void) { +void spi3_init(void) +{ spi_init(&spi3); spi3_arch_init(); } #endif // USE_SPI3 -void spi_init(struct spi_periph* p) { +void spi_init(struct spi_periph *p) +{ p->trans_insert_idx = 0; p->trans_extract_idx = 0; p->status = SPIIdle; @@ -87,7 +92,8 @@ void spi_init(struct spi_periph* p) { #if USE_SPI0_SLAVE struct spi_periph spi0; -void spi0_slave_init(void) { +void spi0_slave_init(void) +{ spi_slave_init(&spi0); spi0_slave_arch_init(); } @@ -97,7 +103,8 @@ void spi0_slave_init(void) { #if USE_SPI1_SLAVE struct spi_periph spi1; -void spi1_slave_init(void) { +void spi1_slave_init(void) +{ spi_slave_init(&spi1); spi1_slave_arch_init(); } @@ -107,7 +114,8 @@ void spi1_slave_init(void) { #if USE_SPI2_SLAVE struct spi_periph spi2; -void spi2_slave_init(void) { +void spi2_slave_init(void) +{ spi_slave_init(&spi2); spi2_slave_arch_init(); } @@ -117,14 +125,16 @@ void spi2_slave_init(void) { #if USE_SPI3_SLAVE struct spi_periph spi3; -void spi3_slave_init(void) { +void spi3_slave_init(void) +{ spi_slave_init(&spi3); spi3_slave_arch_init(); } #endif // USE_SPI3_SLAVE -extern void spi_slave_init(struct spi_periph* p) { +extern void spi_slave_init(struct spi_periph *p) +{ p->trans_insert_idx = 0; p->trans_extract_idx = 0; p->status = SPIIdle; diff --git a/sw/airborne/mcu_periph/spi.h b/sw/airborne/mcu_periph/spi.h index 369d6a9b9b..d070c60b24 100644 --- a/sw/airborne/mcu_periph/spi.h +++ b/sw/airborne/mcu_periph/spi.h @@ -127,7 +127,7 @@ struct spi_transaction; * If not NULL (or 0), call function (with transaction as parameter) * before or after transaction, e.g to allow execution of hardware specific actions */ -typedef void (*SPICallback)( struct spi_transaction *trans ); +typedef void (*SPICallback)(struct spi_transaction *trans); /** SPI transaction structure. * - Use this structure to store a request of SPI transaction @@ -140,8 +140,8 @@ typedef void (*SPICallback)( struct spi_transaction *trans ); * 0 is sent for the remaining words */ struct spi_transaction { - volatile uint8_t* input_buf; ///< pointer to receive buffer for DMA - volatile uint8_t* output_buf; ///< pointer to transmit buffer for DMA + volatile uint8_t *input_buf; ///< pointer to receive buffer for DMA + volatile uint8_t *output_buf; ///< pointer to transmit buffer for DMA uint8_t input_length; ///< number of data words to read uint8_t output_length; ///< number of data words to write uint8_t slave_idx; ///< slave id: #SPI_SLAVE0 to #SPI_SLAVE4 @@ -167,7 +167,7 @@ struct spi_transaction { */ struct spi_periph { /** circular buffer holding transactions */ - struct spi_transaction* trans[SPI_TRANSACTION_QUEUE_LEN]; + struct spi_transaction *trans[SPI_TRANSACTION_QUEUE_LEN]; uint8_t trans_insert_idx; uint8_t trans_extract_idx; /** internal state of the peripheral */ @@ -244,7 +244,7 @@ extern void spi3_arch_init(void); /** Initialize a spi peripheral. * @param p spi peripheral to be configured */ -extern void spi_init(struct spi_periph* p); +extern void spi_init(struct spi_periph *p); /** Initialize all used slaves and uselect them. */ @@ -256,7 +256,7 @@ extern void spi_init_slaves(void); * @param t spi transaction * @return TRUE if insertion to the transaction queue succeded */ -extern bool_t spi_submit(struct spi_periph* p, struct spi_transaction* t); +extern bool_t spi_submit(struct spi_periph *p, struct spi_transaction *t); /** Select a slave. * @param slave slave id @@ -276,7 +276,7 @@ extern void spi_slave_unselect(uint8_t slave); * @param slave slave id * @return true if correctly locked */ -extern bool_t spi_lock(struct spi_periph* p, uint8_t slave); +extern bool_t spi_lock(struct spi_periph *p, uint8_t slave); /** Resume the SPI fifo. * Only the slave that locks the fifo can unlock it. @@ -284,7 +284,7 @@ extern bool_t spi_lock(struct spi_periph* p, uint8_t slave); * @param slave slave id * @return true if correctly unlocked */ -extern bool_t spi_resume(struct spi_periph* p, uint8_t slave); +extern bool_t spi_resume(struct spi_periph *p, uint8_t slave); #endif /* SPI_MASTER */ @@ -341,7 +341,7 @@ extern void spi3_slave_arch_init(void); /** Initialize a spi peripheral in slave mode. * @param p spi peripheral to be configured */ -extern void spi_slave_init(struct spi_periph* p); +extern void spi_slave_init(struct spi_periph *p); /** Register a spi transaction in slave mode (only one transaction can be registered). * Must be implemented by the underlying architecture @@ -349,7 +349,7 @@ extern void spi_slave_init(struct spi_periph* p); * @param t spi transaction * @return return true if registered with success */ -extern bool_t spi_slave_register(struct spi_periph* p, struct spi_transaction* t); +extern bool_t spi_slave_register(struct spi_periph *p, struct spi_transaction *t); /** Initialized and wait for the next transaction. * If a transaction is registered for this peripheral, the spi will be @@ -357,7 +357,7 @@ extern bool_t spi_slave_register(struct spi_periph* p, struct spi_transaction* t * @param p spi peripheral to be used * @return return true if a transaction was register for this peripheral */ -extern bool_t spi_slave_wait(struct spi_periph* p); +extern bool_t spi_slave_wait(struct spi_periph *p); #endif /* SPI_SLAVE */ diff --git a/sw/airborne/mcu_periph/sys_time.c b/sw/airborne/mcu_periph/sys_time.c index 81c3e58398..026b622a1a 100644 --- a/sw/airborne/mcu_periph/sys_time.c +++ b/sw/airborne/mcu_periph/sys_time.c @@ -34,10 +34,11 @@ PRINT_CONFIG_VAR(SYS_TIME_FREQUENCY) struct sys_time sys_time; -int sys_time_register_timer(float duration, sys_time_cb cb) { +int sys_time_register_timer(float duration, sys_time_cb cb) +{ uint32_t start_time = sys_time.nb_tick; - for (int i = 0; i< SYS_TIME_NB_TIMER; i++) { + for (int i = 0; i < SYS_TIME_NB_TIMER; i++) { if (!sys_time.timer[i].in_use) { sys_time.timer[i].cb = cb; sys_time.timer[i].elapsed = FALSE; @@ -51,7 +52,8 @@ int sys_time_register_timer(float duration, sys_time_cb cb) { } -void sys_time_cancel_timer(tid_t id) { +void sys_time_cancel_timer(tid_t id) +{ sys_time.timer[id].in_use = FALSE; sys_time.timer[id].cb = NULL; sys_time.timer[id].elapsed = FALSE; @@ -60,14 +62,16 @@ void sys_time_cancel_timer(tid_t id) { } // FIXME: race condition ?? -void sys_time_update_timer(tid_t id, float duration) { +void sys_time_update_timer(tid_t id, float duration) +{ mcu_int_disable(); sys_time.timer[id].end_time -= (sys_time.timer[id].duration - sys_time_ticks_of_sec(duration)); sys_time.timer[id].duration = sys_time_ticks_of_sec(duration); mcu_int_enable(); } -void sys_time_init( void ) { +void sys_time_init(void) +{ sys_time.nb_sec = 0; sys_time.nb_sec_rem = 0; sys_time.nb_tick = 0; @@ -75,7 +79,7 @@ void sys_time_init( void ) { sys_time.ticks_per_sec = SYS_TIME_FREQUENCY; sys_time.resolution = 1.0 / sys_time.ticks_per_sec; - for (unsigned int i=0; irx_insert_idx = 0; p->rx_extract_idx = 0; p->tx_insert_idx = 0; @@ -200,19 +210,23 @@ void uart_periph_init(struct uart_periph* p) { #endif } -bool_t uart_check_free_space(struct uart_periph* p, uint8_t len) { +bool_t uart_check_free_space(struct uart_periph *p, uint8_t len) +{ int16_t space = p->tx_extract_idx - p->tx_insert_idx; - if (space <= 0) + if (space <= 0) { space += UART_TX_BUFFER_SIZE; + } return (uint16_t)(space - 1) >= len; } -uint8_t uart_getch(struct uart_periph* p) { +uint8_t uart_getch(struct uart_periph *p) +{ uint8_t ret = p->rx_buf[p->rx_extract_idx]; p->rx_extract_idx = (p->rx_extract_idx + 1) % UART_RX_BUFFER_SIZE; return ret; } -void WEAK uart_event(void) { +void WEAK uart_event(void) +{ } diff --git a/sw/airborne/mcu_periph/uart.h b/sw/airborne/mcu_periph/uart.h index 371b22babf..b4c5ca0919 100644 --- a/sw/airborne/mcu_periph/uart.h +++ b/sw/airborne/mcu_periph/uart.h @@ -64,7 +64,7 @@ struct uart_periph { uint16_t tx_extract_idx; uint8_t tx_running; /** UART Register */ - void* reg_addr; + void *reg_addr; /** UART Dev (linux) */ char dev[UART_DEV_NAME_SIZE]; volatile uint16_t ore; ///< overrun error counter @@ -75,23 +75,25 @@ struct uart_periph { }; -extern void uart_periph_init(struct uart_periph* p); -extern void uart_periph_set_baudrate(struct uart_periph* p, uint32_t baud); -extern void uart_periph_set_bits_stop_parity(struct uart_periph* p, uint8_t bits, uint8_t stop, uint8_t parity); -extern void uart_periph_set_mode(struct uart_periph* p, bool_t tx_enabled, bool_t rx_enabled, bool_t hw_flow_control); -extern void uart_transmit(struct uart_periph* p, uint8_t data); -extern bool_t uart_check_free_space(struct uart_periph* p, uint8_t len); -extern uint8_t uart_getch(struct uart_periph* p); +extern void uart_periph_init(struct uart_periph *p); +extern void uart_periph_set_baudrate(struct uart_periph *p, uint32_t baud); +extern void uart_periph_set_bits_stop_parity(struct uart_periph *p, uint8_t bits, uint8_t stop, uint8_t parity); +extern void uart_periph_set_mode(struct uart_periph *p, bool_t tx_enabled, bool_t rx_enabled, bool_t hw_flow_control); +extern void uart_transmit(struct uart_periph *p, uint8_t data); +extern bool_t uart_check_free_space(struct uart_periph *p, uint8_t len); +extern uint8_t uart_getch(struct uart_periph *p); extern void uart_event(void); /** * Check UART for available chars in receive buffer. * @return number of chars in the buffer */ -static inline uint16_t uart_char_available(struct uart_periph* p) { +static inline uint16_t uart_char_available(struct uart_periph *p) +{ int16_t available = p->rx_insert_idx - p->rx_extract_idx; - if (available < 0) + if (available < 0) { available += UART_RX_BUFFER_SIZE; + } return (uint16_t)available; } diff --git a/sw/airborne/mcu_periph/udp.c b/sw/airborne/mcu_periph/udp.c index 69b45dfcef..7854731a21 100644 --- a/sw/airborne/mcu_periph/udp.c +++ b/sw/airborne/mcu_periph/udp.c @@ -27,7 +27,7 @@ #include "mcu_periph/udp.h" -void udp_periph_init(struct udp_periph* p, char* host, int port_out, int port_in, bool_t broadcast) +void udp_periph_init(struct udp_periph *p, char *host, int port_out, int port_in, bool_t broadcast) { p->rx_insert_idx = 0; p->rx_extract_idx = 0; @@ -41,12 +41,12 @@ void udp_periph_init(struct udp_periph* p, char* host, int port_out, int port_in udp_arch_periph_init(p, host, port_out, port_in, broadcast); } -bool_t udp_check_free_space(struct udp_periph* p, uint8_t len) +bool_t udp_check_free_space(struct udp_periph *p, uint8_t len) { return (UDP_TX_BUFFER_SIZE - p->tx_insert_idx) >= len; } -void udp_transmit(struct udp_periph* p, uint8_t data) +void udp_transmit(struct udp_periph *p, uint8_t data) { if (p->tx_insert_idx >= UDP_TX_BUFFER_SIZE) { return; // no room @@ -56,7 +56,7 @@ void udp_transmit(struct udp_periph* p, uint8_t data) p->tx_insert_idx++; } -uint16_t udp_char_available(struct udp_periph* p) +uint16_t udp_char_available(struct udp_periph *p) { int16_t available = p->rx_insert_idx - p->rx_extract_idx; if (available < 0) { @@ -65,7 +65,7 @@ uint16_t udp_char_available(struct udp_periph* p) return (uint16_t)available; } -uint8_t udp_getch(struct udp_periph* p) +uint8_t udp_getch(struct udp_periph *p) { uint8_t ret = p->rx_buf[p->rx_extract_idx]; p->rx_extract_idx = (p->rx_extract_idx + 1) % UDP_RX_BUFFER_SIZE; diff --git a/sw/airborne/mcu_periph/udp.h b/sw/airborne/mcu_periph/udp.h index d32113e69a..344b00bc40 100644 --- a/sw/airborne/mcu_periph/udp.h +++ b/sw/airborne/mcu_periph/udp.h @@ -44,20 +44,20 @@ struct udp_periph { uint8_t tx_buf[UDP_TX_BUFFER_SIZE]; uint16_t tx_insert_idx; /** UDP network */ - void* network; + void *network; /** Generic device interface */ struct link_device device; }; -extern void udp_periph_init(struct udp_periph* p, char* host, int port_out, int port_in, bool_t broadcast); -extern bool_t udp_check_free_space(struct udp_periph* p, uint8_t len); -extern void udp_transmit(struct udp_periph* p, uint8_t data); -extern uint16_t udp_char_available(struct udp_periph* p); -extern uint8_t udp_getch(struct udp_periph* p); +extern void udp_periph_init(struct udp_periph *p, char *host, int port_out, int port_in, bool_t broadcast); +extern bool_t udp_check_free_space(struct udp_periph *p, uint8_t len); +extern void udp_transmit(struct udp_periph *p, uint8_t data); +extern uint16_t udp_char_available(struct udp_periph *p); +extern uint8_t udp_getch(struct udp_periph *p); extern void udp_event(void); -extern void udp_arch_periph_init(struct udp_periph* p, char* host, int port_out, int port_in, bool_t broadcast); -extern void udp_send_message(struct udp_periph* p); -extern void udp_receive(struct udp_periph* p); +extern void udp_arch_periph_init(struct udp_periph *p, char *host, int port_out, int port_in, bool_t broadcast); +extern void udp_send_message(struct udp_periph *p); +extern void udp_receive(struct udp_periph *p); #if USE_UDP0 extern struct udp_periph udp0; diff --git a/sw/airborne/modules/MPPT/MPPT.c b/sw/airborne/modules/MPPT/MPPT.c index 6e1442e775..3d51e8e3aa 100644 --- a/sw/airborne/modules/MPPT/MPPT.c +++ b/sw/airborne/modules/MPPT/MPPT.c @@ -70,13 +70,15 @@ static uint8_t MPPT_status; static uint8_t data_index = 0xff; static int16_t MPPT_data[NB_DATA]; -void MPPT_init( void ) { +void MPPT_init(void) +{ MPPT_mode = 0; MPPT_status = MPPT_STATUS_IDLE; } -static void MPPT_ask( void ) { +static void MPPT_ask(void) +{ data_index++; if (data_index >= NB_I2C_DATA) { /* Setting the current value */ @@ -92,7 +94,8 @@ static void MPPT_ask( void ) { MPPT_status = MPPT_STATUS_ASKING; } -void MPPT_periodic( void ) { +void MPPT_periodic(void) +{ if (mppt_trans.status == I2CTransSuccess) { switch (MPPT_status) { @@ -122,8 +125,9 @@ void MPPT_periodic( void ) { case MPPT_STATUS_READING: /* We got 2 bytes */ - if (data_index < NB_I2C_DATA) - MPPT_data[data_index] = (mppt_trans.buf[0]<<8) | mppt_trans.buf[1]; + if (data_index < NB_I2C_DATA) { + MPPT_data[data_index] = (mppt_trans.buf[0] << 8) | mppt_trans.buf[1]; + } MPPT_status = MPPT_STATUS_IDLE; break; } diff --git a/sw/airborne/modules/MPPT/MPPT.h b/sw/airborne/modules/MPPT/MPPT.h index c54f343ce6..d1ab056aaa 100644 --- a/sw/airborne/modules/MPPT/MPPT.h +++ b/sw/airborne/modules/MPPT/MPPT.h @@ -43,8 +43,8 @@ extern uint8_t MPPT_mode; -void MPPT_init( void ); -void MPPT_automata( void ); -void MPPT_periodic( void ); +void MPPT_init(void); +void MPPT_automata(void); +void MPPT_periodic(void); #endif // MPPT_H diff --git a/sw/airborne/modules/MPPT/sim_MPPT.c b/sw/airborne/modules/MPPT/sim_MPPT.c index bdc7827ec3..e4a8e7d9f1 100644 --- a/sw/airborne/modules/MPPT/sim_MPPT.c +++ b/sw/airborne/modules/MPPT/sim_MPPT.c @@ -31,14 +31,17 @@ uint8_t MPPT_mode; static int16_t MPPT_data[NB_DATA]; -void MPPT_init( void ) { +void MPPT_init(void) +{ uint8_t i = 0; - for(i = 0; i < NB_DATA; i++) + for (i = 0; i < NB_DATA; i++) { MPPT_data[i] = 42 + i; + } } -void MPPT_periodic( void ) { +void MPPT_periodic(void) +{ MPPT_data[MPPT_ITOTAL_INDEX] = MPPT_data[MPPT_IBAT_INDEX] + MPPT_data[MPPT_ICONV_INDEX]; RunOnceEvery(8, DOWNLINK_SEND_MPPT(DefaultChannel, DefaultDevice, NB_DATA, MPPT_data)); diff --git a/sw/airborne/modules/adcs/adc_generic.c b/sw/airborne/modules/adcs/adc_generic.c index 1fc6596b46..318de1995e 100644 --- a/sw/airborne/modules/adcs/adc_generic.c +++ b/sw/airborne/modules/adcs/adc_generic.c @@ -27,7 +27,8 @@ static struct adc_buf buf_generic1; static struct adc_buf buf_generic2; #endif -void adc_generic_init( void ) { +void adc_generic_init(void) +{ #ifdef ADC_CHANNEL_GENERIC1 adc_buf_channel(ADC_CHANNEL_GENERIC1, &buf_generic1, ADC_CHANNEL_GENERIC_NB_SAMPLES); #endif @@ -36,7 +37,8 @@ void adc_generic_init( void ) { #endif } -void adc_generic_periodic( void ) { +void adc_generic_periodic(void) +{ #ifdef ADC_CHANNEL_GENERIC1 adc_generic_val1 = buf_generic1.sum / buf_generic1.av_nb_sample; #endif diff --git a/sw/airborne/modules/adcs/adc_generic.h b/sw/airborne/modules/adcs/adc_generic.h index 8f3daa7599..4b722dad06 100644 --- a/sw/airborne/modules/adcs/adc_generic.h +++ b/sw/airborne/modules/adcs/adc_generic.h @@ -5,7 +5,7 @@ extern uint16_t adc_generic_val1; extern uint16_t adc_generic_val2; -void adc_generic_init( void ); -void adc_generic_periodic( void ); +void adc_generic_init(void); +void adc_generic_periodic(void); #endif /* ADC_GENERIC_H */ diff --git a/sw/airborne/modules/adcs/max11040.c b/sw/airborne/modules/adcs/max11040.c index 31c05227f5..b714c359cc 100644 --- a/sw/airborne/modules/adcs/max11040.c +++ b/sw/airborne/modules/adcs/max11040.c @@ -41,7 +41,8 @@ volatile uint32_t max11040_buf_in; volatile uint32_t max11040_buf_out; -void max11040_init( void ) { +void max11040_init(void) +{ max11040_status = MAX11040_RESET; max11040_data = MAX11040_RESET; max11040_count = 0; @@ -51,36 +52,37 @@ void max11040_init( void ) { max11040_hw_init(); } -void max11040_periodic(void) { +void max11040_periodic(void) +{ #ifdef MAX11040_DEBUG float max11040_values_f[16]; int i; if (max11040_data == MAX11040_DATA_AVAILABLE) { // LED_TOGGLE(3); - for (i=0; i<16; i++) { + for (i = 0; i < 16; i++) { /* we assume that the buffer will be full always in this test mode anyway */ max11040_values_f[i] = (max11040_values[max11040_buf_in][i] * 2.2) / 8388608.0; } DOWNLINK_SEND_TURB_PRESSURE_VOLTAGE( - DefaultChannel, DefaultDevice, - &max11040_values_f[0], - &max11040_values_f[1], - &max11040_values_f[2], - &max11040_values_f[3], - &max11040_values_f[4], - &max11040_values_f[5], - &max11040_values_f[6], - &max11040_values_f[7], - &max11040_values_f[8], - &max11040_values_f[9], - &max11040_values_f[10], - &max11040_values_f[11], - &max11040_values_f[12], - &max11040_values_f[13], - &max11040_values_f[14], - &max11040_values_f[15] ); + DefaultChannel, DefaultDevice, + &max11040_values_f[0], + &max11040_values_f[1], + &max11040_values_f[2], + &max11040_values_f[3], + &max11040_values_f[4], + &max11040_values_f[5], + &max11040_values_f[6], + &max11040_values_f[7], + &max11040_values_f[8], + &max11040_values_f[9], + &max11040_values_f[10], + &max11040_values_f[11], + &max11040_values_f[12], + &max11040_values_f[13], + &max11040_values_f[14], + &max11040_values_f[15]); max11040_data = MAX11040_IDLE; } #endif diff --git a/sw/airborne/modules/adcs/max11040.h b/sw/airborne/modules/adcs/max11040.h index 4ea44141c7..067a134b1a 100644 --- a/sw/airborne/modules/adcs/max11040.h +++ b/sw/airborne/modules/adcs/max11040.h @@ -28,7 +28,7 @@ extern volatile uint32_t max11040_buf_out; #define MAX11040_IDLE 0 #define MAX11040_DATA_AVAILABLE 1 -void max11040_init( void ); +void max11040_init(void); void max11040_periodic(void); diff --git a/sw/airborne/modules/air_data/air_data.c b/sw/airborne/modules/air_data/air_data.c index e7576d4a46..41a4945bdc 100644 --- a/sw/airborne/modules/air_data/air_data.c +++ b/sw/airborne/modules/air_data/air_data.c @@ -108,7 +108,7 @@ static void pressure_abs_cb(uint8_t __attribute__((unused)) sender_id, const flo if (air_data.calc_amsl_baro && air_data.qnh > 0) { air_data.amsl_baro = pprz_isa_height_of_pressure_full(air_data.pressure, - air_data.qnh * 100.f); + air_data.qnh * 100.f); air_data.amsl_baro_valid = TRUE; } @@ -218,8 +218,7 @@ void air_data_periodic(void) // Watchdog on baro if (baro_health_counter > 0) { baro_health_counter--; - } - else { + } else { air_data.amsl_baro_valid = FALSE; } } diff --git a/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c b/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c index ecfd075b88..1111b73277 100644 --- a/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c +++ b/sw/airborne/modules/airborne_ant_track/airborne_ant_track.c @@ -29,28 +29,30 @@ typedef struct { - float fx; - float fy; - float fz;} VECTOR; + float fx; + float fy; + float fz; +} VECTOR; typedef struct { - float fx1; float fx2; float fx3; - float fy1; float fy2; float fy3; - float fz1; float fz2; float fz3;} MATRIX; + float fx1; float fx2; float fx3; + float fy1; float fy2; float fy3; + float fz1; float fz2; float fz3; +} MATRIX; float airborne_ant_pan; static bool_t ant_pan_positive = 0; void ant_point(void); -static void vSubtractVectors(VECTOR* svA, VECTOR svB, VECTOR svC); -static void vMultiplyMatrixByVector(VECTOR* svA, MATRIX smB, VECTOR svC); +static void vSubtractVectors(VECTOR *svA, VECTOR svB, VECTOR svC); +static void vMultiplyMatrixByVector(VECTOR *svA, MATRIX smB, VECTOR svC); /******************************************************************* ; function name: vSubtractVectors ; description: subtracts two vectors a = b - c ; parameters: ;*******************************************************************/ -static void vSubtractVectors(VECTOR* svA, VECTOR svB, VECTOR svC) +static void vSubtractVectors(VECTOR *svA, VECTOR svB, VECTOR svC) { svA->fx = svB.fx - svC.fx; svA->fy = svB.fy - svC.fy; @@ -62,26 +64,27 @@ static void vSubtractVectors(VECTOR* svA, VECTOR svB, VECTOR svC) ; description: multiplies matrix by vector svA = smB * svC ; parameters: ;*******************************************************************/ -static void vMultiplyMatrixByVector(VECTOR* svA, MATRIX smB, VECTOR svC) +static void vMultiplyMatrixByVector(VECTOR *svA, MATRIX smB, VECTOR svC) { svA->fx = smB.fx1 * svC.fx + smB.fx2 * svC.fy + smB.fx3 * svC.fz; svA->fy = smB.fy1 * svC.fx + smB.fy2 * svC.fy + smB.fy3 * svC.fz; svA->fz = smB.fz1 * svC.fx + smB.fz2 * svC.fy + smB.fz3 * svC.fz; } -void airborne_ant_point_init(void){ +void airborne_ant_point_init(void) +{ -return; + return; } void airborne_ant_point_periodic(void) { -float airborne_ant_pan_servo = 0; + float airborne_ant_pan_servo = 0; static VECTOR svPlanePosition, - Home_Position, - Home_PositionForPlane, - Home_PositionForPlane2; + Home_Position, + Home_PositionForPlane, + Home_PositionForPlane2; static MATRIX smRotation; @@ -110,32 +113,32 @@ float airborne_ant_pan_servo = 0; vMultiplyMatrixByVector(&Home_PositionForPlane2, smRotation, Home_PositionForPlane); -/* - * This is for one axis pan antenna mechanisms. The default is to - * circle clockwise so view is right. The pan servo neutral makes - * the antenna look to the right with 0° given, 90° is to the back and - * -90° is to the front. - * - * - * - * plane front - * - * 90 - ^ - * I - * 135 I 45° - * \ I / - * \I/ - * 180-------I------- 0° - * /I\ - * / I \ - * -135 I -45° - * I - * -90 - * plane back - * - * - */ + /* + * This is for one axis pan antenna mechanisms. The default is to + * circle clockwise so view is right. The pan servo neutral makes + * the antenna look to the right with 0° given, 90° is to the back and + * -90° is to the front. + * + * + * + * plane front + * + * 90 + ^ + * I + * 135 I 45° + * \ I / + * \I/ + * 180-------I------- 0° + * /I\ + * / I \ + * -135 I -45° + * I + * -90 + * plane back + * + * + */ /* fPan = 0 -> antenna looks along the wing 90 -> antenna looks in flight direction @@ -144,24 +147,25 @@ float airborne_ant_pan_servo = 0; /* fixed to the plane*/ airborne_ant_pan = (float)(atan2(Home_PositionForPlane2.fx, (Home_PositionForPlane2.fy))); - // I need to avoid oscillations around the 180 degree mark. - if (airborne_ant_pan > 0 && airborne_ant_pan <= RadOfDeg(175)){ ant_pan_positive = 1; } - if (airborne_ant_pan < 0 && airborne_ant_pan >= RadOfDeg(-175)){ ant_pan_positive = 0; } + // I need to avoid oscillations around the 180 degree mark. + if (airborne_ant_pan > 0 && airborne_ant_pan <= RadOfDeg(175)) { ant_pan_positive = 1; } + if (airborne_ant_pan < 0 && airborne_ant_pan >= RadOfDeg(-175)) { ant_pan_positive = 0; } - if (airborne_ant_pan > RadOfDeg(175) && ant_pan_positive == 0){ - airborne_ant_pan = RadOfDeg(-180); + if (airborne_ant_pan > RadOfDeg(175) && ant_pan_positive == 0) { + airborne_ant_pan = RadOfDeg(-180); - }else if (airborne_ant_pan < RadOfDeg(-175) && ant_pan_positive){ - airborne_ant_pan = RadOfDeg(180); - ant_pan_positive = 0; - } + } else if (airborne_ant_pan < RadOfDeg(-175) && ant_pan_positive) { + airborne_ant_pan = RadOfDeg(180); + ant_pan_positive = 0; + } #ifdef ANT_PAN_NEUTRAL airborne_ant_pan = airborne_ant_pan - RadOfDeg(ANT_PAN_NEUTRAL); - if (airborne_ant_pan > 0) + if (airborne_ant_pan > 0) { airborne_ant_pan_servo = MAX_PPRZ * (airborne_ant_pan / (RadOfDeg(ANT_PAN_MAX - ANT_PAN_NEUTRAL))); - else + } else { airborne_ant_pan_servo = MIN_PPRZ * (airborne_ant_pan / (RadOfDeg(ANT_PAN_MIN - ANT_PAN_NEUTRAL))); + } #endif airborne_ant_pan_servo = TRIM_PPRZ(airborne_ant_pan_servo); @@ -171,7 +175,7 @@ float airborne_ant_pan_servo = 0; #endif -return; + return; } #endif diff --git a/sw/airborne/modules/bat_checker/bat_checker.c b/sw/airborne/modules/bat_checker/bat_checker.c index bdc716cb4d..0de6fe633b 100644 --- a/sw/airborne/modules/bat_checker/bat_checker.c +++ b/sw/airborne/modules/bat_checker/bat_checker.c @@ -38,18 +38,21 @@ #endif -void init_bat_checker(void) { +void init_bat_checker(void) +{ LED_INIT(BAT_CHECKER_LED); LED_OFF(BAT_CHECKER_LED); } -void bat_checker_periodic(void) { +void bat_checker_periodic(void) +{ - if (electrical.bat_critical) + if (electrical.bat_critical) { LED_ON(BAT_CHECKER_LED); - else if (electrical.bat_low) + } else if (electrical.bat_low) { LED_TOGGLE(BAT_CHECKER_LED); - else + } else { LED_OFF(BAT_CHECKER_LED); + } } diff --git a/sw/airborne/modules/benchmark/flight_benchmark.c b/sw/airborne/modules/benchmark/flight_benchmark.c index bdc3bf0c84..a3efe06679 100644 --- a/sw/airborne/modules/benchmark/flight_benchmark.c +++ b/sw/airborne/modules/benchmark/flight_benchmark.c @@ -34,7 +34,8 @@ bool_t benchm_go; -void flight_benchmark_init( void ) { +void flight_benchmark_init(void) +{ SquareSumErr_airspeed = 0; SquareSumErr_altitude = 0; SquareSumErr_position = 0; @@ -45,76 +46,79 @@ void flight_benchmark_init( void ) { benchm_go = 0; } -void flight_benchmark_periodic( void ) { +void flight_benchmark_periodic(void) +{ float Err_airspeed = 0; float Err_altitude = 0; float Err_position = 0; - if (benchm_reset){ + if (benchm_reset) { flight_benchmark_reset(); benchm_reset = 0; } - if (benchm_go){ - #if USE_AIRSPEED && defined(BENCHMARK_AIRSPEED) + 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; + if (Err_airspeed > ToleranceAispeed) { + Err_airspeed = Err_airspeed - ToleranceAispeed; SquareSumErr_airspeed += (Err_airspeed * Err_airspeed); } - #endif +#endif - #ifdef BENCHMARK_ALTITUDE +#ifdef BENCHMARK_ALTITUDE Err_altitude = fabs(stateGetPositionUtm_f()->alt - v_ctl_altitude_setpoint); - if (Err_altitude>ToleranceAltitude){ - Err_altitude = Err_altitude-ToleranceAltitude; + if (Err_altitude > ToleranceAltitude) { + Err_altitude = Err_altitude - ToleranceAltitude; SquareSumErr_altitude += (Err_altitude * Err_altitude); } - #endif +#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; + // 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)); -// } +// 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)); +// } -// 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); -// } - if (Err_position_circle < Err_position_segment){ +// 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); +// } + if (Err_position_circle < Err_position_segment) { Err_position = Err_position_circle; - } - else + } else { Err_position = Err_position_segment; + } - if (Err_position>TolerancePosition){ + if (Err_position > TolerancePosition) { SquareSumErr_position += (Err_position * Err_position); } - #endif +#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 ) { +void flight_benchmark_reset(void) +{ SquareSumErr_airspeed = 0; SquareSumErr_altitude = 0; SquareSumErr_position = 0; diff --git a/sw/airborne/modules/benchmark/flight_benchmark.h b/sw/airborne/modules/benchmark/flight_benchmark.h index 60afa0fab6..d2cc190ae5 100644 --- a/sw/airborne/modules/benchmark/flight_benchmark.h +++ b/sw/airborne/modules/benchmark/flight_benchmark.h @@ -3,9 +3,9 @@ #include -void flight_benchmark_init( void ); -void flight_benchmark_periodic( void ); -void flight_benchmark_reset( void ); +void flight_benchmark_init(void); +void flight_benchmark_periodic(void); +void flight_benchmark_reset(void); extern float ToleranceAispeed; extern float ToleranceAltitude; diff --git a/sw/airborne/modules/benchmark/i2c_abuse_test.c b/sw/airborne/modules/benchmark/i2c_abuse_test.c index 268fcf302a..a0a6758fcd 100644 --- a/sw/airborne/modules/benchmark/i2c_abuse_test.c +++ b/sw/airborne/modules/benchmark/i2c_abuse_test.c @@ -42,7 +42,8 @@ struct i2c_transaction i2c_test2; volatile uint8_t i2c_abuse_test_counter = 0; volatile uint32_t i2c_abuse_test_bitrate = 1000; -void init_i2c_abuse_test(void) { +void init_i2c_abuse_test(void) +{ //LED_INIT(DEMO_MODULE_LED); //LED_OFF(DEMO_MODULE_LED); @@ -63,179 +64,167 @@ static void i2c_abuse_send_transaction(uint8_t _init) i2c_test1.len_w = 0; i2c_test1.len_r = 0; - switch (_init) - { - case 1: - i2c_test1.type = I2CTransTx; - i2c_test1.buf[0] = 0x00; // set to rate to 50Hz - i2c_test1.buf[1] = 0x00 | (0x06 << 2); - i2c_test1.buf[2] = 0x01<<5; - i2c_test1.buf[3] = 0x00; - i2c_test1.len_w = 4; - i2c_submit(&I2C_ABUSE_PORT,&i2c_test1); - break; - case 2: - i2c_test1.type = I2CTransTx; - i2c_test1.buf[0] = 0x01; // set to gain to 1 Gauss - i2c_test1.buf[1] = 0x01<<5; - i2c_test1.len_w = 2; - i2c_submit(&I2C_ABUSE_PORT,&i2c_test1); - break; - case 3: - i2c_test1.type = I2CTransTx; - i2c_test1.buf[0] = 0x00; // set to continuous mode - i2c_test1.len_w = 1; - i2c_submit(&I2C_ABUSE_PORT,&i2c_test1); - break; - case 4: - i2c_test1.type = I2CTransRx; - i2c_test1.len_r = 1; - i2c_submit(&I2C_ABUSE_PORT,&i2c_test1); - break; - case 5: - i2c_test1.type = I2CTransRx; - i2c_test1.len_r = 2; - i2c_submit(&I2C_ABUSE_PORT,&i2c_test1); - break; - case 6: - i2c_test1.type = I2CTransRx; - i2c_test1.len_r = 3; - i2c_submit(&I2C_ABUSE_PORT,&i2c_test1); - break; - case 7: - i2c_test1.type = I2CTransRx; - i2c_test1.len_r = 4; - i2c_submit(&I2C_ABUSE_PORT,&i2c_test1); - break; - case 8: - i2c_test1.type = I2CTransRx; - i2c_test1.len_r = 5; - i2c_submit(&I2C_ABUSE_PORT,&i2c_test1); - break; - case 9: - // bad addr - i2c_test1.slave_addr = 0x3C + 2; - i2c_test1.type = I2CTransTx; - i2c_test1.len_w = 1; - i2c_submit(&I2C_ABUSE_PORT,&i2c_test1); - break; - case 10: - // 2 consecutive - i2c_test1.type = I2CTransTx; - i2c_test1.buf[0] = 0x00; // set to continuous mode - i2c_test1.len_w = 1; - i2c_submit(&I2C_ABUSE_PORT,&i2c_test1); - break; - case 11: - i2c_test1.slave_addr = 0x3C; - i2c_test1.type = I2CTransTxRx; - i2c_test1.len_r = 1; - i2c_test1.len_w = 1; - i2c_test1.buf[0] = 0x03; - i2c_submit(&I2C_ABUSE_PORT, &i2c_test1); - break; - case 12: - i2c_test1.slave_addr = 0x3C; - i2c_test1.type = I2CTransTxRx; - i2c_test1.len_r = 2; - i2c_test1.len_w = 1; - i2c_test1.buf[0] = 0x03; - i2c_submit(&I2C_ABUSE_PORT, &i2c_test1); - break; - case 13: - i2c_test1.slave_addr = 0x3C; - i2c_test1.type = I2CTransTxRx; - i2c_test1.len_r = 3; - i2c_test1.len_w = 1; - i2c_test1.buf[0] = 0x03; - i2c_submit(&I2C_ABUSE_PORT, &i2c_test1); - break; - case 14: - i2c_test1.slave_addr = 0x3C; - i2c_test1.type = I2CTransTxRx; - i2c_test1.len_r = 4; - i2c_test1.len_w = 1; - i2c_test1.buf[0] = 0x03; - i2c_submit(&I2C_ABUSE_PORT, &i2c_test1); - break; - case 15: - i2c_test1.slave_addr = 0x3C; - i2c_test1.type = I2CTransTxRx; - i2c_test1.len_r = 4; - i2c_test1.len_w = 2; - i2c_test1.buf[0] = 0x03; - i2c_submit(&I2C_ABUSE_PORT, &i2c_test1); - break; - default: - i2c_test1.slave_addr = 0x3C; - i2c_test1.type = I2CTransTxRx; - i2c_test1.len_r = 5; - i2c_test1.len_w = 1; - i2c_test1.buf[0] = 0x03; - i2c_submit(&I2C_ABUSE_PORT, &i2c_test1); + switch (_init) { + case 1: + i2c_test1.type = I2CTransTx; + i2c_test1.buf[0] = 0x00; // set to rate to 50Hz + i2c_test1.buf[1] = 0x00 | (0x06 << 2); + i2c_test1.buf[2] = 0x01 << 5; + i2c_test1.buf[3] = 0x00; + i2c_test1.len_w = 4; + i2c_submit(&I2C_ABUSE_PORT, &i2c_test1); + break; + case 2: + i2c_test1.type = I2CTransTx; + i2c_test1.buf[0] = 0x01; // set to gain to 1 Gauss + i2c_test1.buf[1] = 0x01 << 5; + i2c_test1.len_w = 2; + i2c_submit(&I2C_ABUSE_PORT, &i2c_test1); + break; + case 3: + i2c_test1.type = I2CTransTx; + i2c_test1.buf[0] = 0x00; // set to continuous mode + i2c_test1.len_w = 1; + i2c_submit(&I2C_ABUSE_PORT, &i2c_test1); + break; + case 4: + i2c_test1.type = I2CTransRx; + i2c_test1.len_r = 1; + i2c_submit(&I2C_ABUSE_PORT, &i2c_test1); + break; + case 5: + i2c_test1.type = I2CTransRx; + i2c_test1.len_r = 2; + i2c_submit(&I2C_ABUSE_PORT, &i2c_test1); + break; + case 6: + i2c_test1.type = I2CTransRx; + i2c_test1.len_r = 3; + i2c_submit(&I2C_ABUSE_PORT, &i2c_test1); + break; + case 7: + i2c_test1.type = I2CTransRx; + i2c_test1.len_r = 4; + i2c_submit(&I2C_ABUSE_PORT, &i2c_test1); + break; + case 8: + i2c_test1.type = I2CTransRx; + i2c_test1.len_r = 5; + i2c_submit(&I2C_ABUSE_PORT, &i2c_test1); + break; + case 9: + // bad addr + i2c_test1.slave_addr = 0x3C + 2; + i2c_test1.type = I2CTransTx; + i2c_test1.len_w = 1; + i2c_submit(&I2C_ABUSE_PORT, &i2c_test1); + break; + case 10: + // 2 consecutive + i2c_test1.type = I2CTransTx; + i2c_test1.buf[0] = 0x00; // set to continuous mode + i2c_test1.len_w = 1; + i2c_submit(&I2C_ABUSE_PORT, &i2c_test1); + break; + case 11: + i2c_test1.slave_addr = 0x3C; + i2c_test1.type = I2CTransTxRx; + i2c_test1.len_r = 1; + i2c_test1.len_w = 1; + i2c_test1.buf[0] = 0x03; + i2c_submit(&I2C_ABUSE_PORT, &i2c_test1); + break; + case 12: + i2c_test1.slave_addr = 0x3C; + i2c_test1.type = I2CTransTxRx; + i2c_test1.len_r = 2; + i2c_test1.len_w = 1; + i2c_test1.buf[0] = 0x03; + i2c_submit(&I2C_ABUSE_PORT, &i2c_test1); + break; + case 13: + i2c_test1.slave_addr = 0x3C; + i2c_test1.type = I2CTransTxRx; + i2c_test1.len_r = 3; + i2c_test1.len_w = 1; + i2c_test1.buf[0] = 0x03; + i2c_submit(&I2C_ABUSE_PORT, &i2c_test1); + break; + case 14: + i2c_test1.slave_addr = 0x3C; + i2c_test1.type = I2CTransTxRx; + i2c_test1.len_r = 4; + i2c_test1.len_w = 1; + i2c_test1.buf[0] = 0x03; + i2c_submit(&I2C_ABUSE_PORT, &i2c_test1); + break; + case 15: + i2c_test1.slave_addr = 0x3C; + i2c_test1.type = I2CTransTxRx; + i2c_test1.len_r = 4; + i2c_test1.len_w = 2; + i2c_test1.buf[0] = 0x03; + i2c_submit(&I2C_ABUSE_PORT, &i2c_test1); + break; + default: + i2c_test1.slave_addr = 0x3C; + i2c_test1.type = I2CTransTxRx; + i2c_test1.len_r = 5; + i2c_test1.len_w = 1; + i2c_test1.buf[0] = 0x03; + i2c_submit(&I2C_ABUSE_PORT, &i2c_test1); } } void event_i2c_abuse_test(void) { - if (i2c_idle(&I2C_ABUSE_PORT)) - { - LED_ON(5); // green = idle + if (i2c_idle(&I2C_ABUSE_PORT)) { + LED_ON(5); // green = idle LED_OFF(4); - } - else - { + } else { LED_ON(4); // red = busy LED_OFF(5); } // 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)) - { + 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); + 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++; + 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; + + 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; + } } - 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_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); + } } } -void periodic_50Hz_i2c_abuse_test(void) { +void periodic_50Hz_i2c_abuse_test(void) +{ } diff --git a/sw/airborne/modules/benchmark/imu_quality_assessment.c b/sw/airborne/modules/benchmark/imu_quality_assessment.c index cedda0e530..dd75030b5f 100644 --- a/sw/airborne/modules/benchmark/imu_quality_assessment.c +++ b/sw/airborne/modules/benchmark/imu_quality_assessment.c @@ -28,32 +28,33 @@ struct imu_quality_assessment_data_struct imu_quality_assessment_data; -void imu_quality_assessment_init(void) { +void imu_quality_assessment_init(void) +{ } #define IMU_QUALITY_ASSESSMENT_FILTER_ORDER 2 #define PEAK_TRACKER(_Value, _Peak) { \ - if ( (_Value) > (_Peak) ) \ - _Peak = _Value; \ - else if ( -(_Value) > (_Peak) ) \ - _Peak = -(_Value); \ -} + if ( (_Value) > (_Peak) ) \ + _Peak = _Value; \ + else if ( -(_Value) > (_Peak) ) \ + _Peak = -(_Value); \ + } void imu_quality_assessment_periodic(void) { - static int32_t lx[IMU_QUALITY_ASSESSMENT_FILTER_ORDER+1]; - static int32_t fx[IMU_QUALITY_ASSESSMENT_FILTER_ORDER+1]; - const int32_t A[IMU_QUALITY_ASSESSMENT_FILTER_ORDER+1] = {16384, -25576, 10508}; - const int32_t B[IMU_QUALITY_ASSESSMENT_FILTER_ORDER+1] = {13117, -26234, 13117}; + static int32_t lx[IMU_QUALITY_ASSESSMENT_FILTER_ORDER + 1]; + static int32_t fx[IMU_QUALITY_ASSESSMENT_FILTER_ORDER + 1]; + const int32_t A[IMU_QUALITY_ASSESSMENT_FILTER_ORDER + 1] = {16384, -25576, 10508}; + const int32_t B[IMU_QUALITY_ASSESSMENT_FILTER_ORDER + 1] = {13117, -26234, 13117}; // Peak tracking - PEAK_TRACKER( imu.accel.x, imu_quality_assessment_data.q_ax); - PEAK_TRACKER( imu.accel.y, imu_quality_assessment_data.q_ay); - PEAK_TRACKER( imu.accel.z, imu_quality_assessment_data.q_az); + PEAK_TRACKER(imu.accel.x, imu_quality_assessment_data.q_ax); + PEAK_TRACKER(imu.accel.y, imu_quality_assessment_data.q_ay); + PEAK_TRACKER(imu.accel.z, imu_quality_assessment_data.q_az); // High frequency high-pass filter @@ -70,8 +71,8 @@ void imu_quality_assessment_periodic(void) fx[0] = B[0] * lx[0] + B[1] * lx[1] + B[2] * lx[2] - A[1] * fx[1] - A[2] * fx[2]; fx[0] = fx[0] >> 14; - int32_t filt_x = ((fx[0])*IMU_ACCEL_X_SENS_NUM)/IMU_ACCEL_X_SENS_DEN; - PEAK_TRACKER( filt_x, imu_quality_assessment_data.q); + int32_t filt_x = ((fx[0]) * IMU_ACCEL_X_SENS_NUM) / IMU_ACCEL_X_SENS_DEN; + PEAK_TRACKER(filt_x, imu_quality_assessment_data.q); } diff --git a/sw/airborne/modules/benchmark/imu_quality_assessment.h b/sw/airborne/modules/benchmark/imu_quality_assessment.h index a14aa9aeb5..c64f6a4694 100644 --- a/sw/airborne/modules/benchmark/imu_quality_assessment.h +++ b/sw/airborne/modules/benchmark/imu_quality_assessment.h @@ -29,21 +29,21 @@ extern void imu_quality_assessment_init(void); extern void imu_quality_assessment_periodic(void); struct imu_quality_assessment_data_struct { - int q_ax; - int q_ay; - int q_az; + int q_ax; + int q_ay; + int q_az; - int q_m; + int q_m; - int q; + int q; }; extern struct imu_quality_assessment_data_struct imu_quality_assessment_data; #define imu_quality_assessment_Reset(_v) { \ - imu_quality_assessment_data.q_ax = 0; \ - imu_quality_assessment_data.q_ay = 0; \ - imu_quality_assessment_data.q_az = 0; \ -} + imu_quality_assessment_data.q_ax = 0; \ + imu_quality_assessment_data.q_ay = 0; \ + imu_quality_assessment_data.q_az = 0; \ + } #endif /* IMU_QUALITY_ASSESSMENT_H_ */ diff --git a/sw/airborne/modules/calibration/send_imu_mag_current.c b/sw/airborne/modules/calibration/send_imu_mag_current.c index 9e36d0c505..38ef498e05 100644 --- a/sw/airborne/modules/calibration/send_imu_mag_current.c +++ b/sw/airborne/modules/calibration/send_imu_mag_current.c @@ -31,11 +31,12 @@ #include "mcu_periph/uart.h" #include "subsystems/datalink/downlink.h" -void send_imu_mag_current(void) { +void send_imu_mag_current(void) +{ DOWNLINK_SEND_IMU_MAG_CURRENT_CALIBRATION(DefaultChannel, DefaultDevice, - &imu.mag_unscaled.x, - &imu.mag_unscaled.y, - &imu.mag_unscaled.z, - &electrical.current); + &imu.mag_unscaled.x, + &imu.mag_unscaled.y, + &imu.mag_unscaled.z, + &electrical.current); } diff --git a/sw/airborne/modules/cam_control/cam.c b/sw/airborne/modules/cam_control/cam.c index 5c9689f161..1707a27ada 100644 --- a/sw/airborne/modules/cam_control/cam.c +++ b/sw/airborne/modules/cam_control/cam.c @@ -100,7 +100,8 @@ void cam_target(void); void cam_waypoint_target(void); void cam_ac_target(void); -static void send_cam(struct transport_tx *trans, struct link_device *dev) { +static void send_cam(struct transport_tx *trans, struct link_device *dev) +{ int16_t x = cam_target_x; int16_t y = cam_target_y; int16_t phi = DegOfRad(cam_phi_c); @@ -109,13 +110,15 @@ static void send_cam(struct transport_tx *trans, struct link_device *dev) { } #ifdef SHOW_CAM_COORDINATES -static void send_cam_point(struct transport_tx *trans, struct link_device *dev) { +static void send_cam_point(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_CAM_POINT(trans, dev, AC_ID, - &cam_point_distance_from_home, &cam_point_lat, &cam_point_lon); + &cam_point_distance_from_home, &cam_point_lat, &cam_point_lon); } #endif -void cam_init( void ) { +void cam_init(void) +{ cam_mode = CAM_MODE0; register_periodic_telemetry(DefaultPeriodic, "CAM", send_cam); @@ -124,49 +127,50 @@ void cam_init( void ) { #endif } -void cam_periodic( void ) { +void cam_periodic(void) +{ #if defined(CAM_FIXED_FOR_FPV_IN_AUTO1) && CAM_FIXED_FOR_FPV_IN_AUTO1 == 1 //Position the camera for straight view. - if (pprz_mode == PPRZ_MODE_AUTO2){ + if (pprz_mode == PPRZ_MODE_AUTO2) { #endif switch (cam_mode) { - case CAM_MODE_OFF: - cam_pan_c = RadOfDeg(CAM_PAN0); - cam_tilt_c = RadOfDeg(CAM_TILT0); - cam_angles(); - break; - case CAM_MODE_ANGLES: - cam_angles(); - break; - case CAM_MODE_NADIR: - cam_nadir(); - break; - case CAM_MODE_XY_TARGET: - cam_target(); - break; - case CAM_MODE_WP_TARGET: - cam_waypoint_target(); - break; - case CAM_MODE_AC_TARGET: - cam_ac_target(); - break; - // In this mode the target coordinates are calculated continuously from the pan and tilt radio channels. - // The "TARGET" waypoint coordinates are not used. - // If the "-DSHOW_CAM_COORDINATES" is defined then the coordinates of where the camera is looking are calculated. - case CAM_MODE_STABILIZED: - cam_waypoint_target(); - break; - // In this mode the angles come from the pan and tilt radio channels. - // The "TARGET" waypoint coordinates are not used but i need to call the "cam_waypoint_target()" function - // in order to calculate the coordinates of where the camera is looking. - case CAM_MODE_RC: - cam_waypoint_target(); - break; - default: - break; + case CAM_MODE_OFF: + cam_pan_c = RadOfDeg(CAM_PAN0); + cam_tilt_c = RadOfDeg(CAM_TILT0); + cam_angles(); + break; + case CAM_MODE_ANGLES: + cam_angles(); + break; + case CAM_MODE_NADIR: + cam_nadir(); + break; + case CAM_MODE_XY_TARGET: + cam_target(); + break; + case CAM_MODE_WP_TARGET: + cam_waypoint_target(); + break; + case CAM_MODE_AC_TARGET: + cam_ac_target(); + break; + // In this mode the target coordinates are calculated continuously from the pan and tilt radio channels. + // The "TARGET" waypoint coordinates are not used. + // If the "-DSHOW_CAM_COORDINATES" is defined then the coordinates of where the camera is looking are calculated. + case CAM_MODE_STABILIZED: + cam_waypoint_target(); + break; + // In this mode the angles come from the pan and tilt radio channels. + // The "TARGET" waypoint coordinates are not used but i need to call the "cam_waypoint_target()" function + // in order to calculate the coordinates of where the camera is looking. + case CAM_MODE_RC: + cam_waypoint_target(); + break; + default: + break; } #if defined(CAM_FIXED_FOR_FPV_IN_AUTO1) && CAM_FIXED_FOR_FPV_IN_AUTO1 == 1 - }else if (pprz_mode == PPRZ_MODE_AUTO1){ + } else if (pprz_mode == PPRZ_MODE_AUTO1) { //Position the camera for straight view. #if defined(CAM_TILT_POSITION_FOR_FPV) @@ -190,48 +194,54 @@ void cam_periodic( void ) { #if defined(COMMAND_CAM_PWR_SW) - if(video_tx_state){ ap_state->commands[COMMAND_CAM_PWR_SW] = MAX_PPRZ; }else{ ap_state->commands[COMMAND_CAM_PWR_SW] = MIN_PPRZ; } + if (video_tx_state) { ap_state->commands[COMMAND_CAM_PWR_SW] = MAX_PPRZ; } else { ap_state->commands[COMMAND_CAM_PWR_SW] = MIN_PPRZ; } #elif defined(VIDEO_TX_SWITCH) - if(video_tx_state){ LED_OFF(VIDEO_TX_SWITCH); }else{ LED_ON(VIDEO_TX_SWITCH); } + if (video_tx_state) { LED_OFF(VIDEO_TX_SWITCH); } else { LED_ON(VIDEO_TX_SWITCH); } #endif } /** Computes the servo values from cam_pan_c and cam_tilt_c */ -void cam_angles( void ) { +void cam_angles(void) +{ float cam_pan = 0; float cam_tilt = 0; if (cam_pan_c > RadOfDeg(CAM_PAN_MAX)) { cam_pan_c = RadOfDeg(CAM_PAN_MAX); } else { - if (cam_pan_c < RadOfDeg(CAM_PAN_MIN)) + if (cam_pan_c < RadOfDeg(CAM_PAN_MIN)) { cam_pan_c = RadOfDeg(CAM_PAN_MIN); + } } - if (cam_tilt_c > RadOfDeg(CAM_TILT_MAX)){ + if (cam_tilt_c > RadOfDeg(CAM_TILT_MAX)) { cam_tilt_c = RadOfDeg(CAM_TILT_MAX); } else { - if (cam_tilt_c < RadOfDeg(CAM_TILT_MIN)) + if (cam_tilt_c < RadOfDeg(CAM_TILT_MIN)) { cam_tilt_c = RadOfDeg(CAM_TILT_MIN); + } } #ifdef CAM_PAN_NEUTRAL float pan_diff = cam_pan_c - RadOfDeg(CAM_PAN_NEUTRAL); - if (pan_diff > 0) + if (pan_diff > 0) { cam_pan = MAX_PPRZ * (pan_diff / (RadOfDeg(CAM_PAN_MAX - CAM_PAN_NEUTRAL))); - else + } else { cam_pan = MIN_PPRZ * (pan_diff / (RadOfDeg(CAM_PAN_MIN - CAM_PAN_NEUTRAL))); + } #else - cam_pan = ((float)RadOfDeg(cam_pan_c - CAM_PAN_MIN)) * ((float)MAX_PPRZ / (float)RadOfDeg(CAM_PAN_MAX-CAM_PAN_MIN) ); + cam_pan = ((float)RadOfDeg(cam_pan_c - CAM_PAN_MIN)) * ((float)MAX_PPRZ / (float)RadOfDeg(CAM_PAN_MAX - CAM_PAN_MIN)); #endif #ifdef CAM_TILT_NEUTRAL float tilt_diff = cam_tilt_c - RadOfDeg(CAM_TILT_NEUTRAL); - if (tilt_diff > 0) + if (tilt_diff > 0) { cam_tilt = MAX_PPRZ * (tilt_diff / (RadOfDeg(CAM_TILT_MAX - CAM_TILT_NEUTRAL))); - else + } else { cam_tilt = MIN_PPRZ * (tilt_diff / (RadOfDeg(CAM_TILT_MIN - CAM_TILT_NEUTRAL))); + } #else - cam_tilt = ((float)RadOfDeg(cam_tilt_c - CAM_TILT_MIN)) * ((float)MAX_PPRZ / (float)RadOfDeg(CAM_TILT_MAX-CAM_TILT_MIN) ); + cam_tilt = ((float)RadOfDeg(cam_tilt_c - CAM_TILT_MIN)) * ((float)MAX_PPRZ / (float)RadOfDeg( + CAM_TILT_MAX - CAM_TILT_MIN)); #endif cam_pan = TRIM_PPRZ(cam_pan); @@ -249,15 +259,16 @@ void cam_angles( void ) { } /** Computes the right angles from target_x, target_y, target_alt */ -void cam_target( void ) { +void cam_target(void) +{ #ifdef TEST_CAM vPoint(test_cam_estimator_x, test_cam_estimator_y, test_cam_estimator_z, test_cam_estimator_phi, test_cam_estimator_theta, test_cam_estimator_hspeed_dir, cam_target_x, cam_target_y, cam_target_alt, &cam_pan_c, &cam_tilt_c); #else - struct EnuCoor_f* pos = stateGetPositionEnu_f(); - struct FloatEulers* att = stateGetNedToBodyEulers_f(); + struct EnuCoor_f *pos = stateGetPositionEnu_f(); + struct FloatEulers *att = stateGetNedToBodyEulers_f(); vPoint(pos->x, pos->y, stateGetPositionUtm_f()->alt, att->phi, att->theta, *stateGetHorizontalSpeedDir_f(), cam_target_x, cam_target_y, cam_target_alt, @@ -267,8 +278,9 @@ void cam_target( void ) { } /** Point straight down */ -void cam_nadir( void ) { - struct EnuCoor_f* pos = stateGetPositionEnu_f(); +void cam_nadir(void) +{ + struct EnuCoor_f *pos = stateGetPositionEnu_f(); #ifdef TEST_CAM cam_target_x = test_cam_estimator_x; cam_target_y = test_cam_estimator_y; @@ -281,7 +293,8 @@ void cam_nadir( void ) { } -void cam_waypoint_target( void ) { +void cam_waypoint_target(void) +{ if (cam_target_wp < nb_waypoint) { cam_target_x = WaypointX(cam_target_wp); cam_target_y = WaypointY(cam_target_wp); @@ -290,7 +303,8 @@ void cam_waypoint_target( void ) { cam_target(); } -void cam_ac_target( void ) { +void cam_ac_target(void) +{ #ifdef TRAFFIC_INFO struct ac_info_ * ac = get_ac_info(cam_target_ac); cam_target_x = ac->east; diff --git a/sw/airborne/modules/cam_control/cam.h b/sw/airborne/modules/cam_control/cam.h index 9517d0740a..3ebd0396be 100644 --- a/sw/airborne/modules/cam_control/cam.h +++ b/sw/airborne/modules/cam_control/cam.h @@ -71,8 +71,8 @@ extern uint8_t cam_target_wp; extern uint8_t cam_target_ac; /** For CAM_MODE_AC_TARGET mode */ -void cam_periodic( void ); -void cam_init( void ); +void cam_periodic(void); +void cam_init(void); extern int16_t cam_pan_command; #define cam_SetPanCommand(x) { ap_state->commands[COMMAND_CAM_PAN] = cam_pan_command = x;} diff --git a/sw/airborne/modules/cam_control/cam_roll.c b/sw/airborne/modules/cam_control/cam_roll.c index 7427cc64c9..3f77767830 100644 --- a/sw/airborne/modules/cam_control/cam_roll.c +++ b/sw/airborne/modules/cam_control/cam_roll.c @@ -54,20 +54,22 @@ float target_x, target_y, target_alt; uint8_t cam_roll_mode; -void cam_init( void ) { +void cam_init(void) +{ cam_roll_mode = CAM_ROLL_START_MODE; } -void cam_periodic( void ) { +void cam_periodic(void) +{ switch (cam_roll_mode) { - case MODE_STABILIZED: - phi_c = cam_roll_phi + stateGetNedToBodyEulers_f()->phi; - break; - case MODE_MANUAL: - phi_c = cam_roll_phi; - break; - default: - phi_c = 0; + case MODE_STABILIZED: + phi_c = cam_roll_phi + stateGetNedToBodyEulers_f()->phi; + break; + case MODE_MANUAL: + phi_c = cam_roll_phi; + break; + default: + phi_c = 0; } ap_state->commands[COMMAND_CAM_ROLL] = TRIM_PPRZ(phi_c * MAX_PPRZ / CAM_PHI_MAX); } diff --git a/sw/airborne/modules/cam_control/cam_segment.c b/sw/airborne/modules/cam_control/cam_segment.c index 192efa6fff..4e69f3f44c 100644 --- a/sw/airborne/modules/cam_control/cam_segment.c +++ b/sw/airborne/modules/cam_control/cam_segment.c @@ -29,14 +29,17 @@ #include "modules/cam_control/cam.h" #include "firmwares/fixedwing/nav.h" -void cam_segment_init( void ) { +void cam_segment_init(void) +{ } -void cam_segment_stop ( void ) { +void cam_segment_stop(void) +{ cam_mode = CAM_MODE_OFF; } -void cam_segment_periodic( void ) { +void cam_segment_periodic(void) +{ cam_mode = CAM_MODE_XY_TARGET; cam_target_x = desired_x; cam_target_y = desired_y; diff --git a/sw/airborne/modules/cam_control/cam_segment.h b/sw/airborne/modules/cam_control/cam_segment.h index 9d781e47d3..4a00fce79c 100644 --- a/sw/airborne/modules/cam_control/cam_segment.h +++ b/sw/airborne/modules/cam_control/cam_segment.h @@ -28,8 +28,8 @@ #ifndef CAM_SEGMENT_H #define CAM_SEGMENT_H -extern void cam_segment_init( void ); -extern void cam_segment_stop ( void ); -extern void cam_segment_periodic( void ); +extern void cam_segment_init(void); +extern void cam_segment_stop(void); +extern void cam_segment_periodic(void); #endif diff --git a/sw/airborne/modules/cam_control/point.c b/sw/airborne/modules/cam_control/point.c index df96419253..ebb5f42707 100644 --- a/sw/airborne/modules/cam_control/point.c +++ b/sw/airborne/modules/cam_control/point.c @@ -81,14 +81,16 @@ #include "math/pprz_geodetic_float.h" typedef struct { - float fx; - float fy; - float fz;} VECTOR; + float fx; + float fy; + float fz; +} VECTOR; typedef struct { - float fx1; float fx2; float fx3; - float fy1; float fy2; float fy3; - float fz1; float fz2; float fz3;} MATRIX; + float fx1; float fx2; float fx3; + float fy1; float fy2; float fy3; + float fz1; float fz2; float fz3; +} MATRIX; float cam_theta; float cam_phi; @@ -102,15 +104,15 @@ float cam_point_lon, cam_point_lat; float distance_correction = 1; #endif -void vSubtractVectors(VECTOR* svA, VECTOR svB, VECTOR svC); -void vMultiplyMatrixByVector(VECTOR* svA, MATRIX smB, VECTOR svC); +void vSubtractVectors(VECTOR *svA, VECTOR svB, VECTOR svC); +void vMultiplyMatrixByVector(VECTOR *svA, MATRIX smB, VECTOR svC); /******************************************************************* ; function name: vSubtractVectors ; description: subtracts two vectors a = b - c ; parameters: ;*******************************************************************/ -void vSubtractVectors(VECTOR* svA, VECTOR svB, VECTOR svC) +void vSubtractVectors(VECTOR *svA, VECTOR svB, VECTOR svC) { svA->fx = svB.fx - svC.fx; svA->fy = svB.fy - svC.fy; @@ -122,7 +124,7 @@ void vSubtractVectors(VECTOR* svA, VECTOR svB, VECTOR svC) ; description: multiplies matrix by vector svA = smB * svC ; parameters: ;*******************************************************************/ -void vMultiplyMatrixByVector(VECTOR* svA, MATRIX smB, VECTOR svC) +void vMultiplyMatrixByVector(VECTOR *svA, MATRIX smB, VECTOR svC) { svA->fx = smB.fx1 * svC.fx + smB.fx2 * svC.fy + smB.fx3 * svC.fz; svA->fy = smB.fy1 * svC.fx + smB.fy2 * svC.fy + smB.fy3 * svC.fz; @@ -164,44 +166,45 @@ void vPoint(float fPlaneEast, float fPlaneNorth, float fPlaneAltitude, float *fPan, float *fTilt) { static VECTOR svPlanePosition, - svObjectPosition, - svObjectPositionForPlane, - svObjectPositionForPlane2; + svObjectPosition, + svObjectPositionForPlane, + svObjectPositionForPlane2; static VECTOR sv_cam_projection, - sv_cam_projection_buf; + sv_cam_projection_buf; static MATRIX smRotation; -/*** BELOW IS THE CODE THAT READS THE RC PAN AND TILT CHANNELS AND CONVERTS THEM TO ANGLES (RADIANS) ***/ -/*** IT IS USED FOR CALCULATING THE COORDINATES OF THE POINT WHERE THE CAMERA IS LOOKING AT ***/ -/*** THIS IS DONE ONLY FOR THE CAM_MODE_STABILIZED OR CAM_MODE_RC. ***/ -/*** IN OTHER MODES ONLY THE CAM_POINT WAYPOINT AND THE DISTANCE FROM TARGET IS UPDATED ***/ -if (cam_mode == CAM_MODE_STABILIZED || cam_mode == CAM_MODE_RC || cam_mode == CAM_MODE_WP_TARGET || cam_mode == CAM_MODE_XY_TARGET ){ + /*** BELOW IS THE CODE THAT READS THE RC PAN AND TILT CHANNELS AND CONVERTS THEM TO ANGLES (RADIANS) ***/ + /*** IT IS USED FOR CALCULATING THE COORDINATES OF THE POINT WHERE THE CAMERA IS LOOKING AT ***/ + /*** THIS IS DONE ONLY FOR THE CAM_MODE_STABILIZED OR CAM_MODE_RC. ***/ + /*** IN OTHER MODES ONLY THE CAM_POINT WAYPOINT AND THE DISTANCE FROM TARGET IS UPDATED ***/ + if (cam_mode == CAM_MODE_STABILIZED || cam_mode == CAM_MODE_RC || cam_mode == CAM_MODE_WP_TARGET + || cam_mode == CAM_MODE_XY_TARGET) { -if (cam_mode == CAM_MODE_STABILIZED || cam_mode == CAM_MODE_RC ){ + if (cam_mode == CAM_MODE_STABILIZED || cam_mode == CAM_MODE_RC) { -/*######################################## TILT CONTROL INPUT #############################################*/ + /*######################################## TILT CONTROL INPUT #############################################*/ #ifdef CAM_TILT_NEUTRAL #if defined(RADIO_TILT) - if ((*fbw_state).channels[RADIO_TILT] >= 0){ - cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_TILT] /(float)MAX_PPRZ) * - (float)(RadOfDeg(CAM_TILT_MAX - CAM_TILT_NEUTRAL)) ); + if ((*fbw_state).channels[RADIO_TILT] >= 0) { + cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + (((float)(*fbw_state).channels[RADIO_TILT] / (float)MAX_PPRZ) * + (float)(RadOfDeg(CAM_TILT_MAX - CAM_TILT_NEUTRAL))); - }else{ - cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_TILT] /(float)MIN_PPRZ) * - (float)(RadOfDeg(CAM_TILT_MIN - CAM_TILT_NEUTRAL)) ); - } + } else { + cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + (((float)(*fbw_state).channels[RADIO_TILT] / (float)MIN_PPRZ) * + (float)(RadOfDeg(CAM_TILT_MIN - CAM_TILT_NEUTRAL))); + } #elif defined(RADIO_PITCH) - if ((*fbw_state).channels[RADIO_PITCH] >= 0){ - cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_PITCH] /(float)MAX_PPRZ) * - (float)(RadOfDeg(CAM_TILT_MAX - CAM_TILT_NEUTRAL)) ); + if ((*fbw_state).channels[RADIO_PITCH] >= 0) { + cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + (((float)(*fbw_state).channels[RADIO_PITCH] / (float)MAX_PPRZ) * + (float)(RadOfDeg(CAM_TILT_MAX - CAM_TILT_NEUTRAL))); - }else{ - cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_PITCH] /(float)MIN_PPRZ) * - (float)(RadOfDeg(CAM_TILT_MIN - CAM_TILT_NEUTRAL)) ); - } + } else { + cam_theta = (float)RadOfDeg(CAM_TILT_NEUTRAL) + (((float)(*fbw_state).channels[RADIO_PITCH] / (float)MIN_PPRZ) * + (float)(RadOfDeg(CAM_TILT_MIN - CAM_TILT_NEUTRAL))); + } #else #error RADIO_TILT or RADIO_PITCH not defined. #endif @@ -209,37 +212,39 @@ if (cam_mode == CAM_MODE_STABILIZED || cam_mode == CAM_MODE_RC ){ #else //#ifdef CAM_TILT_NEUTRAL #if defined(RADIO_TILT) - cam_theta = RadOfDeg(CAM_TILT_MIN) + (RadOfDeg(CAM_TILT_MAX - CAM_TILT_MIN) * ((float)(*fbw_state).channels[RADIO_TILT] /(float)MAX_PPRZ)); + cam_theta = RadOfDeg(CAM_TILT_MIN) + (RadOfDeg(CAM_TILT_MAX - CAM_TILT_MIN) * ((float)( + *fbw_state).channels[RADIO_TILT] / (float)MAX_PPRZ)); #elif defined(RADIO_PITCH) - cam_theta = RadOfDeg(CAM_TILT_MIN) + (RadOfDeg(CAM_TILT_MAX - CAM_TILT_MIN) * ((float)(*fbw_state).channels[RADIO_PITCH] /(float)MAX_PPRZ)); + cam_theta = RadOfDeg(CAM_TILT_MIN) + (RadOfDeg(CAM_TILT_MAX - CAM_TILT_MIN) * ((float)( + *fbw_state).channels[RADIO_PITCH] / (float)MAX_PPRZ)); #else #error RADIO_TILT or RADIO_PITCH not defined. #endif #endif //#ifdef CAM_TILT_NEUTRAL -/*######################################## END OF TILT CONTROL INPUT ########################################*/ + /*######################################## END OF TILT CONTROL INPUT ########################################*/ -/*########################################### PAN CONTROL INPUT #############################################*/ + /*########################################### PAN CONTROL INPUT #############################################*/ #ifdef CAM_PAN_NEUTRAL #if defined(RADIO_PAN) - if ((*fbw_state).channels[RADIO_PAN] >= 0){ - cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_PAN] /(float)MAX_PPRZ) * - RadOfDeg(CAM_PAN_MAX - CAM_PAN_NEUTRAL) ); + if ((*fbw_state).channels[RADIO_PAN] >= 0) { + cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + (((float)(*fbw_state).channels[RADIO_PAN] / (float)MAX_PPRZ) * + RadOfDeg(CAM_PAN_MAX - CAM_PAN_NEUTRAL)); - }else{ - cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_PAN] /(float)MIN_PPRZ) * - RadOfDeg(CAM_PAN_MIN - CAM_PAN_NEUTRAL) ); - } + } else { + cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + (((float)(*fbw_state).channels[RADIO_PAN] / (float)MIN_PPRZ) * + RadOfDeg(CAM_PAN_MIN - CAM_PAN_NEUTRAL)); + } #elif defined(RADIO_ROLL) - if ((*fbw_state).channels[RADIO_ROLL] >= 0){ - cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_ROLL] /(float)MAX_PPRZ) * - RadOfDeg(CAM_PAN_MAX - CAM_PAN_NEUTRAL) ); + if ((*fbw_state).channels[RADIO_ROLL] >= 0) { + cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + (((float)(*fbw_state).channels[RADIO_ROLL] / (float)MAX_PPRZ) * + RadOfDeg(CAM_PAN_MAX - CAM_PAN_NEUTRAL)); - }else{ - cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + ( ((float)(*fbw_state).channels[RADIO_ROLL] /(float)MIN_PPRZ) * - RadOfDeg(CAM_PAN_MIN - CAM_PAN_NEUTRAL) ); - } + } else { + cam_phi = RadOfDeg(CAM_PAN_NEUTRAL) + (((float)(*fbw_state).channels[RADIO_ROLL] / (float)MIN_PPRZ) * + RadOfDeg(CAM_PAN_MIN - CAM_PAN_NEUTRAL)); + } #else #error RADIO_PAN or RADIO_ROLL not defined. #endif @@ -247,190 +252,194 @@ if (cam_mode == CAM_MODE_STABILIZED || cam_mode == CAM_MODE_RC ){ #else //#ifdef CAM_PAN_NEUTRAL #if defined(RADIO_PAN) - cam_phi = RadOfDeg(CAM_PAN_MIN) + (RadOfDeg(CAM_PAN_MAX - CAM_PAN_MIN) * ((float)(*fbw_state).channels[RADIO_PAN] /(float)MAX_PPRZ)); + cam_phi = RadOfDeg(CAM_PAN_MIN) + (RadOfDeg(CAM_PAN_MAX - CAM_PAN_MIN) * ((float)(*fbw_state).channels[RADIO_PAN] / + (float)MAX_PPRZ)); #elif defined(RADIO_ROLL) - cam_phi = RadOfDeg(CAM_PAN_MIN) + (RadOfDeg(CAM_PAN_MAX - CAM_PAN_MIN) * ((float)(*fbw_state).channels[RADIO_ROLL] /(float)MAX_PPRZ)); + cam_phi = RadOfDeg(CAM_PAN_MIN) + (RadOfDeg(CAM_PAN_MAX - CAM_PAN_MIN) * ((float)(*fbw_state).channels[RADIO_ROLL] / + (float)MAX_PPRZ)); #else #error RADIO_PAN or RADIO_ROLL not defined. #endif #endif //#ifdef CAM_PAN_NEUTRAL -/*######################################## END OF PAN CONTROL INPUT #############################################*/ + /*######################################## END OF PAN CONTROL INPUT #############################################*/ - // Bound Pan and Tilt angles. - if (cam_theta > RadOfDeg(CAM_TILT_MAX)){ - cam_theta = RadOfDeg(CAM_TILT_MAX); + // Bound Pan and Tilt angles. + if (cam_theta > RadOfDeg(CAM_TILT_MAX)) { + cam_theta = RadOfDeg(CAM_TILT_MAX); - }else if(cam_theta < RadOfDeg(CAM_TILT_MIN)){ cam_theta = RadOfDeg(CAM_TILT_MIN); } + } else if (cam_theta < RadOfDeg(CAM_TILT_MIN)) { cam_theta = RadOfDeg(CAM_TILT_MIN); } - if (cam_phi > RadOfDeg(CAM_PAN_MAX)){ - cam_phi = RadOfDeg(CAM_PAN_MAX); + if (cam_phi > RadOfDeg(CAM_PAN_MAX)) { + cam_phi = RadOfDeg(CAM_PAN_MAX); - }else if(cam_phi < RadOfDeg(CAM_PAN_MIN)){ cam_phi = RadOfDeg(CAM_PAN_MIN); } + } else if (cam_phi < RadOfDeg(CAM_PAN_MIN)) { cam_phi = RadOfDeg(CAM_PAN_MIN); } - svPlanePosition.fx = fPlaneEast; - svPlanePosition.fy = fPlaneNorth; - svPlanePosition.fz = fPlaneAltitude; + svPlanePosition.fx = fPlaneEast; + svPlanePosition.fy = fPlaneNorth; + svPlanePosition.fz = fPlaneAltitude; // FOR TESTING ANGLES IN THE SIMULATOR ONLY CODE UNCOMMENT THE TWO BELOW LINES //cam_phi = RadOfDeg(90); // LOOK 45 DEGREES TO THE LEFT, -X IS TO THE LEFT AND +X IS TO THE RIGHT //cam_theta = RadOfDeg(70); // LOOK 45 DEGREES DOWN, 0 IS STRAIGHT DOWN 90 IS STRAIGHT IN FRONT - if ( cam_theta > RadOfDeg(80) && cam_mode == CAM_MODE_RC){ // Not much to see after 80 degrees of tilt so stop tracking. - *fPan = cam_phi; - *fTilt = cam_theta; + if (cam_theta > RadOfDeg(80) && cam_mode == CAM_MODE_RC) { // Not much to see after 80 degrees of tilt so stop tracking. + *fPan = cam_phi; + *fTilt = cam_theta; #ifdef SHOW_CAM_COORDINATES - cam_point_distance_from_home = 0; - cam_point_lon = 0; - cam_point_lat = 0; + cam_point_distance_from_home = 0; + cam_point_lon = 0; + cam_point_lat = 0; #endif - return; + return; - }else{ - sv_cam_projection_buf.fx = svPlanePosition.fx + (tanf(cam_theta)*(fPlaneAltitude-ground_alt)); - sv_cam_projection_buf.fy = svPlanePosition.fy; - } + } else { + sv_cam_projection_buf.fx = svPlanePosition.fx + (tanf(cam_theta) * (fPlaneAltitude - ground_alt)); + sv_cam_projection_buf.fy = svPlanePosition.fy; + } #if defined(WP_CAM_POINT) - sv_cam_projection_buf.fz = waypoints[WP_CAM_POINT].a; + sv_cam_projection_buf.fz = waypoints[WP_CAM_POINT].a; #else - sv_cam_projection_buf.fz = ground_alt; + sv_cam_projection_buf.fz = ground_alt; #endif - /* distance between plane and camera projection */ - vSubtractVectors(&sv_cam_projection, sv_cam_projection_buf, svPlanePosition); + /* distance between plane and camera projection */ + vSubtractVectors(&sv_cam_projection, sv_cam_projection_buf, svPlanePosition); - float heading_radians = RadOfDeg(90) - fYawAngle; //Convert the gps heading (radians) to standard mathematical notation. - if(heading_radians > RadOfDeg(180)){ heading_radians -= RadOfDeg(360); } - if(heading_radians < RadOfDeg(-180)){ heading_radians += RadOfDeg(360); } - //heading_radians += cam_theta; + float heading_radians = RadOfDeg(90) - fYawAngle; //Convert the gps heading (radians) to standard mathematical notation. + if (heading_radians > RadOfDeg(180)) { heading_radians -= RadOfDeg(360); } + if (heading_radians < RadOfDeg(-180)) { heading_radians += RadOfDeg(360); } + //heading_radians += cam_theta; - /* camera pan angle correction, using a clockwise rotation */ - smRotation.fx1 = (float)(cos(cam_phi)); - smRotation.fx2 = (float)(sin(cam_phi)); - smRotation.fx3 = 0.; - smRotation.fy1 = -smRotation.fx2; - smRotation.fy2 = smRotation.fx1; - smRotation.fy3 = 0.; - smRotation.fz1 = 0.; - smRotation.fz2 = 0.; - smRotation.fz3 = 1.; + /* camera pan angle correction, using a clockwise rotation */ + smRotation.fx1 = (float)(cos(cam_phi)); + smRotation.fx2 = (float)(sin(cam_phi)); + smRotation.fx3 = 0.; + smRotation.fy1 = -smRotation.fx2; + smRotation.fy2 = smRotation.fx1; + smRotation.fy3 = 0.; + smRotation.fz1 = 0.; + smRotation.fz2 = 0.; + smRotation.fz3 = 1.; - vMultiplyMatrixByVector(&sv_cam_projection_buf, smRotation, sv_cam_projection); + vMultiplyMatrixByVector(&sv_cam_projection_buf, smRotation, sv_cam_projection); - /* yaw correction using a counter clockwise rotation*/ - smRotation.fx1 = (float)(cos(heading_radians)); - smRotation.fx2 = -(float)(sin(heading_radians)); - smRotation.fx3 = 0.; - smRotation.fy1 = -smRotation.fx2; - smRotation.fy2 = smRotation.fx1; - smRotation.fy3 = 0.; - smRotation.fz1 = 0.; - smRotation.fz2 = 0.; - smRotation.fz3 = 1.; + /* yaw correction using a counter clockwise rotation*/ + smRotation.fx1 = (float)(cos(heading_radians)); + smRotation.fx2 = -(float)(sin(heading_radians)); + smRotation.fx3 = 0.; + smRotation.fy1 = -smRotation.fx2; + smRotation.fy2 = smRotation.fx1; + smRotation.fy3 = 0.; + smRotation.fz1 = 0.; + smRotation.fz2 = 0.; + smRotation.fz3 = 1.; - vMultiplyMatrixByVector(&sv_cam_projection, smRotation, sv_cam_projection_buf); + vMultiplyMatrixByVector(&sv_cam_projection, smRotation, sv_cam_projection_buf); #if defined(RADIO_CAM_LOCK) - if( (float)(*fbw_state).channels[RADIO_CAM_LOCK] > MAX_PPRZ/2)) && pprz_mode == PPRZ_MODE_AUTO2){ cam_lock = TRUE; } - if( (float)(*fbw_state).channels[RADIO_CAM_LOCK] < MIN_PPRZ/2 && pprz_mode == PPRZ_MODE_AUTO2){ cam_lock = FALSE; } + if ((float)(*fbw_state).channels[RADIO_CAM_LOCK] > MAX_PPRZ / 2)) && pprz_mode == PPRZ_MODE_AUTO2) { cam_lock = TRUE; } + if ((float)(*fbw_state).channels[RADIO_CAM_LOCK] < MIN_PPRZ / 2 && pprz_mode == PPRZ_MODE_AUTO2) { cam_lock = FALSE; } #endif - // When the variable "cam_lock" is set then the last calculated position is set as the target waypoint. - if (cam_lock == FALSE){ + // When the variable "cam_lock" is set then the last calculated position is set as the target waypoint. + if (cam_lock == FALSE) { fObjectEast = (fPlaneEast + sv_cam_projection.fx) ; - fObjectNorth = (fPlaneNorth + sv_cam_projection.fy) ; - fAltitude = ground_alt; - memory_x = fObjectEast; - memory_y = fObjectNorth; - memory_z = fAltitude; + fObjectNorth = (fPlaneNorth + sv_cam_projection.fy) ; + fAltitude = ground_alt; + memory_x = fObjectEast; + memory_y = fObjectNorth; + memory_z = fAltitude; #if defined(WP_CAM_POINT) - waypoints[WP_CAM_POINT].x = fObjectEast; - waypoints[WP_CAM_POINT].y = fObjectNorth; - waypoints[WP_CAM_POINT].a = ground_alt; + waypoints[WP_CAM_POINT].x = fObjectEast; + waypoints[WP_CAM_POINT].y = fObjectNorth; + waypoints[WP_CAM_POINT].a = ground_alt; #endif #if defined(SHOW_CAM_COORDINATES) - cam_point_x = fObjectEast; - cam_point_y = fObjectNorth; + cam_point_x = fObjectEast; + cam_point_y = fObjectNorth; - cam_point_distance_from_home = distance_correction * (uint16_t) (sqrt((cam_point_x*cam_point_x) + (cam_point_y*cam_point_y) )); - - struct UtmCoor_f utm; - utm.east = gps.utm_pos.east/100. + sv_cam_projection.fx; - utm.north = gps.utm_pos.north/100. + sv_cam_projection.fy; - utm.zone = gps.utm_pos.zone; - struct LlaCoor_f lla; - lla_of_utm_f(&lla, &utm); - cam_point_lon = lla.lon*(180/M_PI); - cam_point_lat = lla.lat*(180/M_PI); -#endif - - }else{ - fObjectEast = memory_x; - fObjectNorth = memory_y; - fAltitude = memory_z; -#if defined(WP_CAM_POINT) - waypoints[WP_CAM_POINT].x = fObjectEast; - waypoints[WP_CAM_POINT].y = fObjectNorth; - waypoints[WP_CAM_POINT].a = fAltitude; -#endif - } - -if (cam_mode == CAM_MODE_RC && cam_lock == 0 ){ - *fPan = cam_phi; - *fTilt = cam_theta; - return; -} - -#if defined(WP_CAM_POINT) - else{ - waypoints[WP_CAM_POINT].x = fObjectEast; - waypoints[WP_CAM_POINT].y = fObjectNorth; - waypoints[WP_CAM_POINT].a = fAltitude; - } -#endif -/*** END OF THE CODE THAT CALCULATES THE COORDINATES OF WHERE THE CAMERA IS LOOKING AT ***/ -}else{ -/*** THE BELOW CODE IS ONLY EXECUTED IN CAM_MODE_WP_TARGET OR CAM_MODE_XY_TARGET ***/ -#ifdef SHOW_CAM_COORDINATES - cam_point_distance_from_home = distance_correction * (uint16_t) fabs( ((uint16_t) (sqrt((fObjectNorth*fObjectNorth) + (fObjectEast*fObjectEast) ))) - - ((uint16_t) (sqrt((fPlaneNorth*fPlaneNorth) + (fPlaneEast*fPlaneEast) ))) ); + cam_point_distance_from_home = distance_correction * (uint16_t)(sqrt((cam_point_x * cam_point_x) + + (cam_point_y * cam_point_y))); struct UtmCoor_f utm; - utm.east = nav_utm_east0 + fObjectEast; - utm.north = nav_utm_north0 + fObjectNorth; + utm.east = gps.utm_pos.east / 100. + sv_cam_projection.fx; + utm.north = gps.utm_pos.north / 100. + sv_cam_projection.fy; utm.zone = gps.utm_pos.zone; struct LlaCoor_f lla; lla_of_utm_f(&lla, &utm); - cam_point_lon = lla.lon*(180/M_PI); - cam_point_lat = lla.lat*(180/M_PI); + cam_point_lon = lla.lon * (180 / M_PI); + cam_point_lat = lla.lat * (180 / M_PI); #endif - + } else { + fObjectEast = memory_x; + fObjectNorth = memory_y; + fAltitude = memory_z; #if defined(WP_CAM_POINT) waypoints[WP_CAM_POINT].x = fObjectEast; waypoints[WP_CAM_POINT].y = fObjectNorth; waypoints[WP_CAM_POINT].a = fAltitude; #endif + } - } + if (cam_mode == CAM_MODE_RC && cam_lock == 0) { + *fPan = cam_phi; + *fTilt = cam_theta; + return; + } -} +#if defined(WP_CAM_POINT) + else { + waypoints[WP_CAM_POINT].x = fObjectEast; + waypoints[WP_CAM_POINT].y = fObjectNorth; + waypoints[WP_CAM_POINT].a = fAltitude; + } +#endif + /*** END OF THE CODE THAT CALCULATES THE COORDINATES OF WHERE THE CAMERA IS LOOKING AT ***/ + } else { + /*** THE BELOW CODE IS ONLY EXECUTED IN CAM_MODE_WP_TARGET OR CAM_MODE_XY_TARGET ***/ +#ifdef SHOW_CAM_COORDINATES + cam_point_distance_from_home = distance_correction * (uint16_t) fabs(((uint16_t)(sqrt((fObjectNorth * fObjectNorth) + + (fObjectEast * fObjectEast)))) - + ((uint16_t)(sqrt((fPlaneNorth * fPlaneNorth) + (fPlaneEast * fPlaneEast))))); + + struct UtmCoor_f utm; + utm.east = nav_utm_east0 + fObjectEast; + utm.north = nav_utm_north0 + fObjectNorth; + utm.zone = gps.utm_pos.zone; + struct LlaCoor_f lla; + lla_of_utm_f(&lla, &utm); + cam_point_lon = lla.lon * (180 / M_PI); + cam_point_lat = lla.lat * (180 / M_PI); +#endif + + +#if defined(WP_CAM_POINT) + waypoints[WP_CAM_POINT].x = fObjectEast; + waypoints[WP_CAM_POINT].y = fObjectNorth; + waypoints[WP_CAM_POINT].a = fAltitude; +#endif + + } + + } //************************************************************************************************ //************************************************************************************************ //************************************************************************************************ //************************************************************************************************ -/* -By swapping coordinates (fx=fPlaneNorth, fy=fPlaneEast) we make the the circle angle go from 0 (0 is to the top of the circle) -to 360 degrees or from 0 radians to 2 PI radians in a clockwise rotation. This way the GPS reported angle can be directly -applied to the rotation matrices (in radians). -In standard mathematical notation 0 is to the right (East) of the circle, -90 is to the bottom, +-180 is to the left -and +90 is to the top (counterclockwise rotation). -When reading back the actual rotated coordinates sv_cam_projection.fx has the y coordinate and sv_cam_projection.fy has the x -represented on a circle in standard mathematical notation. -*/ + /* + By swapping coordinates (fx=fPlaneNorth, fy=fPlaneEast) we make the the circle angle go from 0 (0 is to the top of the circle) + to 360 degrees or from 0 radians to 2 PI radians in a clockwise rotation. This way the GPS reported angle can be directly + applied to the rotation matrices (in radians). + In standard mathematical notation 0 is to the right (East) of the circle, -90 is to the bottom, +-180 is to the left + and +90 is to the top (counterclockwise rotation). + When reading back the actual rotated coordinates sv_cam_projection.fx has the y coordinate and sv_cam_projection.fy has the x + represented on a circle in standard mathematical notation. + */ svPlanePosition.fx = fPlaneNorth; svPlanePosition.fy = fPlaneEast; svPlanePosition.fz = fPlaneAltitude; @@ -507,12 +516,12 @@ represented on a circle in standard mathematical notation. -90 -> camera looks backward */ #if 0 //we roll away anyways - *fTilt = (float)(atan2( svObjectPositionForPlane2.fx, - sqrt( svObjectPositionForPlane2.fy * svObjectPositionForPlane2.fy - + svObjectPositionForPlane2.fz * svObjectPositionForPlane2.fz ) + *fTilt = (float)(atan2(svObjectPositionForPlane2.fx, + sqrt(svObjectPositionForPlane2.fy * svObjectPositionForPlane2.fy + + svObjectPositionForPlane2.fz * svObjectPositionForPlane2.fz) )); #else - *fTilt = (float)(atan2( svObjectPositionForPlane2.fx, -svObjectPositionForPlane2.fz )); + *fTilt = (float)(atan2(svObjectPositionForPlane2.fx, -svObjectPositionForPlane2.fz)); #endif /* fPan is deactivated @@ -538,12 +547,12 @@ represented on a circle in standard mathematical notation. * */ #if 1 // have to check if it helps - *fTilt = (float)(atan2( svObjectPositionForPlane2.fy, - sqrt( svObjectPositionForPlane2.fx * svObjectPositionForPlane2.fx - + svObjectPositionForPlane2.fz * svObjectPositionForPlane2.fz ) + *fTilt = (float)(atan2(svObjectPositionForPlane2.fy, + sqrt(svObjectPositionForPlane2.fx * svObjectPositionForPlane2.fx + + svObjectPositionForPlane2.fz * svObjectPositionForPlane2.fz) )); #else - *fTilt = (float)(atan2( svObjectPositionForPlane2.fy, -svObjectPositionForPlane2.fz)); + *fTilt = (float)(atan2(svObjectPositionForPlane2.fy, -svObjectPositionForPlane2.fz)); #endif /* fPan is deactivated @@ -552,126 +561,126 @@ represented on a circle in standard mathematical notation. #else #ifdef POINT_CAM_YAW_PITCH_NOSE -/* - -45 0 45 - \ | / - \ | / - \|/ + /* + -45 0 45 + \ | / + \ | / + \|/ ## ## - _____________##______________ -left tip|_____________________________|right wing tip + _____________##______________ + left tip|_____________________________|right wing tip ## ## ## ## - ______##______ - |_____|_|_____| - | -*/ + ______##______ + |_____|_|_____| + | + */ #if defined(CAM_PAN_MODE) && CAM_PAN_MODE == 360 /* fixed to the plane*/ *fPan = (float)(atan2(svObjectPositionForPlane2.fy, (svObjectPositionForPlane2.fx))); - *fTilt = (float)(atan2( sqrt( svObjectPositionForPlane2.fx * svObjectPositionForPlane2.fx - + svObjectPositionForPlane2.fy * svObjectPositionForPlane2.fy ), - -svObjectPositionForPlane2.fz + *fTilt = (float)(atan2(sqrt(svObjectPositionForPlane2.fx * svObjectPositionForPlane2.fx + + svObjectPositionForPlane2.fy * svObjectPositionForPlane2.fy), + -svObjectPositionForPlane2.fz )); - // I need to avoid oscillations around the 180 degree mark. -/* - if (*fPan > 0 && *fPan <= RadOfDeg(175)){ heading_positive = 1; } - if (*fPan < 0 && *fPan >= RadOfDeg(-175)){ heading_positive = 0; } + // I need to avoid oscillations around the 180 degree mark. + /* + if (*fPan > 0 && *fPan <= RadOfDeg(175)){ heading_positive = 1; } + if (*fPan < 0 && *fPan >= RadOfDeg(-175)){ heading_positive = 0; } - if (*fPan > RadOfDeg(175) && heading_positive == 0){ - *fPan = RadOfDeg(-180); + if (*fPan > RadOfDeg(175) && heading_positive == 0){ + *fPan = RadOfDeg(-180); - }else if (*fPan < RadOfDeg(-175) && heading_positive){ - *fPan = RadOfDeg(180); - heading_positive = 0; - } -*/ + }else if (*fPan < RadOfDeg(-175) && heading_positive){ + *fPan = RadOfDeg(180); + heading_positive = 0; + } + */ #else *fPan = (float)(atan2(svObjectPositionForPlane2.fy, fabs(svObjectPositionForPlane2.fx))); - *fTilt = (float)(atan2( sqrt( svObjectPositionForPlane2.fx * svObjectPositionForPlane2.fx - + svObjectPositionForPlane2.fy * svObjectPositionForPlane2.fy ), - -svObjectPositionForPlane2.fz + *fTilt = (float)(atan2(sqrt(svObjectPositionForPlane2.fx * svObjectPositionForPlane2.fx + + svObjectPositionForPlane2.fy * svObjectPositionForPlane2.fy), + -svObjectPositionForPlane2.fz )); - if (svObjectPositionForPlane2.fx < 0) - { - *fPan = -*fPan; - *fTilt = -*fTilt; + if (svObjectPositionForPlane2.fx < 0) { + *fPan = -*fPan; + *fTilt = -*fTilt; } - // I need to avoid oscillations around the 180 degree mark. -/* - if (*fPan > 0 && *fPan <= RadOfDeg(85)){ heading_positive = 1; } - if (*fPan < 0 && *fPan >= RadOfDeg(-85)){ heading_positive = 0; } + // I need to avoid oscillations around the 180 degree mark. + /* + if (*fPan > 0 && *fPan <= RadOfDeg(85)){ heading_positive = 1; } + if (*fPan < 0 && *fPan >= RadOfDeg(-85)){ heading_positive = 0; } - if (*fPan > RadOfDeg(85) && heading_positive == 0){ - *fPan = RadOfDeg(-90); + if (*fPan > RadOfDeg(85) && heading_positive == 0){ + *fPan = RadOfDeg(-90); - }else if (*fPan < RadOfDeg(-85) && heading_positive){ - *fPan = RadOfDeg(90); - heading_positive = 0; - } -*/ + }else if (*fPan < RadOfDeg(-85) && heading_positive){ + *fPan = RadOfDeg(90); + heading_positive = 0; + } + */ #endif #else #ifdef POINT_CAM_YAW_PITCH -/* - * This is for two axes pan/tilt camera mechanisms. The default is to - * circle clockwise so view is right. The pan servo neutral makes - * the camera look to the right with 0° given, 90° is to the back and - * -90° is to the front. The tilt servo neutral makes the camera look - * down with 0° given, 90° is to the right and -90° is to the left (all - * values are used in radian in the software). If the camera looks to - * the right side of the plane, the picture is upright. It is upside - * down when looking to the left. That is corrected with the MPEG - * decoding software on the laptop by mirroring. The pan servo is fixed - * in the plane and the tilt servo is moved by the pan servo and moves - * the camera. - * - * - * pan servo, tilt set to 90°, looking from top: - * - * plane front - * - * ^ - * I - * I 45° - * I / - * I/ - * I------- 0° - * I\ - * I \ - * I -45° - * I - * - * plane back - * - * - * tilt servo, pan set to 0°, looking from back: - * - * plane left --------------- plane right - * / I \ - * / I \ - * -45° I 45° - * 0° - * - */ + /* + * This is for two axes pan/tilt camera mechanisms. The default is to + * circle clockwise so view is right. The pan servo neutral makes + * the camera look to the right with 0° given, 90° is to the back and + * -90° is to the front. The tilt servo neutral makes the camera look + * down with 0° given, 90° is to the right and -90° is to the left (all + * values are used in radian in the software). If the camera looks to + * the right side of the plane, the picture is upright. It is upside + * down when looking to the left. That is corrected with the MPEG + * decoding software on the laptop by mirroring. The pan servo is fixed + * in the plane and the tilt servo is moved by the pan servo and moves + * the camera. + * + * + * pan servo, tilt set to 90°, looking from top: + * + * plane front + * + * ^ + * I + * I 45° + * I / + * I/ + * I------- 0° + * I\ + * I \ + * I -45° + * I + * + * plane back + * + * + * tilt servo, pan set to 0°, looking from back: + * + * plane left --------------- plane right + * / I \ + * / I \ + * -45° I 45° + * 0° + * + */ /* fPan = 0 -> camera looks along the wing 90 -> camera looks in flight direction -90 -> camera looks backwards */ /* fixed to the plane*/ - *fPan = (float)(atan2(svObjectPositionForPlane2.fx, fabs(svObjectPositionForPlane2.fy)));// Or is it the opposite??? (CEF) + *fPan = (float)(atan2(svObjectPositionForPlane2.fx, + fabs(svObjectPositionForPlane2.fy)));// Or is it the opposite??? (CEF) // (CEF) It turned out that Object_North is loaded with x and Object_East with y (reverse). See line #155 // this means that: // *fPan = (float)(atan2(svObjectPositionForPlane2.fy, svObjectPositionForPlane2.fy)); // makes the camera 0 to look to the nose? @@ -681,47 +690,46 @@ left tip|_____________________________|right wing tip -90 -> camera looks into left hemispere actually the camera always looks more or less downwards, but never upwards */ - *fTilt = (float)(atan2( sqrt( svObjectPositionForPlane2.fx * svObjectPositionForPlane2.fx - + svObjectPositionForPlane2.fy * svObjectPositionForPlane2.fy ), - -svObjectPositionForPlane2.fz + *fTilt = (float)(atan2(sqrt(svObjectPositionForPlane2.fx * svObjectPositionForPlane2.fx + + svObjectPositionForPlane2.fy * svObjectPositionForPlane2.fy), + -svObjectPositionForPlane2.fz )); - if (svObjectPositionForPlane2.fy < 0) - { - *fPan = -*fPan; - *fTilt = -*fTilt; + if (svObjectPositionForPlane2.fy < 0) { + *fPan = -*fPan; + *fTilt = -*fTilt; } #else #ifdef POINT_CAM_PITCH_ROLL -/* - * This is for another two axes camera mechanisms. The tilt servo is fixed to - * the fuselage and moves the pan servo. - * - * tilt servo, looking from left: - * - * plane front <--------------- plane back - * / I \ - * / I \ - * 45° I -45° - * 0° - * - * - * pan servo, looking from back: - * - * plane left --------------- plane right - * / I \ - * / I \ - * 45° I -45° - * 0° - * - */ + /* + * This is for another two axes camera mechanisms. The tilt servo is fixed to + * the fuselage and moves the pan servo. + * + * tilt servo, looking from left: + * + * plane front <--------------- plane back + * / I \ + * / I \ + * 45° I -45° + * 0° + * + * + * pan servo, looking from back: + * + * plane left --------------- plane right + * / I \ + * / I \ + * 45° I -45° + * 0° + * + */ - *fTilt = (float)(atan2( svObjectPositionForPlane2.fx, -svObjectPositionForPlane2.fz)); + *fTilt = (float)(atan2(svObjectPositionForPlane2.fx, -svObjectPositionForPlane2.fz)); *fPan = (float)(atan2(-svObjectPositionForPlane2.fy, - sqrt( svObjectPositionForPlane2.fx * svObjectPositionForPlane2.fx - + svObjectPositionForPlane2.fz * svObjectPositionForPlane2.fz ) + sqrt(svObjectPositionForPlane2.fx * svObjectPositionForPlane2.fx + + svObjectPositionForPlane2.fz * svObjectPositionForPlane2.fz) )); #else diff --git a/sw/airborne/modules/cam_control/rotorcraft_cam.c b/sw/airborne/modules/cam_control/rotorcraft_cam.c index 0fe477a782..448cf9786c 100644 --- a/sw/airborne/modules/cam_control/rotorcraft_cam.c +++ b/sw/airborne/modules/cam_control/rotorcraft_cam.c @@ -88,24 +88,26 @@ int16_t rotorcraft_cam_pan; #define ROTORCRAFT_CAM_PAN_MIN 0 #define ROTORCRAFT_CAM_PAN_MAX INT32_ANGLE_2_PI -static void send_cam(struct transport_tx *trans, struct link_device *dev) { +static void send_cam(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_ROTORCRAFT_CAM(trans, dev, AC_ID, - &rotorcraft_cam_tilt,&rotorcraft_cam_pan); + &rotorcraft_cam_tilt, &rotorcraft_cam_pan); } -void rotorcraft_cam_set_mode(uint8_t mode) { +void rotorcraft_cam_set_mode(uint8_t mode) +{ rotorcraft_cam_mode = mode; #ifdef ROTORCRAFT_CAM_SWITCH_GPIO if (rotorcraft_cam_mode == ROTORCRAFT_CAM_MODE_NONE) { ROTORCRAFT_CAM_OFF(ROTORCRAFT_CAM_SWITCH_GPIO); - } - else { + } else { ROTORCRAFT_CAM_ON(ROTORCRAFT_CAM_SWITCH_GPIO); } #endif } -void rotorcraft_cam_init(void) { +void rotorcraft_cam_init(void) +{ #ifdef ROTORCRAFT_CAM_SWITCH_GPIO gpio_setup_output(ROTORCRAFT_CAM_SWITCH_GPIO); #endif @@ -122,7 +124,8 @@ void rotorcraft_cam_init(void) { register_periodic_telemetry(DefaultPeriodic, "ROTORCRAFT_CAM", send_cam); } -void rotorcraft_cam_periodic(void) { +void rotorcraft_cam_periodic(void) +{ switch (rotorcraft_cam_mode) { case ROTORCRAFT_CAM_MODE_NONE: @@ -138,8 +141,9 @@ void rotorcraft_cam_periodic(void) { break; case ROTORCRAFT_CAM_MODE_HEADING: #if ROTORCRAFT_CAM_USE_TILT_ANGLES - Bound(rotorcraft_cam_tilt,CT_MIN,CT_MAX); - rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN); + Bound(rotorcraft_cam_tilt, CT_MIN, CT_MAX); + rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) / + (CAM_TA_MAX - CAM_TA_MIN); #endif #if ROTORCRAFT_CAM_USE_PAN INT32_COURSE_NORMALIZE(rotorcraft_cam_pan); @@ -151,7 +155,7 @@ void rotorcraft_cam_periodic(void) { { struct Int32Vect2 diff; VECT2_DIFF(diff, waypoints[ROTORCRAFT_CAM_TRACK_WP], *stateGetPositionEnu_i()); - INT32_VECT2_RSHIFT(diff,diff,INT32_POS_FRAC); + INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC); rotorcraft_cam_pan = int32_atan2(diff.x, diff.y); nav_heading = rotorcraft_cam_pan; #if ROTORCRAFT_CAM_USE_TILT_ANGLES @@ -160,7 +164,8 @@ void rotorcraft_cam_periodic(void) { height = (waypoints[ROTORCRAFT_CAM_TRACK_WP].z - stateGetPositionEnu_i()->z) >> INT32_POS_FRAC; rotorcraft_cam_tilt = int32_atan2(height, dist); Bound(rotorcraft_cam_tilt, CAM_TA_MIN, CAM_TA_MAX); - rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN); + rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) / + (CAM_TA_MAX - CAM_TA_MIN); #endif } #endif diff --git a/sw/airborne/modules/cam_control/rotorcraft_cam.h b/sw/airborne/modules/cam_control/rotorcraft_cam.h index c11044289b..199261d53c 100644 --- a/sw/airborne/modules/cam_control/rotorcraft_cam.h +++ b/sw/airborne/modules/cam_control/rotorcraft_cam.h @@ -103,7 +103,7 @@ extern void rotorcraft_cam_set_mode(uint8_t mode); */ #define rotorcraft_cam_SetCamMode(_v) { \ rotorcraft_cam_set_mode(_v); \ -} + } /** Cam control from datalink message. * camera tilt and pan are incremented by STICK_TILT_INC and STICK_PAN_INC @@ -117,10 +117,10 @@ extern void rotorcraft_cam_set_mode(uint8_t mode); #endif #define ROTORCRAFT_CAM_STICK_PARSE(_dl_buffer) { \ - rotorcraft_cam_tilt += (int16_t)((ANGLE_BFP_OF_REAL(ROTORCRAFT_CAM_STICK_TILT_INC)/127.)*(float)DL_ROTORCRAFT_CAM_STICK_tilt(_dl_buffer)); \ - rotorcraft_cam_pan += (int16_t)((ANGLE_BFP_OF_REAL(ROTORCRAFT_CAM_STICK_PAN_INC)/127.)*(float)DL_ROTORCRAFT_CAM_STICK_pan(dl_buffer)); \ - INT32_COURSE_NORMALIZE(rotorcraft_cam_pan); \ -} + rotorcraft_cam_tilt += (int16_t)((ANGLE_BFP_OF_REAL(ROTORCRAFT_CAM_STICK_TILT_INC)/127.)*(float)DL_ROTORCRAFT_CAM_STICK_tilt(_dl_buffer)); \ + rotorcraft_cam_pan += (int16_t)((ANGLE_BFP_OF_REAL(ROTORCRAFT_CAM_STICK_PAN_INC)/127.)*(float)DL_ROTORCRAFT_CAM_STICK_pan(dl_buffer)); \ + INT32_COURSE_NORMALIZE(rotorcraft_cam_pan); \ + } #endif /* ROTORCRAFT_CAM_H */ diff --git a/sw/airborne/modules/cartography/cartography.c b/sw/airborne/modules/cartography/cartography.c index 7cce1532df..40f2257746 100644 --- a/sw/airborne/modules/cartography/cartography.c +++ b/sw/airborne/modules/cartography/cartography.c @@ -45,17 +45,17 @@ // Be carefull not to use printf function in ap compilation, only use it in sim compilation // the DEBUG_PRINTF should be defined only in the sim part of the makefile airframe file #ifdef DEBUG_PRINTF -int CPTDEBUG=0; +int CPTDEBUG = 0; #define PRTDEB(TYPE,EXP) \ -printf("%5d: " #EXP ": %"#TYPE"\n",CPTDEBUG,EXP);fflush(stdout);CPTDEBUG++; + printf("%5d: " #EXP ": %"#TYPE"\n",CPTDEBUG,EXP);fflush(stdout);CPTDEBUG++; #define PRTDEBSTR(EXP) \ -printf("%5d: STR: "#EXP"\n",CPTDEBUG);fflush(stdout);CPTDEBUG++; + printf("%5d: STR: "#EXP"\n",CPTDEBUG);fflush(stdout);CPTDEBUG++; #else #define PRTDEB(TYPE,EXP) \ -; + ; #define PRTDEBSTR(EXP) \ -; + ; #endif /* @@ -69,10 +69,10 @@ printf("%5d: STR: "#EXP"\n",CPTDEBUG);fflush(stdout);CPTDEBUG++; //////////////////////////////////////////////////////////////////////////////////////////////// #define DISTXY(P1X,P1Y,P2X,P2Y)\ -(sqrt( ( (P2X-P1X) * (P2X-P1X) ) + ( (P2Y-P1Y) * (P2Y-P1Y) ) ) ) + (sqrt( ( (P2X-P1X) * (P2X-P1X) ) + ( (P2Y-P1Y) * (P2Y-P1Y) ) ) ) #define NORMXY(P1X,P1Y)\ -(sqrt( ( (P1X) * (P1X) ) + ( (P1Y) * (P1Y) ) ) ) + (sqrt( ( (P1X) * (P1X) ) + ( (P1Y) * (P1Y) ) ) ) //max distance between the estimator position and an objective points to consider that the objective points is atteined @@ -81,14 +81,15 @@ printf("%5d: STR: "#EXP"\n",CPTDEBUG);fflush(stdout);CPTDEBUG++; //////////////////////////////////////////////////////////////////////////////////////////////// -uint16_t railnumberSinceBoot=1; //used to count the number of rails the plane has achieved since the boot, to number the sequences of saved images +uint16_t railnumberSinceBoot = + 1; //used to count the number of rails the plane has achieved since the boot, to number the sequences of saved images //the number 1 is reserved for snapshot fonctions that take only one image, the 2-65535 numbers are used to number the following sequences //////////////////////////////////////////////////////////////////////////////////////////////// #define USE_ONBOARD_CAMERA 1 #if USE_ONBOARD_CAMERA -uint16_t camera_snapshot_image_number=0; +uint16_t camera_snapshot_image_number = 0; #endif @@ -103,26 +104,27 @@ int numberofrailtodo; float distrail; //distance between adjacent rails in meters, the value is set in the init function float distplus; //distance that the aircraft should travel before and after a rail before turning to the next rails in meters, the value is set in the init function -float distrailinteractif=60; //a cheangable value that can be set interactively in the GCS, not used at that time, it can be used to choose the right value while the aircraft is flying +float distrailinteractif = + 60; //a cheangable value that can be set interactively in the GCS, not used at that time, it can be used to choose the right value while the aircraft is flying -static struct point point1,point2,point3; // 3 2D points used for navigation -static struct point pointA,pointB,pointC; // 3 2D points used for navigation -static struct point vec12,vec13; -float norm12,norm13; // norms of 12 & 13 vectors +static struct point point1, point2, point3; // 3 2D points used for navigation +static struct point pointA, pointB, pointC; // 3 2D points used for navigation +static struct point vec12, vec13; +float norm12, norm13; // norms of 12 & 13 vectors -float tempx,tempy; //temporary points for exchanges +float tempx, tempy; //temporary points for exchanges float angle1213; //angle between 12 & 13 vectors float signforturn; //can be +1 or -1, it is used to compute the direction of the UTURN between rails float tempcircleradius;// used to compute the radius of the UTURN after a rail -float circleradiusmin=40; +float circleradiusmin = 40; //////////////////////////////////////////////////////////////////////////////////////////////// -float normBM,normAM,distancefromrail; +float normBM, normAM, distancefromrail; float course_next_rail; @@ -143,17 +145,21 @@ bool_t ProjectionInsideLimitOfRail; //////////////////////////////////////////////////////////////////////////////////////////////// -void init_carto(void) { +void init_carto(void) +{ } -void periodic_downlink_carto(void) { - DOWNLINK_SEND_CAMERA_SNAPSHOT(DefaultChannel, DefaultDevice,&camera_snapshot_image_number); +void periodic_downlink_carto(void) +{ + DOWNLINK_SEND_CAMERA_SNAPSHOT(DefaultChannel, DefaultDevice, &camera_snapshot_image_number); } -void start_carto(void) { +void start_carto(void) +{ } -void stop_carto(void) { +void stop_carto(void) +{ } @@ -167,7 +173,7 @@ bool_t nav_survey_Inc_railnumberSinceBoot(void) /////////////////////////////////////////////////////////////////////////////////////////////// bool_t nav_survey_Snapshoot(void) { - camera_snapshot_image_number=railnumberSinceBoot; + camera_snapshot_image_number = railnumberSinceBoot; PRTDEBSTR(SNAPSHOT) cartography_periodic_downlink_carto_status = MODULES_START; return FALSE; @@ -176,7 +182,7 @@ bool_t nav_survey_Snapshoot(void) /////////////////////////////////////////////////////////////////////////////////////////////// bool_t nav_survey_Snapshoot_Continu(void) { - camera_snapshot_image_number=railnumberSinceBoot; + camera_snapshot_image_number = railnumberSinceBoot; PRTDEBSTR(SNAPSHOT) cartography_periodic_downlink_carto_status = MODULES_START; return TRUE; @@ -185,7 +191,7 @@ bool_t nav_survey_Snapshoot_Continu(void) /////////////////////////////////////////////////////////////////////////////////////////////// bool_t nav_survey_StopSnapshoot(void) { - camera_snapshot_image_number=0; + camera_snapshot_image_number = 0; PRTDEBSTR(STOP SNAPSHOT) cartography_periodic_downlink_carto_status = MODULES_START; return FALSE; @@ -193,44 +199,46 @@ bool_t nav_survey_StopSnapshoot(void) } /////////////////////////////////////////////////////////////////////////////////////////////// -bool_t nav_survey_computefourth_corner(uint8_t wp1, uint8_t wp2, uint8_t wp3, uint8_t wp4 ) +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) + PRTDEB(f, waypoints[wp4].x) + PRTDEB(f, waypoints[wp4].y) return FALSE; } //////////////////////////////////////////////////////////////////////////////////////////////// -bool_t nav_survey_ComputeProjectionOnLine(struct point pointAf,struct point pointBf,float pos_xf,float pos_yf,float *normAMf,float *normBMf,float *distancefromrailf); +bool_t nav_survey_ComputeProjectionOnLine(struct point pointAf, struct point pointBf, float pos_xf, float pos_yf, + float *normAMf, float *normBMf, float *distancefromrailf); -bool_t nav_survey_ComputeProjectionOnLine(struct point pointAf,struct point pointBf,float pos_xf,float pos_yf,float *normAMf,float *normBMf,float *distancefromrailf) +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 a, b, c, xa, xb, xc, ya, yb, yc; float f; float AA1; float BB1; float YP; float XP; - float AMx,AMy,BMx,BMy; + 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; @@ -240,30 +248,32 @@ bool_t nav_survey_ComputeProjectionOnLine(struct point pointAf,struct point poi //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 + 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) + 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 + } else { return 0; - } - else + } + } else { return 0; + } @@ -273,29 +283,26 @@ bool_t nav_survey_ComputeProjectionOnLine(struct point pointAf,struct point poi //+++++++++++++++++++++++++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)) - { + if (((*normAMf) + (*normBMf)) > 1.05 * DISTXY(pointBf.x, pointBf.y, pointAf.x, pointAf.y)) { PRTDEBSTR(NOT INSIDE) return 0; - } - else - { + } else { PRTDEBSTR(INSIDE) return 1; } @@ -306,93 +313,96 @@ bool_t nav_survey_ComputeProjectionOnLine(struct point pointAf,struct point poi 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; + 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); + 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; + // 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) + if (norm12 < 1e-15) { return FALSE; - if (norm13<1e-15) + } + 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 @@ -410,50 +420,46 @@ bool_t nav_survey_losange_carto(void) //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; + camera_snapshot_image_number = 0; - PRTDEB(f,distrail) + PRTDEB(f, distrail) PRTDEBSTR(nav_survey_losange_carto) - PRTDEB(d,railnumber) + PRTDEB(d, railnumber) - PRTDEB(d,railnumberSinceBoot) + PRTDEB(d, railnumberSinceBoot) //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) + if (norm13 < 1e-15) { + PRTDEBSTR(norm13 < 1e-15) return FALSE; } - if (norm12<1e-15) - { - PRTDEBSTR(norm13<1e-15) + if (norm12 < 1e-15) { + PRTDEBSTR(norm13 < 1e-15) return FALSE; } - if (distrail<1e-15) - { - PRTDEBSTR(distrail<1e-15) + 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 + 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é + 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) @@ -464,243 +470,227 @@ bool_t nav_survey_losange_carto(void) //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); + 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 - { + } else { PRTDEBSTR(debut rail 0) //un fois arrivé, on commence le 1° rail; - railnumber=0; + 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 - { + 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 - { + } 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; + 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)))) + if (!((DISTXY(stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, pointB.x, pointB.y) < DISTLIMIT) + || (ProjectionInsideLimitOfRail && (normAM > (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); + 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); + // nav_survey_ComputeProjectionOnLine(pointA,pointB,stateGetPositionEnu_f()->x,stateGetPositionEnu_f()->y,&normAM,&normBM,&distancefromrail); - if ( (normAM> distplus) && (normBM> distplus) && (distancefromrail distplus) && (normBM > distplus) && (distancefromrail < distrail / 2)) { //CAMERA_SNAPSHOT_REQUIERED=TRUE; //camera_snapshot_image_number++; - camera_snapshot_image_number=railnumberSinceBoot; + camera_snapshot_image_number = railnumberSinceBoot; PRTDEBSTR(SNAPSHOT) } } - else // virage - { + else { // virage //PRTDEBSTR(debut rail suivant) railnumber++; railnumberSinceBoot++; - PRTDEB(d,railnumber) - PRTDEB(d,railnumberSinceBoot) + PRTDEB(d, railnumber) + PRTDEB(d, railnumberSinceBoot) //CAMERA_SNAPSHOT_REQUIERED=TRUE; //camera_snapshot_image_number++; PRTDEBSTR(UTURN) - survey_losange_uturn=TRUE; + survey_losange_uturn = TRUE; } - if (railnumber>numberofrailtodo) - { + if (railnumber > 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) - { + } else { // (survey_losange_uturn==TRUE) - if (distrail<200) - { + 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) + 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); + 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); + 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; + 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 = (course_next_rail - (*stateGetHorizontalSpeedDir_f())); + while (angle_between > 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 ) + 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) ) - { + NavCircleWaypoint(0, signforturn * tempcircleradius); + if ((DISTXY(stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, pointB.x, pointB.y) < DISTLIMIT) + || (angle_between > -10 && angle_between < 10)) { //l'avion fait le rail suivant - survey_losange_uturn=FALSE; - PRTDEBSTR(FIN UTURN-IMPAIR) + 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) + } 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); + 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); + 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; + 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 = (course_next_rail - (*stateGetHorizontalSpeedDir_f())); + while (angle_between > 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 ) + 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) ) - { + NavCircleWaypoint(0, signforturn * (-1)*tempcircleradius); + if ((DISTXY(stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, pointB.x, pointB.y) < DISTLIMIT) + || (angle_between > -10 && angle_between < 10)) { //l'avion fait le rail suivant - survey_losange_uturn=FALSE; - PRTDEBSTR(FIN UTURN-PAIR) + 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 + } 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) + 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); + 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) < DISTLIMIT) { //l'avion fait le rail suivant - survey_losange_uturn=FALSE; - PRTDEBSTR(FIN TRANSIT-IMPAIR) + survey_losange_uturn = FALSE; + PRTDEBSTR(FIN TRANSIT - IMPAIR) } - } - else //if ((railnumber %2)==0) //rail précédent n1, 3, 5, donc seconde direction, de wp2 vers wp1 - { - PRTDEBSTR(TRANSIT-PAIR) + } else { //if ((railnumber %2)==0) //rail précédent n1, 3, 5, donc seconde direction, de wp2 vers wp1 + PRTDEBSTR(TRANSIT - 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); + 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); - 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) < DISTLIMIT) { //l'avion fait le rail suivant - survey_losange_uturn=FALSE; - PRTDEBSTR(FIN TRANSIT-PAIR) + survey_losange_uturn = FALSE; + PRTDEBSTR(FIN TRANSIT - PAIR) } } diff --git a/sw/airborne/modules/cartography/cartography.h b/sw/airborne/modules/cartography/cartography.h index ab0312560b..578b1d04d0 100644 --- a/sw/airborne/modules/cartography/cartography.h +++ b/sw/airborne/modules/cartography/cartography.h @@ -32,11 +32,12 @@ extern bool_t nav_survey_Inc_railnumberSinceBoot(void); extern bool_t nav_survey_Snapshoot(void); bool_t nav_survey_Snapshoot_Continu(void); extern bool_t nav_survey_StopSnapshoot(void); -extern bool_t nav_survey_computefourth_corner(uint8_t wp1, uint8_t wp2, uint8_t wp3, uint8_t wp4 ); +extern bool_t nav_survey_computefourth_corner(uint8_t wp1, uint8_t wp2, uint8_t wp3, uint8_t wp4); extern bool_t nav_survey_losange_carto_init(uint8_t wp1, uint8_t wp2, uint8_t wp3, float distrail, float distplus); -extern bool_t nav_survey_losange_carto(void); // !!!! important il faut mettre void en parametres d'entrée, sinon le compilo dit: attention : function declaration isn»t a prototype +extern bool_t nav_survey_losange_carto( + void); // !!!! important il faut mettre void en parametres d'entrée, sinon le compilo dit: attention : function declaration isn»t a prototype //(uint8_t wp1, uint8_t wp2, uint8_t wp3); diff --git a/sw/airborne/modules/cartography/photogrammetry_calculator.c b/sw/airborne/modules/cartography/photogrammetry_calculator.c index e6a8580f03..b09692b0ce 100644 --- a/sw/airborne/modules/cartography/photogrammetry_calculator.c +++ b/sw/airborne/modules/cartography/photogrammetry_calculator.c @@ -94,10 +94,11 @@ void photogrammetry_calculator_update_camera2flightplan(void) // Flightplan Variables photogrammetry_height = ((float) photogrammetry_resolution) / pixel_projection_width / 1000.0f; - if (photogrammetry_height > photogrammetry_height_max) + if (photogrammetry_height > photogrammetry_height_max) { photogrammetry_height = photogrammetry_height_max; - else if (photogrammetry_height < photogrammetry_height_min) + } else if (photogrammetry_height < photogrammetry_height_min) { photogrammetry_height = photogrammetry_height_min; + } photogrammetry_sidestep = viewing_ratio_width * photogrammetry_height * (1.0f - photogrammetry_sidelap_f); photogrammetry_triggerstep = viewing_ratio_height * photogrammetry_height * (1.0f - photogrammetry_overlap_f); diff --git a/sw/airborne/modules/cartography/photogrammetry_calculator.h b/sw/airborne/modules/cartography/photogrammetry_calculator.h index e9a33e6e65..e3a49928af 100644 --- a/sw/airborne/modules/cartography/photogrammetry_calculator.h +++ b/sw/airborne/modules/cartography/photogrammetry_calculator.h @@ -28,8 +28,8 @@ Add to airframe file:
- - + + @@ -54,9 +54,9 @@ Add to flightplan or airframe file: Add to flightplan @verbatim
-#define PHOTOGRAMMETRY_SWEEP_ANGLE RadOfDeg(53) // angle in radians from the North -#define PHOTOGRAMMETRY_OVERLAP 50 // 1-99 Procent -#define PHOTOGRAMMETRY_SIDELAP 50 // 1-99 Procent +#define PHOTOGRAMMETRY_SWEEP_ANGLE RadOfDeg(53) // angle in radians from the North +#define PHOTOGRAMMETRY_OVERLAP 50 // 1-99 Procent +#define PHOTOGRAMMETRY_SIDELAP 50 // 1-99 Procent #define PHOTOGRAMMETRY_RESOLUTION 80 // mm pixel projection size
@@ -103,50 +103,50 @@ void photogrammetry_calculator_update_camera2flightplan(void); void photogrammetry_calculator_update_flightplan2camera(void); // Update Flightplan on Camera Change -#define photogrammetry_calculator_UpdateSideLap(X) { \ - photogrammetry_sidelap = X; \ - photogrammetry_calculator_update_camera2flightplan(); \ -} +#define photogrammetry_calculator_UpdateSideLap(X) { \ + photogrammetry_sidelap = X; \ + photogrammetry_calculator_update_camera2flightplan(); \ + } -#define photogrammetry_calculator_UpdateOverLap(X) { \ - photogrammetry_overlap = X; \ - photogrammetry_calculator_update_camera2flightplan(); \ -} +#define photogrammetry_calculator_UpdateOverLap(X) { \ + photogrammetry_overlap = X; \ + photogrammetry_calculator_update_camera2flightplan(); \ + } -#define photogrammetry_calculator_UpdateResolution(X) { \ - photogrammetry_resolution = X; \ - photogrammetry_calculator_update_camera2flightplan(); \ -} +#define photogrammetry_calculator_UpdateResolution(X) { \ + photogrammetry_resolution = X; \ + photogrammetry_calculator_update_camera2flightplan(); \ + } // Update Camera on Flightplan Change -#define photogrammetry_calculator_UpdateHeight(X) { \ - photogrammetry_height = X; \ - photogrammetry_calculator_update_flightplan2camera(); \ -} +#define photogrammetry_calculator_UpdateHeight(X) { \ + photogrammetry_height = X; \ + photogrammetry_calculator_update_flightplan2camera(); \ + } -#define photogrammetry_calculator_UpdateSideStep(X) { \ - photogrammetry_sidestep = X; \ - photogrammetry_calculator_update_flightplan2camera(); \ -} +#define photogrammetry_calculator_UpdateSideStep(X) { \ + photogrammetry_sidestep = X; \ + photogrammetry_calculator_update_flightplan2camera(); \ + } -#define photogrammetry_calculator_UpdateTriggerStep(X) { \ - photogrammetry_triggerstep = X; \ - photogrammetry_calculator_update_flightplan2camera(); \ -} +#define photogrammetry_calculator_UpdateTriggerStep(X) { \ + photogrammetry_triggerstep = X; \ + photogrammetry_calculator_update_flightplan2camera(); \ + } // Flightplan Routine Wrappers -#define PhotogrammetryCalculatorPolygonSurveyOsam(_WP, _COUNT) { \ - WaypointAlt(_WP) = photogrammetry_height + GROUND_ALT; \ - int _ang = 90 - DegOfRad(photogrammetry_sweep_angle); \ - while (_ang > 90) _ang -= 180; while (_ang < -90) _ang += 180; \ - nav_survey_poly_osam_setup((_WP), (_COUNT), 2*photogrammetry_sidestep, _ang); \ -} +#define PhotogrammetryCalculatorPolygonSurveyOsam(_WP, _COUNT) { \ + WaypointAlt(_WP) = photogrammetry_height + GROUND_ALT; \ + int _ang = 90 - DegOfRad(photogrammetry_sweep_angle); \ + while (_ang > 90) _ang -= 180; while (_ang < -90) _ang += 180; \ + nav_survey_poly_osam_setup((_WP), (_COUNT), 2*photogrammetry_sidestep, _ang); \ + } -#define PhotogrammetryCalculatorPolygonSurvey(_WP, _COUNT) { \ - nav_survey_polygon_setup((_WP), (_COUNT), DegOfRad(photogrammetry_sweep_angle), \ - photogrammetry_sidestep, photogrammetry_triggerstep, \ - photogrammetry_radius_min, photogrammetry_height + GROUND_ALT); \ -} +#define PhotogrammetryCalculatorPolygonSurvey(_WP, _COUNT) { \ + nav_survey_polygon_setup((_WP), (_COUNT), DegOfRad(photogrammetry_sweep_angle), \ + photogrammetry_sidestep, photogrammetry_triggerstep, \ + photogrammetry_radius_min, photogrammetry_height + GROUND_ALT); \ + } #endif diff --git a/sw/airborne/modules/com/generic_com.c b/sw/airborne/modules/com/generic_com.c index 7df8ed9b88..216461d710 100644 --- a/sw/airborne/modules/com/generic_com.c +++ b/sw/airborne/modules/com/generic_com.c @@ -49,37 +49,39 @@ struct i2c_transaction com_trans; bool_t active_com; -void generic_com_init( void ) { +void generic_com_init(void) +{ active_com = FALSE; com_trans.status = I2CTransDone; } #define FillBufWith32bit(_buf, _index, _value) { \ - _buf[_index] = (uint8_t) (_value); \ - _buf[_index+1] = (uint8_t) ((_value) >> 8); \ - _buf[_index+2] = (uint8_t) ((_value) >> 16); \ - _buf[_index+3] = (uint8_t) ((_value) >> 24); \ -} + _buf[_index] = (uint8_t) (_value); \ + _buf[_index+1] = (uint8_t) ((_value) >> 8); \ + _buf[_index+2] = (uint8_t) ((_value) >> 16); \ + _buf[_index+3] = (uint8_t) ((_value) >> 24); \ + } #define FillBufWith16bit(_buf, _index, _value) { \ - _buf[_index] = (uint8_t) (_value); \ - _buf[_index+1] = (uint8_t) ((_value) >> 8); \ -} + _buf[_index] = (uint8_t) (_value); \ + _buf[_index+1] = (uint8_t) ((_value) >> 8); \ + } -void generic_com_periodic( void ) { +void generic_com_periodic(void) +{ if (com_trans.status != I2CTransDone) { return; } com_trans.buf[0] = active_com; FillBufWith32bit(com_trans.buf, 1, gps.lla_pos.lat); FillBufWith32bit(com_trans.buf, 5, gps.lla_pos.lon); - FillBufWith16bit(com_trans.buf, 9, (int16_t)(gps.lla_pos.alt/1000)); // altitude (meters) + FillBufWith16bit(com_trans.buf, 9, (int16_t)(gps.lla_pos.alt / 1000)); // altitude (meters) FillBufWith16bit(com_trans.buf, 11, gps.gspeed); // ground speed (cm/s) - FillBufWith16bit(com_trans.buf, 13, (int16_t)(gps.course/1e4)); // course (1e3rad) - FillBufWith16bit(com_trans.buf, 15, (uint16_t)((*stateGetAirspeed_f())*100)); // TAS (cm/s) + FillBufWith16bit(com_trans.buf, 13, (int16_t)(gps.course / 1e4)); // course (1e3rad) + FillBufWith16bit(com_trans.buf, 15, (uint16_t)((*stateGetAirspeed_f()) * 100)); // TAS (cm/s) com_trans.buf[17] = electrical.vsupply; // decivolts - com_trans.buf[18] = (uint8_t)(energy/100); // deciAh - com_trans.buf[19] = (uint8_t)(ap_state->commands[COMMAND_THROTTLE]*100/MAX_PPRZ); + com_trans.buf[18] = (uint8_t)(energy / 100); // deciAh + com_trans.buf[19] = (uint8_t)(ap_state->commands[COMMAND_THROTTLE] * 100 / MAX_PPRZ); com_trans.buf[20] = pprz_mode; com_trans.buf[21] = nav_block; FillBufWith16bit(com_trans.buf, 22, autopilot_flight_time); @@ -87,19 +89,22 @@ void generic_com_periodic( void ) { } -void generic_com_event( void ) { +void generic_com_event(void) +{ // Handle I2C event if (com_trans.status == I2CTransSuccess || com_trans.status == I2CTransFailed) { com_trans.status = I2CTransDone; } } -void start_com( void ) { +void start_com(void) +{ active_com = TRUE; com_trans.status = I2CTransDone; } -void stop_com( void ) { +void stop_com(void) +{ active_com = FALSE; com_trans.buf[0] = active_com; i2c_transmit(&GENERIC_COM_I2C_DEV, &com_trans, GENERIC_COM_SLAVE_ADDR, 1); diff --git a/sw/airborne/modules/com/generic_com.h b/sw/airborne/modules/com/generic_com.h index 2221a7c8f5..00b6dedfe9 100644 --- a/sw/airborne/modules/com/generic_com.h +++ b/sw/airborne/modules/com/generic_com.h @@ -25,10 +25,10 @@ #ifndef GENERIC_COM_H #define GENERIC_COM_H -void generic_com_init( void ); -void generic_com_periodic( void ); -void generic_com_event( void ); -void start_com( void ); -void stop_com( void ); +void generic_com_init(void); +void generic_com_periodic(void); +void generic_com_event(void); +void start_com(void); +void stop_com(void); #endif // GENERIC_COM_H diff --git a/sw/airborne/modules/com/usb_serial_stm32_example2.c b/sw/airborne/modules/com/usb_serial_stm32_example2.c index c44b02fc9b..54f7efe8f0 100644 --- a/sw/airborne/modules/com/usb_serial_stm32_example2.c +++ b/sw/airborne/modules/com/usb_serial_stm32_example2.c @@ -35,7 +35,8 @@ uint8_t run; uint8_t prompt = '$'; /* A lot of text */ -uint8_t big_buffer[] = " ASCII stands for American Standard Code for Information Interchange. Computers can only understand numbers, so an ASCII code is the numerical representation of a character such as 'a' or '@' or an action of some sort. ASCII was developed a long time ago and now the non-printing characters are rarely used for their original purpose. Below is the ASCII character table and this includes descriptions of the first 32 non-printing characters. ASCII was actually designed for use with teletypes and so the descriptions are somewhat obscure. If someone says they want your CV however in ASCII format, all this means is they want 'plain' text with no formatting such as tabs, bold or underscoring - the raw format that any computer can understand. This is usually so they can easily import the file into their own applications without issues. Notepad.exe creates ASCII text, or in MS Word you can save a file as 'text only' "; +uint8_t big_buffer[] = + " ASCII stands for American Standard Code for Information Interchange. Computers can only understand numbers, so an ASCII code is the numerical representation of a character such as 'a' or '@' or an action of some sort. ASCII was developed a long time ago and now the non-printing characters are rarely used for their original purpose. Below is the ASCII character table and this includes descriptions of the first 32 non-printing characters. ASCII was actually designed for use with teletypes and so the descriptions are somewhat obscure. If someone says they want your CV however in ASCII format, all this means is they want 'plain' text with no formatting such as tabs, bold or underscoring - the raw format that any computer can understand. This is usually so they can easily import the file into their own applications without issues. Notepad.exe creates ASCII text, or in MS Word you can save a file as 'text only' "; /** * Init module, call VCOM_init() from here diff --git a/sw/airborne/modules/computer_vision/cv/color.h b/sw/airborne/modules/computer_vision/cv/color.h index 61a78bbca4..46ffdb413a 100644 --- a/sw/airborne/modules/computer_vision/cv/color.h +++ b/sw/airborne/modules/computer_vision/cv/color.h @@ -23,69 +23,64 @@ #include #include "image.h" -inline void grayscale_uyvy(struct img_struct* input, struct img_struct* output); -inline void grayscale_uyvy(struct img_struct* input, struct img_struct* output) +inline void grayscale_uyvy(struct img_struct *input, struct img_struct *output); +inline void grayscale_uyvy(struct img_struct *input, struct img_struct *output) { uint8_t *source = input->buf; uint8_t *dest = output->buf; source++; - for (int y=0;yh;y++) - { - for (int x=0;xw;x++) - { + for (int y = 0; y < output->h; y++) { + for (int x = 0; x < output->w; x++) { // UYVY *dest++ = 127; // U *dest++ = *source; // Y - source+=2; + source += 2; } } } -inline int colorfilt_uyvy(struct img_struct* input, struct img_struct* output, uint8_t y_m, uint8_t y_M, uint8_t u_m, uint8_t u_M, uint8_t v_m, uint8_t v_M); -inline int colorfilt_uyvy(struct img_struct* input, struct img_struct* output, uint8_t y_m, uint8_t y_M, uint8_t u_m, uint8_t u_M, uint8_t v_m, uint8_t v_M) +inline int colorfilt_uyvy(struct img_struct *input, struct img_struct *output, uint8_t y_m, uint8_t y_M, uint8_t u_m, + uint8_t u_M, uint8_t v_m, uint8_t v_M); +inline int colorfilt_uyvy(struct img_struct *input, struct img_struct *output, uint8_t y_m, uint8_t y_M, uint8_t u_m, + uint8_t u_M, uint8_t v_m, uint8_t v_M) { int cnt = 0; uint8_t *source = input->buf; uint8_t *dest = output->buf; - for (int y=0;yh;y++) - { - for (int x=0;xw;x+=2) - { + for (int y = 0; y < output->h; y++) { + for (int x = 0; x < output->w; x += 2) { // Color Check: if ( - // Light - (dest[1] >= y_m) - && (dest[1] <= y_M) - && (dest[0] >= u_m) - && (dest[0] <= u_M) - && (dest[2] >= v_m) - && (dest[2] <= v_M) - )// && (dest[2] > 128)) - { + // Light + (dest[1] >= y_m) + && (dest[1] <= y_M) + && (dest[0] >= u_m) + && (dest[0] <= u_M) + && (dest[2] >= v_m) + && (dest[2] <= v_M) + ) { // && (dest[2] > 128)) cnt ++; // UYVY dest[0] = 64; // U dest[1] = source[1]; // Y dest[2] = 255; // V dest[3] = source[3]; // Y - } - else - { + } else { // UYVY - char u = source[0]-127; - u/=4; + char u = source[0] - 127; + u /= 4; dest[0] = 127; // U dest[1] = source[1]; // Y - u = source[2]-127; - u/=4; + u = source[2] - 127; + u /= 4; dest[2] = 127; // V dest[3] = source[3]; // Y } - dest+=4; - source+=4; + dest += 4; + source += 4; } } return cnt; diff --git a/sw/airborne/modules/computer_vision/cv/encoding/jpeg.c b/sw/airborne/modules/computer_vision/cv/encoding/jpeg.c index b9c32cb1d6..119f4bc2cf 100644 --- a/sw/airborne/modules/computer_vision/cv/encoding/jpeg.c +++ b/sw/airborne/modules/computer_vision/cv/encoding/jpeg.c @@ -19,59 +19,63 @@ static inline unsigned char svs_size_code(int w) { // 1=(40,30) 2=(128,96) 3=(160,120) 5=(320,240) 7=(640,480) 9=(1280,1024); - if (w<=40) + if (w <= 40) { return '1'; - if (w<=128) + } + if (w <= 128) { return '2'; - if (w<=160) + } + if (w <= 160) { return '3'; - if (w<=320) + } + if (w <= 320) { return '4'; - if (w<=640) + } + if (w <= 640) { return '7'; + } return '9'; } -int create_svs_jpeg_header(unsigned char* jpegbuf, int32_t size, int w) +int create_svs_jpeg_header(unsigned char *jpegbuf, int32_t size, int w) { // SVS Surveyor Jpeg UDP format uint32_t s = size; - uint8_t* p = (uint8_t*) & s; - jpegbuf[0]='#'; - jpegbuf[1]='#'; - jpegbuf[2]='I'; - jpegbuf[3]='M'; - jpegbuf[4]='J'; - jpegbuf[5]=svs_size_code(w); - jpegbuf[6]=p[0]; - jpegbuf[7]=p[1]; - jpegbuf[8]=p[2]; - jpegbuf[9]=0x00; + uint8_t *p = (uint8_t *) & s; + jpegbuf[0] = '#'; + jpegbuf[1] = '#'; + jpegbuf[2] = 'I'; + jpegbuf[3] = 'M'; + jpegbuf[4] = 'J'; + jpegbuf[5] = svs_size_code(w); + jpegbuf[6] = p[0]; + jpegbuf[7] = p[1]; + jpegbuf[8] = p[2]; + jpegbuf[9] = 0x00; return size + 10; } -typedef struct JPEG_ENCODER_STRUCTURE -{ - uint16_t mcu_width; - uint16_t mcu_height; - uint16_t horizontal_mcus; - uint16_t vertical_mcus; - uint16_t cols_in_right_mcus; - uint16_t rows_in_bottom_mcus; +typedef struct JPEG_ENCODER_STRUCTURE { + uint16_t mcu_width; + uint16_t mcu_height; + uint16_t horizontal_mcus; + uint16_t vertical_mcus; + uint16_t cols_in_right_mcus; + uint16_t rows_in_bottom_mcus; - uint16_t rows; - uint16_t cols; + uint16_t rows; + uint16_t cols; - uint16_t length_minus_mcu_width; - uint16_t length_minus_width; - uint16_t incr; - uint16_t mcu_width_size; - uint16_t offset; + uint16_t length_minus_mcu_width; + uint16_t length_minus_width; + uint16_t incr; + uint16_t mcu_width_size; + uint16_t offset; - int16_t ldc1; - int16_t ldc2; - int16_t ldc3; + int16_t ldc1; + int16_t ldc2; + int16_t ldc3; } JPEG_ENCODER_STRUCTURE; @@ -94,221 +98,208 @@ extern int16_t Temp [BLOCK_SIZE]; extern uint32_t lcode; extern uint16_t bitindex; -void initialization (JPEG_ENCODER_STRUCTURE *, uint32_t, uint32_t, uint32_t); +void initialization(JPEG_ENCODER_STRUCTURE *, uint32_t, uint32_t, uint32_t); -void initialize_quantization_tables (uint32_t); +void initialize_quantization_tables(uint32_t); -uint8_t* write_markers (uint8_t *, uint32_t, uint32_t, uint32_t); +uint8_t *write_markers(uint8_t *, uint32_t, uint32_t, uint32_t); -void read_400_format (JPEG_ENCODER_STRUCTURE *, uint8_t *); -void read_422_format (JPEG_ENCODER_STRUCTURE *, uint8_t *); +void read_400_format(JPEG_ENCODER_STRUCTURE *, uint8_t *); +void read_422_format(JPEG_ENCODER_STRUCTURE *, uint8_t *); -uint8_t* encodeMCU (JPEG_ENCODER_STRUCTURE *, uint32_t, uint8_t *); +uint8_t *encodeMCU(JPEG_ENCODER_STRUCTURE *, uint32_t, uint8_t *); -void levelshift (int16_t *); -void DCT (int16_t *); +void levelshift(int16_t *); +void DCT(int16_t *); -void quantization (int16_t *, uint16_t *); -uint8_t* huffman (JPEG_ENCODER_STRUCTURE *, uint16_t, uint8_t *); +void quantization(int16_t *, uint16_t *); +uint8_t *huffman(JPEG_ENCODER_STRUCTURE *, uint16_t, uint8_t *); -uint8_t* close_bitstream (uint8_t *); +uint8_t *close_bitstream(uint8_t *); -int16_t fdct_coeff[8] = {0x5a82,0x5a82,0x30fb,0x7641,0x18f8,0x7d8a,0x471c,0x6a6d}; +int16_t fdct_coeff[8] = {0x5a82, 0x5a82, 0x30fb, 0x7641, 0x18f8, 0x7d8a, 0x471c, 0x6a6d}; int16_t fdct_temp[64]; -uint16_t luminance_dc_code_table [] = -{ - 0x0000, 0x0002, 0x0003, 0x0004, 0x0005, 0x0006, - 0x000E, 0x001E, 0x003E, 0x007E, 0x00FE, 0x01FE +uint16_t luminance_dc_code_table [] = { + 0x0000, 0x0002, 0x0003, 0x0004, 0x0005, 0x0006, + 0x000E, 0x001E, 0x003E, 0x007E, 0x00FE, 0x01FE }; -uint16_t luminance_dc_size_table [] = -{ - 0x0002, 0x0003, 0x0003, 0x0003, 0x0003, 0x0003, - 0x0004, 0x0005, 0x0006, 0x0007, 0x0008, 0x0009 +uint16_t luminance_dc_size_table [] = { + 0x0002, 0x0003, 0x0003, 0x0003, 0x0003, 0x0003, + 0x0004, 0x0005, 0x0006, 0x0007, 0x0008, 0x0009 }; -uint16_t chrominance_dc_code_table [] = -{ - 0x0000, 0x0001, 0x0002, 0x0006, 0x000E, 0x001E, - 0x003E, 0x007E, 0x00FE, 0x01FE, 0x03FE, 0x07FE +uint16_t chrominance_dc_code_table [] = { + 0x0000, 0x0001, 0x0002, 0x0006, 0x000E, 0x001E, + 0x003E, 0x007E, 0x00FE, 0x01FE, 0x03FE, 0x07FE }; -uint16_t chrominance_dc_size_table [] = -{ - 0x0002, 0x0002, 0x0002, 0x0003, 0x0004, 0x0005, - 0x0006, 0x0007, 0x0008, 0x0009, 0x000A, 0x000B +uint16_t chrominance_dc_size_table [] = { + 0x0002, 0x0002, 0x0002, 0x0003, 0x0004, 0x0005, + 0x0006, 0x0007, 0x0008, 0x0009, 0x000A, 0x000B }; -uint16_t luminance_ac_code_table [] = -{ - 0x000A, - 0x0000, 0x0001, 0x0004, 0x000B, 0x001A, 0x0078, 0x00F8, 0x03F6, 0xFF82, 0xFF83, - 0x000C, 0x001B, 0x0079, 0x01F6, 0x07F6, 0xFF84, 0xFF85, 0xFF86, 0xFF87, 0xFF88, - 0x001C, 0x00F9, 0x03F7, 0x0FF4, 0xFF89, 0xFF8A, 0xFF8b, 0xFF8C, 0xFF8D, 0xFF8E, - 0x003A, 0x01F7, 0x0FF5, 0xFF8F, 0xFF90, 0xFF91, 0xFF92, 0xFF93, 0xFF94, 0xFF95, - 0x003B, 0x03F8, 0xFF96, 0xFF97, 0xFF98, 0xFF99, 0xFF9A, 0xFF9B, 0xFF9C, 0xFF9D, - 0x007A, 0x07F7, 0xFF9E, 0xFF9F, 0xFFA0, 0xFFA1, 0xFFA2, 0xFFA3, 0xFFA4, 0xFFA5, - 0x007B, 0x0FF6, 0xFFA6, 0xFFA7, 0xFFA8, 0xFFA9, 0xFFAA, 0xFFAB, 0xFFAC, 0xFFAD, - 0x00FA, 0x0FF7, 0xFFAE, 0xFFAF, 0xFFB0, 0xFFB1, 0xFFB2, 0xFFB3, 0xFFB4, 0xFFB5, - 0x01F8, 0x7FC0, 0xFFB6, 0xFFB7, 0xFFB8, 0xFFB9, 0xFFBA, 0xFFBB, 0xFFBC, 0xFFBD, - 0x01F9, 0xFFBE, 0xFFBF, 0xFFC0, 0xFFC1, 0xFFC2, 0xFFC3, 0xFFC4, 0xFFC5, 0xFFC6, - 0x01FA, 0xFFC7, 0xFFC8, 0xFFC9, 0xFFCA, 0xFFCB, 0xFFCC, 0xFFCD, 0xFFCE, 0xFFCF, - 0x03F9, 0xFFD0, 0xFFD1, 0xFFD2, 0xFFD3, 0xFFD4, 0xFFD5, 0xFFD6, 0xFFD7, 0xFFD8, - 0x03FA, 0xFFD9, 0xFFDA, 0xFFDB, 0xFFDC, 0xFFDD, 0xFFDE, 0xFFDF, 0xFFE0, 0xFFE1, - 0x07F8, 0xFFE2, 0xFFE3, 0xFFE4, 0xFFE5, 0xFFE6, 0xFFE7, 0xFFE8, 0xFFE9, 0xFFEA, - 0xFFEB, 0xFFEC, 0xFFED, 0xFFEE, 0xFFEF, 0xFFF0, 0xFFF1, 0xFFF2, 0xFFF3, 0xFFF4, - 0xFFF5, 0xFFF6, 0xFFF7, 0xFFF8, 0xFFF9, 0xFFFA, 0xFFFB, 0xFFFC, 0xFFFD, 0xFFFE, - 0x07F9 +uint16_t luminance_ac_code_table [] = { + 0x000A, + 0x0000, 0x0001, 0x0004, 0x000B, 0x001A, 0x0078, 0x00F8, 0x03F6, 0xFF82, 0xFF83, + 0x000C, 0x001B, 0x0079, 0x01F6, 0x07F6, 0xFF84, 0xFF85, 0xFF86, 0xFF87, 0xFF88, + 0x001C, 0x00F9, 0x03F7, 0x0FF4, 0xFF89, 0xFF8A, 0xFF8b, 0xFF8C, 0xFF8D, 0xFF8E, + 0x003A, 0x01F7, 0x0FF5, 0xFF8F, 0xFF90, 0xFF91, 0xFF92, 0xFF93, 0xFF94, 0xFF95, + 0x003B, 0x03F8, 0xFF96, 0xFF97, 0xFF98, 0xFF99, 0xFF9A, 0xFF9B, 0xFF9C, 0xFF9D, + 0x007A, 0x07F7, 0xFF9E, 0xFF9F, 0xFFA0, 0xFFA1, 0xFFA2, 0xFFA3, 0xFFA4, 0xFFA5, + 0x007B, 0x0FF6, 0xFFA6, 0xFFA7, 0xFFA8, 0xFFA9, 0xFFAA, 0xFFAB, 0xFFAC, 0xFFAD, + 0x00FA, 0x0FF7, 0xFFAE, 0xFFAF, 0xFFB0, 0xFFB1, 0xFFB2, 0xFFB3, 0xFFB4, 0xFFB5, + 0x01F8, 0x7FC0, 0xFFB6, 0xFFB7, 0xFFB8, 0xFFB9, 0xFFBA, 0xFFBB, 0xFFBC, 0xFFBD, + 0x01F9, 0xFFBE, 0xFFBF, 0xFFC0, 0xFFC1, 0xFFC2, 0xFFC3, 0xFFC4, 0xFFC5, 0xFFC6, + 0x01FA, 0xFFC7, 0xFFC8, 0xFFC9, 0xFFCA, 0xFFCB, 0xFFCC, 0xFFCD, 0xFFCE, 0xFFCF, + 0x03F9, 0xFFD0, 0xFFD1, 0xFFD2, 0xFFD3, 0xFFD4, 0xFFD5, 0xFFD6, 0xFFD7, 0xFFD8, + 0x03FA, 0xFFD9, 0xFFDA, 0xFFDB, 0xFFDC, 0xFFDD, 0xFFDE, 0xFFDF, 0xFFE0, 0xFFE1, + 0x07F8, 0xFFE2, 0xFFE3, 0xFFE4, 0xFFE5, 0xFFE6, 0xFFE7, 0xFFE8, 0xFFE9, 0xFFEA, + 0xFFEB, 0xFFEC, 0xFFED, 0xFFEE, 0xFFEF, 0xFFF0, 0xFFF1, 0xFFF2, 0xFFF3, 0xFFF4, + 0xFFF5, 0xFFF6, 0xFFF7, 0xFFF8, 0xFFF9, 0xFFFA, 0xFFFB, 0xFFFC, 0xFFFD, 0xFFFE, + 0x07F9 }; -uint16_t luminance_ac_size_table [] = -{ - 0x0004, - 0x0002, 0x0002, 0x0003, 0x0004, 0x0005, 0x0007, 0x0008, 0x000A, 0x0010, 0x0010, - 0x0004, 0x0005, 0x0007, 0x0009, 0x000B, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, - 0x0005, 0x0008, 0x000A, 0x000C, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, - 0x0006, 0x0009, 0x000C, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, - 0x0006, 0x000A, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, - 0x0007, 0x000B, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, - 0x0007, 0x000C, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, - 0x0008, 0x000C, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, - 0x0009, 0x000F, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, - 0x0009, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, - 0x0009, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, - 0x000A, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, - 0x000A, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, - 0x000B, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, - 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, - 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, - 0x000B +uint16_t luminance_ac_size_table [] = { + 0x0004, + 0x0002, 0x0002, 0x0003, 0x0004, 0x0005, 0x0007, 0x0008, 0x000A, 0x0010, 0x0010, + 0x0004, 0x0005, 0x0007, 0x0009, 0x000B, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, + 0x0005, 0x0008, 0x000A, 0x000C, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, + 0x0006, 0x0009, 0x000C, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, + 0x0006, 0x000A, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, + 0x0007, 0x000B, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, + 0x0007, 0x000C, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, + 0x0008, 0x000C, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, + 0x0009, 0x000F, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, + 0x0009, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, + 0x0009, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, + 0x000A, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, + 0x000A, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, + 0x000B, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, + 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, + 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, + 0x000B }; -uint16_t chrominance_ac_code_table [] = -{ - 0x0000, - 0x0001, 0x0004, 0x000A, 0x0018, 0x0019, 0x0038, 0x0078, 0x01F4, 0x03F6, 0x0FF4, - 0x000B, 0x0039, 0x00F6, 0x01F5, 0x07F6, 0x0FF5, 0xFF88, 0xFF89, 0xFF8A, 0xFF8B, - 0x001A, 0x00F7, 0x03F7, 0x0FF6, 0x7FC2, 0xFF8C, 0xFF8D, 0xFF8E, 0xFF8F, 0xFF90, - 0x001B, 0x00F8, 0x03F8, 0x0FF7, 0xFF91, 0xFF92, 0xFF93, 0xFF94, 0xFF95, 0xFF96, - 0x003A, 0x01F6, 0xFF97, 0xFF98, 0xFF99, 0xFF9A, 0xFF9B, 0xFF9C, 0xFF9D, 0xFF9E, - 0x003B, 0x03F9, 0xFF9F, 0xFFA0, 0xFFA1, 0xFFA2, 0xFFA3, 0xFFA4, 0xFFA5, 0xFFA6, - 0x0079, 0x07F7, 0xFFA7, 0xFFA8, 0xFFA9, 0xFFAA, 0xFFAB, 0xFFAC, 0xFFAD, 0xFFAE, - 0x007A, 0x07F8, 0xFFAF, 0xFFB0, 0xFFB1, 0xFFB2, 0xFFB3, 0xFFB4, 0xFFB5, 0xFFB6, - 0x00F9, 0xFFB7, 0xFFB8, 0xFFB9, 0xFFBA, 0xFFBB, 0xFFBC, 0xFFBD, 0xFFBE, 0xFFBF, - 0x01F7, 0xFFC0, 0xFFC1, 0xFFC2, 0xFFC3, 0xFFC4, 0xFFC5, 0xFFC6, 0xFFC7, 0xFFC8, - 0x01F8, 0xFFC9, 0xFFCA, 0xFFCB, 0xFFCC, 0xFFCD, 0xFFCE, 0xFFCF, 0xFFD0, 0xFFD1, - 0x01F9, 0xFFD2, 0xFFD3, 0xFFD4, 0xFFD5, 0xFFD6, 0xFFD7, 0xFFD8, 0xFFD9, 0xFFDA, - 0x01FA, 0xFFDB, 0xFFDC, 0xFFDD, 0xFFDE, 0xFFDF, 0xFFE0, 0xFFE1, 0xFFE2, 0xFFE3, - 0x07F9, 0xFFE4, 0xFFE5, 0xFFE6, 0xFFE7, 0xFFE8, 0xFFE9, 0xFFEA, 0xFFEb, 0xFFEC, - 0x3FE0, 0xFFED, 0xFFEE, 0xFFEF, 0xFFF0, 0xFFF1, 0xFFF2, 0xFFF3, 0xFFF4, 0xFFF5, - 0x7FC3, 0xFFF6, 0xFFF7, 0xFFF8, 0xFFF9, 0xFFFA, 0xFFFB, 0xFFFC, 0xFFFD, 0xFFFE, - 0x03FA +uint16_t chrominance_ac_code_table [] = { + 0x0000, + 0x0001, 0x0004, 0x000A, 0x0018, 0x0019, 0x0038, 0x0078, 0x01F4, 0x03F6, 0x0FF4, + 0x000B, 0x0039, 0x00F6, 0x01F5, 0x07F6, 0x0FF5, 0xFF88, 0xFF89, 0xFF8A, 0xFF8B, + 0x001A, 0x00F7, 0x03F7, 0x0FF6, 0x7FC2, 0xFF8C, 0xFF8D, 0xFF8E, 0xFF8F, 0xFF90, + 0x001B, 0x00F8, 0x03F8, 0x0FF7, 0xFF91, 0xFF92, 0xFF93, 0xFF94, 0xFF95, 0xFF96, + 0x003A, 0x01F6, 0xFF97, 0xFF98, 0xFF99, 0xFF9A, 0xFF9B, 0xFF9C, 0xFF9D, 0xFF9E, + 0x003B, 0x03F9, 0xFF9F, 0xFFA0, 0xFFA1, 0xFFA2, 0xFFA3, 0xFFA4, 0xFFA5, 0xFFA6, + 0x0079, 0x07F7, 0xFFA7, 0xFFA8, 0xFFA9, 0xFFAA, 0xFFAB, 0xFFAC, 0xFFAD, 0xFFAE, + 0x007A, 0x07F8, 0xFFAF, 0xFFB0, 0xFFB1, 0xFFB2, 0xFFB3, 0xFFB4, 0xFFB5, 0xFFB6, + 0x00F9, 0xFFB7, 0xFFB8, 0xFFB9, 0xFFBA, 0xFFBB, 0xFFBC, 0xFFBD, 0xFFBE, 0xFFBF, + 0x01F7, 0xFFC0, 0xFFC1, 0xFFC2, 0xFFC3, 0xFFC4, 0xFFC5, 0xFFC6, 0xFFC7, 0xFFC8, + 0x01F8, 0xFFC9, 0xFFCA, 0xFFCB, 0xFFCC, 0xFFCD, 0xFFCE, 0xFFCF, 0xFFD0, 0xFFD1, + 0x01F9, 0xFFD2, 0xFFD3, 0xFFD4, 0xFFD5, 0xFFD6, 0xFFD7, 0xFFD8, 0xFFD9, 0xFFDA, + 0x01FA, 0xFFDB, 0xFFDC, 0xFFDD, 0xFFDE, 0xFFDF, 0xFFE0, 0xFFE1, 0xFFE2, 0xFFE3, + 0x07F9, 0xFFE4, 0xFFE5, 0xFFE6, 0xFFE7, 0xFFE8, 0xFFE9, 0xFFEA, 0xFFEb, 0xFFEC, + 0x3FE0, 0xFFED, 0xFFEE, 0xFFEF, 0xFFF0, 0xFFF1, 0xFFF2, 0xFFF3, 0xFFF4, 0xFFF5, + 0x7FC3, 0xFFF6, 0xFFF7, 0xFFF8, 0xFFF9, 0xFFFA, 0xFFFB, 0xFFFC, 0xFFFD, 0xFFFE, + 0x03FA }; -uint16_t chrominance_ac_size_table [] = -{ - 0x0002, - 0x0002, 0x0003, 0x0004, 0x0005, 0x0005, 0x0006, 0x0007, 0x0009, 0x000A, 0x000C, - 0x0004, 0x0006, 0x0008, 0x0009, 0x000B, 0x000C, 0x0010, 0x0010, 0x0010, 0x0010, - 0x0005, 0x0008, 0x000A, 0x000C, 0x000F, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, - 0x0005, 0x0008, 0x000A, 0x000C, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, - 0x0006, 0x0009, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, - 0x0006, 0x000A, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, - 0x0007, 0x000B, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, - 0x0007, 0x000B, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, - 0x0008, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, - 0x0009, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, - 0x0009, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, - 0x0009, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, - 0x0009, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, - 0x000B, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, - 0x000E, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, - 0x000F, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, - 0x000A +uint16_t chrominance_ac_size_table [] = { + 0x0002, + 0x0002, 0x0003, 0x0004, 0x0005, 0x0005, 0x0006, 0x0007, 0x0009, 0x000A, 0x000C, + 0x0004, 0x0006, 0x0008, 0x0009, 0x000B, 0x000C, 0x0010, 0x0010, 0x0010, 0x0010, + 0x0005, 0x0008, 0x000A, 0x000C, 0x000F, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, + 0x0005, 0x0008, 0x000A, 0x000C, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, + 0x0006, 0x0009, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, + 0x0006, 0x000A, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, + 0x0007, 0x000B, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, + 0x0007, 0x000B, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, + 0x0008, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, + 0x0009, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, + 0x0009, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, + 0x0009, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, + 0x0009, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, + 0x000B, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, + 0x000E, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, + 0x000F, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, 0x0010, + 0x000A }; -uint8_t bitsize [] = -{ - 0, 1, 2, 2, 3, 3, 3, 3, - 4, 4, 4, 4, 4, 4, 4, 4, - 5, 5, 5, 5, 5, 5, 5, 5, - 5, 5, 5, 5, 5, 5, 5, 5, - 6, 6, 6, 6, 6, 6, 6, 6, - 6, 6, 6, 6, 6, 6, 6, 6, - 6, 6, 6, 6, 6, 6, 6, 6, - 6, 6, 6, 6, 6, 6, 6, 6, - 7, 7, 7, 7, 7, 7, 7, 7, - 7, 7, 7, 7, 7, 7, 7, 7, - 7, 7, 7, 7, 7, 7, 7, 7, - 7, 7, 7, 7, 7, 7, 7, 7, - 7, 7, 7, 7, 7, 7, 7, 7, - 7, 7, 7, 7, 7, 7, 7, 7, - 7, 7, 7, 7, 7, 7, 7, 7, - 7, 7, 7, 7, 7, 7, 7, 7, - 8, 8, 8, 8, 8, 8, 8, 8, - 8, 8, 8, 8, 8, 8, 8, 8, - 8, 8, 8, 8, 8, 8, 8, 8, - 8, 8, 8, 8, 8, 8, 8, 8, - 8, 8, 8, 8, 8, 8, 8, 8, - 8, 8, 8, 8, 8, 8, 8, 8, - 8, 8, 8, 8, 8, 8, 8, 8, - 8, 8, 8, 8, 8, 8, 8, 8, - 8, 8, 8, 8, 8, 8, 8, 8, - 8, 8, 8, 8, 8, 8, 8, 8, - 8, 8, 8, 8, 8, 8, 8, 8, - 8, 8, 8, 8, 8, 8, 8, 8, - 8, 8, 8, 8, 8, 8, 8, 8, - 8, 8, 8, 8, 8, 8, 8, 8, - 8, 8, 8, 8, 8, 8, 8, 8, - 8, 8, 8, 8, 8, 8, 8, 8 +uint8_t bitsize [] = { + 0, 1, 2, 2, 3, 3, 3, 3, + 4, 4, 4, 4, 4, 4, 4, 4, + 5, 5, 5, 5, 5, 5, 5, 5, + 5, 5, 5, 5, 5, 5, 5, 5, + 6, 6, 6, 6, 6, 6, 6, 6, + 6, 6, 6, 6, 6, 6, 6, 6, + 6, 6, 6, 6, 6, 6, 6, 6, + 6, 6, 6, 6, 6, 6, 6, 6, + 7, 7, 7, 7, 7, 7, 7, 7, + 7, 7, 7, 7, 7, 7, 7, 7, + 7, 7, 7, 7, 7, 7, 7, 7, + 7, 7, 7, 7, 7, 7, 7, 7, + 7, 7, 7, 7, 7, 7, 7, 7, + 7, 7, 7, 7, 7, 7, 7, 7, + 7, 7, 7, 7, 7, 7, 7, 7, + 7, 7, 7, 7, 7, 7, 7, 7, + 8, 8, 8, 8, 8, 8, 8, 8, + 8, 8, 8, 8, 8, 8, 8, 8, + 8, 8, 8, 8, 8, 8, 8, 8, + 8, 8, 8, 8, 8, 8, 8, 8, + 8, 8, 8, 8, 8, 8, 8, 8, + 8, 8, 8, 8, 8, 8, 8, 8, + 8, 8, 8, 8, 8, 8, 8, 8, + 8, 8, 8, 8, 8, 8, 8, 8, + 8, 8, 8, 8, 8, 8, 8, 8, + 8, 8, 8, 8, 8, 8, 8, 8, + 8, 8, 8, 8, 8, 8, 8, 8, + 8, 8, 8, 8, 8, 8, 8, 8, + 8, 8, 8, 8, 8, 8, 8, 8, + 8, 8, 8, 8, 8, 8, 8, 8, + 8, 8, 8, 8, 8, 8, 8, 8, + 8, 8, 8, 8, 8, 8, 8, 8 }; -uint8_t markerdata [] = -{ -0xFF, 0xC4, 0x00, 0x1F, 0x00, 0x00, 0x01, 0x05, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, +uint8_t markerdata [] = { + 0xFF, 0xC4, 0x00, 0x1F, 0x00, 0x00, 0x01, 0x05, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, -0xFF, 0xC4, 0x00, 0xB5, 0x10, 0x00, 0x02, 0x01, 0x03, 0x03, 0x02, 0x04, 0x03, 0x05, 0x05, 0x04, 0x04, 0x00, 0x00, 0x01, 0x7D, 0x01, 0x02, 0x03, 0x00, 0x04, 0x11, 0x05, 0x12, 0x21, 0x31, 0x41, 0x06, 0x13, 0x51, 0x61, 0x07, 0x22, 0x71, 0x14, 0x32, 0x81, 0x91, 0xA1, 0x08, 0x23, 0x42, 0xB1, 0xC1, 0x15, 0x52, 0xD1, 0xF0, 0x24, 0x33, 0x62, 0x72, 0x82, 0x09, 0x0A, 0x16, 0x17, 0x18, 0x19, 0x1A, 0x25, 0x26, 0x27, 0x28, 0x29, 0x2A, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39, 0x3A, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49, 0x4A, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59, 0x5A, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68, 0x69, 0x6A, 0x73, 0x74, 0x75, 0x76, 0x77, 0x78, 0x79, 0x7A, 0x83, 0x84, 0x85, 0x86, 0x87, 0x88, 0x89, 0x8A, 0x92, 0x93, 0x94, 0x95, 0x96, 0x97, 0x98, 0x99, 0x9A, 0xA2, 0xA3, 0xA4, 0xA5, 0xA6, 0xA7, 0xA8, 0xA9, 0xAA, 0xB2, 0xB3, 0xB4, 0xB5, 0xB6, 0xB7, 0xB8, 0xB9, 0xBA, 0xC2, 0xC3, 0xC4, 0xC5, 0xC6, 0xC7, 0xC8, 0xC9, 0xCA, 0xD2, 0xD3, 0xD4, 0xD5, 0xD6, 0xD7, 0xD8, 0xD9, 0xDA, 0xE1, 0xE2, 0xE3, 0xE4, 0xE5, 0xE6, 0xE7, 0xE8, 0xE9, 0xEA, 0xF1, 0xF2, 0xF3, 0xF4, 0xF5, 0xF6, 0xF7, 0xF8, 0xF9, 0xFA, + 0xFF, 0xC4, 0x00, 0xB5, 0x10, 0x00, 0x02, 0x01, 0x03, 0x03, 0x02, 0x04, 0x03, 0x05, 0x05, 0x04, 0x04, 0x00, 0x00, 0x01, 0x7D, 0x01, 0x02, 0x03, 0x00, 0x04, 0x11, 0x05, 0x12, 0x21, 0x31, 0x41, 0x06, 0x13, 0x51, 0x61, 0x07, 0x22, 0x71, 0x14, 0x32, 0x81, 0x91, 0xA1, 0x08, 0x23, 0x42, 0xB1, 0xC1, 0x15, 0x52, 0xD1, 0xF0, 0x24, 0x33, 0x62, 0x72, 0x82, 0x09, 0x0A, 0x16, 0x17, 0x18, 0x19, 0x1A, 0x25, 0x26, 0x27, 0x28, 0x29, 0x2A, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39, 0x3A, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49, 0x4A, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59, 0x5A, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68, 0x69, 0x6A, 0x73, 0x74, 0x75, 0x76, 0x77, 0x78, 0x79, 0x7A, 0x83, 0x84, 0x85, 0x86, 0x87, 0x88, 0x89, 0x8A, 0x92, 0x93, 0x94, 0x95, 0x96, 0x97, 0x98, 0x99, 0x9A, 0xA2, 0xA3, 0xA4, 0xA5, 0xA6, 0xA7, 0xA8, 0xA9, 0xAA, 0xB2, 0xB3, 0xB4, 0xB5, 0xB6, 0xB7, 0xB8, 0xB9, 0xBA, 0xC2, 0xC3, 0xC4, 0xC5, 0xC6, 0xC7, 0xC8, 0xC9, 0xCA, 0xD2, 0xD3, 0xD4, 0xD5, 0xD6, 0xD7, 0xD8, 0xD9, 0xDA, 0xE1, 0xE2, 0xE3, 0xE4, 0xE5, 0xE6, 0xE7, 0xE8, 0xE9, 0xEA, 0xF1, 0xF2, 0xF3, 0xF4, 0xF5, 0xF6, 0xF7, 0xF8, 0xF9, 0xFA, -0xFF, 0xC4, 0x00, 0x1F, 0x01, 0x00, 0x03, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, + 0xFF, 0xC4, 0x00, 0x1F, 0x01, 0x00, 0x03, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, -0xFF, 0xC4, 0x00, 0xB5, 0x11, 0x00, 0x02, 0x01, 0x02, 0x04, 0x04, 0x03, 0x04, 0x07, 0x05, 0x04, 0x04, 0x00, 0x01, 0x02, 0x77, 0x00, 0x01, 0x02, 0x03, 0x11, 0x04, 0x05, 0x21, 0x31, 0x06, 0x12, 0x41, 0x51, 0x07, 0x61, 0x71, 0x13, 0x22, 0x32, 0x81, 0x08, 0x14, 0x42, 0x91, 0xA1, 0xB1, 0xC1, 0x09, 0x23, 0x33, 0x52, 0xF0, 0x15, 0x62, 0x72, 0xD1, 0x0A, 0x16, 0x24, 0x34, 0xE1, 0x25, 0xF1, 0x17, 0x18, 0x19, 0x1A, 0x26, 0x27, 0x28, 0x29, 0x2A, 0x35, 0x36, 0x37, 0x38, 0x39, 0x3A, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49, 0x4A, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59, 0x5A, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68, 0x69, 0x6A, 0x73, 0x74, 0x75, 0x76, 0x77, 0x78, 0x79, 0x7A, 0x82, 0x83, 0x84, 0x85, 0x86, 0x87, 0x88, 0x89, 0x8A, 0x92, 0x93, 0x94, 0x95, 0x96, 0x97, 0x98, 0x99, 0x9A, 0xA2, 0xA3, 0xA4, 0xA5, 0xA6, 0xA7, 0xA8, 0xA9, 0xAA, 0xB2, 0xB3, 0xB4, 0xB5, 0xB6, 0xB7, 0xB8, 0xB9, 0xBA, 0xC2, 0xC3, 0xC4, 0xC5, 0xC6, 0xC7, 0xC8, 0xC9, 0xCA, 0xD2, 0xD3, 0xD4, 0xD5, 0xD6, 0xD7, 0xD8, 0xD9, 0xDA, 0xE2, 0xE3, 0xE4, 0xE5, 0xE6, 0xE7, 0xE8, 0xE9, 0xEA, 0xF2, 0xF3, 0xF4, 0xF5, 0xF6, 0xF7, 0xF8, 0xF9, 0xFA, + 0xFF, 0xC4, 0x00, 0xB5, 0x11, 0x00, 0x02, 0x01, 0x02, 0x04, 0x04, 0x03, 0x04, 0x07, 0x05, 0x04, 0x04, 0x00, 0x01, 0x02, 0x77, 0x00, 0x01, 0x02, 0x03, 0x11, 0x04, 0x05, 0x21, 0x31, 0x06, 0x12, 0x41, 0x51, 0x07, 0x61, 0x71, 0x13, 0x22, 0x32, 0x81, 0x08, 0x14, 0x42, 0x91, 0xA1, 0xB1, 0xC1, 0x09, 0x23, 0x33, 0x52, 0xF0, 0x15, 0x62, 0x72, 0xD1, 0x0A, 0x16, 0x24, 0x34, 0xE1, 0x25, 0xF1, 0x17, 0x18, 0x19, 0x1A, 0x26, 0x27, 0x28, 0x29, 0x2A, 0x35, 0x36, 0x37, 0x38, 0x39, 0x3A, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49, 0x4A, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59, 0x5A, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68, 0x69, 0x6A, 0x73, 0x74, 0x75, 0x76, 0x77, 0x78, 0x79, 0x7A, 0x82, 0x83, 0x84, 0x85, 0x86, 0x87, 0x88, 0x89, 0x8A, 0x92, 0x93, 0x94, 0x95, 0x96, 0x97, 0x98, 0x99, 0x9A, 0xA2, 0xA3, 0xA4, 0xA5, 0xA6, 0xA7, 0xA8, 0xA9, 0xAA, 0xB2, 0xB3, 0xB4, 0xB5, 0xB6, 0xB7, 0xB8, 0xB9, 0xBA, 0xC2, 0xC3, 0xC4, 0xC5, 0xC6, 0xC7, 0xC8, 0xC9, 0xCA, 0xD2, 0xD3, 0xD4, 0xD5, 0xD6, 0xD7, 0xD8, 0xD9, 0xDA, 0xE2, 0xE3, 0xE4, 0xE5, 0xE6, 0xE7, 0xE8, 0xE9, 0xEA, 0xF2, 0xF3, 0xF4, 0xF5, 0xF6, 0xF7, 0xF8, 0xF9, 0xFA, }; -uint8_t zigzag_table [] = -{ - 0, 1, 5, 6, 14, 15, 27, 28, - 2, 4, 7, 13, 16, 26, 29, 42, - 3, 8, 12, 17, 25, 30, 41, 43, - 9, 11, 18, 24, 31, 40, 44, 53, - 10, 19, 23, 32, 39, 45, 52, 54, - 20, 22, 33, 38, 46, 51, 55, 60, - 21, 34, 37, 47, 50, 56, 59, 61, - 35, 36, 48, 49, 57, 58, 62, 63 +uint8_t zigzag_table [] = { + 0, 1, 5, 6, 14, 15, 27, 28, + 2, 4, 7, 13, 16, 26, 29, 42, + 3, 8, 12, 17, 25, 30, 41, 43, + 9, 11, 18, 24, 31, 40, 44, 53, + 10, 19, 23, 32, 39, 45, 52, 54, + 20, 22, 33, 38, 46, 51, 55, 60, + 21, 34, 37, 47, 50, 56, 59, 61, + 35, 36, 48, 49, 57, 58, 62, 63 }; -uint8_t luminance_quant_table [] = -{ - 16, 11, 10, 16, 24, 40, 51, 61, - 12, 12, 14, 19, 26, 58, 60, 55, - 14, 13, 16, 24, 40, 57, 69, 56, - 14, 17, 22, 29, 51, 87, 80, 62, - 18, 22, 37, 56, 68, 109, 103, 77, - 24, 35, 55, 64, 81, 104, 113, 92, - 49, 64, 78, 87, 103, 121, 120, 101, - 72, 92, 95, 98, 112, 100, 103, 99 +uint8_t luminance_quant_table [] = { + 16, 11, 10, 16, 24, 40, 51, 61, + 12, 12, 14, 19, 26, 58, 60, 55, + 14, 13, 16, 24, 40, 57, 69, 56, + 14, 17, 22, 29, 51, 87, 80, 62, + 18, 22, 37, 56, 68, 109, 103, 77, + 24, 35, 55, 64, 81, 104, 113, 92, + 49, 64, 78, 87, 103, 121, 120, 101, + 72, 92, 95, 98, 112, 100, 103, 99 }; -uint8_t chrominance_quant_table [] = -{ - 17, 18, 24, 47, 99, 99, 99, 99, - 18, 21, 26, 66, 99, 99, 99, 99, - 24, 26, 56, 99, 99, 99, 99, 99, - 47, 66, 99, 99, 99, 99, 99, 99, - 99, 99, 99, 99, 99, 99, 99, 99, - 99, 99, 99, 99, 99, 99, 99, 99, - 99, 99, 99, 99, 99, 99, 99, 99, - 99, 99, 99, 99, 99, 99, 99, 99 +uint8_t chrominance_quant_table [] = { + 17, 18, 24, 47, 99, 99, 99, 99, + 18, 21, 26, 66, 99, 99, 99, 99, + 24, 26, 56, 99, 99, 99, 99, 99, + 47, 66, 99, 99, 99, 99, 99, 99, + 99, 99, 99, 99, 99, 99, 99, 99, + 99, 99, 99, 99, 99, 99, 99, 99, + 99, 99, 99, 99, 99, 99, 99, 99, + 99, 99, 99, 99, 99, 99, 99, 99 }; uint8_t Lqt [BLOCK_SIZE]; @@ -327,49 +318,46 @@ uint32_t lcode = 0; uint16_t bitindex = 0; -void (*read_format) (JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure, uint8_t *input_ptr); +void (*read_format)(JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure, uint8_t *input_ptr); -void initialization (JPEG_ENCODER_STRUCTURE *jpeg, uint32_t image_format, uint32_t image_width, uint32_t image_height) +void initialization(JPEG_ENCODER_STRUCTURE *jpeg, uint32_t image_format, uint32_t image_width, uint32_t image_height) { - uint16_t mcu_width, mcu_height, bytes_per_pixel; + uint16_t mcu_width, mcu_height, bytes_per_pixel; - if (image_format == FOUR_ZERO_ZERO) - { - jpeg->mcu_width = mcu_width = 8; - jpeg->mcu_height = mcu_height = 8; + if (image_format == FOUR_ZERO_ZERO) { + jpeg->mcu_width = mcu_width = 8; + jpeg->mcu_height = mcu_height = 8; - jpeg->horizontal_mcus = (uint16_t) ((image_width + mcu_width - 1) >> 3); - jpeg->vertical_mcus = (uint16_t) ((image_height + mcu_height - 1) >> 3); + jpeg->horizontal_mcus = (uint16_t)((image_width + mcu_width - 1) >> 3); + jpeg->vertical_mcus = (uint16_t)((image_height + mcu_height - 1) >> 3); - bytes_per_pixel = 1; - read_format = read_400_format; - } - else - { - jpeg->mcu_width = mcu_width = 16; - jpeg->horizontal_mcus = (uint16_t) ((image_width + mcu_width - 1) >> 4); + bytes_per_pixel = 1; + read_format = read_400_format; + } else { + jpeg->mcu_width = mcu_width = 16; + jpeg->horizontal_mcus = (uint16_t)((image_width + mcu_width - 1) >> 4); - jpeg->mcu_height = mcu_height = 8; - jpeg->vertical_mcus = (uint16_t) ((image_height + mcu_height - 1) >> 3); - bytes_per_pixel = 2; - read_format = read_422_format; - } + jpeg->mcu_height = mcu_height = 8; + jpeg->vertical_mcus = (uint16_t)((image_height + mcu_height - 1) >> 3); + bytes_per_pixel = 2; + read_format = read_422_format; + } - jpeg->rows_in_bottom_mcus = (uint16_t) (image_height - (jpeg->vertical_mcus - 1) * mcu_height); - jpeg->cols_in_right_mcus = (uint16_t) (image_width - (jpeg->horizontal_mcus - 1) * mcu_width); + jpeg->rows_in_bottom_mcus = (uint16_t)(image_height - (jpeg->vertical_mcus - 1) * mcu_height); + jpeg->cols_in_right_mcus = (uint16_t)(image_width - (jpeg->horizontal_mcus - 1) * mcu_width); - jpeg->length_minus_mcu_width = (uint16_t) ((image_width - mcu_width) * bytes_per_pixel); - jpeg->length_minus_width = (uint16_t) ((image_width - jpeg->cols_in_right_mcus) * bytes_per_pixel); + jpeg->length_minus_mcu_width = (uint16_t)((image_width - mcu_width) * bytes_per_pixel); + jpeg->length_minus_width = (uint16_t)((image_width - jpeg->cols_in_right_mcus) * bytes_per_pixel); - jpeg->mcu_width_size = (uint16_t) (mcu_width * bytes_per_pixel); + jpeg->mcu_width_size = (uint16_t)(mcu_width * bytes_per_pixel); - jpeg->offset = (uint16_t) ((image_width * (mcu_height - 1) - (mcu_width - jpeg->cols_in_right_mcus)) * bytes_per_pixel); + jpeg->offset = (uint16_t)((image_width * (mcu_height - 1) - (mcu_width - jpeg->cols_in_right_mcus)) * bytes_per_pixel); - jpeg->ldc1 = 0; - jpeg->ldc2 = 0; - jpeg->ldc3 = 0; - lcode = 0; - bitindex = 0; + jpeg->ldc1 = 0; + jpeg->ldc2 = 0; + jpeg->ldc3 = 0; + lcode = 0; + bitindex = 0; } ///////////////////////////////////////////////////////////// @@ -381,28 +369,28 @@ void initialization (JPEG_ENCODER_STRUCTURE *jpeg, uint32_t image_format, uint32 * Table K.1 from JPEG spec. */ static const int jpeg_luma_quantizer[64] = { - 16, 11, 10, 16, 24, 40, 51, 61, - 12, 12, 14, 19, 26, 58, 60, 55, - 14, 13, 16, 24, 40, 57, 69, 56, - 14, 17, 22, 29, 51, 87, 80, 62, - 18, 22, 37, 56, 68, 109, 103, 77, - 24, 35, 55, 64, 81, 104, 113, 92, - 49, 64, 78, 87, 103, 121, 120, 101, - 72, 92, 95, 98, 112, 100, 103, 99 + 16, 11, 10, 16, 24, 40, 51, 61, + 12, 12, 14, 19, 26, 58, 60, 55, + 14, 13, 16, 24, 40, 57, 69, 56, + 14, 17, 22, 29, 51, 87, 80, 62, + 18, 22, 37, 56, 68, 109, 103, 77, + 24, 35, 55, 64, 81, 104, 113, 92, + 49, 64, 78, 87, 103, 121, 120, 101, + 72, 92, 95, 98, 112, 100, 103, 99 }; /* * Table K.2 from JPEG spec. */ static const int jpeg_chroma_quantizer[64] = { - 17, 18, 24, 47, 99, 99, 99, 99, - 18, 21, 26, 66, 99, 99, 99, 99, - 24, 26, 56, 99, 99, 99, 99, 99, - 47, 66, 99, 99, 99, 99, 99, 99, - 99, 99, 99, 99, 99, 99, 99, 99, - 99, 99, 99, 99, 99, 99, 99, 99, - 99, 99, 99, 99, 99, 99, 99, 99, - 99, 99, 99, 99, 99, 99, 99, 99 + 17, 18, 24, 47, 99, 99, 99, 99, + 18, 21, 26, 66, 99, 99, 99, 99, + 24, 26, 56, 99, 99, 99, 99, 99, + 47, 66, 99, 99, 99, 99, 99, 99, + 99, 99, 99, 99, 99, 99, 99, 99, + 99, 99, 99, 99, 99, 99, 99, 99, + 99, 99, 99, 99, 99, 99, 99, 99, + 99, 99, 99, 99, 99, 99, 99, 99 }; /* @@ -414,26 +402,27 @@ void MakeTables(int q) int i; int factor = q; - if (q < 1) factor = 1; - if (q > 99) factor = 99; - if (q < 50) + if (q < 1) { factor = 1; } + if (q > 99) { factor = 99; } + if (q < 50) { q = 5000 / factor; - else - q = 200 - factor*2; + } else { + q = 200 - factor * 2; + } - for (i=0; i < 64; i++) { + for (i = 0; i < 64; i++) { int lq = (jpeg_luma_quantizer[i] * q + 50) / 100; int cq = (jpeg_chroma_quantizer[i] * q + 50) / 100; /* Limit the quantizers to 1 <= q <= 255 */ - if (lq < 1) lq = 1; - else if (lq > 255) lq = 255; + if (lq < 1) { lq = 1; } + else if (lq > 255) { lq = 255; } Lqt [i] = (uint8_t) lq; ILqt [i] = 0x8000 / lq; - if (cq < 1) cq = 1; - else if (cq > 255) cq = 255; + if (cq < 1) { cq = 1; } + else if (cq > 255) { cq = 255; } Cqt [i] = (uint8_t) cq; //ICqt [i] = DSP_Division (0x8000, value); ICqt [i] = 0x8000 / cq; @@ -444,659 +433,631 @@ void MakeTables(int q) -uint8_t* encode_image (uint8_t *input_ptr, uint8_t *output_ptr, uint32_t quality_factor, uint32_t image_format, uint32_t image_width, uint32_t image_height, uint8_t add_dri_header) +uint8_t *encode_image(uint8_t *input_ptr, uint8_t *output_ptr, uint32_t quality_factor, uint32_t image_format, + uint32_t image_width, uint32_t image_height, uint8_t add_dri_header) { - uint16_t i, j; + uint16_t i, j; - JPEG_ENCODER_STRUCTURE JpegStruct; - JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure = &JpegStruct; + JPEG_ENCODER_STRUCTURE JpegStruct; + JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure = &JpegStruct; - /* Initialization of JPEG control structure */ - initialization (jpeg_encoder_structure,image_format,image_width,image_height); + /* Initialization of JPEG control structure */ + initialization(jpeg_encoder_structure, image_format, image_width, image_height); - /* Quantization Table Initialization */ - //initialize_quantization_tables (quality_factor); + /* Quantization Table Initialization */ + //initialize_quantization_tables (quality_factor); - MakeTables( quality_factor); + MakeTables(quality_factor); - /* Writing Marker Data */ - if (add_dri_header) - { - output_ptr = write_markers (output_ptr, image_format, image_width, image_height); + /* Writing Marker Data */ + if (add_dri_header) { + output_ptr = write_markers(output_ptr, image_format, image_width, image_height); + } + + for (i = 1; i <= jpeg_encoder_structure->vertical_mcus; i++) { + if (i < jpeg_encoder_structure->vertical_mcus) { + jpeg_encoder_structure->rows = jpeg_encoder_structure->mcu_height; + } else { + jpeg_encoder_structure->rows = jpeg_encoder_structure->rows_in_bottom_mcus; } - for (i=1; i<=jpeg_encoder_structure->vertical_mcus; i++) - { - if (i < jpeg_encoder_structure->vertical_mcus) - jpeg_encoder_structure->rows = jpeg_encoder_structure->mcu_height; - else - jpeg_encoder_structure->rows = jpeg_encoder_structure->rows_in_bottom_mcus; + for (j = 1; j <= jpeg_encoder_structure->horizontal_mcus; j++) { + if (j < jpeg_encoder_structure->horizontal_mcus) { + jpeg_encoder_structure->cols = jpeg_encoder_structure->mcu_width; + jpeg_encoder_structure->incr = jpeg_encoder_structure->length_minus_mcu_width; + } else { + jpeg_encoder_structure->cols = jpeg_encoder_structure->cols_in_right_mcus; + jpeg_encoder_structure->incr = jpeg_encoder_structure->length_minus_width; + } - for (j=1; j<=jpeg_encoder_structure->horizontal_mcus; j++) - { - if (j < jpeg_encoder_structure->horizontal_mcus) - { - jpeg_encoder_structure->cols = jpeg_encoder_structure->mcu_width; - jpeg_encoder_structure->incr = jpeg_encoder_structure->length_minus_mcu_width; - } - else - { - jpeg_encoder_structure->cols = jpeg_encoder_structure->cols_in_right_mcus; - jpeg_encoder_structure->incr = jpeg_encoder_structure->length_minus_width; - } + read_format(jpeg_encoder_structure, input_ptr); - read_format (jpeg_encoder_structure, input_ptr); + /* Encode the data in MCU */ + output_ptr = encodeMCU(jpeg_encoder_structure, image_format, output_ptr); - /* Encode the data in MCU */ - output_ptr = encodeMCU (jpeg_encoder_structure, image_format, output_ptr); - - input_ptr += jpeg_encoder_structure->mcu_width_size; - } - - input_ptr += jpeg_encoder_structure->offset; + input_ptr += jpeg_encoder_structure->mcu_width_size; } - /* Close Routine */ - output_ptr = close_bitstream (output_ptr); - return output_ptr; + input_ptr += jpeg_encoder_structure->offset; + } + + /* Close Routine */ + output_ptr = close_bitstream(output_ptr); + return output_ptr; } -uint8_t* encodeMCU (JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure, uint32_t image_format, uint8_t *output_ptr) +uint8_t *encodeMCU(JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure, uint32_t image_format, uint8_t *output_ptr) { - levelshift (Y1); - DCT (Y1); - quantization (Y1, ILqt); - output_ptr = huffman (jpeg_encoder_structure, 1, output_ptr); + levelshift(Y1); + DCT(Y1); + quantization(Y1, ILqt); + output_ptr = huffman(jpeg_encoder_structure, 1, output_ptr); - if (image_format == FOUR_TWO_TWO) - { - levelshift (Y2); - DCT (Y2); - quantization (Y2, ILqt); - output_ptr = huffman (jpeg_encoder_structure, 1, output_ptr); + if (image_format == FOUR_TWO_TWO) { + levelshift(Y2); + DCT(Y2); + quantization(Y2, ILqt); + output_ptr = huffman(jpeg_encoder_structure, 1, output_ptr); - levelshift (CB); - DCT (CB); - quantization (CB, ICqt); - output_ptr = huffman (jpeg_encoder_structure, 2, output_ptr); + levelshift(CB); + DCT(CB); + quantization(CB, ICqt); + output_ptr = huffman(jpeg_encoder_structure, 2, output_ptr); - levelshift (CR); - DCT (CR); - quantization (CR, ICqt); - output_ptr = huffman (jpeg_encoder_structure, 3, output_ptr); - } - return output_ptr; + levelshift(CR); + DCT(CR); + quantization(CR, ICqt); + output_ptr = huffman(jpeg_encoder_structure, 3, output_ptr); + } + return output_ptr; } /* Level shifting to get 8 bit SIGNED values for the data */ -void levelshift (int16_t* const data) +void levelshift(int16_t *const data) { - int16_t i; + int16_t i; - for (i=63; i>=0; i--) - data [i] -= 128; + for (i = 63; i >= 0; i--) { + data [i] -= 128; + } } /* DCT for One block(8x8) */ -void DCT (int16_t *data) +void DCT(int16_t *data) { - //_r8x8dct(data, fdct_coeff, fdct_temp); + //_r8x8dct(data, fdct_coeff, fdct_temp); - uint16_t i; - int32_t x0, x1, x2, x3, x4, x5, x6, x7, x8; + uint16_t i; + int32_t x0, x1, x2, x3, x4, x5, x6, x7, x8; - static const uint16_t c1=1420; // cos PI/16 * root(2) - static const uint16_t c2=1338; // cos PI/8 * root(2) - static const uint16_t c3=1204; // cos 3PI/16 * root(2) - static const uint16_t c5=805; // cos 5PI/16 * root(2) - static const uint16_t c6=554; // cos 3PI/8 * root(2) - static const uint16_t c7=283; // cos 7PI/16 * root(2) + static const uint16_t c1 = 1420; // cos PI/16 * root(2) + static const uint16_t c2 = 1338; // cos PI/8 * root(2) + static const uint16_t c3 = 1204; // cos 3PI/16 * root(2) + static const uint16_t c5 = 805; // cos 5PI/16 * root(2) + static const uint16_t c6 = 554; // cos 3PI/8 * root(2) + static const uint16_t c7 = 283; // cos 7PI/16 * root(2) - static const uint16_t s1=3; - static const uint16_t s2=10; - static const uint16_t s3=13; + static const uint16_t s1 = 3; + static const uint16_t s2 = 10; + static const uint16_t s3 = 13; - for (i=8; i>0; i--) - { - x8 = data [0] + data [7]; - x0 = data [0] - data [7]; + for (i = 8; i > 0; i--) { + x8 = data [0] + data [7]; + x0 = data [0] - data [7]; - x7 = data [1] + data [6]; - x1 = data [1] - data [6]; + x7 = data [1] + data [6]; + x1 = data [1] - data [6]; - x6 = data [2] + data [5]; - x2 = data [2] - data [5]; + x6 = data [2] + data [5]; + x2 = data [2] - data [5]; - x5 = data [3] + data [4]; - x3 = data [3] - data [4]; + x5 = data [3] + data [4]; + x3 = data [3] - data [4]; - x4 = x8 + x5; - x8 -= x5; + x4 = x8 + x5; + x8 -= x5; - x5 = x7 + x6; - x7 -= x6; + x5 = x7 + x6; + x7 -= x6; - data [0] = (int16_t) (x4 + x5); - data [4] = (int16_t) (x4 - x5); + data [0] = (int16_t)(x4 + x5); + data [4] = (int16_t)(x4 - x5); - data [2] = (int16_t) ((x8*c2 + x7*c6) >> s2); - data [6] = (int16_t) ((x8*c6 - x7*c2) >> s2); + data [2] = (int16_t)((x8 * c2 + x7 * c6) >> s2); + data [6] = (int16_t)((x8 * c6 - x7 * c2) >> s2); - data [7] = (int16_t) ((x0*c7 - x1*c5 + x2*c3 - x3*c1) >> s2); - data [5] = (int16_t) ((x0*c5 - x1*c1 + x2*c7 + x3*c3) >> s2); - data [3] = (int16_t) ((x0*c3 - x1*c7 - x2*c1 - x3*c5) >> s2); - data [1] = (int16_t) ((x0*c1 + x1*c3 + x2*c5 + x3*c7) >> s2); + data [7] = (int16_t)((x0 * c7 - x1 * c5 + x2 * c3 - x3 * c1) >> s2); + data [5] = (int16_t)((x0 * c5 - x1 * c1 + x2 * c7 + x3 * c3) >> s2); + data [3] = (int16_t)((x0 * c3 - x1 * c7 - x2 * c1 - x3 * c5) >> s2); + data [1] = (int16_t)((x0 * c1 + x1 * c3 + x2 * c5 + x3 * c7) >> s2); - data += 8; - } + data += 8; + } - data -= 64; + data -= 64; - for (i=8; i>0; i--) - { - x8 = data [0] + data [56]; - x0 = data [0] - data [56]; + for (i = 8; i > 0; i--) { + x8 = data [0] + data [56]; + x0 = data [0] - data [56]; - x7 = data [8] + data [48]; - x1 = data [8] - data [48]; + x7 = data [8] + data [48]; + x1 = data [8] - data [48]; - x6 = data [16] + data [40]; - x2 = data [16] - data [40]; + x6 = data [16] + data [40]; + x2 = data [16] - data [40]; - x5 = data [24] + data [32]; - x3 = data [24] - data [32]; + x5 = data [24] + data [32]; + x3 = data [24] - data [32]; - x4 = x8 + x5; - x8 -= x5; + x4 = x8 + x5; + x8 -= x5; - x5 = x7 + x6; - x7 -= x6; + x5 = x7 + x6; + x7 -= x6; - data [0] = (int16_t) ((x4 + x5) >> s1); - data [32] = (int16_t) ((x4 - x5) >> s1); + data [0] = (int16_t)((x4 + x5) >> s1); + data [32] = (int16_t)((x4 - x5) >> s1); - data [16] = (int16_t) ((x8*c2 + x7*c6) >> s3); - data [48] = (int16_t) ((x8*c6 - x7*c2) >> s3); + data [16] = (int16_t)((x8 * c2 + x7 * c6) >> s3); + data [48] = (int16_t)((x8 * c6 - x7 * c2) >> s3); - data [56] = (int16_t) ((x0*c7 - x1*c5 + x2*c3 - x3*c1) >> s3); - data [40] = (int16_t) ((x0*c5 - x1*c1 + x2*c7 + x3*c3) >> s3); - data [24] = (int16_t) ((x0*c3 - x1*c7 - x2*c1 - x3*c5) >> s3); - data [8] = (int16_t) ((x0*c1 + x1*c3 + x2*c5 + x3*c7) >> s3); + data [56] = (int16_t)((x0 * c7 - x1 * c5 + x2 * c3 - x3 * c1) >> s3); + data [40] = (int16_t)((x0 * c5 - x1 * c1 + x2 * c7 + x3 * c3) >> s3); + data [24] = (int16_t)((x0 * c3 - x1 * c7 - x2 * c1 - x3 * c5) >> s3); + data [8] = (int16_t)((x0 * c1 + x1 * c3 + x2 * c5 + x3 * c7) >> s3); - data++; - } + data++; + } } #define PUTBITS \ -{ \ + { \ bits_in_next_word = (int16_t) (bitindex + numbits - 32); \ if (bits_in_next_word < 0) \ { \ - lcode = (lcode << numbits) | data; \ - bitindex += numbits; \ + lcode = (lcode << numbits) | data; \ + bitindex += numbits; \ } \ else \ { \ - lcode = (lcode << (32 - bitindex)) | (data >> bits_in_next_word); \ - if ((*output_ptr++ = (uint8_t)(lcode >> 24)) == 0xff) \ - *output_ptr++ = 0; \ - if ((*output_ptr++ = (uint8_t)(lcode >> 16)) == 0xff) \ - *output_ptr++ = 0; \ - if ((*output_ptr++ = (uint8_t)(lcode >> 8)) == 0xff) \ - *output_ptr++ = 0; \ - if ((*output_ptr++ = (uint8_t) lcode) == 0xff) \ - *output_ptr++ = 0; \ - lcode = data; \ - bitindex = bits_in_next_word; \ + lcode = (lcode << (32 - bitindex)) | (data >> bits_in_next_word); \ + if ((*output_ptr++ = (uint8_t)(lcode >> 24)) == 0xff) \ + *output_ptr++ = 0; \ + if ((*output_ptr++ = (uint8_t)(lcode >> 16)) == 0xff) \ + *output_ptr++ = 0; \ + if ((*output_ptr++ = (uint8_t)(lcode >> 8)) == 0xff) \ + *output_ptr++ = 0; \ + if ((*output_ptr++ = (uint8_t) lcode) == 0xff) \ + *output_ptr++ = 0; \ + lcode = data; \ + bitindex = bits_in_next_word; \ } \ -} + } -uint8_t* huffman (JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure, uint16_t component, uint8_t *output_ptr) +uint8_t *huffman(JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure, uint16_t component, uint8_t *output_ptr) { - uint16_t i; - uint16_t *DcCodeTable, *DcSizeTable, *AcCodeTable, *AcSizeTable; + uint16_t i; + uint16_t *DcCodeTable, *DcSizeTable, *AcCodeTable, *AcSizeTable; - int16_t *Temp_Ptr, Coeff, LastDc; - uint16_t AbsCoeff, HuffCode, HuffSize, RunLength=0, DataSize=0, index; + int16_t *Temp_Ptr, Coeff, LastDc; + uint16_t AbsCoeff, HuffCode, HuffSize, RunLength = 0, DataSize = 0, index; - int16_t bits_in_next_word; - uint16_t numbits; - uint32_t data; + int16_t bits_in_next_word; + uint16_t numbits; + uint32_t data; - Temp_Ptr = Temp; - Coeff = *Temp_Ptr++; + Temp_Ptr = Temp; + Coeff = *Temp_Ptr++; - if (component == 1) - { - DcCodeTable = luminance_dc_code_table; - DcSizeTable = luminance_dc_size_table; - AcCodeTable = luminance_ac_code_table; - AcSizeTable = luminance_ac_size_table; + if (component == 1) { + DcCodeTable = luminance_dc_code_table; + DcSizeTable = luminance_dc_size_table; + AcCodeTable = luminance_ac_code_table; + AcSizeTable = luminance_ac_size_table; - LastDc = jpeg_encoder_structure->ldc1; - jpeg_encoder_structure->ldc1 = Coeff; + LastDc = jpeg_encoder_structure->ldc1; + jpeg_encoder_structure->ldc1 = Coeff; + } else { + DcCodeTable = chrominance_dc_code_table; + DcSizeTable = chrominance_dc_size_table; + AcCodeTable = chrominance_ac_code_table; + AcSizeTable = chrominance_ac_size_table; + + if (component == 2) { + LastDc = jpeg_encoder_structure->ldc2; + jpeg_encoder_structure->ldc2 = Coeff; + } else { + LastDc = jpeg_encoder_structure->ldc3; + jpeg_encoder_structure->ldc3 = Coeff; } - else - { - DcCodeTable = chrominance_dc_code_table; - DcSizeTable = chrominance_dc_size_table; - AcCodeTable = chrominance_ac_code_table; - AcSizeTable = chrominance_ac_size_table; + } - if (component == 2) - { - LastDc = jpeg_encoder_structure->ldc2; - jpeg_encoder_structure->ldc2 = Coeff; - } - else - { - LastDc = jpeg_encoder_structure->ldc3; - jpeg_encoder_structure->ldc3 = Coeff; - } - } + Coeff -= LastDc; - Coeff -= LastDc; + AbsCoeff = (Coeff < 0) ? -Coeff-- : Coeff; - AbsCoeff = (Coeff < 0) ? -Coeff-- : Coeff; + while (AbsCoeff != 0) { + AbsCoeff >>= 1; + DataSize++; + } - while (AbsCoeff != 0) - { - AbsCoeff >>= 1; - DataSize++; - } + HuffCode = DcCodeTable [DataSize]; + HuffSize = DcSizeTable [DataSize]; - HuffCode = DcCodeTable [DataSize]; - HuffSize = DcSizeTable [DataSize]; + Coeff &= (1 << DataSize) - 1; + data = (HuffCode << DataSize) | Coeff; + numbits = HuffSize + DataSize; - Coeff &= (1 << DataSize) - 1; - data = (HuffCode << DataSize) | Coeff; - numbits = HuffSize + DataSize; + PUTBITS - PUTBITS - - for (i=63; i>0; i--) - { - if ((Coeff = *Temp_Ptr++) != 0) - { - while (RunLength > 15) - { - RunLength -= 16; - data = AcCodeTable [161]; - numbits = AcSizeTable [161]; - PUTBITS - } - - AbsCoeff = (Coeff < 0) ? -Coeff-- : Coeff; - - if (AbsCoeff >> 8 == 0) - DataSize = bitsize [AbsCoeff]; - else - DataSize = bitsize [AbsCoeff >> 8] + 8; - - index = RunLength * 10 + DataSize; - HuffCode = AcCodeTable [index]; - HuffSize = AcSizeTable [index]; - - Coeff &= (1 << DataSize) - 1; - data = (HuffCode << DataSize) | Coeff; - numbits = HuffSize + DataSize; - - PUTBITS - RunLength = 0; - } - else - RunLength++; - } - - if (RunLength != 0) - { - data = AcCodeTable [0]; - numbits = AcSizeTable [0]; + for (i = 63; i > 0; i--) { + if ((Coeff = *Temp_Ptr++) != 0) { + while (RunLength > 15) { + RunLength -= 16; + data = AcCodeTable [161]; + numbits = AcSizeTable [161]; PUTBITS + } + + AbsCoeff = (Coeff < 0) ? -Coeff-- : Coeff; + + if (AbsCoeff >> 8 == 0) { + DataSize = bitsize [AbsCoeff]; + } else { + DataSize = bitsize [AbsCoeff >> 8] + 8; + } + + index = RunLength * 10 + DataSize; + HuffCode = AcCodeTable [index]; + HuffSize = AcSizeTable [index]; + + Coeff &= (1 << DataSize) - 1; + data = (HuffCode << DataSize) | Coeff; + numbits = HuffSize + DataSize; + + PUTBITS + RunLength = 0; + } else { + RunLength++; } - return output_ptr; + } + + if (RunLength != 0) { + data = AcCodeTable [0]; + numbits = AcSizeTable [0]; + PUTBITS + } + return output_ptr; } /* For bit Stuffing and EOI marker */ -uint8_t* close_bitstream (uint8_t *output_ptr) +uint8_t *close_bitstream(uint8_t *output_ptr) { - uint16_t i, count; - uint8_t *ptr; + uint16_t i, count; + uint8_t *ptr; - if (bitindex > 0) - { - lcode <<= (32 - bitindex); - //for (i=0; i 0) { + lcode <<= (32 - bitindex); + //for (i=0; i> 3; + count = (bitindex + 7) >> 3; - ptr = (uint8_t *) &lcode + 3; + ptr = (uint8_t *) &lcode + 3; - for (i=0; i> 8); + *output_ptr++ = (uint8_t) header_length; + + // Precision (P) + *output_ptr++ = 0x08; + + // image height + *output_ptr++ = (uint8_t)(image_height >> 8); + *output_ptr++ = (uint8_t) image_height; + + // image width + *output_ptr++ = (uint8_t)(image_width >> 8); + *output_ptr++ = (uint8_t) image_width; + + // Nf + *output_ptr++ = number_of_components; + + if (image_format == FOUR_ZERO_ZERO) { + *output_ptr++ = 0x01; + *output_ptr++ = 0x11; *output_ptr++ = 0x00; - *output_ptr++ = 0x43; - - // Pq, Tq - *output_ptr++ = 0x00; - - // Lqt table - for (i=0; i<64; i++) - *output_ptr++ = Lqt [i]; - - // Quantization table marker - *output_ptr++ = 0xFF; - *output_ptr++ = 0xDB; - - // Quantization table length - *output_ptr++ = 0x00; - *output_ptr++ = 0x43; - - // Pq, Tq + } else { *output_ptr++ = 0x01; - // Cqt table - for (i=0; i<64; i++) - *output_ptr++ = Cqt [i]; - - if (image_format == FOUR_ZERO_ZERO) - number_of_components = 1; - else - number_of_components = 3; - - // Frame header(SOF) - - // Start of frame marker - *output_ptr++ = 0xFF; - *output_ptr++ = 0xC0; - - header_length = (uint16_t) (8 + 3 * number_of_components); - - // Frame header length - *output_ptr++ = (uint8_t) (header_length >> 8); - *output_ptr++ = (uint8_t) header_length; - - // Precision (P) - *output_ptr++ = 0x08; - - // image height - *output_ptr++ = (uint8_t) (image_height >> 8); - *output_ptr++ = (uint8_t) image_height; - - // image width - *output_ptr++ = (uint8_t) (image_width >> 8); - *output_ptr++ = (uint8_t) image_width; - - // Nf - *output_ptr++ = number_of_components; - - if (image_format == FOUR_ZERO_ZERO) - { - *output_ptr++ = 0x01; - *output_ptr++ = 0x11; - *output_ptr++ = 0x00; - } - else - { - *output_ptr++ = 0x01; - - if (image_format == FOUR_TWO_TWO) - *output_ptr++ = 0x21; - else - *output_ptr++ = 0x11; - - *output_ptr++ = 0x00; - - *output_ptr++ = 0x02; - *output_ptr++ = 0x11; - *output_ptr++ = 0x01; - - *output_ptr++ = 0x03; - *output_ptr++ = 0x11; - *output_ptr++ = 0x01; - } - - // huffman table(DHT) - - for (i=0; i> 8); - *output_ptr++ = (uint8_t) header_length; - - // Ns - *output_ptr++ = number_of_components; - - if (image_format == FOUR_ZERO_ZERO) - { - *output_ptr++ = 0x01; - *output_ptr++ = 0x00; - } - else - { - *output_ptr++ = 0x01; - *output_ptr++ = 0x00; - - *output_ptr++ = 0x02; - *output_ptr++ = 0x11; - - *output_ptr++ = 0x03; - *output_ptr++ = 0x11; + if (image_format == FOUR_TWO_TWO) { + *output_ptr++ = 0x21; + } else { + *output_ptr++ = 0x11; } *output_ptr++ = 0x00; - *output_ptr++ = 0x3F; + + *output_ptr++ = 0x02; + *output_ptr++ = 0x11; + *output_ptr++ = 0x01; + + *output_ptr++ = 0x03; + *output_ptr++ = 0x11; + *output_ptr++ = 0x01; + } + + // huffman table(DHT) + + for (i = 0; i < sizeof(markerdata); i++) { + *output_ptr++ = markerdata [i]; + } + + + // Scan header(SOF) + + // Start of scan marker + *output_ptr++ = 0xFF; + *output_ptr++ = 0xDA; + + header_length = (uint16_t)(6 + (number_of_components << 1)); + + // Scan header length + *output_ptr++ = (uint8_t)(header_length >> 8); + *output_ptr++ = (uint8_t) header_length; + + // Ns + *output_ptr++ = number_of_components; + + if (image_format == FOUR_ZERO_ZERO) { + *output_ptr++ = 0x01; *output_ptr++ = 0x00; - return output_ptr; + } else { + *output_ptr++ = 0x01; + *output_ptr++ = 0x00; + + *output_ptr++ = 0x02; + *output_ptr++ = 0x11; + + *output_ptr++ = 0x03; + *output_ptr++ = 0x11; + } + + *output_ptr++ = 0x00; + *output_ptr++ = 0x3F; + *output_ptr++ = 0x00; + return output_ptr; } /* Multiply Quantization table with quality factor to get LQT and CQT factor ranges from 1 to 8; 1 = highest quality, 8 = lowest quality */ -void initialize_quantization_tables (uint32_t quality_factor) +void initialize_quantization_tables(uint32_t quality_factor) { - uint16_t i, index; - uint32_t value; + uint16_t i, index; + uint32_t value; - if (quality_factor < 1) - quality_factor = 1; - if (quality_factor > 8) - quality_factor = 8; - quality_factor = ((quality_factor * 3) - 2) * 128; //converts range[1:8] to [1:22] + if (quality_factor < 1) { + quality_factor = 1; + } + if (quality_factor > 8) { + quality_factor = 8; + } + quality_factor = ((quality_factor * 3) - 2) * 128; //converts range[1:8] to [1:22] - for (i=0; i<64; i++) - { - index = zigzag_table [i]; + for (i = 0; i < 64; i++) { + index = zigzag_table [i]; - /* luminance quantization table * quality factor */ - value = luminance_quant_table [i] * quality_factor; - value = (value + 0x200) >> 10; + /* luminance quantization table * quality factor */ + value = luminance_quant_table [i] * quality_factor; + value = (value + 0x200) >> 10; - if (value < 2) - value = 2; - else if (value > 255) - value = 255; - - Lqt [index] = (uint8_t) value; - //ILqt [i] = DSP_Division (0x8000, value); - ILqt [i] = 0x8000 / value; - - /* chrominance quantization table * quality factor */ - value = chrominance_quant_table [i] * quality_factor; - value = (value + 0x200) >> 10; - - if (value < 2) - value = 2; - else if (value > 255) - value = 255; - - Cqt [index] = (uint8_t) value; - //ICqt [i] = DSP_Division (0x8000, value); - ICqt [i] = 0x8000 / value; + if (value < 2) { + value = 2; + } else if (value > 255) { + value = 255; } + + Lqt [index] = (uint8_t) value; + //ILqt [i] = DSP_Division (0x8000, value); + ILqt [i] = 0x8000 / value; + + /* chrominance quantization table * quality factor */ + value = chrominance_quant_table [i] * quality_factor; + value = (value + 0x200) >> 10; + + if (value < 2) { + value = 2; + } else if (value > 255) { + value = 255; + } + + Cqt [index] = (uint8_t) value; + //ICqt [i] = DSP_Division (0x8000, value); + ICqt [i] = 0x8000 / value; + } } /* multiply DCT Coefficients with Quantization table and store in ZigZag location */ -void quantization (int16_t* const data, uint16_t* const quant_table_ptr) +void quantization(int16_t *const data, uint16_t *const quant_table_ptr) { - int16_t i; - int32_t value; + int16_t i; + int32_t value; - for (i=63; i>=0; i--) - { - value = data [i] * quant_table_ptr [i]; - value = (value + 0x4000) >> 15; + for (i = 63; i >= 0; i--) { + value = data [i] * quant_table_ptr [i]; + value = (value + 0x4000) >> 15; - Temp [zigzag_table [i]] = (int16_t) value; - } + Temp [zigzag_table [i]] = (int16_t) value; + } } -void read_400_format (JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure, uint8_t *input_ptr) +void read_400_format(JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure, uint8_t *input_ptr) { - int32_t i, j; - int16_t *Y1_Ptr = Y1; + int32_t i, j; + int16_t *Y1_Ptr = Y1; - uint16_t rows = jpeg_encoder_structure->rows; - uint16_t cols = jpeg_encoder_structure->cols; - uint16_t incr = jpeg_encoder_structure->incr; + uint16_t rows = jpeg_encoder_structure->rows; + uint16_t cols = jpeg_encoder_structure->cols; + uint16_t incr = jpeg_encoder_structure->incr; - for (i=rows; i>0; i--) - { - for (j=cols; j>0; j--) - *Y1_Ptr++ = *input_ptr++; - - for (j=8-cols; j>0; j--) { - *Y1_Ptr = *(Y1_Ptr - 1); - Y1_Ptr++; - } - - input_ptr += incr; + for (i = rows; i > 0; i--) { + for (j = cols; j > 0; j--) { + *Y1_Ptr++ = *input_ptr++; } - for (i=8-rows; i>0; i--) - { - for (j=8; j>0; j--) { - *Y1_Ptr = *(Y1_Ptr - 8); - Y1_Ptr++; - } + for (j = 8 - cols; j > 0; j--) { + *Y1_Ptr = *(Y1_Ptr - 1); + Y1_Ptr++; } + + input_ptr += incr; + } + + for (i = 8 - rows; i > 0; i--) { + for (j = 8; j > 0; j--) { + *Y1_Ptr = *(Y1_Ptr - 8); + Y1_Ptr++; + } + } } -void read_422_format (JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure, uint8_t *input_ptr) +void read_422_format(JPEG_ENCODER_STRUCTURE *jpeg_encoder_structure, uint8_t *input_ptr) { - int32_t i, j; - uint16_t Y1_cols, Y2_cols; + int32_t i, j; + uint16_t Y1_cols, Y2_cols; - int16_t *Y1_Ptr = Y1; - int16_t *Y2_Ptr = Y2; - int16_t *CB_Ptr = CB; - int16_t *CR_Ptr = CR; + int16_t *Y1_Ptr = Y1; + int16_t *Y2_Ptr = Y2; + int16_t *CB_Ptr = CB; + int16_t *CR_Ptr = CR; - uint16_t rows = jpeg_encoder_structure->rows; - uint16_t cols = jpeg_encoder_structure->cols; - uint16_t incr = jpeg_encoder_structure->incr; + uint16_t rows = jpeg_encoder_structure->rows; + uint16_t cols = jpeg_encoder_structure->cols; + uint16_t incr = jpeg_encoder_structure->incr; - if (cols <= 8) - { - Y1_cols = cols; - Y2_cols = 0; - } - else - { - Y1_cols = 8; - Y2_cols = (uint16_t) (cols - 8); + if (cols <= 8) { + Y1_cols = cols; + Y2_cols = 0; + } else { + Y1_cols = 8; + Y2_cols = (uint16_t)(cols - 8); + } + + for (i = rows; i > 0; i--) { + for (j = Y1_cols >> 1; j > 0; j--) { + *CB_Ptr++ = *input_ptr++; + *Y1_Ptr++ = *input_ptr++; + *CR_Ptr++ = *input_ptr++; + *Y1_Ptr++ = *input_ptr++; } - for (i=rows; i>0; i--) - { - for (j=Y1_cols>>1; j>0; j--) - { - *CB_Ptr++ = *input_ptr++; - *Y1_Ptr++ = *input_ptr++; - *CR_Ptr++ = *input_ptr++; - *Y1_Ptr++ = *input_ptr++; - } - - for (j=Y2_cols>>1; j>0; j--) - { - *CB_Ptr++ = *input_ptr++; - *Y2_Ptr++ = *input_ptr++; - *CR_Ptr++ = *input_ptr++; - *Y2_Ptr++ = *input_ptr++; - } - - if (cols <= 8) - { - for (j=8-Y1_cols; j>0; j--) { - *Y1_Ptr = *(Y1_Ptr - 1); - Y1_Ptr++; - } - - for (j=8-Y2_cols; j>0; j--) { - *Y2_Ptr = *(Y1_Ptr - 1); - Y2_Ptr++; - } - } - else - { - for (j=8-Y2_cols; j>0; j--) { - *Y2_Ptr = *(Y2_Ptr - 1); - Y2_Ptr++; - } - } - - for (j=(16-cols)>>1; j>0; j--) - { - *CB_Ptr = *(CB_Ptr-1); CB_Ptr++; - *CR_Ptr = *(CR_Ptr-1); CR_Ptr++; - } - - input_ptr += incr; + for (j = Y2_cols >> 1; j > 0; j--) { + *CB_Ptr++ = *input_ptr++; + *Y2_Ptr++ = *input_ptr++; + *CR_Ptr++ = *input_ptr++; + *Y2_Ptr++ = *input_ptr++; } - for (i=8-rows; i>0; i--) - { - for (j=8; j>0; j--) - { - *Y1_Ptr = *(Y1_Ptr - 8); Y1_Ptr++; - *Y2_Ptr = *(Y2_Ptr - 8); Y2_Ptr++; - *CB_Ptr = *(CB_Ptr - 8); CB_Ptr++; - *CR_Ptr = *(CR_Ptr - 8); CR_Ptr++; - } + if (cols <= 8) { + for (j = 8 - Y1_cols; j > 0; j--) { + *Y1_Ptr = *(Y1_Ptr - 1); + Y1_Ptr++; + } + + for (j = 8 - Y2_cols; j > 0; j--) { + *Y2_Ptr = *(Y1_Ptr - 1); + Y2_Ptr++; + } + } else { + for (j = 8 - Y2_cols; j > 0; j--) { + *Y2_Ptr = *(Y2_Ptr - 1); + Y2_Ptr++; + } } + + for (j = (16 - cols) >> 1; j > 0; j--) { + *CB_Ptr = *(CB_Ptr - 1); CB_Ptr++; + *CR_Ptr = *(CR_Ptr - 1); CR_Ptr++; + } + + input_ptr += incr; + } + + for (i = 8 - rows; i > 0; i--) { + for (j = 8; j > 0; j--) { + *Y1_Ptr = *(Y1_Ptr - 8); Y1_Ptr++; + *Y2_Ptr = *(Y2_Ptr - 8); Y2_Ptr++; + *CB_Ptr = *(CB_Ptr - 8); CB_Ptr++; + *CR_Ptr = *(CR_Ptr - 8); CR_Ptr++; + } + } } diff --git a/sw/airborne/modules/computer_vision/cv/encoding/jpeg.h b/sw/airborne/modules/computer_vision/cv/encoding/jpeg.h index e275c28fd9..d7c2f2591b 100644 --- a/sw/airborne/modules/computer_vision/cv/encoding/jpeg.h +++ b/sw/airborne/modules/computer_vision/cv/encoding/jpeg.h @@ -10,36 +10,36 @@ #define FOUR_FOUR_FOUR 3 #define RGB 4 -unsigned char *encode_image ( - uint8_t* in, - uint8_t* out, - uint32_t q, // image quality 1-8 - uint32_t fmt, // image format code - uint32_t width, // image width - uint32_t height, // image height - uint8_t add_dri_header // data only or full jpeg file +unsigned char *encode_image( + uint8_t *in, + uint8_t *out, + uint32_t q, // image quality 1-8 + uint32_t fmt, // image format code + uint32_t width, // image width + uint32_t height, // image height + uint8_t add_dri_header // data only or full jpeg file ); -unsigned char *encode_image_rtp ( - uint8_t* in, - uint8_t* out, - uint32_t q, // image quality 1-8 - uint32_t fmt, // image format code - uint32_t width, // image width - uint32_t height, // image height - uint8_t add_dri_header // data only or full jpeg file +unsigned char *encode_image_rtp( + uint8_t *in, + uint8_t *out, + uint32_t q, // image quality 1-8 + uint32_t fmt, // image format code + uint32_t width, // image width + uint32_t height, // image height + uint8_t add_dri_header // data only or full jpeg file ); -unsigned char *encode_image_std_qt ( - uint8_t* in, - uint8_t* out, - uint32_t q, // image quality 0 to 99 - uint32_t fmt, // image format code - uint32_t width, // image width - uint32_t height, // image height - uint8_t add_dri_header // data only or full jpeg file +unsigned char *encode_image_std_qt( + uint8_t *in, + uint8_t *out, + uint32_t q, // image quality 0 to 99 + uint32_t fmt, // image format code + uint32_t width, // image width + uint32_t height, // image height + uint8_t add_dri_header // data only or full jpeg file ); -int create_svs_jpeg_header(unsigned char* buf, int32_t size, int w); +int create_svs_jpeg_header(unsigned char *buf, int32_t size, int w); #endif diff --git a/sw/airborne/modules/computer_vision/cv/encoding/rtp.c b/sw/airborne/modules/computer_vision/cv/encoding/rtp.c index aa66af2051..2221126818 100644 --- a/sw/airborne/modules/computer_vision/cv/encoding/rtp.c +++ b/sw/airborne/modules/computer_vision/cv/encoding/rtp.c @@ -7,7 +7,9 @@ #include "rtp.h" -void send_rtp_packet(struct UdpSocket *sock, uint8_t* Jpeg, int JpegLen, uint32_t m_SequenceNumber, uint32_t m_Timestamp, uint32_t m_offset, uint8_t marker_bit, int w, int h, uint8_t format_code, uint8_t quality_code, uint8_t has_dri_header); +void send_rtp_packet(struct UdpSocket *sock, uint8_t *Jpeg, int JpegLen, uint32_t m_SequenceNumber, + uint32_t m_Timestamp, uint32_t m_offset, uint8_t marker_bit, int w, int h, uint8_t format_code, uint8_t quality_code, + uint8_t has_dri_header); // http://www.ietf.org/rfc/rfc3550.txt @@ -15,25 +17,23 @@ void send_rtp_packet(struct UdpSocket *sock, uint8_t* Jpeg, int JpegLen, uint32_ #define KJpegCh2ScanDataLen 56 // RGB JPEG images as RTP payload - 64x48 pixel -uint8_t JpegScanDataCh2A[KJpegCh2ScanDataLen] = -{ - 0xf8, 0xbe, 0x8a, 0x28, 0xaf, 0xe5, 0x33, 0xfd, - 0xfc, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2, - 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2, - 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2, - 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2, - 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2, - 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x3f, 0xff, 0xd9 +uint8_t JpegScanDataCh2A[KJpegCh2ScanDataLen] = { + 0xf8, 0xbe, 0x8a, 0x28, 0xaf, 0xe5, 0x33, 0xfd, + 0xfc, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2, + 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2, + 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2, + 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2, + 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2, + 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x3f, 0xff, 0xd9 }; -uint8_t JpegScanDataCh2B[KJpegCh2ScanDataLen] = -{ - 0xf5, 0x8a, 0x28, 0xa2, 0xbf, 0xca, 0xf3, 0xfc, - 0x53, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2, - 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2, - 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2, - 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2, - 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2, - 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x3f, 0xff, 0xd9 +uint8_t JpegScanDataCh2B[KJpegCh2ScanDataLen] = { + 0xf5, 0x8a, 0x28, 0xa2, 0xbf, 0xca, 0xf3, 0xfc, + 0x53, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2, + 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2, + 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2, + 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2, + 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x0a, 0x28, 0xa2, + 0x80, 0x0a, 0x28, 0xa2, 0x80, 0x3f, 0xff, 0xd9 }; @@ -50,20 +50,20 @@ void test_rtp_frame(struct UdpSocket *sock) uint8_t format_code = 0x01; uint8_t quality_code = 0x54; - if (toggle) - { - send_rtp_packet(sock, JpegScanDataCh2A,KJpegCh2ScanDataLen,framecounter, timecounter, 0, 1, 64, 48, format_code, quality_code, 0); - } - else - { - send_rtp_packet(sock, JpegScanDataCh2B,KJpegCh2ScanDataLen,framecounter, timecounter, 0, 1, 64, 48, format_code, quality_code, 0); + if (toggle) { + send_rtp_packet(sock, JpegScanDataCh2A, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code, + quality_code, 0); + } else { + send_rtp_packet(sock, JpegScanDataCh2B, KJpegCh2ScanDataLen, framecounter, timecounter, 0, 1, 64, 48, format_code, + quality_code, 0); } framecounter++; - timecounter+=3600; + timecounter += 3600; } -void send_rtp_frame(struct UdpSocket *sock, uint8_t * Jpeg, uint32_t JpegLen, int w, int h, uint8_t format_code, uint8_t quality_code, uint8_t has_dri_header, uint32_t delta_t) +void send_rtp_frame(struct UdpSocket *sock, uint8_t *Jpeg, uint32_t JpegLen, int w, int h, uint8_t format_code, + uint8_t quality_code, uint8_t has_dri_header, uint32_t delta_t) { static uint32_t packetcounter = 0; static uint32_t timecounter = 0; @@ -71,27 +71,25 @@ void send_rtp_frame(struct UdpSocket *sock, uint8_t * Jpeg, uint32_t JpegLen, in #define MAX_PACKET_SIZE 1400 - if (delta_t <= 0) - { + if (delta_t <= 0) { struct timeval tv; gettimeofday(&tv, 0); - uint32_t t = (tv.tv_sec % (256*256)) * 90000 + tv.tv_usec * 9 / 100; + uint32_t t = (tv.tv_sec % (256 * 256)) * 90000 + tv.tv_usec * 9 / 100; timecounter = t; } // Split frame into packets - for (;JpegLen > 0;) - { + for (; JpegLen > 0;) { uint32_t len = MAX_PACKET_SIZE; uint8_t lastpacket = 0; - if (JpegLen <= len) - { + if (JpegLen <= len) { lastpacket = 1; len = JpegLen; } - send_rtp_packet(sock, Jpeg,len,packetcounter, timecounter, offset, lastpacket, w, h, format_code, quality_code, has_dri_header); + send_rtp_packet(sock, Jpeg, len, packetcounter, timecounter, offset, lastpacket, w, h, format_code, quality_code, + has_dri_header); JpegLen -= len; Jpeg += len; @@ -100,10 +98,9 @@ void send_rtp_frame(struct UdpSocket *sock, uint8_t * Jpeg, uint32_t JpegLen, in } - if (delta_t > 0) - { + if (delta_t > 0) { // timestamp = 1 / 90 000 seconds - timecounter+=delta_t; + timecounter += delta_t; } } @@ -115,13 +112,13 @@ void send_rtp_frame(struct UdpSocket *sock, uint8_t * Jpeg, uint32_t JpegLen, in */ void send_rtp_packet( - struct UdpSocket *sock, - uint8_t * Jpeg, int JpegLen, - uint32_t m_SequenceNumber, uint32_t m_Timestamp, - uint32_t m_offset, uint8_t marker_bit, - int w, int h, - uint8_t format_code, uint8_t quality_code, - uint8_t has_dri_header) + struct UdpSocket *sock, + uint8_t *Jpeg, int JpegLen, + uint32_t m_SequenceNumber, uint32_t m_Timestamp, + uint32_t m_offset, uint8_t marker_bit, + int w, int h, + uint8_t format_code, uint8_t quality_code, + uint8_t has_dri_header) { #define KRtpHeaderSize 12 // size of the RTP header @@ -130,7 +127,7 @@ void send_rtp_packet( uint8_t RtpBuf[2048]; int RtpPacketSize = JpegLen + KRtpHeaderSize + KJpegHeaderSize; - memset(RtpBuf,0x00,sizeof(RtpBuf)); + memset(RtpBuf, 0x00, sizeof(RtpBuf)); /* The RTP header has the following format: @@ -151,7 +148,7 @@ void send_rtp_packet( // Prepare the 12 byte RTP header RtpBuf[0] = 0x80; // RTP version - RtpBuf[1] = 0x1a + (marker_bit<<7); // JPEG payload (26) and marker bit + RtpBuf[1] = 0x1a + (marker_bit << 7); // JPEG payload (26) and marker bit RtpBuf[2] = m_SequenceNumber >> 8; RtpBuf[3] = m_SequenceNumber & 0x0FF; // each packet is counted with a sequence counter RtpBuf[4] = (m_Timestamp & 0xFF000000) >> 24; // each image gets a timestamp @@ -184,13 +181,14 @@ void send_rtp_packet( RtpBuf[16] = 0x00; // type: 0 422 or 1 421 RtpBuf[17] = 60; // quality scale factor RtpBuf[16] = format_code; // type: 0 422 or 1 421 - if (has_dri_header) - RtpBuf[16] |= 0x40; // DRI flag + if (has_dri_header) { + RtpBuf[16] |= 0x40; // DRI flag + } RtpBuf[17] = quality_code; // quality scale factor - RtpBuf[18] = w/8; // width / 8 -> 48 pixel - RtpBuf[19] = h/8; // height / 8 -> 32 pixel + RtpBuf[18] = w / 8; // width / 8 -> 48 pixel + RtpBuf[19] = h / 8; // height / 8 -> 32 pixel // append the JPEG scan data to the RTP buffer - memcpy(&RtpBuf[20],Jpeg,JpegLen); + memcpy(&RtpBuf[20], Jpeg, JpegLen); - udp_write(sock,RtpBuf,RtpPacketSize); + udp_write(sock, RtpBuf, RtpPacketSize); }; diff --git a/sw/airborne/modules/computer_vision/cv/encoding/rtp.h b/sw/airborne/modules/computer_vision/cv/encoding/rtp.h index 66559ed17b..0266aa65e4 100644 --- a/sw/airborne/modules/computer_vision/cv/encoding/rtp.h +++ b/sw/airborne/modules/computer_vision/cv/encoding/rtp.h @@ -5,13 +5,13 @@ #include "udp/socket.h" void send_rtp_frame( - struct UdpSocket *sock, // socket - uint8_t * Jpeg, uint32_t JpegLen, // jpeg data - int w, int h, // width and height - uint8_t format_code, // 0=422, 1=421 - uint8_t quality_code, // 0-99 of 128 for custom (include - uint8_t has_dri_header, // Does Jpeg data include Header Info? - uint32_t delta_t // time step 90kHz + struct UdpSocket *sock, // socket + uint8_t *Jpeg, uint32_t JpegLen, // jpeg data + int w, int h, // width and height + uint8_t format_code, // 0=422, 1=421 + uint8_t quality_code, // 0-99 of 128 for custom (include + uint8_t has_dri_header, // Does Jpeg data include Header Info? + uint32_t delta_t // time step 90kHz ); void test_rtp_frame(struct UdpSocket *sock); diff --git a/sw/airborne/modules/computer_vision/cv/resize.h b/sw/airborne/modules/computer_vision/cv/resize.h index d6f900c1eb..e2bbf7096f 100644 --- a/sw/airborne/modules/computer_vision/cv/resize.h +++ b/sw/airborne/modules/computer_vision/cv/resize.h @@ -23,28 +23,26 @@ #include #include "image.h" -inline void resize_uyuv(struct img_struct* input, struct img_struct* output, int downsample); -inline void resize_uyuv(struct img_struct* input, struct img_struct* output, int downsample) +inline void resize_uyuv(struct img_struct *input, struct img_struct *output, int downsample); +inline void resize_uyuv(struct img_struct *input, struct img_struct *output, int downsample) { uint8_t *source = input->buf; uint8_t *dest = output->buf; - int pixelskip = downsample-1; - for (int y=0;yh;y++) - { - for (int x=0;xw;x+=2) - { + int pixelskip = downsample - 1; + for (int y = 0; y < output->h; y++) { + for (int x = 0; x < output->w; x += 2) { // YUYV *dest++ = *source++; // U *dest++ = *source++; // Y // now skip 3 pixels - if (pixelskip) source+=(pixelskip+1)*2; + if (pixelskip) { source += (pixelskip + 1) * 2; } *dest++ = *source++; // U *dest++ = *source++; // V - if (pixelskip) source+=(pixelskip-1)*2; + if (pixelskip) { source += (pixelskip - 1) * 2; } } // skip 3 rows - if (pixelskip) source += pixelskip * input->w * 2; + if (pixelskip) { source += pixelskip * input->w * 2; } } } diff --git a/sw/airborne/modules/computer_vision/image_nc_send.c b/sw/airborne/modules/computer_vision/image_nc_send.c index 372b4a2032..88b10a9ec1 100644 --- a/sw/airborne/modules/computer_vision/image_nc_send.c +++ b/sw/airborne/modules/computer_vision/image_nc_send.c @@ -81,23 +81,23 @@ void image_nc_send_run(void) {} pthread_t computervision_thread; volatile uint8_t computervision_thread_status = 0; volatile uint8_t computer_vision_thread_command = 0; -void *computervision_thread_main(void* data); -void *computervision_thread_main(void* data) +void *computervision_thread_main(void *data); +void *computervision_thread_main(void *data) { // Video Input struct vid_struct vid; - vid.device = (char*)"/dev/video1"; - vid.w=1280; - vid.h=720; + vid.device = (char *)"/dev/video1"; + vid.w = 1280; + vid.h = 720; vid.n_buffers = 4; - if (video_init(&vid)<0) { + if (video_init(&vid) < 0) { printf("Error initialising video\n"); computervision_thread_status = -1; return 0; } // Frame Grabbing - struct img_struct* img_new = video_create_image(&vid); + struct img_struct *img_new = video_create_image(&vid); // Frame Resizing uint8_t quality_factor = IMAGE_QUALITY_FACTOR; @@ -106,18 +106,17 @@ void *computervision_thread_main(void* data) struct img_struct small; small.w = vid.w / IMAGE_DOWNSIZE_FACTOR; small.h = vid.h / IMAGE_DOWNSIZE_FACTOR; - small.buf = (uint8_t*)malloc(small.w*small.h*2); + small.buf = (uint8_t *)malloc(small.w * small.h * 2); // Commpressed image buffer - uint8_t* jpegbuf = (uint8_t*)malloc(vid.h*vid.w*2); + uint8_t *jpegbuf = (uint8_t *)malloc(vid.h * vid.w * 2); // file index (search from 0) int file_index = 0; int microsleep = (int)(1000000. / IMAGE_FPS); - while (computer_vision_thread_command > 0) - { + while (computer_vision_thread_command > 0) { usleep(microsleep); video_grab_image(&vid, img_new); @@ -126,17 +125,17 @@ void *computervision_thread_main(void* data) // JPEG encode the image: uint32_t image_format = FOUR_TWO_TWO; // format (in jpeg.h) - uint8_t* end = encode_image (small.buf, jpegbuf, quality_factor, image_format, small.w, small.h, dri_jpeg_header); - uint32_t size = end-(jpegbuf); + uint8_t *end = encode_image(small.buf, jpegbuf, quality_factor, image_format, small.w, small.h, dri_jpeg_header); + uint32_t size = end - (jpegbuf); #if IMAGE_SAVE - FILE* save; + FILE *save; char save_name[128]; if (system("mkdir -p /data/video/images") == 0) { // search available index (max is 99) - for ( ; file_index < 99; file_index++) { - printf("search %d\n",file_index); - sprintf(save_name,"/data/video/images/img_%02d.jpg",file_index); + for (; file_index < 99; file_index++) { + printf("search %d\n", file_index); + sprintf(save_name, "/data/video/images/img_%02d.jpg", file_index); // test if file exists or not if (access(save_name, F_OK) == -1) { printf("access\n"); @@ -144,14 +143,12 @@ void *computervision_thread_main(void* data) if (save != NULL) { fwrite(jpegbuf, sizeof(uint8_t), size, save); fclose(save); - } - else { + } else { printf("Error when opening file %s\n", save_name); } // leave for loop break; - } - else {printf("file exists\n");} + } else {printf("file exists\n");} } } #endif @@ -164,23 +161,20 @@ void *computervision_thread_main(void* data) // Open process to send using netcat in child process char nc_cmd[64]; sprintf(nc_cmd, "nc %s %d", IMAGE_SERVER_IP, IMAGE_SERVER_PORT); - FILE* netcat; + FILE *netcat; netcat = popen(nc_cmd, "w"); if (netcat != NULL) { fwrite(jpegbuf, sizeof(uint8_t), size, netcat); if (pclose(netcat) == 0) { printf("Sending image succesfully\n"); } - } - else { + } else { printf("Fail sending image\n"); } exit(0); - } - else if (pid < 0) { + } else if (pid < 0) { printf("Fork failed\n"); - } - else { + } else { // Parent is waiting for child to terminate wait(&status); } @@ -196,7 +190,7 @@ void image_nc_send_start(void) { computer_vision_thread_command = 1; int rc = pthread_create(&computervision_thread, NULL, computervision_thread_main, NULL); - if(rc) { + if (rc) { printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc); } } diff --git a/sw/airborne/modules/computer_vision/lib/udp/socket.c b/sw/airborne/modules/computer_vision/lib/udp/socket.c index e0e182f395..fba489e282 100644 --- a/sw/airborne/modules/computer_vision/lib/udp/socket.c +++ b/sw/airborne/modules/computer_vision/lib/udp/socket.c @@ -22,13 +22,13 @@ # define SOCKET_ERROR -1 # define IO_SOCKET ioctl -struct UdpSocket* udp_socket(const char* str_ip_out, const int port_out, const int port_in, const int broadcast) +struct UdpSocket *udp_socket(const char *str_ip_out, const int port_out, const int port_in, const int broadcast) { - struct UdpSocket* me = malloc(sizeof(struct UdpSocket)); + struct UdpSocket *me = malloc(sizeof(struct UdpSocket)); int so_reuseaddr = 1; - me->socket_out = socket( AF_INET, SOCK_DGRAM, 0); + me->socket_out = socket(AF_INET, SOCK_DGRAM, 0); setsockopt(me->socket_out, SOL_SOCKET, SO_REUSEADDR, &so_reuseaddr, sizeof(so_reuseaddr)); @@ -41,7 +41,7 @@ struct UdpSocket* udp_socket(const char* str_ip_out, const int port_out, const i me->addr_out.sin_port = htons(port_out); me->addr_out.sin_addr.s_addr = inet_addr(str_ip_out); - me->socket_in = socket( AF_INET, SOCK_DGRAM, 0); + me->socket_in = socket(AF_INET, SOCK_DGRAM, 0); setsockopt(me->socket_in, SOL_SOCKET, SO_REUSEADDR, &so_reuseaddr, sizeof(so_reuseaddr)); @@ -58,9 +58,10 @@ struct UdpSocket* udp_socket(const char* str_ip_out, const int port_out, const i //#define UDP_MODE MSG_DONTWAIT #define UDP_MODE 0 -int udp_write(struct UdpSocket* me, unsigned char* buf, int len) { +int udp_write(struct UdpSocket *me, unsigned char *buf, int len) +{ sendto(me->socket_out, buf, len, UDP_MODE, - (struct sockaddr*)&me->addr_out, sizeof(me->addr_out)); + (struct sockaddr *)&me->addr_out, sizeof(me->addr_out)); //printf("sendto ret=%d\n",ret); return len; } @@ -68,11 +69,11 @@ int udp_write(struct UdpSocket* me, unsigned char* buf, int len) { unsigned long MIN(unsigned long a, unsigned long b); unsigned long MIN(unsigned long a, unsigned long b) { - if (a0 in order to start the reading loop @@ -81,22 +82,22 @@ int udp_read(struct UdpSocket* me, unsigned char* buf, int len) int status; // if socket is connected - for (;btr>0;) - { + for (; btr > 0;) { // Check Status status = IO_SOCKET(me->socket_in, FIONREAD, &toread); - if(status == SOCKET_ERROR) { + if (status == SOCKET_ERROR) { printf("problem receiving from socket\n"); break; } //printf("UDP has %d bytes\n", toread); - if (toread <= 0) + if (toread <= 0) { break; + } // If status: ok and new data: read it - btr = MIN(toread,(unsigned long)len); - recvfrom(me->socket_in, buf, btr, 0, (struct sockaddr*)&me->addr_in, (socklen_t *) sizeof(me->addr_in) ); + btr = MIN(toread, (unsigned long)len); + recvfrom(me->socket_in, buf, btr, 0, (struct sockaddr *)&me->addr_in, (socklen_t *) sizeof(me->addr_in)); newbytes += btr; } return newbytes; diff --git a/sw/airborne/modules/computer_vision/lib/udp/socket.h b/sw/airborne/modules/computer_vision/lib/udp/socket.h index 7260e5cfa5..98a831a6ee 100644 --- a/sw/airborne/modules/computer_vision/lib/udp/socket.h +++ b/sw/airborne/modules/computer_vision/lib/udp/socket.h @@ -16,9 +16,9 @@ struct UdpSocket { }; -extern struct UdpSocket* udp_socket(const char* str_ip_out, const int port_out, const int port_in, const int broadcast); -extern int udp_write(struct UdpSocket* me, unsigned char* buf, int len); -extern int udp_read(struct UdpSocket* me, unsigned char* buf, int len); +extern struct UdpSocket *udp_socket(const char *str_ip_out, const int port_out, const int port_in, const int broadcast); +extern int udp_write(struct UdpSocket *me, unsigned char *buf, int len); +extern int udp_read(struct UdpSocket *me, unsigned char *buf, int len); #endif /* SOCKET_H */ diff --git a/sw/airborne/modules/computer_vision/lib/v4l/video.c b/sw/airborne/modules/computer_vision/lib/v4l/video.c index b3a090ced4..e0a9fb5c31 100644 --- a/sw/airborne/modules/computer_vision/lib/v4l/video.c +++ b/sw/airborne/modules/computer_vision/lib/v4l/video.c @@ -43,10 +43,10 @@ pthread_t video_thread; -void *video_thread_main(void* data); -void *video_thread_main(void* data) +void *video_thread_main(void *data); +void *video_thread_main(void *data) { - struct vid_struct* vid = (struct vid_struct*)data; + struct vid_struct *vid = (struct vid_struct *)data; printf("video_thread_main started\n"); while (1) { fd_set fds; @@ -61,7 +61,7 @@ void *video_thread_main(void* data) r = select(vid->fd + 1, &fds, NULL, NULL, &tv); if (-1 == r) { - if (EINTR == errno) continue; + if (EINTR == errno) { continue; } printf("select err\n"); } @@ -84,13 +84,13 @@ void *video_thread_main(void* data) vid->seq++; - if(vid->trigger) { + if (vid->trigger) { // todo add timestamp again //vid->img->timestamp = util_timestamp(); vid->img->seq = vid->seq; - memcpy(vid->img->buf, vid->buffers[buf.index].buf, vid->w*vid->h*2); - vid->trigger=0; + memcpy(vid->img->buf, vid->buffers[buf.index].buf, vid->w * vid->h * 2); + vid->trigger = 0; } if (ioctl(vid->fd, VIDIOC_QBUF, &buf) < 0) { @@ -108,9 +108,9 @@ int video_init(struct vid_struct *vid) unsigned int i; enum v4l2_buf_type type; - vid->seq=0; - vid->trigger=0; - if(vid->n_buffers==0) vid->n_buffers=4; + vid->seq = 0; + vid->trigger = 0; + if (vid->n_buffers == 0) { vid->n_buffers = 4; } vid->fd = open(vid->device, O_RDWR | O_NONBLOCK, 0); @@ -148,7 +148,7 @@ int video_init(struct vid_struct *vid) printf("Buffer count = %d\n", vid->n_buffers); - vid->buffers = (struct buffer_struct*)calloc(vid->n_buffers, sizeof(struct buffer_struct)); + vid->buffers = (struct buffer_struct *)calloc(vid->n_buffers, sizeof(struct buffer_struct)); for (i = 0; i < vid->n_buffers; ++i) { struct v4l2_buffer buf; @@ -164,11 +164,11 @@ int video_init(struct vid_struct *vid) } vid->buffers[i].length = buf.length; - printf("buffer%d.length=%d\n",i,buf.length); - vid->buffers[i].buf = mmap(NULL, buf.length, PROT_READ|PROT_WRITE, MAP_SHARED, vid->fd, buf.m.offset); + printf("buffer%d.length=%d\n", i, buf.length); + vid->buffers[i].buf = mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, vid->fd, buf.m.offset); if (MAP_FAILED == vid->buffers[i].buf) { - printf ("mmap() failed.\n"); + printf("mmap() failed.\n"); return -1; } } @@ -188,14 +188,14 @@ int video_init(struct vid_struct *vid) } type = V4L2_BUF_TYPE_VIDEO_CAPTURE; - if (ioctl(vid->fd, VIDIOC_STREAMON, &type)< 0) { + if (ioctl(vid->fd, VIDIOC_STREAMON, &type) < 0) { printf("ioctl() VIDIOC_STREAMON failed.\n"); return -1; } //start video thread int rc = pthread_create(&video_thread, NULL, video_thread_main, vid); - if(rc) { + if (rc) { printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc); return 202; } @@ -207,27 +207,28 @@ void video_close(struct vid_struct *vid) { int i; for (i = 0; i < (int)vid->n_buffers; ++i) { - if (-1 == munmap(vid->buffers[i].buf, vid->buffers[i].length)) printf("munmap() failed.\n"); + if (-1 == munmap(vid->buffers[i].buf, vid->buffers[i].length)) { printf("munmap() failed.\n"); } } close(vid->fd); } struct img_struct *video_create_image(struct vid_struct *vid) { - struct img_struct* img = (struct img_struct*)malloc(sizeof(struct img_struct)); - img->w=vid->w; - img->h=vid->h; - img->buf = (unsigned char*)malloc(vid->h*vid->w*2); + struct img_struct *img = (struct img_struct *)malloc(sizeof(struct img_struct)); + img->w = vid->w; + img->h = vid->h; + img->buf = (unsigned char *)malloc(vid->h * vid->w * 2); return img; } pthread_mutex_t video_grab_mutex = PTHREAD_MUTEX_INITIALIZER; -void video_grab_image(struct vid_struct *vid, struct img_struct *img) { +void video_grab_image(struct vid_struct *vid, struct img_struct *img) +{ pthread_mutex_lock(&video_grab_mutex); vid->img = img; - vid->trigger=1; + vid->trigger = 1; // while(vid->trigger) pthread_yield(); - while(vid->trigger) usleep(1); + while (vid->trigger) { usleep(1); } pthread_mutex_unlock(&video_grab_mutex); } diff --git a/sw/airborne/modules/computer_vision/lib/v4l/video.h b/sw/airborne/modules/computer_vision/lib/v4l/video.h index 125892ad1b..351ba68522 100644 --- a/sw/airborne/modules/computer_vision/lib/v4l/video.h +++ b/sw/airborne/modules/computer_vision/lib/v4l/video.h @@ -25,7 +25,7 @@ #include "../../cv/image.h" struct buffer_struct { - void * buf; + void *buf; size_t length; }; @@ -39,7 +39,7 @@ struct vid_struct { //private members int trigger; struct img_struct *img; - struct buffer_struct * buffers; + struct buffer_struct *buffers; int fd; }; diff --git a/sw/airborne/modules/computer_vision/viewvideo.c b/sw/airborne/modules/computer_vision/viewvideo.c index b1b0e7eeab..7a5dfd0a95 100644 --- a/sw/airborne/modules/computer_vision/viewvideo.c +++ b/sw/airborne/modules/computer_vision/viewvideo.c @@ -92,23 +92,23 @@ int viewvideo_shot = 0; pthread_t computervision_thread; volatile uint8_t computervision_thread_status = 0; volatile uint8_t computer_vision_thread_command = 0; -void *computervision_thread_main(void* data); -void *computervision_thread_main(void* data) +void *computervision_thread_main(void *data); +void *computervision_thread_main(void *data) { // Video Input struct vid_struct vid; - vid.device = (char*)"/dev/video1"; - vid.w=1280; - vid.h=720; + vid.device = (char *)"/dev/video1"; + vid.w = 1280; + vid.h = 720; vid.n_buffers = 4; - if (video_init(&vid)<0) { + if (video_init(&vid) < 0) { printf("Error initialising video\n"); computervision_thread_status = -1; return 0; } // Video Grabbing - struct img_struct* img_new = video_create_image(&vid); + struct img_struct *img_new = video_create_image(&vid); // Video Resizing uint8_t quality_factor = VIDEO_QUALITY_FACTOR; @@ -118,19 +118,19 @@ void *computervision_thread_main(void* data) struct img_struct small; small.w = vid.w / VIDEO_DOWNSIZE_FACTOR; small.h = vid.h / VIDEO_DOWNSIZE_FACTOR; - small.buf = (uint8_t*)malloc(small.w*small.h*2); + small.buf = (uint8_t *)malloc(small.w * small.h * 2); // Video Compression - uint8_t* jpegbuf = (uint8_t*)malloc(vid.h*vid.w*2); + uint8_t *jpegbuf = (uint8_t *)malloc(vid.h * vid.w * 2); // Network Transmit - struct UdpSocket* vsock; + struct UdpSocket *vsock; vsock = udp_socket(VIDEO_SOCK_IP, VIDEO_SOCK_OUT, VIDEO_SOCK_IN, FMS_BROADCAST); // Create SPD file and make folder if necessary - FILE* sdp; + FILE *sdp; if (system("mkdir -p /data/video/sdp") == 0) { - sdp = fopen("/data/video/sdp/x86_config-mjpeg.sdp","w"); + sdp = fopen("/data/video/sdp/x86_config-mjpeg.sdp", "w"); if (sdp != NULL) { fprintf(sdp, "v=0\n"); fprintf(sdp, "m=video %d RTP/AVP 26\n", (int)(VIDEO_SOCK_OUT)); @@ -146,30 +146,28 @@ void *computervision_thread_main(void* data) struct timeval last_time; gettimeofday(&last_time, NULL); - while (computer_vision_thread_command > 0) - { + while (computer_vision_thread_command > 0) { // compute usleep to have a more stable frame rate struct timeval time; gettimeofday(&time, NULL); - int dt = (int)(time.tv_sec - last_time.tv_sec)*1000000 + (int)(time.tv_usec - last_time.tv_usec); - if (dt < microsleep) usleep(microsleep - dt); + int dt = (int)(time.tv_sec - last_time.tv_sec) * 1000000 + (int)(time.tv_usec - last_time.tv_usec); + if (dt < microsleep) { usleep(microsleep - dt); } last_time = time; // Grab new frame video_grab_image(&vid, img_new); // Save picture on disk - if (computer_vision_thread_command == 2) - { - uint8_t* end = encode_image (img_new->buf, jpegbuf, 99, FOUR_TWO_TWO, vid.w, vid.h, 1); - uint32_t size = end-(jpegbuf); - FILE* save; + if (computer_vision_thread_command == 2) { + uint8_t *end = encode_image(img_new->buf, jpegbuf, 99, FOUR_TWO_TWO, vid.w, vid.h, 1); + uint32_t size = end - (jpegbuf); + FILE *save; char save_name[128]; if (system("mkdir -p /data/video/images") == 0) { // search available index (max is 99) - for ( ; file_index < 99; file_index++) { - printf("search %d\n",file_index); - sprintf(save_name,"/data/video/images/img_%02d.jpg",file_index); + for (; file_index < 99; file_index++) { + printf("search %d\n", file_index); + sprintf(save_name, "/data/video/images/img_%02d.jpg", file_index); // test if file exists or not if (access(save_name, F_OK) == -1) { printf("access\n"); @@ -177,8 +175,7 @@ void *computervision_thread_main(void* data) if (save != NULL) { fwrite(jpegbuf, sizeof(uint8_t), size, save); fclose(save); - } - else { + } else { printf("Error when opening file %s\n", save_name); } // leave for loop @@ -197,20 +194,20 @@ void *computervision_thread_main(void* data) // JPEG encode the image: uint32_t image_format = FOUR_TWO_TWO; // format (in jpeg.h) - uint8_t* end = encode_image (small.buf, jpegbuf, quality_factor, image_format, small.w, small.h, dri_jpeg_header); + uint8_t *end = encode_image(small.buf, jpegbuf, quality_factor, image_format, small.w, small.h, dri_jpeg_header); uint32_t size = end - (jpegbuf); // Send image with RTP - printf("Sending an image ...%u\n",size); + printf("Sending an image ...%u\n", size); send_rtp_frame( - vsock, // UDP - jpegbuf,size, // JPEG - small.w, small.h, // Img Size - 0, // Format 422 - quality_factor, // Jpeg-Quality - dri_jpeg_header, // DRI Header - 1 // 90kHz time increment - ); + vsock, // UDP + jpegbuf, size, // JPEG + small.w, small.h, // Img Size + 0, // Format 422 + quality_factor, // Jpeg-Quality + dri_jpeg_header, // DRI Header + 1 // 90kHz time increment + ); // Extra note: when the time increment is set to 0, // it is automaticaly calculated by the send_rtp_frame function // based on gettimeofday value. This seems to introduce some lag or jitter. @@ -230,7 +227,7 @@ void viewvideo_start(void) { computer_vision_thread_command = 1; int rc = pthread_create(&computervision_thread, NULL, computervision_thread_main, NULL); - if(rc) { + if (rc) { printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc); } } diff --git a/sw/airborne/modules/computer_vision/viewvideo.h b/sw/airborne/modules/computer_vision/viewvideo.h index 35c67ab778..d04b07af1a 100644 --- a/sw/airborne/modules/computer_vision/viewvideo.h +++ b/sw/airborne/modules/computer_vision/viewvideo.h @@ -42,9 +42,9 @@ extern int viewvideo_save_shot(void); extern int viewvideo_shot; #define viewvideo_SaveShot(_v) { \ - viewvideo_shot = 1; \ - viewvideo_save_shot(); \ -} + viewvideo_shot = 1; \ + viewvideo_save_shot(); \ + } #endif /* VIEW_VIDEO_H */ diff --git a/sw/airborne/modules/config/config_mkk_v2.c b/sw/airborne/modules/config/config_mkk_v2.c index 92243aacc9..a5742b4b83 100644 --- a/sw/airborne/modules/config/config_mkk_v2.c +++ b/sw/airborne/modules/config/config_mkk_v2.c @@ -42,8 +42,7 @@ void config_mkk_v2_init(void) void config_mkk_v2_periodic_read_status(void) { // Read Config - if (config_mkk_v2.read_config > 0) - { + if (config_mkk_v2.read_config > 0) { switch (config_mkk_v2.trans.status) { case I2CTransFailed: config_mkk_v2.read_config = 0; @@ -66,13 +65,15 @@ void config_mkk_v2_periodic_read_status(void) void config_mkk_v2_periodic_telemetry(void) { - static uint8_t send_nr = 0; + static uint8_t send_nr = 0; - DOWNLINK_SEND_MKK(DefaultChannel, DefaultDevice, &send_nr, &actuators_mkk_v2.data[send_nr].MaxPWM, &actuators_mkk_v2.data[send_nr].Current, &actuators_mkk_v2.data[send_nr].Temperature); + DOWNLINK_SEND_MKK(DefaultChannel, DefaultDevice, &send_nr, &actuators_mkk_v2.data[send_nr].MaxPWM, + &actuators_mkk_v2.data[send_nr].Current, &actuators_mkk_v2.data[send_nr].Temperature); - send_nr++; - if (send_nr >= ACTUATORS_MKK_V2_NB) - send_nr = 0; + send_nr++; + if (send_nr >= ACTUATORS_MKK_V2_NB) { + send_nr = 0; + } } @@ -102,12 +103,11 @@ extern config_mkk_v2_eeprom_t config_mkk_v2_eeprom; uint8_t config_mkk_v2_crc(uint8_t offset) { - uint8_t crc = 0xaa; - for(int i=offset; i<(offset+7); i++) - { - crc += config_mkk_v2.trans.buf[i]; - } - return crc; + uint8_t crc = 0xaa; + for (int i = offset; i < (offset + 7); i++) { + crc += config_mkk_v2.trans.buf[i]; + } + return crc; } @@ -119,74 +119,73 @@ config_mkk_v2_eeprom_t config_mkk_v2_eeprom; #define RETURN_IF_NOT_KILLMODE() \ -{ \ - if (!actuators_delay_done) \ - return; \ -} + { \ + if (!actuators_delay_done) \ + return; \ + } void config_mkk_v2_read_eeprom(void) { - // Activate decoder - config_mkk_v2.read_config = 1; + // Activate decoder + config_mkk_v2.read_config = 1; - // Do not read config while running - RETURN_IF_NOT_KILLMODE(); + // Do not read config while running + RETURN_IF_NOT_KILLMODE(); - // New I2C Write/Read Transaction - config_mkk_v2.trans.type = I2CTransTxRx; - config_mkk_v2.trans.slave_addr = 0x52 + config_mkk_v2.addr * 2; - config_mkk_v2.trans.len_w = 2; - config_mkk_v2.trans.buf[0] = 0; - config_mkk_v2.trans.buf[1] = (BL_READMODE_CONFIG<<3); - config_mkk_v2.trans.len_r = 8; + // New I2C Write/Read Transaction + config_mkk_v2.trans.type = I2CTransTxRx; + config_mkk_v2.trans.slave_addr = 0x52 + config_mkk_v2.addr * 2; + config_mkk_v2.trans.len_w = 2; + config_mkk_v2.trans.buf[0] = 0; + config_mkk_v2.trans.buf[1] = (BL_READMODE_CONFIG << 3); + config_mkk_v2.trans.len_r = 8; - i2c_submit(&ACTUATORS_MKK_V2_I2C_DEV, &config_mkk_v2.trans); + i2c_submit(&ACTUATORS_MKK_V2_I2C_DEV, &config_mkk_v2.trans); } void config_mkk_v2_parse_eeprom(void) { - config_mkk_v2_eeprom.crc = config_mkk_v2.trans.buf[7]; // checksum - if (config_mkk_v2_crc(0) != config_mkk_v2_eeprom.crc) - { - config_mkk_v2.nb_err++; - } - else - { - config_mkk_v2_eeprom.revision = config_mkk_v2.trans.buf[0]; // must be BL_revision - config_mkk_v2_eeprom.SetMask = config_mkk_v2.trans.buf[1]; // settings mask - config_mkk_v2_eeprom.PwmScaling = config_mkk_v2.trans.buf[2]; // maximum value of control pwm, acts like a thrust limit - config_mkk_v2_eeprom.CurrentLimit = config_mkk_v2.trans.buf[3]; // current limit in A - config_mkk_v2_eeprom.TempLimit = config_mkk_v2.trans.buf[4]; // in °C - config_mkk_v2_eeprom.CurrentScaling = config_mkk_v2.trans.buf[5]; // scaling factor for current measurement - config_mkk_v2_eeprom.BitConfig = config_mkk_v2.trans.buf[6]; // see defines above - } + config_mkk_v2_eeprom.crc = config_mkk_v2.trans.buf[7]; // checksum + if (config_mkk_v2_crc(0) != config_mkk_v2_eeprom.crc) { + config_mkk_v2.nb_err++; + } else { + config_mkk_v2_eeprom.revision = config_mkk_v2.trans.buf[0]; // must be BL_revision + config_mkk_v2_eeprom.SetMask = config_mkk_v2.trans.buf[1]; // settings mask + config_mkk_v2_eeprom.PwmScaling = + config_mkk_v2.trans.buf[2]; // maximum value of control pwm, acts like a thrust limit + config_mkk_v2_eeprom.CurrentLimit = config_mkk_v2.trans.buf[3]; // current limit in A + config_mkk_v2_eeprom.TempLimit = config_mkk_v2.trans.buf[4]; // in °C + config_mkk_v2_eeprom.CurrentScaling = config_mkk_v2.trans.buf[5]; // scaling factor for current measurement + config_mkk_v2_eeprom.BitConfig = config_mkk_v2.trans.buf[6]; // see defines above + } } void config_mkk_v2_send_eeprom(void) { - // Do not upload while running - RETURN_IF_NOT_KILLMODE(); + // Do not upload while running + RETURN_IF_NOT_KILLMODE(); - // Do not upload bad data: - if (config_mkk_v2_eeprom.revision != config_mkk_v2_EEPROM_REVISION) - return; + // Do not upload bad data: + if (config_mkk_v2_eeprom.revision != config_mkk_v2_EEPROM_REVISION) { + return; + } - // New I2C Write Transaction - config_mkk_v2.trans.type = I2CTransTx; - config_mkk_v2.trans.slave_addr = 0x52 + config_mkk_v2.addr * 2; - config_mkk_v2.trans.len_w = 10; - config_mkk_v2.trans.buf[0] = 0; - config_mkk_v2.trans.buf[1] = (BL_READMODE_CONFIG<<3); + // New I2C Write Transaction + config_mkk_v2.trans.type = I2CTransTx; + config_mkk_v2.trans.slave_addr = 0x52 + config_mkk_v2.addr * 2; + config_mkk_v2.trans.len_w = 10; + config_mkk_v2.trans.buf[0] = 0; + config_mkk_v2.trans.buf[1] = (BL_READMODE_CONFIG << 3); - config_mkk_v2.trans.buf[2] = config_mkk_v2_eeprom.revision; - config_mkk_v2.trans.buf[3] = config_mkk_v2_eeprom.SetMask; - config_mkk_v2.trans.buf[4] = config_mkk_v2_eeprom.PwmScaling; - config_mkk_v2.trans.buf[5] = config_mkk_v2_eeprom.CurrentLimit; - config_mkk_v2.trans.buf[6] = config_mkk_v2_eeprom.TempLimit; - config_mkk_v2.trans.buf[7] = config_mkk_v2_eeprom.CurrentScaling; - config_mkk_v2.trans.buf[8] = config_mkk_v2_eeprom.BitConfig; - config_mkk_v2.trans.buf[9] = config_mkk_v2_crc(2); + config_mkk_v2.trans.buf[2] = config_mkk_v2_eeprom.revision; + config_mkk_v2.trans.buf[3] = config_mkk_v2_eeprom.SetMask; + config_mkk_v2.trans.buf[4] = config_mkk_v2_eeprom.PwmScaling; + config_mkk_v2.trans.buf[5] = config_mkk_v2_eeprom.CurrentLimit; + config_mkk_v2.trans.buf[6] = config_mkk_v2_eeprom.TempLimit; + config_mkk_v2.trans.buf[7] = config_mkk_v2_eeprom.CurrentScaling; + config_mkk_v2.trans.buf[8] = config_mkk_v2_eeprom.BitConfig; + config_mkk_v2.trans.buf[9] = config_mkk_v2_crc(2); - i2c_submit(&ACTUATORS_MKK_V2_I2C_DEV, &config_mkk_v2.trans); + i2c_submit(&ACTUATORS_MKK_V2_I2C_DEV, &config_mkk_v2.trans); } diff --git a/sw/airborne/modules/config/config_mkk_v2.h b/sw/airborne/modules/config/config_mkk_v2.h index 2f842545ac..5a4da39694 100644 --- a/sw/airborne/modules/config/config_mkk_v2.h +++ b/sw/airborne/modules/config/config_mkk_v2.h @@ -32,14 +32,13 @@ #include "mcu_periph/i2c.h" -struct config_mkk_v2_struct -{ - uint8_t read_config; - uint8_t addr; +struct config_mkk_v2_struct { + uint8_t read_config; + uint8_t addr; - int nb_err; + int nb_err; - struct i2c_transaction trans; + struct i2c_transaction trans; }; extern struct config_mkk_v2_struct config_mkk_v2; @@ -51,8 +50,7 @@ void config_mkk_v2_periodic_telemetry(void); ////////////////////////////////////////////////////////////////// // MKK Config -typedef struct -{ +typedef struct { uint8_t revision; uint8_t SetMask; uint8_t PwmScaling; @@ -84,42 +82,42 @@ extern void config_mkk_v2_read_eeprom(void); #define config_mkk_v2_ResetDefault(_v) { \ config_mkk_v2_eeprom.SetMask = CONFIG_MKK_V2_MASK_SET_SAVE_EEPROM | CONFIG_MKK_V2_MASK_SET_DEFAULT_PARAMS; \ config_mkk_v2_send_eeprom(); \ -} + } #define config_mkk_v2_SetPwmScaling(_v) { \ config_mkk_v2_eeprom.PwmScaling = _v; \ config_mkk_v2_eeprom.SetMask = CONFIG_MKK_V2_MASK_SET_SAVE_EEPROM | CONFIG_MKK_V2_MASK_SET_PWM_SCALING; \ config_mkk_v2_send_eeprom(); \ -} + } #define config_mkk_v2_SetCurrentLimit(_v) { \ config_mkk_v2_eeprom.CurrentLimit = _v; \ config_mkk_v2_eeprom.SetMask = CONFIG_MKK_V2_MASK_SET_SAVE_EEPROM | CONFIG_MKK_V2_MASK_SET_CURRENT_LIMIT; \ config_mkk_v2_send_eeprom(); \ -} + } #define config_mkk_v2_SetTempLimit(_v) { \ config_mkk_v2_eeprom.TempLimit = _v; \ config_mkk_v2_eeprom.SetMask = CONFIG_MKK_V2_MASK_SET_SAVE_EEPROM | CONFIG_MKK_V2_MASK_SET_TEMP_LIMIT; \ config_mkk_v2_send_eeprom(); \ -} + } #define config_mkk_v2_SetCurrentScaling(_v) { \ config_mkk_v2_eeprom.CurrentScaling = _v; \ config_mkk_v2_eeprom.SetMask = CONFIG_MKK_V2_MASK_SET_SAVE_EEPROM | CONFIG_MKK_V2_MASK_SET_CURRENT_SCALING; \ config_mkk_v2_send_eeprom(); \ -} + } #define config_mkk_v2_SetBitConfig(_v) { \ config_mkk_v2_eeprom.BitConfig = _v; \ config_mkk_v2_eeprom.SetMask = CONFIG_MKK_V2_MASK_SET_SAVE_EEPROM | CONFIG_MKK_V2_MASK_SET_BITCONFIG; \ config_mkk_v2_send_eeprom(); \ -} + } #define config_mkk_v2_GetConfig(_v) { \ config_mkk_v2.addr = _v; \ config_mkk_v2_read_eeprom(); \ -} + } diff --git a/sw/airborne/modules/core/pwm_meas.c b/sw/airborne/modules/core/pwm_meas.c index 0786f9cea5..9654f71414 100644 --- a/sw/airborne/modules/core/pwm_meas.c +++ b/sw/airborne/modules/core/pwm_meas.c @@ -30,7 +30,7 @@ #include "mcu_periph/pwm_input.h" #include "mcu_periph/sys_time.h" -void pwm_meas_init( void ) +void pwm_meas_init(void) { pwm_input_init(); } diff --git a/sw/airborne/modules/core/pwm_meas.h b/sw/airborne/modules/core/pwm_meas.h index 890f4091d6..f81276c46e 100644 --- a/sw/airborne/modules/core/pwm_meas.h +++ b/sw/airborne/modules/core/pwm_meas.h @@ -28,6 +28,6 @@ #ifndef PWM_MEAS_H #define PWM_MEAS_H -void pwm_meas_init( void ); +void pwm_meas_init(void); #endif diff --git a/sw/airborne/modules/core/sys_mon.c b/sw/airborne/modules/core/sys_mon.c index 1d9723a82c..18742e7a2e 100644 --- a/sw/airborne/modules/core/sys_mon.c +++ b/sw/airborne/modules/core/sys_mon.c @@ -38,7 +38,8 @@ static uint32_t sum_time_event; ///< in usec static uint32_t min_time_event; ///< in usec static uint32_t sum_n_event; -void init_sysmon(void) { +void init_sysmon(void) +{ sys_mon.cpu_load = 0; sys_mon.periodic_time = 0; sys_mon.periodic_time_min = 0xFFFF; @@ -62,7 +63,8 @@ void init_sysmon(void) { #include "messages.h" #include "subsystems/datalink/downlink.h" -void periodic_report_sysmon(void) { +void periodic_report_sysmon(void) +{ /** Report system status at low frequency */ if (n_periodic > 0) { sys_mon.periodic_time = Max(sum_time_periodic / n_periodic, 1); @@ -87,7 +89,8 @@ void periodic_report_sysmon(void) { sys_mon.periodic_cycle_max = 0; } -void periodic_sysmon(void) { +void periodic_sysmon(void) +{ /** Estimate periodic task cycle time */ uint32_t periodic_usec = SysTimeTimer(periodic_timer); SysTimeTimerStart(periodic_timer); @@ -98,16 +101,20 @@ void periodic_sysmon(void) { sum_cycle_periodic += periodic_cycle; /* remember min and max periodic times */ - if (periodic_usec < sys_mon.periodic_time_min) + if (periodic_usec < sys_mon.periodic_time_min) { sys_mon.periodic_time_min = periodic_usec; - if (periodic_usec > sys_mon.periodic_time_max) + } + if (periodic_usec > sys_mon.periodic_time_max) { sys_mon.periodic_time_max = periodic_usec; + } /* remember min and max periodic cycle times */ - if (periodic_cycle < sys_mon.periodic_cycle_min) + if (periodic_cycle < sys_mon.periodic_cycle_min) { sys_mon.periodic_cycle_min = periodic_cycle; - if (periodic_cycle > sys_mon.periodic_cycle_max) + } + if (periodic_cycle > sys_mon.periodic_cycle_max) { sys_mon.periodic_cycle_max = periodic_cycle; + } n_periodic++; sum_n_event += n_event; @@ -115,12 +122,14 @@ void periodic_sysmon(void) { sum_time_event = 0; } -void event_sysmon(void) { +void event_sysmon(void) +{ /** Store event calls total time and number of calls between two periodic calls */ if (n_event > 0) { uint32_t t = SysTimeTimer(event_timer); - if (t < min_time_event) + if (t < min_time_event) { min_time_event = t; + } sum_time_event += t; } SysTimeTimerStart(event_timer); diff --git a/sw/airborne/modules/core/trigger_ext.h b/sw/airborne/modules/core/trigger_ext.h index f099f0e8c9..d1fdf07ad7 100644 --- a/sw/airborne/modules/core/trigger_ext.h +++ b/sw/airborne/modules/core/trigger_ext.h @@ -42,7 +42,7 @@ extern uint32_t trigger_t0; extern uint32_t trigger_delta_t0; extern volatile bool_t trigger_ext_valid; -void trigger_ext_init ( void ); +void trigger_ext_init(void); #endif diff --git a/sw/airborne/modules/datalink/extra_pprz_dl.h b/sw/airborne/modules/datalink/extra_pprz_dl.h index f43ea38379..8e278c7fe9 100644 --- a/sw/airborne/modules/datalink/extra_pprz_dl.h +++ b/sw/airborne/modules/datalink/extra_pprz_dl.h @@ -36,10 +36,10 @@ extern struct pprz_transport extra_pprz_tp; /* Datalink Event */ -#define ExtraDatalinkEvent() { \ - PprzCheckAndParse(EXTRA_PPRZ_UART, extra_pprz_tp); \ - DlCheckAndParse(); \ -} +#define ExtraDatalinkEvent() { \ + PprzCheckAndParse(EXTRA_PPRZ_UART, extra_pprz_tp); \ + DlCheckAndParse(); \ + } #endif /* EXTRA_PPRZ_DL_H */ diff --git a/sw/airborne/modules/datalink/mavlink.h b/sw/airborne/modules/datalink/mavlink.h index b767f6f56d..5621f684df 100644 --- a/sw/airborne/modules/datalink/mavlink.h +++ b/sw/airborne/modules/datalink/mavlink.h @@ -71,8 +71,8 @@ void mavlink_event(void); */ static inline void comm_send_ch(mavlink_channel_t chan __attribute__((unused)), uint8_t ch) { - // Send bytes - MAVLink(Transmit(ch)); + // Send bytes + MAVLink(Transmit(ch)); } #endif // DATALINK_MAVLINK_H diff --git a/sw/airborne/modules/datalink/mavlink_decoder.c b/sw/airborne/modules/datalink/mavlink_decoder.c index c0f21a1a58..3d86c5ad17 100644 --- a/sw/airborne/modules/datalink/mavlink_decoder.c +++ b/sw/airborne/modules/datalink/mavlink_decoder.c @@ -29,7 +29,7 @@ struct mavlink_transport mavlink_tp; #ifndef MAVLINK_NO_CRC_EXTRA -uint8_t mavlink_crc_extra[256]={50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 200, 183, 0, 130, 0, 148, 21, 0, 52, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}; +uint8_t mavlink_crc_extra[256] = {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 200, 183, 0, 130, 0, 148, 21, 0, 52, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0}; #endif #if MAVLINK_DECODER_DEBUG @@ -37,7 +37,8 @@ uint8_t mavlink_crc_extra[256]={50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89 #include "messages.h" #include "subsystems/datalink/downlink.h" -void mavlink_send_debug(struct mavlink_transport * t) { +void mavlink_send_debug(struct mavlink_transport *t) +{ DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice, t->trans.payload_len, t->trans.payload); } #endif diff --git a/sw/airborne/modules/datalink/mavlink_decoder.h b/sw/airborne/modules/datalink/mavlink_decoder.h index 4fab5ccb0b..2eda365779 100644 --- a/sw/airborne/modules/datalink/mavlink_decoder.h +++ b/sw/airborne/modules/datalink/mavlink_decoder.h @@ -50,7 +50,7 @@ struct mavlink_message { uint8_t sys_id; uint8_t comp_id; uint8_t msg_id; - uint8_t* payload; + uint8_t *payload; }; #define MAVLINK_PAYLOAD_OFFSET 4 @@ -82,9 +82,9 @@ static inline void mavlink_crc_accumulate(uint8_t data, uint16_t *crcAccum) /*Accumulate one byte of data into the CRC*/ uint8_t tmp; - tmp = data ^ (uint8_t)(*crcAccum &0xff); - tmp ^= (tmp<<4); - *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); + tmp = data ^ (uint8_t)(*crcAccum & 0xff); + tmp ^= (tmp << 4); + *crcAccum = (*crcAccum >> 8) ^ (tmp << 8) ^ (tmp << 3) ^ (tmp >> 4); } /** @@ -92,7 +92,7 @@ static inline void mavlink_crc_accumulate(uint8_t data, uint16_t *crcAccum) * * @param crcAccum the 16 bit X.25 CRC */ -static inline void mavlink_crc_init(uint16_t* crcAccum) +static inline void mavlink_crc_init(uint16_t *crcAccum) { *crcAccum = X25_INIT_CRC; } @@ -104,7 +104,7 @@ static inline void mavlink_crc_init(uint16_t* crcAccum) * @param length length of the byte array * @return the checksum over the buffer bytes **/ -static inline uint16_t mavlink_crc_calculate(const uint8_t* pBuffer, uint16_t length) +static inline uint16_t mavlink_crc_calculate(const uint8_t *pBuffer, uint16_t length) { uint16_t crcTmp; mavlink_crc_init(&crcTmp); @@ -118,7 +118,7 @@ static inline uint16_t mavlink_crc_calculate(const uint8_t* pBuffer, uint16_t le // Mavlink parsing state machine typedef enum { - MAVLINK_PARSE_STATE_UNINIT=0, + MAVLINK_PARSE_STATE_UNINIT = 0, MAVLINK_PARSE_STATE_IDLE, MAVLINK_PARSE_STATE_GOT_STX, MAVLINK_PARSE_STATE_GOT_LENGTH, @@ -131,8 +131,8 @@ typedef enum { struct mavlink_msg_req { uint8_t msg_id; ///< Requested message ID struct mavlink_message msg; ///< Mavlink message - void (*callback)(struct mavlink_message * msg); ///< Callback function - struct mavlink_msg_req * next; + void (*callback)(struct mavlink_message *msg); ///< Callback function + struct mavlink_msg_req *next; }; /** Mavlink transport protocol @@ -145,7 +145,7 @@ struct mavlink_transport { uint8_t payload_idx; uint16_t checksum; // linked list of callbacks - struct mavlink_msg_req * req; + struct mavlink_msg_req *req; }; extern struct mavlink_transport mavlink_tp; @@ -155,12 +155,13 @@ extern struct mavlink_transport mavlink_tp; * * Activated with MAVLINK_DECODER_DEBUG flag */ -extern void mavlink_send_debug(struct mavlink_transport * t); +extern void mavlink_send_debug(struct mavlink_transport *t); #endif /** Register a callback for a mavlink message */ -static inline void mavlink_register_msg(struct mavlink_transport * t, struct mavlink_msg_req * req) { +static inline void mavlink_register_msg(struct mavlink_transport *t, struct mavlink_msg_req *req) +{ // handle linked list of requests req->next = t->req; t->req = req; @@ -168,7 +169,8 @@ static inline void mavlink_register_msg(struct mavlink_transport * t, struct mav /** Mavlink character parser */ -static inline void parse_mavlink(struct mavlink_transport * t, uint8_t c ) { +static inline void parse_mavlink(struct mavlink_transport *t, uint8_t c) +{ switch (t->status) { case MAVLINK_PARSE_STATE_UNINIT: t->status = MAVLINK_PARSE_STATE_IDLE; // directly go to idle state (no break) @@ -183,7 +185,8 @@ static inline void parse_mavlink(struct mavlink_transport * t, uint8_t c ) { t->trans.ovrn++; goto error; } - t->trans.payload_len = c + MAVLINK_PAYLOAD_OFFSET; /* Not Counting STX, CRC1 and CRC2, adding LENGTH, SEQ, SYSID, COMPID, MSGID */ + t->trans.payload_len = c + + MAVLINK_PAYLOAD_OFFSET; /* Not Counting STX, CRC1 and CRC2, adding LENGTH, SEQ, SYSID, COMPID, MSGID */ mavlink_crc_accumulate(c, &(t->checksum)); t->status = MAVLINK_PARSE_STATE_GOT_LENGTH; t->payload_idx = 0; @@ -192,8 +195,9 @@ static inline void parse_mavlink(struct mavlink_transport * t, uint8_t c ) { t->trans.payload[t->payload_idx] = c; mavlink_crc_accumulate(c, &(t->checksum)); t->payload_idx++; - if (t->payload_idx == t->trans.payload_len) + if (t->payload_idx == t->trans.payload_len) { t->status = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + } break; case MAVLINK_PARSE_STATE_GOT_PAYLOAD: #if MAVLINK_DECODER_DEBUG @@ -203,13 +207,15 @@ static inline void parse_mavlink(struct mavlink_transport * t, uint8_t c ) { // add extra CRC mavlink_crc_accumulate(mavlink_crc_extra[(t->trans.payload[MAVLINK_MSG_ID_IDX])], &(t->checksum)); #endif - if (c != (t->checksum & 0xFF)) + if (c != (t->checksum & 0xFF)) { goto error; + } t->status = MAVLINK_PARSE_STATE_GOT_CRC1; break; case MAVLINK_PARSE_STATE_GOT_CRC1: - if (c != (t->checksum >> 8)) + if (c != (t->checksum >> 8)) { goto error; + } t->trans.msg_received = TRUE; goto restart; default: @@ -223,9 +229,10 @@ restart: return; } -static inline void mavlink_parse_payload(struct mavlink_transport * t) { +static inline void mavlink_parse_payload(struct mavlink_transport *t) +{ uint8_t i; - struct mavlink_msg_req * el; + struct mavlink_msg_req *el; // test the linked list and call callback if needed for (el = t->req; el; el = el->next) { if (el->msg_id == t->trans.payload[MAVLINK_MSG_ID_IDX]) { @@ -235,8 +242,9 @@ static inline void mavlink_parse_payload(struct mavlink_transport * t) { el->msg.comp_id = t->trans.payload[MAVLINK_COMP_ID_IDX]; el->msg.msg_id = t->trans.payload[MAVLINK_MSG_ID_IDX]; // copy buffer - for(i = 0; i < t->trans.payload_len; i++) - el->msg.payload[i] = t->trans.payload[i+MAVLINK_PAYLOAD_OFFSET]; + for (i = 0; i < t->trans.payload_len; i++) { + el->msg.payload[i] = t->trans.payload[i + MAVLINK_PAYLOAD_OFFSET]; + } // callback el->callback(&(el->msg)); } @@ -248,14 +256,14 @@ static inline void mavlink_parse_payload(struct mavlink_transport * t) { #define ReadMavlinkBuffer(_dev,_trans) { while (TransportLink(_dev,ChAvailable())&&!(_trans.trans.msg_received)) parse_mavlink(&(_trans),TransportLink(_dev,Getch())); } #define MavlinkCheckAndParse(_dev,_trans) { \ - if (MavlinkBuffer(_dev)) { \ - ReadMavlinkBuffer(_dev,_trans); \ - if (_trans.trans.msg_received) { \ - mavlink_parse_payload(&(_trans)); \ - _trans.trans.msg_received = FALSE; \ - } \ - } \ -} + if (MavlinkBuffer(_dev)) { \ + ReadMavlinkBuffer(_dev,_trans); \ + if (_trans.trans.msg_received) { \ + mavlink_parse_payload(&(_trans)); \ + _trans.trans.msg_received = FALSE; \ + } \ + } \ + } /* Datalink Event Macro */ #define MavlinkDatalinkEvent() MavlinkCheckAndParse(MAVLINK_UART, mavlink_tp) diff --git a/sw/airborne/modules/datalink/xtend_rssi.c b/sw/airborne/modules/datalink/xtend_rssi.c index e13a4c2a50..93ffadb79f 100644 --- a/sw/airborne/modules/datalink/xtend_rssi.c +++ b/sw/airborne/modules/datalink/xtend_rssi.c @@ -41,25 +41,25 @@ #define XTEND_RSSI_PWM_ARRAY_INDEX (XTEND_RSSI_PWM_INPUT_CHANNEL - 1) -void xtend_rssi_periodic( void ) { +void xtend_rssi_periodic(void) +{ -/* get the last duty if valid then reset valid flag (this says if we got another pulse since the last one) - calculate the % and dB from the duty using datasheet specs - send the %, dB, datalink time -*/ + /* get the last duty if valid then reset valid flag (this says if we got another pulse since the last one) + calculate the % and dB from the duty using datasheet specs + send the %, dB, datalink time + */ uint32_t duty_tics = pwm_input_duty_tics[XTEND_RSSI_PWM_ARRAY_INDEX]; uint8_t duty_percent = 0; uint8_t rssi_dB_fade_margin = 0; //shows dB fade margin above rated minimum sensitivity - if (pwm_input_duty_valid[XTEND_RSSI_PWM_ARRAY_INDEX]) - { - duty_percent = (duty_tics * 100) / cpu_ticks_of_usec(XTEND_RSSI_PWM_PERIOD_USEC); - rssi_dB_fade_margin = (2 * duty_percent + 10) / 3; //not sure if this is right, datasheet isn't very informative - pwm_input_duty_valid[XTEND_RSSI_PWM_ARRAY_INDEX] = FALSE; + if (pwm_input_duty_valid[XTEND_RSSI_PWM_ARRAY_INDEX]) { + duty_percent = (duty_tics * 100) / cpu_ticks_of_usec(XTEND_RSSI_PWM_PERIOD_USEC); + rssi_dB_fade_margin = (2 * duty_percent + 10) / 3; //not sure if this is right, datasheet isn't very informative + pwm_input_duty_valid[XTEND_RSSI_PWM_ARRAY_INDEX] = FALSE; } DOWNLINK_SEND_XTEND_RSSI(DefaultChannel, DefaultDevice, - &datalink_time, - &rssi_dB_fade_margin, - &duty_percent ); + &datalink_time, + &rssi_dB_fade_margin, + &duty_percent); } diff --git a/sw/airborne/modules/datalink/xtend_rssi.h b/sw/airborne/modules/datalink/xtend_rssi.h index 1e09d6b3e8..e9521759bc 100644 --- a/sw/airborne/modules/datalink/xtend_rssi.h +++ b/sw/airborne/modules/datalink/xtend_rssi.h @@ -23,13 +23,13 @@ /** \file xtend_rssi.h * * This measures the rssi pwm signal from a Digi XTend radio modem - * and sends a message with the info. + * and sends a message with the info. * */ #ifndef XTEND_RSSI_H #define XTEND_RSSI_H -void xtend_rssi_periodic( void ); +void xtend_rssi_periodic(void); #endif \ No newline at end of file diff --git a/sw/airborne/modules/demo_module/demo_module.c b/sw/airborne/modules/demo_module/demo_module.c index c88fef4d01..99eac4c4d1 100644 --- a/sw/airborne/modules/demo_module/demo_module.c +++ b/sw/airborne/modules/demo_module/demo_module.c @@ -23,25 +23,30 @@ #include "demo_module.h" #include "led.h" -void init_demo(void) { +void init_demo(void) +{ // this part is already done by led_init in fact LED_INIT(DEMO_MODULE_LED); LED_OFF(DEMO_MODULE_LED); } -void periodic_1Hz_demo(void) { +void periodic_1Hz_demo(void) +{ LED_TOGGLE(DEMO_MODULE_LED); } -void periodic_10Hz_demo(void) { +void periodic_10Hz_demo(void) +{ LED_TOGGLE(DEMO_MODULE_LED); } -void start_demo(void) { +void start_demo(void) +{ LED_ON(DEMO_MODULE_LED); } -void stop_demo(void) { +void stop_demo(void) +{ LED_OFF(DEMO_MODULE_LED); } diff --git a/sw/airborne/modules/deploy_sonar_buoy/deploy_sonar_buoy.c b/sw/airborne/modules/deploy_sonar_buoy/deploy_sonar_buoy.c index cfab72eee4..29df594c85 100644 --- a/sw/airborne/modules/deploy_sonar_buoy/deploy_sonar_buoy.c +++ b/sw/airborne/modules/deploy_sonar_buoy/deploy_sonar_buoy.c @@ -33,25 +33,26 @@ bool_t buoy_1; bool_t buoy_2; /* initialises GPIO pins */ -void deploy_sonar_buoy_init(void) { +void deploy_sonar_buoy_init(void) +{ /* initialise peripheral clock for port C */ - RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOC, ENABLE); + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE); GPIO_InitTypeDef GPIO_InitStructure; GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(GPIOC, &GPIO_InitStructure); /* set port C pin 5 to be low */ - GPIO_WriteBit(GPIOC, GPIO_Pin_5 , Bit_RESET ); + GPIO_WriteBit(GPIOC, GPIO_Pin_5 , Bit_RESET); /* initialise peripheral clock for port B */ - RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOB, ENABLE); + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(GPIOB, &GPIO_InitStructure); /* set port B pin 0 to be low */ - GPIO_WriteBit(GPIOB, GPIO_Pin_0 , Bit_RESET ); + GPIO_WriteBit(GPIOB, GPIO_Pin_0 , Bit_RESET); /* set the variables of interest to be FALSE */ buoy_1 = FALSE; @@ -60,7 +61,8 @@ void deploy_sonar_buoy_init(void) { } /* sets GPIO pins */ -void deploy_sonar_buoy_periodic(void) { - GPIO_WriteBit(GPIOC, GPIO_Pin_5 , buoy_1 ? Bit_SET : Bit_RESET ); - GPIO_WriteBit(GPIOB, GPIO_Pin_0 , buoy_2 ? Bit_SET : Bit_RESET ); +void deploy_sonar_buoy_periodic(void) +{ + GPIO_WriteBit(GPIOC, GPIO_Pin_5 , buoy_1 ? Bit_SET : Bit_RESET); + GPIO_WriteBit(GPIOB, GPIO_Pin_0 , buoy_2 ? Bit_SET : Bit_RESET); } diff --git a/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.c b/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.c index d4b967b85d..8fed86bae6 100644 --- a/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.c +++ b/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.c @@ -62,14 +62,13 @@ void atmega_i2c_cam_ctrl_init(void) dc_init(); } -void atmega_i2c_cam_ctrl_periodic (void) +void atmega_i2c_cam_ctrl_periodic(void) { atmega_i2c_cam_ctrl_just_sent_command = 0; dc_periodic_4Hz(); // Request Status - if (atmega_i2c_cam_ctrl_just_sent_command == 0) - { + if (atmega_i2c_cam_ctrl_just_sent_command == 0) { atmega_i2c_cam_ctrl_send(DC_GET_STATUS); } } @@ -84,19 +83,17 @@ void atmega_i2c_cam_ctrl_send(uint8_t cmd) atmega_i2c_cam_ctrl_trans.buf[0] = cmd; i2c_transceive(&ATMEGA_I2C_DEV, &atmega_i2c_cam_ctrl_trans, ATMEGA_SLAVE_ADDR, 1, 1); - if (cmd == DC_SHOOT) - { + if (cmd == DC_SHOOT) { dc_send_shot_position(); } } -void atmega_i2c_cam_ctrl_event( void ) +void atmega_i2c_cam_ctrl_event(void) { - if (atmega_i2c_cam_ctrl_trans.status == I2CTransSuccess) - { + if (atmega_i2c_cam_ctrl_trans.status == I2CTransSuccess) { unsigned char cam_ret[1]; cam_ret[0] = atmega_i2c_cam_ctrl_trans.buf[0]; - RunOnceEvery(6,DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice, 1, cam_ret )); + RunOnceEvery(6, DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice, 1, cam_ret)); atmega_i2c_cam_ctrl_trans.status = I2CTransDone; } } diff --git a/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.h b/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.h index aaccdbb295..b6e71c2c6d 100644 --- a/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.h +++ b/sw/airborne/modules/digital_cam/atmega_i2c_cam_ctrl.h @@ -33,13 +33,13 @@ void atmega_i2c_cam_ctrl_event(void); void atmega_i2c_cam_ctrl_send(uint8_t cmd); // Allow commands to be set by datalink -#define ParseCameraCommand() { \ - { \ - if ( DL_PAYLOAD_COMMAND_command_length(dl_buffer) == 1){ \ - dc_send_command(DL_PAYLOAD_COMMAND_command(dl_buffer)[0]); \ - } \ - } \ -} +#define ParseCameraCommand() { \ + { \ + if ( DL_PAYLOAD_COMMAND_command_length(dl_buffer) == 1){ \ + dc_send_command(DL_PAYLOAD_COMMAND_command(dl_buffer)[0]); \ + } \ + } \ + } #endif diff --git a/sw/airborne/modules/digital_cam/catia/catia.c b/sw/airborne/modules/digital_cam/catia/catia.c index da1ed90490..2cc93adf57 100644 --- a/sw/airborne/modules/digital_cam/catia/catia.c +++ b/sw/airborne/modules/digital_cam/catia/catia.c @@ -14,7 +14,7 @@ // Search&Rescue Onboard Detection Application #define SODA "/root/develop/allthings_obc2014/src/soda/soda" -static void* handle_msg_shoot(void* ptr); +static void *handle_msg_shoot(void *ptr); static inline void send_msg_image_buffer(void); static inline void send_msg_status(void); @@ -22,7 +22,7 @@ static volatile int is_shooting, image_idx, image_count, shooting_idx, shooting_ static char image_buffer[MAX_IMAGE_BUFFERS][IMAGE_SIZE]; static pthread_mutex_t mut = PTHREAD_MUTEX_INITIALIZER; -int main(int argc, char* argv[]) +int main(int argc, char *argv[]) { pthread_t shooting_threads[MAX_PROCESSING_THREADS]; char c; @@ -66,13 +66,13 @@ int main(int argc, char* argv[]) // Shoot an image if not busy if (mora_protocol.msg_id == MORA_SHOOT) { // Parse the shoot message - union dc_shot_union* shoot = (union dc_shot_union*) malloc(sizeof(union dc_shot_union)); + union dc_shot_union *shoot = (union dc_shot_union *) malloc(sizeof(union dc_shot_union)); for (i = 0; i < MORA_SHOOT_MSG_SIZE; i++) { shoot->bin[i] = mora_protocol.payload[i]; } printf("CATIA:\tSHOT %d,%d\n", shoot->data.nr, shoot->data.phi); - pthread_create(&shooting_threads[(shooting_idx++ % MAX_PROCESSING_THREADS)], NULL, handle_msg_shoot, (void*)shoot); + pthread_create(&shooting_threads[(shooting_idx++ % MAX_PROCESSING_THREADS)], NULL, handle_msg_shoot, (void *)shoot); send_msg_status(); } @@ -101,10 +101,10 @@ int main(int argc, char* argv[]) return 0; } -static void* handle_msg_shoot(void* ptr) +static void *handle_msg_shoot(void *ptr) { char filename[MAX_FILENAME], soda_call[512]; - union dc_shot_union* shoot = (union dc_shot_union*) ptr; + union dc_shot_union *shoot = (union dc_shot_union *) ptr; // Test if can shoot pthread_mutex_lock(&mut); @@ -131,7 +131,8 @@ static void* handle_msg_shoot(void* ptr) //Parse the image sprintf(soda_call, "%s %s %d %d %d %d %d %d %d %d %d %d", SODA, filename, - shoot->data.nr, shoot->data.lat, shoot->data.lon, shoot->data.alt, shoot->data.phi, shoot->data.theta, shoot->data.psi, shoot->data.vground, shoot->data.course, shoot->data.groundalt); + shoot->data.nr, shoot->data.lat, shoot->data.lon, shoot->data.alt, shoot->data.phi, shoot->data.theta, shoot->data.psi, + shoot->data.vground, shoot->data.course, shoot->data.groundalt); printf("CATIA-%d:\tCalling '%s'\n", shoot->data.nr, soda_call); short int ret = system(soda_call); printf("CATIA-%d:\tShooting: soda return %d of image %s\n", shoot->data.nr, ret, filename); @@ -166,7 +167,7 @@ static inline void send_msg_status(void) { int i; struct mora_status_struct status_msg; - char* buffer = (char*) &status_msg; + char *buffer = (char *) &status_msg; pthread_mutex_lock(&mut); status_msg.cpu = 0; diff --git a/sw/airborne/modules/digital_cam/catia/chdk_pipe.c b/sw/airborne/modules/digital_cam/catia/chdk_pipe.c index c92def5bfb..846fae3e5a 100644 --- a/sw/airborne/modules/digital_cam/catia/chdk_pipe.c +++ b/sw/airborne/modules/digital_cam/catia/chdk_pipe.c @@ -18,12 +18,13 @@ #define SHELL "/root/develop/allthings_obc2014/src/popcorn/popcorn.sh" -const char* setup = "lua props=require(\"propcase\");print(\"SetupScript\");set_prop(props.ISO_MODE,3200);set_prop(props.FLASH_MODE,2);set_prop(props.RESOLUTION,0);set_prop(props.DATE_STAMP,0);set_prop(props.AF_ASSIST_BEAM,0);set_prop(props.QUALITY,0);print(\"Ready\");\n"; +const char *setup = + "lua props=require(\"propcase\");print(\"SetupScript\");set_prop(props.ISO_MODE,3200);set_prop(props.FLASH_MODE,2);set_prop(props.RESOLUTION,0);set_prop(props.DATE_STAMP,0);set_prop(props.AF_ASSIST_BEAM,0);set_prop(props.QUALITY,0);print(\"Ready\");\n"; static int fo, fi; static void wait_for_cmd(int timeout); -static void wait_for_img(char* filename, int timeout); -static pid_t popen2(const char* command, int* infp, int* outfp); +static void wait_for_img(char *filename, int timeout); +static pid_t popen2(const char *command, int *infp, int *outfp); /*void main(int argc, char ** argv, char ** envp) { @@ -88,7 +89,7 @@ void chdk_pipe_deinit(void) /** * Shoot an image */ -void chdk_pipe_shoot(char* filename) +void chdk_pipe_shoot(char *filename) { write(fi, "rs /root\n", 9); wait_for_img(filename, 10); @@ -98,7 +99,7 @@ void chdk_pipe_shoot(char* filename) * Wait for the image to be available * TODO: add timeout */ -static void wait_for_img(char* filename, int timeout) +static void wait_for_img(char *filename, int timeout) { int hash_cnt = 0; char ch; @@ -133,7 +134,7 @@ static void wait_for_cmd(int timeout) /** * Open a process with stdin and stdout */ -static pid_t popen2(const char* command, int* infp, int* outfp) +static pid_t popen2(const char *command, int *infp, int *outfp) { int p_stdin[2], p_stdout[2]; pid_t pid; diff --git a/sw/airborne/modules/digital_cam/catia/chdk_pipe.h b/sw/airborne/modules/digital_cam/catia/chdk_pipe.h index 501c1fff56..6ebc50e3c7 100644 --- a/sw/airborne/modules/digital_cam/catia/chdk_pipe.h +++ b/sw/airborne/modules/digital_cam/catia/chdk_pipe.h @@ -1,6 +1,6 @@ // CHDKPTP Pipe void chdk_pipe_init(void); -void chdk_pipe_shoot(char* filename); +void chdk_pipe_shoot(char *filename); void chdk_pipe_deinit(void); diff --git a/sw/airborne/modules/digital_cam/catia/protocol.c b/sw/airborne/modules/digital_cam/catia/protocol.c index 969d4ca961..778412190d 100644 --- a/sw/airborne/modules/digital_cam/catia/protocol.c +++ b/sw/airborne/modules/digital_cam/catia/protocol.c @@ -17,7 +17,7 @@ uint8_t mora_ck_a, mora_ck_b; struct mora_transport mora_protocol; -void parse_mora(struct mora_transport* t, uint8_t c) +void parse_mora(struct mora_transport *t, uint8_t c) { //printf("%02X %d %d\n",c, t->status, t->error); diff --git a/sw/airborne/modules/digital_cam/catia/protocol.h b/sw/airborne/modules/digital_cam/catia/protocol.h index da31001ef9..0b976d179b 100644 --- a/sw/airborne/modules/digital_cam/catia/protocol.h +++ b/sw/airborne/modules/digital_cam/catia/protocol.h @@ -141,7 +141,7 @@ struct mora_transport { extern struct mora_transport mora_protocol; -void parse_mora(struct mora_transport* t, uint8_t c); +void parse_mora(struct mora_transport *t, uint8_t c); #endif diff --git a/sw/airborne/modules/digital_cam/catia/serial.c b/sw/airborne/modules/digital_cam/catia/serial.c index d82ba5fdec..7bc6538541 100644 --- a/sw/airborne/modules/digital_cam/catia/serial.c +++ b/sw/airborne/modules/digital_cam/catia/serial.c @@ -25,7 +25,7 @@ int fd; /* File descriptor for the port */ -int serial_init(char* port_name) +int serial_init(char *port_name) { struct termios orig_termios, cur_termios; diff --git a/sw/airborne/modules/digital_cam/catia/serial.h b/sw/airborne/modules/digital_cam/catia/serial.h index 1ce61a441d..5dead8683b 100644 --- a/sw/airborne/modules/digital_cam/catia/serial.h +++ b/sw/airborne/modules/digital_cam/catia/serial.h @@ -4,7 +4,7 @@ extern int fd; #include "std.h" -int serial_init(char* port_name); +int serial_init(char *port_name); static inline int ttyUSB0ChAvailable(void) { diff --git a/sw/airborne/modules/digital_cam/catia/socket.c b/sw/airborne/modules/digital_cam/catia/socket.c index 8d4f0615d4..b9cf9e3be8 100644 --- a/sw/airborne/modules/digital_cam/catia/socket.c +++ b/sw/airborne/modules/digital_cam/catia/socket.c @@ -31,21 +31,21 @@ void socket_init(int is_server) inet_aton("127.0.0.1", &socket_server.sin_addr); if (is_server) { - if (bind(socket_fd, (struct sockaddr*)&socket_server, sizeof(socket_server)) != 0) { + if (bind(socket_fd, (struct sockaddr *)&socket_server, sizeof(socket_server)) != 0) { perror("Socket: bind"); exit(1); } } } -int socket_recv(char* buffer, int len) +int socket_recv(char *buffer, int len) { socklen_t slen = sizeof(socket_server); - return recvfrom(socket_fd, buffer, len, MSG_DONTWAIT, (struct sockaddr*)&socket_server, &slen); + return recvfrom(socket_fd, buffer, len, MSG_DONTWAIT, (struct sockaddr *)&socket_server, &slen); } -void socket_send(char* buffer, int len) +void socket_send(char *buffer, int len) { socklen_t slen = sizeof(socket_server); - sendto(socket_fd, buffer, len, MSG_DONTWAIT, (struct sockaddr*)&socket_server, slen); + sendto(socket_fd, buffer, len, MSG_DONTWAIT, (struct sockaddr *)&socket_server, slen); } diff --git a/sw/airborne/modules/digital_cam/catia/socket.h b/sw/airborne/modules/digital_cam/catia/socket.h index 542a208765..b0938c252a 100644 --- a/sw/airborne/modules/digital_cam/catia/socket.h +++ b/sw/airborne/modules/digital_cam/catia/socket.h @@ -1,4 +1,4 @@ void socket_init(int is_server); -int socket_recv(char* buffer, int len); -void socket_send(char* buffer, int len); +int socket_recv(char *buffer, int len); +void socket_send(char *buffer, int len); diff --git a/sw/airborne/modules/digital_cam/dc.c b/sw/airborne/modules/digital_cam/dc.c index e91130e78a..386011eb54 100644 --- a/sw/airborne/modules/digital_cam/dc.c +++ b/sw/airborne/modules/digital_cam/dc.c @@ -95,9 +95,9 @@ uint16_t dc_photo_nr = 0; void dc_send_shot_position(void) { // angles in decideg - int16_t phi = DegOfRad(stateGetNedToBodyEulers_f()->phi*10.0f); - int16_t theta = DegOfRad(stateGetNedToBodyEulers_f()->theta*10.0f); - int16_t psi = DegOfRad(stateGetNedToBodyEulers_f()->psi*10.0f); + int16_t phi = DegOfRad(stateGetNedToBodyEulers_f()->phi * 10.0f); + int16_t theta = DegOfRad(stateGetNedToBodyEulers_f()->theta * 10.0f); + int16_t psi = DegOfRad(stateGetNedToBodyEulers_f()->psi * 10.0f); // course in decideg int16_t course = DegOfRad(*stateGetHorizontalSpeedDir_f()) * 10; // ground speed in cm/s @@ -135,7 +135,8 @@ void dc_init(void) dc_distance_interval = DC_AUTOSHOOT_DISTANCE_INTERVAL; } -uint8_t dc_info(void) { +uint8_t dc_info(void) +{ #ifdef DOWNLINK_SEND_DC_INFO float course = DegOfRad(stateGetNedToBodyEulers_f()->psi); int16_t mode = dc_autoshoot; @@ -160,7 +161,8 @@ uint8_t dc_info(void) { } /* shoot on distance */ -uint8_t dc_distance(float interval) { +uint8_t dc_distance(float interval) +{ dc_autoshoot = DC_AUTOSHOOT_DISTANCE; dc_gps_count = 0; dc_distance_interval = interval; @@ -172,27 +174,30 @@ uint8_t dc_distance(float interval) { } /* shoot on circle */ -uint8_t dc_circle(float interval, float start) { +uint8_t dc_circle(float interval, float start) +{ dc_autoshoot = DC_AUTOSHOOT_CIRCLE; dc_gps_count = 0; dc_circle_interval = fmodf(fmaxf(interval, 1.), 360.); - if(start == DC_IGNORE) { + if (start == DC_IGNORE) { start = DegOfRad(stateGetNedToBodyEulers_f()->psi); } dc_circle_start_angle = fmodf(start, 360.); - if (start < 0.) + if (start < 0.) { start += 360.; + } //dc_circle_last_block = floorf(dc_circle_start_angle/dc_circle_interval); dc_circle_last_block = 0; - dc_circle_max_blocks = floorf(360./dc_circle_interval); + dc_circle_max_blocks = floorf(360. / dc_circle_interval); dc_info(); return 0; } /* shoot on survey */ -uint8_t dc_survey(float interval, float x, float y) { +uint8_t dc_survey(float interval, float x, float y) +{ dc_autoshoot = DC_AUTOSHOOT_SURVEY; dc_gps_count = 0; dc_survey_interval = interval; @@ -213,19 +218,21 @@ uint8_t dc_survey(float interval, float x, float y) { return 0; } -uint8_t dc_stop(void) { +uint8_t dc_stop(void) +{ dc_autoshoot = DC_AUTOSHOOT_STOP; dc_info(); return 0; } -static float dim_mod(float a, float b, float m) { +static float dim_mod(float a, float b, float m) +{ if (a < b) { float tmp = a; a = b; b = tmp; } - return fminf(a-b, b+m-a); + return fminf(a - b, b + m - a); } void dc_periodic_4Hz(void) @@ -243,49 +250,47 @@ void dc_periodic_4Hz(void) } break; - case DC_AUTOSHOOT_DISTANCE: - { - struct FloatVect2 cur_pos; - cur_pos.x = stateGetPositionEnu_f()->x; - cur_pos.y = stateGetPositionEnu_f()->y; - struct FloatVect2 delta_pos; - VECT2_DIFF(delta_pos, cur_pos, last_shot_pos); - float dist_to_last_shot = float_vect2_norm(&delta_pos); - if (dist_to_last_shot > dc_distance_interval) { - dc_gps_count++; - dc_send_command(DC_SHOOT); - VECT2_COPY(last_shot_pos, cur_pos); - } + case DC_AUTOSHOOT_DISTANCE: { + struct FloatVect2 cur_pos; + cur_pos.x = stateGetPositionEnu_f()->x; + cur_pos.y = stateGetPositionEnu_f()->y; + struct FloatVect2 delta_pos; + VECT2_DIFF(delta_pos, cur_pos, last_shot_pos); + float dist_to_last_shot = float_vect2_norm(&delta_pos); + if (dist_to_last_shot > dc_distance_interval) { + dc_gps_count++; + dc_send_command(DC_SHOOT); + VECT2_COPY(last_shot_pos, cur_pos); } - break; + } + break; - case DC_AUTOSHOOT_CIRCLE: - { - float course = DegOfRad(stateGetNedToBodyEulers_f()->psi) - dc_circle_start_angle; - if (course < 0.) - course += 360.; - float current_block = floorf(course/dc_circle_interval); - - if (dim_mod(current_block, dc_circle_last_block, dc_circle_max_blocks) == 1) { - dc_gps_count++; - dc_circle_last_block = current_block; - dc_send_command(DC_SHOOT); - } + case DC_AUTOSHOOT_CIRCLE: { + float course = DegOfRad(stateGetNedToBodyEulers_f()->psi) - dc_circle_start_angle; + if (course < 0.) { + course += 360.; } - break; + float current_block = floorf(course / dc_circle_interval); - case DC_AUTOSHOOT_SURVEY: - { - float dist_x = dc_gps_x - stateGetPositionEnu_f()->x; - float dist_y = dc_gps_y - stateGetPositionEnu_f()->y; - - if (dist_x*dist_x + dist_y*dist_y >= dc_gps_next_dist*dc_gps_next_dist) { - dc_gps_next_dist += dc_survey_interval; - dc_gps_count++; - dc_send_command(DC_SHOOT); - } + if (dim_mod(current_block, dc_circle_last_block, dc_circle_max_blocks) == 1) { + dc_gps_count++; + dc_circle_last_block = current_block; + dc_send_command(DC_SHOOT); } - break; + } + break; + + case DC_AUTOSHOOT_SURVEY: { + float dist_x = dc_gps_x - stateGetPositionEnu_f()->x; + float dist_y = dc_gps_y - stateGetPositionEnu_f()->y; + + if (dist_x * dist_x + dist_y * dist_y >= dc_gps_next_dist * dc_gps_next_dist) { + dc_gps_next_dist += dc_survey_interval; + dc_gps_count++; + dc_send_command(DC_SHOOT); + } + } + break; default: dc_autoshoot = DC_AUTOSHOOT_STOP; diff --git a/sw/airborne/modules/digital_cam/dc_shoot_rc.c b/sw/airborne/modules/digital_cam/dc_shoot_rc.c index 1cf398a2ab..df7e9470e3 100644 --- a/sw/airborne/modules/digital_cam/dc_shoot_rc.c +++ b/sw/airborne/modules/digital_cam/dc_shoot_rc.c @@ -47,8 +47,7 @@ void dc_shoot_rc_periodic(void) } if ((rd_shoot == 1) && (rd_num < 4)) { rd_num = rd_num + 1; - } - else { + } else { rd_num = 0; rd_shoot = 0; } diff --git a/sw/airborne/modules/digital_cam/gpio_cam_ctrl.c b/sw/airborne/modules/digital_cam/gpio_cam_ctrl.c index 44b0a8c68b..c03e558a0b 100644 --- a/sw/airborne/modules/digital_cam/gpio_cam_ctrl.c +++ b/sw/airborne/modules/digital_cam/gpio_cam_ctrl.c @@ -96,10 +96,10 @@ void gpio_cam_ctrl_init(void) #endif } -void gpio_cam_ctrl_periodic( void ) +void gpio_cam_ctrl_periodic(void) { #ifdef DC_SHOOT_ON_BUTTON_RELEASE - if (dc_timer==1) { + if (dc_timer == 1) { dc_send_shot_position(); } #endif @@ -130,8 +130,7 @@ void gpio_cam_ctrl_periodic( void ) void dc_send_command(uint8_t cmd) { dc_timer = DC_SHUTTER_DELAY; - switch (cmd) - { + switch (cmd) { case DC_SHOOT: DC_PUSH(DC_SHUTTER_GPIO); #ifndef DC_SHOOT_ON_BUTTON_RELEASE diff --git a/sw/airborne/modules/digital_cam/hackhd.c b/sw/airborne/modules/digital_cam/hackhd.c index 2754e89f44..a493baa4de 100644 --- a/sw/airborne/modules/digital_cam/hackhd.c +++ b/sw/airborne/modules/digital_cam/hackhd.c @@ -91,9 +91,9 @@ static inline uint16_t pin_of_gpio(uint32_t __attribute__((unused)) port, uint16 static inline void hackhd_send_shot_position(void) { // angles in decideg - int16_t phi = DegOfRad(stateGetNedToBodyEulers_f()->phi*10.0f); - int16_t theta = DegOfRad(stateGetNedToBodyEulers_f()->theta*10.0f); - int16_t psi = DegOfRad(stateGetNedToBodyEulers_f()->psi*10.0f); + int16_t phi = DegOfRad(stateGetNedToBodyEulers_f()->phi * 10.0f); + int16_t theta = DegOfRad(stateGetNedToBodyEulers_f()->theta * 10.0f); + int16_t psi = DegOfRad(stateGetNedToBodyEulers_f()->psi * 10.0f); // course in decideg int16_t course = DegOfRad(*stateGetHorizontalSpeedDir_f()) * 10; // ground speed in cm/s @@ -129,21 +129,22 @@ static inline void hackhd_log_shot_position(void) struct EnuCoor_f pos = *stateGetPositionEnu_f(); uint32_t time = get_sys_time_msec(); sdLogWriteLog(&pprzLogFile, "%d %d %d %d %d %d %d %u\n", - hackhd.photo_nr, - (int32_t)(DegOfRad(att.phi*10.0f)), - (int32_t)(DegOfRad(att.theta*10.0f)), - (int32_t)(DegOfRad(att.psi*10.0f)), - (int32_t)(pos.x*100.0f), - (int32_t)(pos.y*100.0f), - (int32_t)(pos.z*100.0f), - time); + hackhd.photo_nr, + (int32_t)(DegOfRad(att.phi * 10.0f)), + (int32_t)(DegOfRad(att.theta * 10.0f)), + (int32_t)(DegOfRad(att.psi * 10.0f)), + (int32_t)(pos.x * 100.0f), + (int32_t)(pos.y * 100.0f), + (int32_t)(pos.z * 100.0f), + time); } } #endif struct HackHD hackhd; -void hackhd_init(void) { +void hackhd_init(void) +{ hackhd.status = HACKHD_NONE; hackhd.timer = 0; hackhd.photo_nr = 0; @@ -154,15 +155,16 @@ void hackhd_init(void) { gpio_setup_output(HACKHD_GPIO); // set gpio as open-drain, only possible on stm32f4 gpio_set_output_options( - port_of_gpio(HACKHD_GPIO), - GPIO_OTYPE_OD, - GPIO_OSPEED_25MHZ, - pin_of_gpio(HACKHD_GPIO)); + port_of_gpio(HACKHD_GPIO), + GPIO_OTYPE_OD, + GPIO_OSPEED_25MHZ, + pin_of_gpio(HACKHD_GPIO)); HACKHD_RELEASE(HACKHD_GPIO); #endif } -void hackhd_periodic( void ) { +void hackhd_periodic(void) +{ if (hackhd.timer) { hackhd.timer--; } else { @@ -190,10 +192,10 @@ void hackhd_periodic( void ) { } /* Command the powering and recording */ -void hackhd_command(enum hackhd_status cmd) { +void hackhd_command(enum hackhd_status cmd) +{ hackhd.status = cmd; - switch (cmd) - { + switch (cmd) { case HACKHD_POWER_ON: case HACKHD_POWER_OFF: hackhd.timer = HACKHD_TIMER_OF_DELAY(HACKHD_POWER_DELAY); @@ -216,12 +218,12 @@ void hackhd_command(enum hackhd_status cmd) { } } -void hackhd_autoshoot(void) { - // at least wait a minimum time before two shoots +void hackhd_autoshoot(void) +{ +// at least wait a minimum time before two shoots if (hackhd.autoshoot) { hackhd.autoshoot--; - } - else { + } else { // test distance if needed // or take picture if first of the sequence #ifdef HACKHD_AUTOSHOOT_DIST @@ -229,7 +231,7 @@ void hackhd_autoshoot(void) { struct FloatVect2 d_pos; d_pos.x = pos.x - hackhd.last_shot_pos.x; d_pos.y = pos.y - hackhd.last_shot_pos.y; - if (VECT2_NORM2(d_pos) > (HACKHD_AUTOSHOOT_DIST*HACKHD_AUTOSHOOT_DIST) + if (VECT2_NORM2(d_pos) > (HACKHD_AUTOSHOOT_DIST * HACKHD_AUTOSHOOT_DIST) || hackhd.status == HACKHD_AUTOSHOOT_START) { #endif // take a picture @@ -242,7 +244,8 @@ void hackhd_autoshoot(void) { } } -void hackhd_autoshoot_start(void) { +void hackhd_autoshoot_start(void) +{ // start taking a picture immediately hackhd.autoshoot = 0; hackhd.status = HACKHD_AUTOSHOOT_START; diff --git a/sw/airborne/modules/digital_cam/servo_cam_ctrl.c b/sw/airborne/modules/digital_cam/servo_cam_ctrl.c index c5a073ef18..b95530a85b 100644 --- a/sw/airborne/modules/digital_cam/servo_cam_ctrl.c +++ b/sw/airborne/modules/digital_cam/servo_cam_ctrl.c @@ -29,8 +29,7 @@ uint8_t dc_timer; void dc_send_command(uint8_t cmd) { dc_timer = DC_SHUTTER_DELAY; - switch (cmd) - { + switch (cmd) { case DC_SHOOT: DC_PUSH(DC_SHUTTER_SERVO); #ifndef DC_SHOOT_ON_BUTTON_RELEASE diff --git a/sw/airborne/modules/digital_cam/servo_cam_ctrl.h b/sw/airborne/modules/digital_cam/servo_cam_ctrl.h index 739d6471c9..c2c1af2ee9 100644 --- a/sw/airborne/modules/digital_cam/servo_cam_ctrl.h +++ b/sw/airborne/modules/digital_cam/servo_cam_ctrl.h @@ -58,8 +58,8 @@ static inline void servo_cam_ctrl_init(void) dc_timer = 0; } -#define DC_PUSH(X) ap_state->commands[X] = -MAX_PPRZ; -#define DC_RELEASE(X) ap_state->commands[X] = MAX_PPRZ; +#define DC_PUSH(X) ap_state->commands[X] = -MAX_PPRZ; +#define DC_RELEASE(X) ap_state->commands[X] = MAX_PPRZ; #ifndef DC_SHUTTER_DELAY #define DC_SHUTTER_DELAY 2 /* 4Hz -> 0.5s */ @@ -71,10 +71,10 @@ static inline void servo_cam_ctrl_init(void) /* 4Hz Periodic */ -static inline void servo_cam_ctrl_periodic( void ) +static inline void servo_cam_ctrl_periodic(void) { #ifdef DC_SHOOT_ON_BUTTON_RELEASE - if (dc_timer==1) { + if (dc_timer == 1) { dc_send_shot_position(); } #endif diff --git a/sw/airborne/modules/digital_cam/sim_i2c_cam_ctrl.c b/sw/airborne/modules/digital_cam/sim_i2c_cam_ctrl.c index 59d64a2a9d..111030df1b 100644 --- a/sw/airborne/modules/digital_cam/sim_i2c_cam_ctrl.c +++ b/sw/airborne/modules/digital_cam/sim_i2c_cam_ctrl.c @@ -41,7 +41,7 @@ void atmega_i2c_cam_ctrl_init(void) dc_init(); } -void atmega_i2c_cam_ctrl_periodic (void) +void atmega_i2c_cam_ctrl_periodic(void) { dc_periodic_4Hz(); @@ -57,31 +57,25 @@ void atmega_i2c_cam_ctrl_send(uint8_t cmd) static uint8_t mode = 0; unsigned char cam_ret[1]; - if (cmd == DC_SHOOT) - { + if (cmd == DC_SHOOT) { dc_send_shot_position(); - } - else if (cmd == DC_TALLER) - { + } else if (cmd == DC_TALLER) { zoom = 1; - } - else if (cmd == DC_WIDER) - { + } else if (cmd == DC_WIDER) { zoom = 0; - } - else if (cmd == DC_GET_STATUS) - { + } else if (cmd == DC_GET_STATUS) { mode++; - if (mode > 15) + if (mode > 15) { mode = 0; + } } cam_ret[0] = mode + zoom * 0x20; - RunOnceEvery(6,DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice, 1, cam_ret )); + RunOnceEvery(6, DOWNLINK_SEND_PAYLOAD(DefaultChannel, DefaultDevice, 1, cam_ret)); } -void atmega_i2c_cam_ctrl_event( void ) +void atmega_i2c_cam_ctrl_event(void) { } diff --git a/sw/airborne/modules/display/lcd_dogm.c b/sw/airborne/modules/display/lcd_dogm.c index 82ded69027..a549225994 100644 --- a/sw/airborne/modules/display/lcd_dogm.c +++ b/sw/airborne/modules/display/lcd_dogm.c @@ -38,26 +38,29 @@ #include "lcd_dogm.h" -void lcd_cmd(uint8_t command) { +void lcd_cmd(uint8_t command) +{ uint32_t i; - for (i=0;i<20000;i++); + for (i = 0; i < 20000; i++); lcddogmSelect(); lcddogmCmdMode(); lcd_spi_tx(command); } -void lcd_data(uint8_t data) { +void lcd_data(uint8_t data) +{ uint32_t i; - for (i=0;i<100000;i++); + for (i = 0; i < 100000; i++); lcddogmSelect(); lcddogmDataMode(); lcd_spi_tx(data); } -void lcd_dogm_init( void ) { +void lcd_dogm_init(void) +{ uint32_t i; - for (i=0;i<100000;i++); + for (i = 0; i < 100000; i++); lcd_dogm_init_hw(); /* Write configuration */ diff --git a/sw/airborne/modules/display/lcd_dogm.h b/sw/airborne/modules/display/lcd_dogm.h index 17f68f753d..7a59477237 100644 --- a/sw/airborne/modules/display/lcd_dogm.h +++ b/sw/airborne/modules/display/lcd_dogm.h @@ -7,19 +7,19 @@ #include "led.h" /* EA DOGM163, 3 line LCD at 3.3V */ -#define DOGM_FUN_SET_1 0x39 +#define DOGM_FUN_SET_1 0x39 #define DOGM_BIAS_SET 0x15 #define DOGM_PWR_CTRL 0x55 #define DOGM_FOLLOWER 0x6E #define DOGM_CONTRAST 0x70 -#define DOGM_FUN_SET_2 0x38 -#define DOGM_DISP_ON 0x0C -#define DOGM_CLEAR 0x01 -#define DOGM_ENTRY_MODE 0x06 +#define DOGM_FUN_SET_2 0x38 +#define DOGM_DISP_ON 0x0C +#define DOGM_CLEAR 0x01 +#define DOGM_ENTRY_MODE 0x06 extern void lcd_cmd(uint8_t command); extern void lcd_data(uint8_t data); -extern void lcd_dogm_init( void ); +extern void lcd_dogm_init(void); #endif /* LCD_DOGM_H */ diff --git a/sw/airborne/modules/enose/anemotaxis.c b/sw/airborne/modules/enose/anemotaxis.c index 559f2a9b7e..ba9589009f 100644 --- a/sw/airborne/modules/enose/anemotaxis.c +++ b/sw/airborne/modules/enose/anemotaxis.c @@ -12,31 +12,35 @@ static enum status status; static int8_t sign; static struct point last_plume; -static void last_plume_was_here( void ) { +static void last_plume_was_here(void) +{ last_plume.x = stateGetPositionEnu_f()->x; last_plume.y = stateGetPositionEnu_f()->y; } -bool_t nav_anemotaxis_downwind( uint8_t c, float radius ) { - struct FloatVect2* wind = stateGetHorizontalWindspeed_f(); +bool_t nav_anemotaxis_downwind(uint8_t c, float radius) +{ + struct FloatVect2 *wind = stateGetHorizontalWindspeed_f(); float wind_dir = atan2(wind->x, wind->y); - waypoints[c].x = waypoints[WP_HOME].x + radius*cos(wind_dir); - waypoints[c].y = waypoints[WP_HOME].y + radius*sin(wind_dir); + waypoints[c].x = waypoints[WP_HOME].x + radius * cos(wind_dir); + waypoints[c].y = waypoints[WP_HOME].y + radius * sin(wind_dir); return FALSE; } -bool_t nav_anemotaxis_init( uint8_t c ) { +bool_t nav_anemotaxis_init(uint8_t c) +{ status = UTURN; sign = 1; - struct FloatVect2* wind = stateGetHorizontalWindspeed_f(); + struct FloatVect2 *wind = stateGetHorizontalWindspeed_f(); float wind_dir = atan2(wind->x, wind->y); - waypoints[c].x = stateGetPositionEnu_f()->x + DEFAULT_CIRCLE_RADIUS*cos(wind_dir+M_PI); - waypoints[c].y = stateGetPositionEnu_f()->y + DEFAULT_CIRCLE_RADIUS*sin(wind_dir+M_PI); + waypoints[c].x = stateGetPositionEnu_f()->x + DEFAULT_CIRCLE_RADIUS * cos(wind_dir + M_PI); + waypoints[c].y = stateGetPositionEnu_f()->y + DEFAULT_CIRCLE_RADIUS * sin(wind_dir + M_PI); last_plume_was_here(); return FALSE; } -bool_t nav_anemotaxis( uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume ) { +bool_t nav_anemotaxis(uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume) +{ if (chemo_sensor) { last_plume_was_here(); waypoints[plume].x = stateGetPositionEnu_f()->x; @@ -44,7 +48,7 @@ bool_t nav_anemotaxis( uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume ) { // DownlinkSendWp(plume); } - struct FloatVect2* wind = stateGetHorizontalWindspeed_f(); + struct FloatVect2 *wind = stateGetHorizontalWindspeed_f(); float wind_dir = atan2(wind->x, wind->y) + M_PI; /** Not null even if wind_east=wind_north=0 */ @@ -52,51 +56,52 @@ bool_t nav_anemotaxis( uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume ) { float upwind_y = sin(wind_dir); switch (status) { - case UTURN: - NavCircleWaypoint(c, DEFAULT_CIRCLE_RADIUS*sign); - if (NavQdrCloseTo(DegOfRad(M_PI_2-wind_dir))) { - float crosswind_x = - upwind_y; - float crosswind_y = upwind_x; - waypoints[c1].x = waypoints[c].x + DEFAULT_CIRCLE_RADIUS*upwind_x; - waypoints[c1].y = waypoints[c].y + DEFAULT_CIRCLE_RADIUS*upwind_y; + case UTURN: + NavCircleWaypoint(c, DEFAULT_CIRCLE_RADIUS * sign); + if (NavQdrCloseTo(DegOfRad(M_PI_2 - wind_dir))) { + float crosswind_x = - upwind_y; + float crosswind_y = upwind_x; + waypoints[c1].x = waypoints[c].x + DEFAULT_CIRCLE_RADIUS * upwind_x; + waypoints[c1].y = waypoints[c].y + DEFAULT_CIRCLE_RADIUS * upwind_y; - float width = Max(2*ScalarProduct(upwind_x, upwind_y, stateGetPositionEnu_f()->x-last_plume.x, stateGetPositionEnu_f()->y-last_plume.y), DEFAULT_CIRCLE_RADIUS); + float width = Max(2 * ScalarProduct(upwind_x, upwind_y, stateGetPositionEnu_f()->x - last_plume.x, + stateGetPositionEnu_f()->y - last_plume.y), DEFAULT_CIRCLE_RADIUS); - waypoints[c2].x = waypoints[c1].x - width*crosswind_x*sign; - waypoints[c2].y = waypoints[c1].y - width*crosswind_y*sign; + waypoints[c2].x = waypoints[c1].x - width * crosswind_x * sign; + waypoints[c2].y = waypoints[c1].y - width * crosswind_y * sign; - // DownlinkSendWp(c1); - // DownlinkSendWp(c2); + // DownlinkSendWp(c1); + // DownlinkSendWp(c2); - status = CROSSWIND; - nav_init_stage(); - } - break; + status = CROSSWIND; + nav_init_stage(); + } + break; - case CROSSWIND: - NavSegment(c1, c2); - if (NavApproaching(c2, CARROT)) { - waypoints[c].x = waypoints[c2].x + DEFAULT_CIRCLE_RADIUS*upwind_x; - waypoints[c].y = waypoints[c2].y + DEFAULT_CIRCLE_RADIUS*upwind_y; + case CROSSWIND: + NavSegment(c1, c2); + if (NavApproaching(c2, CARROT)) { + waypoints[c].x = waypoints[c2].x + DEFAULT_CIRCLE_RADIUS * upwind_x; + waypoints[c].y = waypoints[c2].y + DEFAULT_CIRCLE_RADIUS * upwind_y; - // DownlinkSendWp(c); + // DownlinkSendWp(c); - sign = -sign; - status = UTURN; - nav_init_stage(); - } + sign = -sign; + status = UTURN; + nav_init_stage(); + } - if (chemo_sensor) { - waypoints[c].x = stateGetPositionEnu_f()->x + DEFAULT_CIRCLE_RADIUS*upwind_x; - waypoints[c].y = stateGetPositionEnu_f()->y + DEFAULT_CIRCLE_RADIUS*upwind_y; + if (chemo_sensor) { + waypoints[c].x = stateGetPositionEnu_f()->x + DEFAULT_CIRCLE_RADIUS * upwind_x; + waypoints[c].y = stateGetPositionEnu_f()->y + DEFAULT_CIRCLE_RADIUS * upwind_y; - // DownlinkSendWp(c); + // DownlinkSendWp(c); - sign = -sign; - status = UTURN; - nav_init_stage(); - } - break; + sign = -sign; + status = UTURN; + nav_init_stage(); + } + break; } chemo_sensor = 0; return TRUE; diff --git a/sw/airborne/modules/enose/chemo_detect.c b/sw/airborne/modules/enose/chemo_detect.c index e3e3e89991..5cd83b3329 100644 --- a/sw/airborne/modules/enose/chemo_detect.c +++ b/sw/airborne/modules/enose/chemo_detect.c @@ -6,11 +6,13 @@ uint16_t chemo_sensor; -void chemo_init( void ) { +void chemo_init(void) +{ chemo_sensor = 0; } -void chemo_periodic( void ) { +void chemo_periodic(void) +{ #ifdef ENOSE static uint16_t vals[ENOSE_NB_SENSOR][DETECT_PERIOD]; static int idx; @@ -19,15 +21,18 @@ void chemo_periodic( void ) { int dval = enose_val[0] - vals[0][idx]; if (dval < -THRESHOLD) { chemo_sensor = -dval; - } else + } else { chemo_sensor = 0; + } int i; - for(i = 0; i < ENOSE_NB_SENSOR; i++) + for (i = 0; i < ENOSE_NB_SENSOR; i++) { vals[i][idx] = enose_val[i]; + } idx++; - if (idx > DETECT_PERIOD) + if (idx > DETECT_PERIOD) { idx = 0; + } #endif /* ENOSE */ } diff --git a/sw/airborne/modules/enose/chemo_detect.h b/sw/airborne/modules/enose/chemo_detect.h index b0a5e1062c..02451941c4 100644 --- a/sw/airborne/modules/enose/chemo_detect.h +++ b/sw/airborne/modules/enose/chemo_detect.h @@ -6,7 +6,7 @@ extern uint16_t chemo_sensor; #define MAX_CHEMO 400 -void chemo_init( void ); -void chemo_periodic( void ); +void chemo_init(void); +void chemo_periodic(void); #endif /* CHEMO_DETECT_H */ diff --git a/sw/airborne/modules/enose/chemotaxis.c b/sw/airborne/modules/enose/chemotaxis.c index cff12726e5..4c941010af 100644 --- a/sw/airborne/modules/enose/chemotaxis.c +++ b/sw/airborne/modules/enose/chemotaxis.c @@ -15,7 +15,8 @@ static uint8_t last_plume_value; static float radius; static int8_t sign; -bool_t nav_chemotaxis_init( uint8_t c, uint8_t plume ) { +bool_t nav_chemotaxis_init(uint8_t c, uint8_t plume) +{ radius = MAX_RADIUS; last_plume_value = 0; sign = 1; @@ -24,7 +25,8 @@ bool_t nav_chemotaxis_init( uint8_t c, uint8_t plume ) { return FALSE; } -bool_t nav_chemotaxis( uint8_t c, uint8_t plume ) { +bool_t nav_chemotaxis(uint8_t c, uint8_t plume) +{ if (chemo_sensor > last_plume_value) { /* Move the circle in this direction */ @@ -36,10 +38,11 @@ bool_t nav_chemotaxis( uint8_t c, uint8_t plume ) { /* Turn in the right direction */ float dir_x = cos(M_PI_2 - (*stateGetHorizontalSpeedDir_f())); float dir_y = sin(M_PI_2 - (*stateGetHorizontalSpeedDir_f())); - float pvect = dir_x*y - dir_y*x; + float pvect = dir_x * y - dir_y * x; sign = (pvect > 0 ? -1 : 1); /* Reduce the radius */ - radius = sign * (DEFAULT_CIRCLE_RADIUS+(MAX_CHEMO-chemo_sensor)/(float)MAX_CHEMO*(MAX_RADIUS-DEFAULT_CIRCLE_RADIUS)); + radius = sign * (DEFAULT_CIRCLE_RADIUS + (MAX_CHEMO - chemo_sensor) / (float)MAX_CHEMO * + (MAX_RADIUS - DEFAULT_CIRCLE_RADIUS)); /* Store this plume */ diff --git a/sw/airborne/modules/enose/chemotaxis.h b/sw/airborne/modules/enose/chemotaxis.h index 05b2d93ba9..ad0c26e587 100644 --- a/sw/airborne/modules/enose/chemotaxis.h +++ b/sw/airborne/modules/enose/chemotaxis.h @@ -3,7 +3,7 @@ #include "std.h" -extern bool_t nav_chemotaxis_init( uint8_t c, uint8_t plume ); -extern bool_t nav_chemotaxis( uint8_t c, uint8_t plume ); +extern bool_t nav_chemotaxis_init(uint8_t c, uint8_t plume); +extern bool_t nav_chemotaxis(uint8_t c, uint8_t plume); #endif /** CHEMOTAXIS_H */ diff --git a/sw/airborne/modules/enose/enose.c b/sw/airborne/modules/enose/enose.c index d5828070ca..509689729e 100644 --- a/sw/airborne/modules/enose/enose.c +++ b/sw/airborne/modules/enose/enose.c @@ -22,9 +22,10 @@ static volatile bool_t enose_i2c_done; static struct adc_buf buf_PID; -void enose_init( void ) { +void enose_init(void) +{ uint8_t i; - for (i=0; i< ENOSE_NB_SENSOR; i++) { + for (i = 0; i < ENOSE_NB_SENSOR; i++) { enose_heat[i] = ENOSE_HEAT_INIT; enose_val[i] = 0; } @@ -38,7 +39,8 @@ void enose_init( void ) { } -void enose_set_heat(uint8_t no_sensor, uint8_t value) { +void enose_set_heat(uint8_t no_sensor, uint8_t value) +{ enose_heat[no_sensor] = value; enose_conf_requested = TRUE; } @@ -48,41 +50,43 @@ void enose_set_heat(uint8_t no_sensor, uint8_t value) { #include "messages.h" #include "subsystems/datalink/downlink.h" -void enose_periodic( void ) { +void enose_periodic(void) +{ enose_PID_val = buf_PID.sum / buf_PID.av_nb_sample; if (enose_i2c_done) { if (enose_conf_requested) { const uint8_t msg[] = { ENOSE_PWM_ADDR, enose_heat[0], enose_heat[1], enose_heat[2] }; - memcpy((void*)i2c0_buf, msg, sizeof(msg)); + memcpy((void *)i2c0_buf, msg, sizeof(msg)); i2c0_transmit(ENOSE_SLAVE_ADDR, sizeof(msg), &enose_i2c_done); enose_i2c_done = FALSE; enose_conf_requested = FALSE; - } - else if (enose_status == ENOSE_IDLE) { + } else if (enose_status == ENOSE_IDLE) { enose_status = ENOSE_MEASURING_WR; const uint8_t msg[] = { ENOSE_DATA_ADDR }; - memcpy((void*)i2c0_buf, msg, sizeof(msg)); + memcpy((void *)i2c0_buf, msg, sizeof(msg)); i2c0_transmit(ENOSE_SLAVE_ADDR, sizeof(msg), &enose_i2c_done); enose_i2c_done = FALSE; - } - else if (enose_status == ENOSE_MEASURING_WR) { + } else if (enose_status == ENOSE_MEASURING_WR) { enose_status = ENOSE_MEASURING_RD; i2c0_receive(ENOSE_SLAVE_ADDR, 6, &enose_i2c_done); enose_i2c_done = FALSE; - } - else if (enose_status == ENOSE_MEASURING_RD) { - uint16_t val = (i2c0_buf[0]<<8) | i2c0_buf[1]; - if (val < 5000) - enose_val[0] = val; - val = (i2c0_buf[2]<<8) | i2c0_buf[3]; - if (val < 5000) - enose_val[1] = val; - val = (i2c0_buf[4]<<8) | i2c0_buf[5]; - if (val < 5000) - enose_val[2] = val; + } else if (enose_status == ENOSE_MEASURING_RD) { + uint16_t val = (i2c0_buf[0] << 8) | i2c0_buf[1]; + if (val < 5000) { + enose_val[0] = val; + } + val = (i2c0_buf[2] << 8) | i2c0_buf[3]; + if (val < 5000) { + enose_val[1] = val; + } + val = (i2c0_buf[4] << 8) | i2c0_buf[5]; + if (val < 5000) { + enose_val[2] = val; + } enose_status = ENOSE_IDLE; } } - DOWNLINK_SEND_ENOSE_STATUS(DefaultChannel, DefaultDevice,&enose_val[0], &enose_val[1], &enose_val[2], &enose_PID_val, 3, enose_heat); + DOWNLINK_SEND_ENOSE_STATUS(DefaultChannel, DefaultDevice, &enose_val[0], &enose_val[1], &enose_val[2], &enose_PID_val, + 3, enose_heat); } diff --git a/sw/airborne/modules/enose/enose.h b/sw/airborne/modules/enose/enose.h index 96300304fe..b064810945 100644 --- a/sw/airborne/modules/enose/enose.h +++ b/sw/airborne/modules/enose/enose.h @@ -24,10 +24,10 @@ extern uint16_t enose_PID_val; extern uint8_t enose_status; -extern void enose_init( void ); +extern void enose_init(void); extern void enose_set_heat(uint8_t no_sensor, uint8_t value); -extern void enose_periodic( void ); +extern void enose_periodic(void); #define enose_SetHeat0(val) {enose_set_heat(0, val);} #define enose_SetHeat1(val) {enose_set_heat(1, val);} diff --git a/sw/airborne/modules/gain_scheduling/gain_scheduling.c b/sw/airborne/modules/gain_scheduling/gain_scheduling.c index e90663c4ff..514d788bb6 100644 --- a/sw/airborne/modules/gain_scheduling/gain_scheduling.c +++ b/sw/airborne/modules/gain_scheduling/gain_scheduling.c @@ -48,7 +48,8 @@ struct Int32AttitudeGains gainlibrary[NUMBER_OF_GAINSETS]; float scheduling_points[NUMBER_OF_GAINSETS] = SCHEDULING_POINTS; //Get the specified gains in the gainlibrary -void gain_scheduling_init(void) { +void gain_scheduling_init(void) +{ int32_t phi_p[NUMBER_OF_GAINSETS] = PHI_P; int32_t phi_d[NUMBER_OF_GAINSETS] = PHI_D; int32_t phi_i[NUMBER_OF_GAINSETS] = PHI_I; @@ -64,29 +65,30 @@ void gain_scheduling_init(void) { int32_t psi_i[NUMBER_OF_GAINSETS] = PSI_I; int32_t psi_dd[NUMBER_OF_GAINSETS] = PSI_DD; - for(int i = 0; i 1 uint8_t section = 0; //Find out between which gainsets to interpolate - while(FLOAT_OF_BFP(SCHEDULING_VARIABLE,SCHEDULING_VARIABLE_FRAC) > scheduling_points[section]) { + while (FLOAT_OF_BFP(SCHEDULING_VARIABLE, SCHEDULING_VARIABLE_FRAC) > scheduling_points[section]) { section++; - if(section == NUMBER_OF_GAINSETS) break; + if (section == NUMBER_OF_GAINSETS) { break; } } //Get pointers for the two gainsets and the stabilization_gains @@ -94,37 +96,39 @@ void gain_scheduling_periodic(void) { gblend = &stabilization_gains; - if (section == 0) + if (section == 0) { set_gainset(0); - else if (section == NUMBER_OF_GAINSETS) - set_gainset(NUMBER_OF_GAINSETS-1); - else { - ga = &gainlibrary[section-1]; + } else if (section == NUMBER_OF_GAINSETS) { + set_gainset(NUMBER_OF_GAINSETS - 1); + } else { + ga = &gainlibrary[section - 1]; gb = &gainlibrary[section]; //Calculate the ratio between the scheduling points int32_t ratio; - ratio = BFP_OF_REAL((FLOAT_OF_BFP(SCHEDULING_VARIABLE,SCHEDULING_VARIABLE_FRAC) - scheduling_points[section-1])/(scheduling_points[section] - scheduling_points[section-1]),INT32_RATIO_FRAC); + ratio = BFP_OF_REAL((FLOAT_OF_BFP(SCHEDULING_VARIABLE, + SCHEDULING_VARIABLE_FRAC) - scheduling_points[section - 1]) / (scheduling_points[section] - + scheduling_points[section - 1]), INT32_RATIO_FRAC); int64_t g1, g2, gbl; //Loop through the gains and interpolate - for (int i=0; i < (sizeof(struct Int32AttitudeGains)/sizeof(int32_t)); i++) - { - g1 = *(((int32_t*) ga) + i); - g1 *= (1<> INT32_RATIO_FRAC; - *(((int32_t*) gblend) + i) = (int32_t) gbl; + *(((int32_t *) gblend) + i) = (int32_t) gbl; } } #endif } //Set one of the gainsets entirely -void set_gainset(int gainset) { +void set_gainset(int gainset) +{ stabilization_gains = gainlibrary[gainset]; } \ No newline at end of file diff --git a/sw/airborne/modules/gas_engine/gas_engine_idle_trim.c b/sw/airborne/modules/gas_engine/gas_engine_idle_trim.c index d423f06b90..45f8700825 100644 --- a/sw/airborne/modules/gas_engine/gas_engine_idle_trim.c +++ b/sw/airborne/modules/gas_engine/gas_engine_idle_trim.c @@ -28,7 +28,8 @@ int gas_engine_idle_trim_right = 0; #include "inter_mcu.h" -void periodic_gas_engine_idle_trim(void) { +void periodic_gas_engine_idle_trim(void) +{ ap_state->commands[COMMAND_IDLE1] = fbw_state->channels[RADIO_GAIN1]; ap_state->commands[COMMAND_IDLE2] = fbw_state->channels[RADIO_GAIN2]; } diff --git a/sw/airborne/modules/geo_mag/geo_mag.c b/sw/airborne/modules/geo_mag/geo_mag.c index f5760118c9..01da5dfe78 100644 --- a/sw/airborne/modules/geo_mag/geo_mag.c +++ b/sw/airborne/modules/geo_mag/geo_mag.c @@ -36,17 +36,21 @@ bool_t geo_mag_calc_flag; struct GeoMag geo_mag; -void geo_mag_init(void) { +void geo_mag_init(void) +{ geo_mag_calc_flag = FALSE; geo_mag.ready = FALSE; } -void geo_mag_periodic(void) { - if (!geo_mag.ready && gps.fix == GPS_FIX_3D && kill_throttle) +void geo_mag_periodic(void) +{ + if (!geo_mag.ready && gps.fix == GPS_FIX_3D && kill_throttle) { geo_mag_calc_flag = TRUE; + } } -void geo_mag_event(void) { +void geo_mag_event(void) +{ if (geo_mag_calc_flag) { double gha[MAXCOEFF]; // Geomag global variables @@ -54,8 +58,8 @@ void geo_mag_event(void) { /* Current date in decimal year, for example 2012.68 */ double sdate = GPS_EPOCH_BEGIN + - (double)gps.week/WEEKS_IN_YEAR + - (double)gps.tow/1000/SECS_IN_YEAR; + (double)gps.week / WEEKS_IN_YEAR + + (double)gps.tow / 1000 / SECS_IN_YEAR; /* LLA Position in decimal degrees and altitude in km */ double latitude = (double)gps.lla_pos.lat / 1e7; @@ -75,7 +79,8 @@ void geo_mag_event(void) { VECT3_COPY(ahrs_impl.mag_h, geo_mag.vect); #else // convert to MAG_BFP and copy to ahrs - VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(geo_mag.vect.x), MAG_BFP_OF_REAL(geo_mag.vect.y), MAG_BFP_OF_REAL(geo_mag.vect.z)); + VECT3_ASSIGN(ahrs_impl.mag_h, MAG_BFP_OF_REAL(geo_mag.vect.x), MAG_BFP_OF_REAL(geo_mag.vect.y), + MAG_BFP_OF_REAL(geo_mag.vect.z)); #endif geo_mag.ready = TRUE; diff --git a/sw/airborne/modules/gps/gps_ubx_ucenter.c b/sw/airborne/modules/gps/gps_ubx_ucenter.c index 3e15d0ce05..b7cfce8a4d 100644 --- a/sw/airborne/modules/gps/gps_ubx_ucenter.c +++ b/sw/airborne/modules/gps/gps_ubx_ucenter.c @@ -78,8 +78,7 @@ void gps_ubx_ucenter_init(void) gps_ubx_ucenter.hw_ver_h = 0; gps_ubx_ucenter.hw_ver_l = 0; - for (int i=0; i 0) { - DEBUG_PRINT("Initial ublox baudrate found: %u\n", gps_ubx_ucenter.baud_init); - } - else { - DEBUG_PRINT("WARNING: Unable to determine the ublox baudrate. Autoconfiguration is unlikely to work.\n"); - } + if (gps_ubx_ucenter.baud_init > 0) { + DEBUG_PRINT("Initial ublox baudrate found: %u\n", gps_ubx_ucenter.baud_init); + } else { + DEBUG_PRINT("WARNING: Unable to determine the ublox baudrate. Autoconfiguration is unlikely to work.\n"); + } #endif - } - else { - gps_ubx_ucenter.cnt++; - } - break; - // Send Configuration - case GPS_UBX_UCENTER_STATUS_CONFIG: - if (gps_ubx_ucenter_configure(gps_ubx_ucenter.cnt) == FALSE) { - gps_ubx_ucenter.status = GPS_UBX_UCENTER_STATUS_STOPPED; - gps_ubx_ucenter.cnt = 0; - } - else { - gps_ubx_ucenter.cnt++; - } - break; - default: - // stop this module now... - // todo - break; + } else { + gps_ubx_ucenter.cnt++; + } + break; + // Send Configuration + case GPS_UBX_UCENTER_STATUS_CONFIG: + if (gps_ubx_ucenter_configure(gps_ubx_ucenter.cnt) == FALSE) { + gps_ubx_ucenter.status = GPS_UBX_UCENTER_STATUS_STOPPED; + gps_ubx_ucenter.cnt = 0; + } else { + gps_ubx_ucenter.cnt++; + } + break; + default: + // stop this module now... + // todo + break; } } @@ -139,49 +133,48 @@ void gps_ubx_ucenter_periodic(void) void gps_ubx_ucenter_event(void) { // Save processing time inflight - if (gps_ubx_ucenter.status == GPS_UBX_UCENTER_STATUS_STOPPED) + if (gps_ubx_ucenter.status == GPS_UBX_UCENTER_STATUS_STOPPED) { return; + } // Read Configuration Reply's - switch (gps_ubx.msg_class) - { - case UBX_ACK_ID: - if (gps_ubx.msg_id == UBX_ACK_ACK_ID) { - gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_ACK; - DEBUG_PRINT("ACK\n"); - } - else { - gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NACK; - DEBUG_PRINT("NACK\n"); - } - break; - case UBX_MON_ID: - if (gps_ubx.msg_id == UBX_MON_VER_ID ) { - gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_VERSION; - gps_ubx_ucenter.sw_ver_h = UBX_MON_VER_c(gps_ubx.msg_buf,0) - '0'; - gps_ubx_ucenter.sw_ver_l = 10*(UBX_MON_VER_c(gps_ubx.msg_buf,2) - '0'); - gps_ubx_ucenter.sw_ver_l += UBX_MON_VER_c(gps_ubx.msg_buf,3) - '0'; - gps_ubx_ucenter.hw_ver_h = UBX_MON_VER_c(gps_ubx.msg_buf,33) - '0'; - gps_ubx_ucenter.hw_ver_h += 10*(UBX_MON_VER_c(gps_ubx.msg_buf,32) - '0'); - gps_ubx_ucenter.hw_ver_l = UBX_MON_VER_c(gps_ubx.msg_buf,37) - '0'; - gps_ubx_ucenter.hw_ver_l += 10*(UBX_MON_VER_c(gps_ubx.msg_buf,36) - '0'); + switch (gps_ubx.msg_class) { + case UBX_ACK_ID: + if (gps_ubx.msg_id == UBX_ACK_ACK_ID) { + gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_ACK; + DEBUG_PRINT("ACK\n"); + } else { + gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NACK; + DEBUG_PRINT("NACK\n"); + } + break; + case UBX_MON_ID: + if (gps_ubx.msg_id == UBX_MON_VER_ID) { + gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_VERSION; + gps_ubx_ucenter.sw_ver_h = UBX_MON_VER_c(gps_ubx.msg_buf, 0) - '0'; + gps_ubx_ucenter.sw_ver_l = 10 * (UBX_MON_VER_c(gps_ubx.msg_buf, 2) - '0'); + gps_ubx_ucenter.sw_ver_l += UBX_MON_VER_c(gps_ubx.msg_buf, 3) - '0'; + gps_ubx_ucenter.hw_ver_h = UBX_MON_VER_c(gps_ubx.msg_buf, 33) - '0'; + gps_ubx_ucenter.hw_ver_h += 10 * (UBX_MON_VER_c(gps_ubx.msg_buf, 32) - '0'); + gps_ubx_ucenter.hw_ver_l = UBX_MON_VER_c(gps_ubx.msg_buf, 37) - '0'; + gps_ubx_ucenter.hw_ver_l += 10 * (UBX_MON_VER_c(gps_ubx.msg_buf, 36) - '0'); - DEBUG_PRINT("ublox sw_ver: %u.%u\n", gps_ubx_ucenter.sw_ver_h, gps_ubx_ucenter.sw_ver_l); - DEBUG_PRINT("ublox hw_ver: %u.%u\n", gps_ubx_ucenter.hw_ver_h, gps_ubx_ucenter.hw_ver_l); - } - break; - case UBX_CFG_ID: - if (gps_ubx.msg_id == UBX_CFG_PRT_ID) { - gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_CFG_PRT; - gps_ubx_ucenter.port_id = UBX_CFG_PRT_PortId(gps_ubx.msg_buf,0); - gps_ubx_ucenter.baud_run = UBX_CFG_PRT_Baudrate(gps_ubx.msg_buf,0); + DEBUG_PRINT("ublox sw_ver: %u.%u\n", gps_ubx_ucenter.sw_ver_h, gps_ubx_ucenter.sw_ver_l); + DEBUG_PRINT("ublox hw_ver: %u.%u\n", gps_ubx_ucenter.hw_ver_h, gps_ubx_ucenter.hw_ver_l); + } + break; + case UBX_CFG_ID: + if (gps_ubx.msg_id == UBX_CFG_PRT_ID) { + gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_CFG_PRT; + gps_ubx_ucenter.port_id = UBX_CFG_PRT_PortId(gps_ubx.msg_buf, 0); + gps_ubx_ucenter.baud_run = UBX_CFG_PRT_Baudrate(gps_ubx.msg_buf, 0); - DEBUG_PRINT("gps_ubx_ucenter.baud_run: %u\n", gps_ubx_ucenter.baud_run); - DEBUG_PRINT("gps_ubx_ucenter.port_id: %u\n", gps_ubx_ucenter.port_id); - } - break; - default: - break; + DEBUG_PRINT("gps_ubx_ucenter.baud_run: %u\n", gps_ubx_ucenter.baud_run); + DEBUG_PRINT("gps_ubx_ucenter.port_id: %u\n", gps_ubx_ucenter.port_id); + } + break; + default: + break; } } @@ -228,67 +221,66 @@ static inline void gps_ubx_ucenter_enable_msg(uint8_t class, uint8_t id, uint8_t */ static bool_t gps_ubx_ucenter_autobaud(uint8_t nr) { - switch (nr) - { - case 0: - case 1: - // Very important for some modules: - // Give the GPS some time to boot (up to 0.75 second) - break; - case 2: - gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; - GpsUartSetBaudrate(B38400); // Try the most common first? - gps_ubx_ucenter_config_port_poll(); - break; - case 3: - if (gps_ubx_ucenter.reply == GPS_UBX_UCENTER_REPLY_ACK) { - gps_ubx_ucenter.baud_init = gps_ubx_ucenter.baud_run; - return FALSE; - } - gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; - GpsUartSetBaudrate(B9600); // Maybe the factory default? - gps_ubx_ucenter_config_port_poll(); - break; - case 4: - if (gps_ubx_ucenter.reply == GPS_UBX_UCENTER_REPLY_ACK) { - gps_ubx_ucenter.baud_init = gps_ubx_ucenter.baud_run; - return FALSE; - } - gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; - GpsUartSetBaudrate(B57600); // The high-rate default? - gps_ubx_ucenter_config_port_poll(); - break; - case 5: - if (gps_ubx_ucenter.reply == GPS_UBX_UCENTER_REPLY_ACK) { - gps_ubx_ucenter.baud_init = gps_ubx_ucenter.baud_run; - return FALSE; - } - gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; - GpsUartSetBaudrate(B4800); // Default NMEA baudrate? - gps_ubx_ucenter_config_port_poll(); - break; - case 6: - if (gps_ubx_ucenter.reply == GPS_UBX_UCENTER_REPLY_ACK) { - gps_ubx_ucenter.baud_init = gps_ubx_ucenter.baud_run; - return FALSE; - } - gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; - GpsUartSetBaudrate(B115200); // Last possible option for ublox - gps_ubx_ucenter_config_port_poll(); - break; - case 7: - if (gps_ubx_ucenter.reply == GPS_UBX_UCENTER_REPLY_ACK) { - gps_ubx_ucenter.baud_init = gps_ubx_ucenter.baud_run; - return FALSE; - } + switch (nr) { + case 0: + case 1: + // Very important for some modules: + // Give the GPS some time to boot (up to 0.75 second) + break; + case 2: + gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; + GpsUartSetBaudrate(B38400); // Try the most common first? + gps_ubx_ucenter_config_port_poll(); + break; + case 3: + if (gps_ubx_ucenter.reply == GPS_UBX_UCENTER_REPLY_ACK) { + gps_ubx_ucenter.baud_init = gps_ubx_ucenter.baud_run; + return FALSE; + } + gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; + GpsUartSetBaudrate(B9600); // Maybe the factory default? + gps_ubx_ucenter_config_port_poll(); + break; + case 4: + if (gps_ubx_ucenter.reply == GPS_UBX_UCENTER_REPLY_ACK) { + gps_ubx_ucenter.baud_init = gps_ubx_ucenter.baud_run; + return FALSE; + } + gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; + GpsUartSetBaudrate(B57600); // The high-rate default? + gps_ubx_ucenter_config_port_poll(); + break; + case 5: + if (gps_ubx_ucenter.reply == GPS_UBX_UCENTER_REPLY_ACK) { + gps_ubx_ucenter.baud_init = gps_ubx_ucenter.baud_run; + return FALSE; + } + gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; + GpsUartSetBaudrate(B4800); // Default NMEA baudrate? + gps_ubx_ucenter_config_port_poll(); + break; + case 6: + if (gps_ubx_ucenter.reply == GPS_UBX_UCENTER_REPLY_ACK) { + gps_ubx_ucenter.baud_init = gps_ubx_ucenter.baud_run; + return FALSE; + } + gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; + GpsUartSetBaudrate(B115200); // Last possible option for ublox + gps_ubx_ucenter_config_port_poll(); + break; + case 7: + if (gps_ubx_ucenter.reply == GPS_UBX_UCENTER_REPLY_ACK) { + gps_ubx_ucenter.baud_init = gps_ubx_ucenter.baud_run; + return FALSE; + } - // Autoconfig Failed... let's setup the failsafe baudrate - // Should we try even a different baudrate? - gps_ubx_ucenter.baud_init = 0; // Set as zero to indicate that we couldn't verify the baudrate - GpsUartSetBaudrate(B9600); - return FALSE; - default: - break; + // Autoconfig Failed... let's setup the failsafe baudrate + // Should we try even a different baudrate? + gps_ubx_ucenter.baud_init = 0; // Set as zero to indicate that we couldn't verify the baudrate + GpsUartSetBaudrate(B9600); + return FALSE; + default: + break; } return TRUE; } @@ -338,12 +330,12 @@ static inline void gps_ubx_ucenter_config_nav(void) { //New ublox firmware v5 or higher uses CFG_NAV5 message, CFG_NAV is no longer available if (gps_ubx_ucenter.sw_ver_h < 5 && gps_ubx_ucenter.hw_ver_h < 6) { - UbxSend_CFG_NAV(NAV_DYN_AIRBORNE_2G, 3, 16, 24, 20, 5, 0, 0x3C, 0x3C, 0x14, 0x03E8 ,0x0000, 0x0, 0x17, 0x00FA, 0x00FA, 0x0064, 0x012C, 0x000F, 0x00, 0x00); - } - else { + UbxSend_CFG_NAV(NAV_DYN_AIRBORNE_2G, 3, 16, 24, 20, 5, 0, 0x3C, 0x3C, 0x14, 0x03E8 , 0x0000, 0x0, 0x17, 0x00FA, 0x00FA, + 0x0064, 0x012C, 0x000F, 0x00, 0x00); + } else { UbxSend_CFG_NAV5(NAV5_MASK, GPS_UBX_NAV5_DYNAMICS, NAV5_3D_ONLY, IGNORED, IGNORED, NAV5_DEFAULT_MIN_ELEV, RESERVED, - NAV5_DEFAULT_PDOP_MASK, NAV5_DEFAULT_TDOP_MASK, NAV5_DEFAULT_P_ACC, NAV5_DEFAULT_T_ACC, - NAV5_DEFAULT_STATIC_HOLD_THRES, RESERVED, RESERVED, RESERVED, RESERVED); + NAV5_DEFAULT_PDOP_MASK, NAV5_DEFAULT_TDOP_MASK, NAV5_DEFAULT_P_ACC, NAV5_DEFAULT_T_ACC, + NAV5_DEFAULT_STATIC_HOLD_THRES, RESERVED, RESERVED, RESERVED, RESERVED); } } @@ -363,11 +355,11 @@ static inline void gps_ubx_ucenter_config_nav(void) #define NMEA_PROTO_MASK 0x0002 #define RTCM_PROTO_MASK 0x0004 -#define GPS_PORT_DDC 0x00 -#define GPS_PORT_UART1 0x01 -#define GPS_PORT_UART2 0x02 -#define GPS_PORT_USB 0x03 -#define GPS_PORT_SPI 0x04 +#define GPS_PORT_DDC 0x00 +#define GPS_PORT_UART1 0x01 +#define GPS_PORT_UART2 0x02 +#define GPS_PORT_USB 0x03 +#define GPS_PORT_SPI 0x04 #define GPS_PORT_RESERVED 0x05 #define __UBX_GPS_BAUD(_u) _u##_BAUD @@ -380,31 +372,31 @@ static inline void gps_ubx_ucenter_config_nav(void) static inline void gps_ubx_ucenter_config_port(void) { - switch(gps_ubx_ucenter.port_id) - { - // I2C Interface - case GPS_PORT_DDC: + switch (gps_ubx_ucenter.port_id) { + // I2C Interface + case GPS_PORT_DDC: #ifdef GPS_I2C - UbxSend_CFG_PRT(gps_ubx_ucenter.port_id, 0x0, 0x0, GPS_I2C_SLAVE_ADDR, 0x0, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0); + UbxSend_CFG_PRT(gps_ubx_ucenter.port_id, 0x0, 0x0, GPS_I2C_SLAVE_ADDR, 0x0, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0); #else - DEBUG_PRINT("WARNING: Please include the gps_i2c module.\n"); + DEBUG_PRINT("WARNING: Please include the gps_i2c module.\n"); #endif - break; - // UART Interface - case GPS_PORT_UART1: - case GPS_PORT_UART2: - UbxSend_CFG_PRT(gps_ubx_ucenter.port_id, 0x0, 0x0, UBX_UART_MODE_MASK, UART_SPEED(UBX_GPS_BAUD), UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0); - break; - // USB Interface - case GPS_PORT_USB: - UbxSend_CFG_PRT(gps_ubx_ucenter.port_id, 0x0, 0x0, 0x0, 0x0, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0); - break; - case GPS_PORT_SPI: - DEBUG_PRINT("WARNING: ublox SPI port is currently not supported.\n"); - break; - default: - DEBUG_PRINT("WARNING: Unknown ublox port id: %u\n", gps_ubx_ucenter.port_id); - break; + break; + // UART Interface + case GPS_PORT_UART1: + case GPS_PORT_UART2: + UbxSend_CFG_PRT(gps_ubx_ucenter.port_id, 0x0, 0x0, UBX_UART_MODE_MASK, UART_SPEED(UBX_GPS_BAUD), UBX_PROTO_MASK, + UBX_PROTO_MASK, 0x0, 0x0); + break; + // USB Interface + case GPS_PORT_USB: + UbxSend_CFG_PRT(gps_ubx_ucenter.port_id, 0x0, 0x0, 0x0, 0x0, UBX_PROTO_MASK, UBX_PROTO_MASK, 0x0, 0x0); + break; + case GPS_PORT_SPI: + DEBUG_PRINT("WARNING: ublox SPI port is currently not supported.\n"); + break; + default: + DEBUG_PRINT("WARNING: Unknown ublox port id: %u\n", gps_ubx_ucenter.port_id); + break; } } @@ -414,14 +406,15 @@ static inline void gps_ubx_ucenter_config_port(void) #define GPS_SBAS_CORRECTIONS 0x02 #define GPS_SBAS_INTEGRITY 0x04 -#define GPS_SBAS_MAX_SBAS 1 // Default ublox setting uses 3 SBAS channels(?) +#define GPS_SBAS_MAX_SBAS 1 // Default ublox setting uses 3 SBAS channels(?) -#define GPS_SBAS_AUTOSCAN 0x00 +#define GPS_SBAS_AUTOSCAN 0x00 static inline void gps_ubx_ucenter_config_sbas(void) { // Since March 2nd 2011 EGNOS is released for aviation purposes - UbxSend_CFG_SBAS(GPS_SBAS_ENABLED, GPS_SBAS_RANGING | GPS_SBAS_CORRECTIONS | GPS_SBAS_INTEGRITY, GPS_SBAS_MAX_SBAS, GPS_SBAS_AUTOSCAN, GPS_SBAS_AUTOSCAN); + UbxSend_CFG_SBAS(GPS_SBAS_ENABLED, GPS_SBAS_RANGING | GPS_SBAS_CORRECTIONS | GPS_SBAS_INTEGRITY, GPS_SBAS_MAX_SBAS, + GPS_SBAS_AUTOSCAN, GPS_SBAS_AUTOSCAN); //UbxSend_CFG_SBAS(0x00, 0x00, 0x00, 0x00, 0x00); } @@ -430,116 +423,116 @@ static inline void gps_ubx_ucenter_config_sbas(void) static bool_t gps_ubx_ucenter_configure(uint8_t nr) { - DEBUG_PRINT("gps_ubx_ucenter_configure nr: %u\n",nr); + DEBUG_PRINT("gps_ubx_ucenter_configure nr: %u\n", nr); // Store the reply of the last configuration step and reset - if (nr < GPS_UBX_UCENTER_CONFIG_STEPS) + if (nr < GPS_UBX_UCENTER_CONFIG_STEPS) { gps_ubx_ucenter.replies[nr] = gps_ubx_ucenter.reply; + } switch (nr) { - case 0: - // Use old baudrate to issue a baudrate change command - gps_ubx_ucenter_config_port(); - break; - case 1: + case 0: + // Use old baudrate to issue a baudrate change command + gps_ubx_ucenter_config_port(); + break; + case 1: #if PRINT_DEBUG_GPS_UBX_UCENTER - if (gps_ubx_ucenter.reply != GPS_UBX_UCENTER_REPLY_ACK) { - DEBUG_PRINT("ublox did not acknowledge port configuration.\n"); - } - else { - DEBUG_PRINT("Changed ublox baudrate to: %u\n", UART_SPEED(UBX_GPS_BAUD)); - } + if (gps_ubx_ucenter.reply != GPS_UBX_UCENTER_REPLY_ACK) { + DEBUG_PRINT("ublox did not acknowledge port configuration.\n"); + } else { + DEBUG_PRINT("Changed ublox baudrate to: %u\n", UART_SPEED(UBX_GPS_BAUD)); + } #endif - // Now the GPS baudrate should have changed - GpsUartSetBaudrate(UBX_GPS_BAUD); - gps_ubx_ucenter.baud_run = UART_SPEED(UBX_GPS_BAUD); - UbxSend_MON_GET_VER(); - break; - case 2: - case 3: - case 4: - case 5: - // UBX_G5010 takes 0.7 seconds to answer a firmware request - // Version info is important for proper configuration as different firmwares have different settings - break; - case 6: - // Send some debugging info: detected baudrate, software version etc... - gps_ubx_ucenter.replies[0] = (gps_ubx_ucenter.baud_init/1000); - gps_ubx_ucenter.replies[1] = (gps_ubx_ucenter.baud_init - 1000 * gps_ubx_ucenter.replies[0]) / 100; - gps_ubx_ucenter.replies[2] = gps_ubx_ucenter.sw_ver_h; - gps_ubx_ucenter.replies[3] = gps_ubx_ucenter.sw_ver_l; - gps_ubx_ucenter.replies[4] = gps_ubx_ucenter.hw_ver_h; - gps_ubx_ucenter.replies[5] = gps_ubx_ucenter.hw_ver_l; + // Now the GPS baudrate should have changed + GpsUartSetBaudrate(UBX_GPS_BAUD); + gps_ubx_ucenter.baud_run = UART_SPEED(UBX_GPS_BAUD); + UbxSend_MON_GET_VER(); + break; + case 2: + case 3: + case 4: + case 5: + // UBX_G5010 takes 0.7 seconds to answer a firmware request + // Version info is important for proper configuration as different firmwares have different settings + break; + case 6: + // Send some debugging info: detected baudrate, software version etc... + gps_ubx_ucenter.replies[0] = (gps_ubx_ucenter.baud_init / 1000); + gps_ubx_ucenter.replies[1] = (gps_ubx_ucenter.baud_init - 1000 * gps_ubx_ucenter.replies[0]) / 100; + gps_ubx_ucenter.replies[2] = gps_ubx_ucenter.sw_ver_h; + gps_ubx_ucenter.replies[3] = gps_ubx_ucenter.sw_ver_l; + gps_ubx_ucenter.replies[4] = gps_ubx_ucenter.hw_ver_h; + gps_ubx_ucenter.replies[5] = gps_ubx_ucenter.hw_ver_l; #if DEBUG_GPS_UBX_UCENTER - DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,6,gps_ubx_ucenter.replies); + DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice, 6, gps_ubx_ucenter.replies); #endif - // Configure CFG-NAV(5) message - gps_ubx_ucenter_config_nav(); - break; - case 7: - // Geodetic Position Solution - gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_POSLLH_ID,1); - break; - case 8: - // Velocity Solution in NED - gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_VELNED_ID, 1); - break; - case 9: - // Receiver Navigation Status - gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_STATUS_ID, 1); - break; - case 10: - // Space Vehicle Information - gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SVINFO_ID, 4); - break; - case 11: - // Navigation Solution Information + // Configure CFG-NAV(5) message + gps_ubx_ucenter_config_nav(); + break; + case 7: + // Geodetic Position Solution + gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_POSLLH_ID, 1); + break; + case 8: + // Velocity Solution in NED + gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_VELNED_ID, 1); + break; + case 9: + // Receiver Navigation Status + gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_STATUS_ID, 1); + break; + case 10: + // Space Vehicle Information + gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SVINFO_ID, 4); + break; + case 11: + // Navigation Solution Information #if GPS_UBX_UCENTER_SLOW_NAV_SOL - gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SOL_ID, 8); + gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SOL_ID, 8); #else - gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SOL_ID, 1); + gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_SOL_ID, 1); #endif - break; - case 12: - // Disable UTM on old Lea4P - gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_POSUTM_ID, 0); - break; - case 13: - // SBAS Configuration - gps_ubx_ucenter_config_sbas(); - break; - case 14: - // Poll Navigation/Measurement Rate Settings - UbxSend_CFG_RATE(GPS_UBX_UCENTER_RATE, 0x0001, 0x0000); - break; - case 15: - // Raw Measurement Data + break; + case 12: + // Disable UTM on old Lea4P + gps_ubx_ucenter_enable_msg(UBX_NAV_ID, UBX_NAV_POSUTM_ID, 0); + break; + case 13: + // SBAS Configuration + gps_ubx_ucenter_config_sbas(); + break; + case 14: + // Poll Navigation/Measurement Rate Settings + UbxSend_CFG_RATE(GPS_UBX_UCENTER_RATE, 0x0001, 0x0000); + break; + case 15: + // Raw Measurement Data #if USE_GPS_UBX_RXM_RAW - gps_ubx_ucenter_enable_msg(UBX_RXM_ID, UBX_RXM_RAW_ID, 1); + gps_ubx_ucenter_enable_msg(UBX_RXM_ID, UBX_RXM_RAW_ID, 1); #endif - break; - case 16: - // Subframe Buffer + break; + case 16: + // Subframe Buffer #if USE_GPS_UBX_RXM_SFRB - gps_ubx_ucenter_enable_msg(UBX_RXM_ID, UBX_RXM_SFRB_ID, 1); + gps_ubx_ucenter_enable_msg(UBX_RXM_ID, UBX_RXM_SFRB_ID, 1); #endif - break; - case 17: - // Try to save on non-ROM devices... - UbxSend_CFG_CFG(0x00000000,0xffffffff,0x00000000); - break; - case 18: + break; + case 17: + // Try to save on non-ROM devices... + UbxSend_CFG_CFG(0x00000000, 0xffffffff, 0x00000000); + break; + case 18: #if DEBUG_GPS_UBX_UCENTER - // Debug Downlink the result of all configuration steps: see messages - // To view, enable DEBUG message in your telemetry configuration .xml - DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,GPS_UBX_UCENTER_CONFIG_STEPS,gps_ubx_ucenter.replies); - for (int i = 0; i < GPS_UBX_UCENTER_CONFIG_STEPS; i++) { - DEBUG_PRINT("%u\n", gps_ubx_ucenter.replies[i]); - } + // Debug Downlink the result of all configuration steps: see messages + // To view, enable DEBUG message in your telemetry configuration .xml + DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice, GPS_UBX_UCENTER_CONFIG_STEPS, gps_ubx_ucenter.replies); + for (int i = 0; i < GPS_UBX_UCENTER_CONFIG_STEPS; i++) { + DEBUG_PRINT("%u\n", gps_ubx_ucenter.replies[i]); + } #endif - return FALSE; - default: - break; + return FALSE; + default: + break; } gps_ubx_ucenter.reply = GPS_UBX_UCENTER_REPLY_NONE; diff --git a/sw/airborne/modules/gps/gps_ubx_ucenter.h b/sw/airborne/modules/gps/gps_ubx_ucenter.h index d7c92ab1df..e068e15cbb 100644 --- a/sw/airborne/modules/gps/gps_ubx_ucenter.h +++ b/sw/airborne/modules/gps/gps_ubx_ucenter.h @@ -31,10 +31,9 @@ #include "std.h" /** U-Center Variables */ -#define GPS_UBX_UCENTER_CONFIG_STEPS 19 +#define GPS_UBX_UCENTER_CONFIG_STEPS 19 -struct gps_ubx_ucenter_struct -{ +struct gps_ubx_ucenter_struct { uint8_t status; uint8_t reply; uint8_t cnt; diff --git a/sw/airborne/modules/gps_i2c/gps_i2c.c b/sw/airborne/modules/gps_i2c/gps_i2c.c index e26425f672..fa1ad69c61 100644 --- a/sw/airborne/modules/gps_i2c/gps_i2c.c +++ b/sw/airborne/modules/gps_i2c/gps_i2c.c @@ -44,15 +44,16 @@ bool_t gps_i2c_done, gps_i2c_data_ready_to_transmit; #define GPS_I2C_STATUS_READING_DATA 4 #define gps_i2c_AddCharToRxBuf(_x) { \ - gps_i2c_rx_buf[gps_i2c_rx_insert_idx] = _x; \ - gps_i2c_rx_insert_idx++; /* size=256, No check for buf overflow */ \ -} + gps_i2c_rx_buf[gps_i2c_rx_insert_idx] = _x; \ + gps_i2c_rx_insert_idx++; /* size=256, No check for buf overflow */ \ + } static uint8_t gps_i2c_status; //static uint16_t gps_i2c_nb_avail_bytes; /* size buffer =~ 12k */ //static uint8_t data_buf_len; -void gps_i2c_init(void) { +void gps_i2c_init(void) +{ gps_i2c_status = GPS_I2C_STATUS_IDLE; gps_i2c_done = TRUE; gps_i2c_data_ready_to_transmit = FALSE; @@ -65,83 +66,85 @@ void gps_i2c_init(void) { #endif } -void gps_i2c_periodic(void) { -/* - if (gps_i2c_done && gps_i2c_status == GPS_I2C_STATUS_IDLE) { - i2c0_buf[0] = GPS_I2C_ADDR_NB_AVAIL_BYTES; - i2c0_transmit_no_stop(GPS_I2C_SLAVE_ADDR, 1, &gps_i2c_done); - gps_i2c_done = FALSE; - gps_i2c_status = GPS_I2C_STATUS_ASKING_NB_AVAIL_BYTES; - } -*/ - -} - -void gps_i2c_event(void) { -/* - * switch (gps_i2c_status) { - case GPS_I2C_STATUS_IDLE: - if (gps_i2c_data_ready_to_transmit) { - // Copy data from our buffer to the i2c buffer - uint8_t data_size = Min(gps_i2c_tx_insert_idx-gps_i2c_tx_extract_idx, I2C0_BUF_LEN); - uint8_t i; - for(i = 0; i < data_size; i++, gps_i2c_tx_extract_idx++) - i2c0_buf[i] = gps_i2c_tx_buf[gps_i2c_tx_extract_idx]; - - // Start i2c transmit - i2c0_transmit(GPS_I2C_SLAVE_ADDR, data_size, &gps_i2c_done); +void gps_i2c_periodic(void) +{ + /* + if (gps_i2c_done && gps_i2c_status == GPS_I2C_STATUS_IDLE) { + i2c0_buf[0] = GPS_I2C_ADDR_NB_AVAIL_BYTES; + i2c0_transmit_no_stop(GPS_I2C_SLAVE_ADDR, 1, &gps_i2c_done); gps_i2c_done = FALSE; - - // Reset flag if finished - if (gps_i2c_tx_extract_idx >= gps_i2c_tx_insert_idx) { - gps_i2c_data_ready_to_transmit = FALSE; - gps_i2c_tx_insert_idx = 0; - } + gps_i2c_status = GPS_I2C_STATUS_ASKING_NB_AVAIL_BYTES; } - break; - - case GPS_I2C_STATUS_ASKING_NB_AVAIL_BYTES: - i2c0_receive(GPS_I2C_SLAVE_ADDR, 2, &gps_i2c_done); - gps_i2c_done = FALSE; - gps_i2c_status = GPS_I2C_STATUS_READING_NB_AVAIL_BYTES; - break; - - case GPS_I2C_STATUS_READING_NB_AVAIL_BYTES: - gps_i2c_nb_avail_bytes = (i2c0_buf[0]<<8) | i2c0_buf[1]; - - if (gps_i2c_nb_avail_bytes) - goto continue_reading; - else - gps_i2c_status = GPS_I2C_STATUS_IDLE; - break; - - continue_reading: - - case GPS_I2C_STATUS_ASKING_DATA: - data_buf_len = Min(gps_i2c_nb_avail_bytes, I2C0_BUF_LEN); - gps_i2c_nb_avail_bytes -= data_buf_len; - - i2c0_receive(GPS_I2C_SLAVE_ADDR, data_buf_len, &gps_i2c_done); - gps_i2c_done = FALSE; - gps_i2c_status = GPS_I2C_STATUS_READING_DATA; - break; - - case GPS_I2C_STATUS_READING_DATA: { - uint8_t i; - for(i = 0; i < data_buf_len; i++) { - gps_i2c_AddCharToRxBuf(i2c0_buf[i]); - } - - if (gps_i2c_nb_avail_bytes) - goto continue_reading; - else - gps_i2c_status = GPS_I2C_STATUS_IDLE; - break; - } - - default: - return; - } -*/ + */ + +} + +void gps_i2c_event(void) +{ + /* + * switch (gps_i2c_status) { + case GPS_I2C_STATUS_IDLE: + if (gps_i2c_data_ready_to_transmit) { + // Copy data from our buffer to the i2c buffer + uint8_t data_size = Min(gps_i2c_tx_insert_idx-gps_i2c_tx_extract_idx, I2C0_BUF_LEN); + uint8_t i; + for(i = 0; i < data_size; i++, gps_i2c_tx_extract_idx++) + i2c0_buf[i] = gps_i2c_tx_buf[gps_i2c_tx_extract_idx]; + + // Start i2c transmit + i2c0_transmit(GPS_I2C_SLAVE_ADDR, data_size, &gps_i2c_done); + gps_i2c_done = FALSE; + + // Reset flag if finished + if (gps_i2c_tx_extract_idx >= gps_i2c_tx_insert_idx) { + gps_i2c_data_ready_to_transmit = FALSE; + gps_i2c_tx_insert_idx = 0; + } + } + break; + + case GPS_I2C_STATUS_ASKING_NB_AVAIL_BYTES: + i2c0_receive(GPS_I2C_SLAVE_ADDR, 2, &gps_i2c_done); + gps_i2c_done = FALSE; + gps_i2c_status = GPS_I2C_STATUS_READING_NB_AVAIL_BYTES; + break; + + case GPS_I2C_STATUS_READING_NB_AVAIL_BYTES: + gps_i2c_nb_avail_bytes = (i2c0_buf[0]<<8) | i2c0_buf[1]; + + if (gps_i2c_nb_avail_bytes) + goto continue_reading; + else + gps_i2c_status = GPS_I2C_STATUS_IDLE; + break; + + continue_reading: + + case GPS_I2C_STATUS_ASKING_DATA: + data_buf_len = Min(gps_i2c_nb_avail_bytes, I2C0_BUF_LEN); + gps_i2c_nb_avail_bytes -= data_buf_len; + + i2c0_receive(GPS_I2C_SLAVE_ADDR, data_buf_len, &gps_i2c_done); + gps_i2c_done = FALSE; + gps_i2c_status = GPS_I2C_STATUS_READING_DATA; + break; + + case GPS_I2C_STATUS_READING_DATA: { + uint8_t i; + for(i = 0; i < data_buf_len; i++) { + gps_i2c_AddCharToRxBuf(i2c0_buf[i]); + } + + if (gps_i2c_nb_avail_bytes) + goto continue_reading; + else + gps_i2c_status = GPS_I2C_STATUS_IDLE; + break; + } + + default: + return; + } + */ } diff --git a/sw/airborne/modules/gps_i2c/gps_i2c.h b/sw/airborne/modules/gps_i2c/gps_i2c.h index 9414a4add8..f89fe03d9f 100644 --- a/sw/airborne/modules/gps_i2c/gps_i2c.h +++ b/sw/airborne/modules/gps_i2c/gps_i2c.h @@ -44,13 +44,13 @@ void gps_i2c_periodic(void); #define gps_i2cChAvailable() (gps_i2c_rx_insert_idx != gps_i2c_rx_extract_idx) #define gps_i2cGetch() (gps_i2c_rx_buf[gps_i2c_rx_extract_idx++]) #define gps_i2cTransmit(_char) { \ - if (! gps_i2c_data_ready_to_transmit) /* Else transmitting, overrun*/ \ - gps_i2c_tx_buf[gps_i2c_tx_insert_idx++] = _char; \ -} + if (! gps_i2c_data_ready_to_transmit) /* Else transmitting, overrun*/ \ + gps_i2c_tx_buf[gps_i2c_tx_insert_idx++] = _char; \ + } #define gps_i2cSendMessage() { \ - gps_i2c_data_ready_to_transmit = TRUE; \ - gps_i2c_tx_extract_idx = 0; \ -} + gps_i2c_data_ready_to_transmit = TRUE; \ + gps_i2c_tx_extract_idx = 0; \ + } // #define gps_i2cTxRunning (gps_i2c_data_ready_to_transmit) // #define gps_i2cInitParam(_baud, _uart_prm1, _uart_prm2) {} diff --git a/sw/airborne/modules/gsm/gsm.c b/sw/airborne/modules/gsm/gsm.c index 7eb1623b6c..bd5fc57b0a 100644 --- a/sw/airborne/modules/gsm/gsm.c +++ b/sw/airborne/modules/gsm/gsm.c @@ -81,7 +81,7 @@ Receiving: #define GSMTransmit(_c) GSMLink(Transmit(_c)) -#define CTRLZ 0x1A +#define CTRLZ 0x1A #define GSM_ORIGIN_MAXLEN 32 #define DATA_MAXLEN 128 @@ -97,7 +97,7 @@ static bool waiting_for_reply; /* An AT command has been sent and an answer is e // static char msg_date[32]; static char expected_ack[10]; -static char gsm_buf[GSM_MAX_PAYLOAD] __attribute__ ((aligned)); +static char gsm_buf[GSM_MAX_PAYLOAD] __attribute__((aligned)); static uint8_t gsm_buf_idx, gsm_buf_len; static char origin[GSM_ORIGIN_MAXLEN]; static char data_to_send[DATA_MAXLEN]; @@ -105,7 +105,7 @@ static char data_to_send[DATA_MAXLEN]; #define STATUS_NONE 0 #define STATUS_CSQ 1 #define STATUS_REQUESTING_MESSAGE 2 -#define STATUS_SEND_AT 3 +#define STATUS_SEND_AT 3 #define STATUS_SEND_CMGF 4 #define STATUS_SEND_CNMI 5 #define STATUS_SEND_CPMS 6 @@ -134,7 +134,7 @@ static void request_for_msg(void); static void Send_CSQ(void); static void Send(const char string[]); static void parse_msg_header(void); -static char* indexn(char*, char, uint8_t); +static char *indexn(char *, char, uint8_t); static uint8_t gcs_index; @@ -142,17 +142,18 @@ static uint8_t gcs_index_max; /*****************************************************************************/ -void gsm_init(void) { +void gsm_init(void) +{ if (gsm_status == STATUS_NONE) { /* First call */ LED_ON(GSM_ONOFF_LED); gsm_status = STATUS_POWERON; - //} else { /* Second call */ - // gsm_buf_idx = 0; - // gsm_line_received = false; - // - // Send_AT(); - // gsm_status = STATUS_SEND_AT; - // gsm_gsm_init_status = FALSE; + //} else { /* Second call */ + // gsm_buf_idx = 0; + // gsm_line_received = false; + // + // Send_AT(); + // gsm_status = STATUS_SEND_AT; + // gsm_gsm_init_status = FALSE; } gcs_index = 0; gcs_index_max = 0; @@ -164,7 +165,8 @@ void gsm_init(void) { #endif } -void gsm_init_report(void) { /* Second call */ +void gsm_init_report(void) /* Second call */ +{ if (gsm_status != STATUS_NONE) { gsm_buf_idx = 0; gsm_line_received = false; @@ -175,13 +177,14 @@ void gsm_init_report(void) { /* Second call */ } } -void gsm_event(void) { +void gsm_event(void) +{ if (GSMBuffer()) { ReadGSMBuffer(); } if (gsm_line_received) { - if (gsm_buf_len > 0) DOWNLINK_SEND_DEBUG_GSM_RECEIVE(DefaultChannel, DefaultDevice, gsm_buf_len, gsm_buf); + if (gsm_buf_len > 0) { DOWNLINK_SEND_DEBUG_GSM_RECEIVE(DefaultChannel, DefaultDevice, gsm_buf_len, gsm_buf); } gsm_got_line(); gsm_line_received = false; } else if (prompt_received) { @@ -200,12 +203,12 @@ static void gsm_got_line(void) Suppr_SMS(index_msg); gsm_status = STATUS_DELETE_SMS; } else if (gsm_status == STATUS_IDLE - && strncmp(CMTI, gsm_buf, strlen(CMTI)) == 0) { + && strncmp(CMTI, gsm_buf, strlen(CMTI)) == 0) { /* A SMS is available */ /* Extracting the index of the message */ - char * first_comma = indexn(gsm_buf, ',',MAXLEN_CMTI_ANSWER); + char *first_comma = indexn(gsm_buf, ',', MAXLEN_CMTI_ANSWER); if (first_comma) { - index_msg = atoi(first_comma+1); + index_msg = atoi(first_comma + 1); request_for_msg(); gsm_status = STATUS_REQUESTING_MESSAGE; } @@ -216,7 +219,7 @@ static void gsm_got_line(void) if (gsm_answer) { waiting_for_reply = false; - switch(gsm_status) { + switch (gsm_status) { case STATUS_CSQ : gsm_send_report_continue(); gsm_status = STATUS_WAITING_PROMPT; @@ -291,42 +294,41 @@ static void gsm_receive_content(void) // Checking the number of the sender if ( //#if ! (defined GCS_NUMBER_1 || defined GCS_NUMBER_2 || defined SAFETY_NUMBER_1 || defined SAFETY_NUMBER_2) - true + true //#else // false //#endif #ifdef GCS_NUMBER_1 - || strncmp((char*)GCS_NUMBER_1, origin, strlen(GCS_NUMBER_1)) == 0 + || strncmp((char *)GCS_NUMBER_1, origin, strlen(GCS_NUMBER_1)) == 0 #endif #ifdef GCS_NUMBER_2 - || strncmp((char*)GCS_NUMBER_2, origin, strlen(GCS_NUMBER_2)) == 0 + || strncmp((char *)GCS_NUMBER_2, origin, strlen(GCS_NUMBER_2)) == 0 #endif #ifdef SAFETY_NUMBER_1 - || strncmp((char*)SAFETY_NUMBER_1, origin, strlen(SAFETY_NUMBER_1)) == 0 + || strncmp((char *)SAFETY_NUMBER_1, origin, strlen(SAFETY_NUMBER_1)) == 0 #endif #ifdef SAFETY_NUMBER_2 - || strncmp((char*)SAFETY_NUMBER_2, origin, strlen(SAFETY_NUMBER_2)) == 0 + || strncmp((char *)SAFETY_NUMBER_2, origin, strlen(SAFETY_NUMBER_2)) == 0 #endif - ) { + ) { // Decoding the message ... // Search for the instruction switch (gsm_buf[0]) { - case 'B' : - { - uint8_t block_index = atoi(gsm_buf+1); - if (block_index > 0) /* Warning: no way to go to the first block */ - nav_goto_block(block_index); - break; + case 'B' : { + uint8_t block_index = atoi(gsm_buf + 1); + if (block_index > 0) { /* Warning: no way to go to the first block */ + nav_goto_block(block_index); } - case 'S' : - { - uint8_t var_index = atoi(gsm_buf+1); - if (var_index > 0) { - float value = atof(indexn(gsm_buf, ' ',MAXLEN_SMS_CONTENT)+1); - DlSetting(var_index, value); - } + break; + } + case 'S' : { + uint8_t var_index = atoi(gsm_buf + 1); + if (var_index > 0) { + float value = atof(indexn(gsm_buf, ' ', MAXLEN_SMS_CONTENT) + 1); + DlSetting(var_index, value); } + } default: // Report an error ??? @@ -352,7 +354,7 @@ void Suppr_SMS(int index_) static void gsm_got_prompt(void) { if (gsm_status == STATUS_WAITING_PROMPT) { // We were waiting for a prompt - char string[strlen(data_to_send) +3]; + char string[strlen(data_to_send) + 3]; sprintf(string, "%s%c", data_to_send, CTRLZ); Send(string); @@ -384,7 +386,7 @@ static void parse_msg_header(void) void gsm_send_report() { gsm_status = STATUS_IDLE; - if(gsm_status == STATUS_IDLE) { + if (gsm_status == STATUS_IDLE) { // Checking the network coverage Send_CSQ(); gsm_status = STATUS_CSQ; @@ -402,7 +404,8 @@ void gsm_send_report_continue(void) // Donnee GPS :ne sont pas envoyes gps_mode, gps.tow, gps.utm_pos.zone, gps_nb_ovrn // Donnees batterie (seuls vsupply et autopilot_flight_time sont envoyes) // concatenation de toutes les infos en un seul message à transmettre - sprintf(data_to_send, "%ld %ld %d %ld %d %d %d %d %d", gps.utm_pos.east, gps.utm_pos.north, gps_course, gps.hmsl, gps.gspeed, -gps.ned_vel.z, vsupply, autopilot_flight_time, rssi); + sprintf(data_to_send, "%ld %ld %d %ld %d %d %d %d %d", gps.utm_pos.east, gps.utm_pos.north, gps_course, gps.hmsl, + gps.gspeed, -gps.ned_vel.z, vsupply, autopilot_flight_time, rssi); // send the number and wait for the prompt char buf[32]; @@ -424,7 +427,7 @@ void gsm_send_report_continue(void) break; } gcs_index++; - if (gcs_index == gcs_index_max) gcs_index = 0; + if (gcs_index == gcs_index_max) { gcs_index = 0; } } @@ -468,8 +471,9 @@ static void Send_CPMS(void) } -static void gsm_parse(uint8_t c) { - switch(c) { +static void gsm_parse(uint8_t c) +{ + switch (c) { case GSM_CMD_LINE_TERMINATION: break; case '>': @@ -479,7 +483,7 @@ static void gsm_parse(uint8_t c) { gsm_buf[gsm_buf_idx] = '\0'; gsm_line_received = true; gsm_buf_len = gsm_buf_idx; - gsm_buf_idx=0; + gsm_buf_idx = 0; break; default: if (gsm_buf_idx < GSM_MAX_PAYLOAD) { @@ -496,8 +500,9 @@ static void Send(const char string[]) { int i = 0; - while(string[i]) + while (string[i]) { GSMTransmit(string[i++]); + } GSMTransmit(GSM_CMD_LINE_TERMINATION); DOWNLINK_SEND_DEBUG_GSM_SEND(DefaultChannel, DefaultDevice, i, string); @@ -505,8 +510,9 @@ static void Send(const char string[]) /* Returns a pointer to the first occurrence of the character c in the firtn n chars of string s. Return NULL if not found */ -static char* indexn(char* s, char c, uint8_t n) { - while(n && (*s != c)) { +static char *indexn(char *s, char c, uint8_t n) +{ + while (n && (*s != c)) { n--; s++; } diff --git a/sw/airborne/modules/gumstix_interface/qr_code_spi_link.c b/sw/airborne/modules/gumstix_interface/qr_code_spi_link.c index c0a1a92526..9a4fdc30ae 100644 --- a/sw/airborne/modules/gumstix_interface/qr_code_spi_link.c +++ b/sw/airborne/modules/gumstix_interface/qr_code_spi_link.c @@ -34,12 +34,13 @@ struct spi_transaction qr_code_spi_link_transaction; static volatile bool_t qr_code_spi_data_available = FALSE; -uint8_t testDataOut[3] = {1,2,3}; -uint8_t testDataIn[3] = {9,9,9}; +uint8_t testDataOut[3] = {1, 2, 3}; +uint8_t testDataIn[3] = {9, 9, 9}; -static void qr_code_spi_link_trans_cb( struct spi_transaction *trans ); +static void qr_code_spi_link_trans_cb(struct spi_transaction *trans); -void qr_code_spi_link_init(void) { +void qr_code_spi_link_init(void) +{ qr_code_spi_link_transaction.cpol = SPICpolIdleHigh; qr_code_spi_link_transaction.cpha = SPICphaEdge2; qr_code_spi_link_transaction.dss = SPIDss8bit; @@ -54,18 +55,20 @@ void qr_code_spi_link_init(void) { } -void qr_code_spi_link_periodic(void) { +void qr_code_spi_link_periodic(void) +{ if (qr_code_spi_data_available) { - qr_code_spi_data_available = FALSE; - uint16_t x,y; - memcpy(&x,qr_code_spi_link_transaction.input_buf,2); - memcpy(&y,qr_code_spi_link_transaction.input_buf+2,2); - DOWNLINK_SEND_VISUALTARGET(DefaultChannel, DefaultDevice, &x, &y); - spi_slave_register(&spi1, &qr_code_spi_link_transaction); + qr_code_spi_data_available = FALSE; + uint16_t x, y; + memcpy(&x, qr_code_spi_link_transaction.input_buf, 2); + memcpy(&y, qr_code_spi_link_transaction.input_buf + 2, 2); + DOWNLINK_SEND_VISUALTARGET(DefaultChannel, DefaultDevice, &x, &y); + spi_slave_register(&spi1, &qr_code_spi_link_transaction); } } -static void qr_code_spi_link_trans_cb( struct spi_transaction *trans __attribute__ ((unused)) ) { +static void qr_code_spi_link_trans_cb(struct spi_transaction *trans __attribute__((unused))) +{ qr_code_spi_data_available = TRUE; } diff --git a/sw/airborne/modules/hott/hott.c b/sw/airborne/modules/hott/hott.c index b186ddc6b6..b715af4be9 100644 --- a/sw/airborne/modules/hott/hott.c +++ b/sw/airborne/modules/hott/hott.c @@ -88,7 +88,8 @@ static struct HOTT_VARIO_MSG hott_vario_msg; /** * Initializes a HoTT GPS message (Receiver answer type !Not Smartbox) */ -static void hott_msgs_init(void) { +static void hott_msgs_init(void) +{ #if HOTT_SIM_EAM_SENSOR hott_init_eam_msg(&hott_eam_msg); @@ -107,60 +108,66 @@ static void hott_msgs_init(void) { #endif } -static void hott_enable_transmitter(void) { +static void hott_enable_transmitter(void) +{ //enables serial transmitter, disables receiver uart_periph_set_mode(&HOTT_PORT, TRUE, FALSE, FALSE); } -static void hott_enable_receiver(void) { +static void hott_enable_receiver(void) +{ //enables serial receiver, disables transmitter uart_periph_set_mode(&HOTT_PORT, TRUE, TRUE, FALSE); } -void hott_init(void) { +void hott_init(void) +{ hott_msgs_init(); } /**/ -void hott_periodic(void) { +void hott_periodic(void) +{ #if HOTT_SIM_EAM_SENSOR if ((hott_telemetry_sendig_msgs_id != HOTT_TELEMETRY_EAM_SENSOR_ID) && - HOTT_REQ_UPDATE_EAM == TRUE) { + HOTT_REQ_UPDATE_EAM == TRUE) { hott_update_eam_msg(&hott_eam_msg); HOTT_REQ_UPDATE_EAM = FALSE; } #endif #if HOTT_SIM_GAM_SENSOR if ((hott_telemetry_sendig_msgs_id != HOTT_TELEMETRY_GAM_SENSOR_ID) && - HOTT_REQ_UPDATE_GAM == TRUE) { + HOTT_REQ_UPDATE_GAM == TRUE) { hott_update_gam_msg(&hott_gam_msg); HOTT_REQ_UPDATE_GAM = FALSE; } #endif #if HOTT_SIM_GPS_SENSOR if ((hott_telemetry_sendig_msgs_id != HOTT_TELEMETRY_GPS_SENSOR_ID) && - HOTT_REQ_UPDATE_GPS == TRUE) { + HOTT_REQ_UPDATE_GPS == TRUE) { hott_update_gps_msg(&hott_gam_msg); HOTT_REQ_UPDATE_GPS = FALSE; } #endif #if HOTT_SIM_VARIO_SENSOR if ((hott_telemetry_sendig_msgs_id != HOTT_TELEMETRY_VARIO_SENSOR_ID) && - HOTT_REQ_UPDATE_VARIO == TRUE) { + HOTT_REQ_UPDATE_VARIO == TRUE) { hott_update_vario_msg(&hott_gam_msg); HOTT_REQ_UPDATE_VARIO = FALSE; } #endif } -static void hott_send_msg(int8_t *buffer, int16_t len) { - if (hott_telemetry_is_sending == TRUE) return; +static void hott_send_msg(int8_t *buffer, int16_t len) +{ + if (hott_telemetry_is_sending == TRUE) { return; } hott_msg_ptr = buffer; hott_msg_len = len + 1; //len + 1 byte for crc hott_telemetry_sendig_msgs_id = buffer[1]; //HoTT msgs id is the 2. byte } -static void hott_send_telemetry_data(void) { +static void hott_send_telemetry_data(void) +{ static int16_t msg_crc = 0; if (!hott_telemetry_is_sending) { hott_telemetry_is_sending = TRUE; @@ -173,36 +180,39 @@ static void hott_send_telemetry_data(void) { hott_telemetry_sendig_msgs_id = 0; msg_crc = 0; hott_enable_receiver(); - while(uart_char_available(&HOTT_PORT)) + while (uart_char_available(&HOTT_PORT)) { uart_getch(&HOTT_PORT); - } - else { + } + } else { --hott_msg_len; if (hott_msg_len != 0) { msg_crc += *hott_msg_ptr; uart_transmit(&HOTT_PORT, *hott_msg_ptr++); - } else + } else { uart_transmit(&HOTT_PORT, (int8_t)msg_crc); + } } } -static void hott_check_serial_data(uint32_t tnow) { +static void hott_check_serial_data(uint32_t tnow) +{ static uint32_t hott_serial_request_timer = 0; - if (hott_telemetry_is_sending == TRUE) return; + if (hott_telemetry_is_sending == TRUE) { return; } if (uart_char_available(&HOTT_PORT) > 1) { if (uart_char_available(&HOTT_PORT) == 2) { if (hott_serial_request_timer == 0) { hott_serial_request_timer = tnow; return; } else { - if (tnow - hott_serial_request_timer < 4600) //wait ca. 5ms + if (tnow - hott_serial_request_timer < 4600) { //wait ca. 5ms return; + } hott_serial_request_timer = 0; } uint8_t c = uart_getch(&HOTT_PORT); uint8_t addr = uart_getch(&HOTT_PORT); - switch(c) { + switch (c) { #if HOTT_SIM_TEXTMODE case HOTT_TEXT_MODE_REQUEST_ID: //Text mode, handle only if not armed! @@ -252,31 +262,34 @@ static void hott_check_serial_data(uint32_t tnow) { default: break; } - } - else { - while(uart_char_available(&HOTT_PORT)) + } else { + while (uart_char_available(&HOTT_PORT)) { uart_getch(&HOTT_PORT); + } hott_serial_request_timer = 0; } } } -static void hott_periodic_event(uint32_t tnow) { +static void hott_periodic_event(uint32_t tnow) +{ static uint32_t hott_serial_timer; hott_check_serial_data(tnow); - if (hott_msg_ptr == 0) return; + if (hott_msg_ptr == 0) { return; } if (hott_telemetry_is_sending) { - if (tnow - hott_serial_timer < 3000) //delay ca. 3,5 mS. 19200 baud = 520uS / int8_t + 3ms required delay + if (tnow - hott_serial_timer < 3000) { //delay ca. 3,5 mS. 19200 baud = 520uS / int8_t + 3ms required delay return; - } - else + } + } else { tnow = get_sys_time_usec(); + } hott_send_telemetry_data(); hott_serial_timer = tnow; } -void hott_event(void) { +void hott_event(void) +{ if (SysTimeTimer(hott_event_timer) > 1000) { SysTimeTimerStart(hott_event_timer); hott_periodic_event(hott_event_timer); diff --git a/sw/airborne/modules/hott/hott_eam.h b/sw/airborne/modules/hott/hott_eam.h index 643555dfa9..060a6863c9 100644 --- a/sw/airborne/modules/hott/hott_eam.h +++ b/sw/airborne/modules/hott/hott_eam.h @@ -39,47 +39,47 @@ struct HOTT_EAM_MSG { int8_t start_byte; //#01 start int8_t int8_t eam_sensor_id; //#02 EAM sensort id. constat value 0x8e int8_t warning_beeps; //#03 1=A 2=B ... or 'A' - 0x40 = 1 - // Q Min cell voltage sensor 1 - // R Min Battery 1 voltage sensor 1 - // J Max Battery 1 voltage sensor 1 - // F Mim temperature sensor 1 - // H Max temperature sensor 1 - // S Min cell voltage sensor 2 - // K Max cell voltage sensor 2 - // G Min temperature sensor 2 - // I Max temperature sensor 2 - // W Max current - // V Max capacity mAh - // P Min main power voltage - // X Max main power voltage - // O Min altitude - // Z Max altitude - // C (negative) sink rate m/sec to high - // B (negative) sink rate m/3sec to high - // N climb rate m/sec to high - // M climb rate m/3sec to high + // Q Min cell voltage sensor 1 + // R Min Battery 1 voltage sensor 1 + // J Max Battery 1 voltage sensor 1 + // F Mim temperature sensor 1 + // H Max temperature sensor 1 + // S Min cell voltage sensor 2 + // K Max cell voltage sensor 2 + // G Min temperature sensor 2 + // I Max temperature sensor 2 + // W Max current + // V Max capacity mAh + // P Min main power voltage + // X Max main power voltage + // O Min altitude + // Z Max altitude + // C (negative) sink rate m/sec to high + // B (negative) sink rate m/3sec to high + // N climb rate m/sec to high + // M climb rate m/3sec to high int8_t sensor_id; //#04 constant value 0xe0 int8_t alarm_invers1; //#05 alarm bitmask. Value is displayed inverted - //Bit# Alarm field - // 0 mAh - // 1 Battery 1 - // 2 Battery 2 - // 3 Temperature 1 - // 4 Temperature 2 - // 5 Altitude - // 6 Current - // 7 Main power voltage + //Bit# Alarm field + // 0 mAh + // 1 Battery 1 + // 2 Battery 2 + // 3 Temperature 1 + // 4 Temperature 2 + // 5 Altitude + // 6 Current + // 7 Main power voltage int8_t alarm_invers2; //#06 alarm bitmask. Value is displayed inverted - //Bit# Alarm Field - // 0 m/s - // 1 m/3s - // 2 Altitude (duplicate?) - // 3 m/s (duplicate?) - // 4 m/3s (duplicate?) - // 5 unknown/unused - // 6 unknown/unused - // 7 "ON" sign/text msg active + //Bit# Alarm Field + // 0 m/s + // 1 m/3s + // 2 Altitude (duplicate?) + // 3 m/s (duplicate?) + // 4 m/3s (duplicate?) + // 5 unknown/unused + // 6 unknown/unused + // 7 "ON" sign/text msg active int8_t cell1_L; //#07 cell 1 voltage lower value. 0.02V steps, 124=2.48V int8_t cell2_L; //#08 @@ -125,10 +125,11 @@ struct HOTT_EAM_MSG { int8_t speed_H; //#42 int8_t stop_byte; //#43 stop int8_t - //#44 CRC/Parity + //#44 CRC/Parity }; -static void hott_init_eam_msg(struct HOTT_EAM_MSG* hott_eam_msg) { +static void hott_init_eam_msg(struct HOTT_EAM_MSG *hott_eam_msg) +{ memset(hott_eam_msg, 0, sizeof(struct HOTT_EAM_MSG)); hott_eam_msg->start_byte = 0x7C; hott_eam_msg->eam_sensor_id = HOTT_TELEMETRY_EAM_SENSOR_ID; @@ -136,7 +137,8 @@ static void hott_init_eam_msg(struct HOTT_EAM_MSG* hott_eam_msg) { hott_eam_msg->stop_byte = 0x7D; } -static void hott_update_eam_msg(struct HOTT_EAM_MSG* hott_eam_msg) { +static void hott_update_eam_msg(struct HOTT_EAM_MSG *hott_eam_msg) +{ hott_eam_msg->batt1_voltage = electrical.vsupply; hott_eam_msg->batt2_voltage = electrical.vsupply; diff --git a/sw/airborne/modules/hott/hott_gam.h b/sw/airborne/modules/hott/hott_gam.h index 9a32f9acc0..9b53a5c3bc 100644 --- a/sw/airborne/modules/hott/hott_gam.h +++ b/sw/airborne/modules/hott/hott_gam.h @@ -36,49 +36,49 @@ struct HOTT_GAM_MSG { int8_t start_byte; //#01 start int8_t constant value 0x7c int8_t gam_sensor_id; //#02 EAM sensort id. constat value 0x8d int8_t warning_beeps; //#03 1=A 2=B ... 0x1a=Z 0 = no alarm - // Q Min cell voltage sensor 1 - // R Min Battery 1 voltage sensor 1 - // J Max Battery 1 voltage sensor 1 - // F Min temperature sensor 1 - // H Max temperature sensor 1 - // S Min Battery 2 voltage sensor 2 - // K Max Battery 2 voltage sensor 2 - // G Min temperature sensor 2 - // I Max temperature sensor 2 - // W Max current - // V Max capacity mAh - // P Min main power voltage - // X Max main power voltage - // O Min altitude - // Z Max altitude - // C negative difference m/s too high - // A negative difference m/3s too high - // N positive difference m/s too high - // L positive difference m/3s too high - // T Minimum RPM - // Y Maximum RPM + // Q Min cell voltage sensor 1 + // R Min Battery 1 voltage sensor 1 + // J Max Battery 1 voltage sensor 1 + // F Min temperature sensor 1 + // H Max temperature sensor 1 + // S Min Battery 2 voltage sensor 2 + // K Max Battery 2 voltage sensor 2 + // G Min temperature sensor 2 + // I Max temperature sensor 2 + // W Max current + // V Max capacity mAh + // P Min main power voltage + // X Max main power voltage + // O Min altitude + // Z Max altitude + // C negative difference m/s too high + // A negative difference m/3s too high + // N positive difference m/s too high + // L positive difference m/3s too high + // T Minimum RPM + // Y Maximum RPM int8_t sensor_id; //#04 constant value 0xd0 int8_t alarm_invers1; //#05 alarm bitmask. Value is displayed inverted - //Bit# Alarm field - // 0 all cell voltage - // 1 Battery 1 - // 2 Battery 2 - // 3 Temperature 1 - // 4 Temperature 2 - // 5 Fuel - // 6 mAh - // 7 Altitude + //Bit# Alarm field + // 0 all cell voltage + // 1 Battery 1 + // 2 Battery 2 + // 3 Temperature 1 + // 4 Temperature 2 + // 5 Fuel + // 6 mAh + // 7 Altitude int8_t alarm_invers2; //#06 alarm bitmask. Value is displayed inverted - //Bit# Alarm Field - // 0 main power current - // 1 main power voltage - // 2 Altitude - // 3 m/s - // 4 m/3s - // 5 unknown - // 6 unknown - // 7 "ON" sign/text msg active + //Bit# Alarm Field + // 0 main power current + // 1 main power voltage + // 2 Altitude + // 3 m/s + // 4 m/3s + // 5 unknown + // 6 unknown + // 7 "ON" sign/text msg active int8_t cell1; //#07 cell 1 voltage lower value. 0.02V steps, 124=2.48V int8_t cell2; //#08 @@ -93,7 +93,7 @@ struct HOTT_GAM_MSG { int8_t temperature1; //#17 temperature 1. offset of 20. a value of 20 = 0°C int8_t temperature2; //#18 temperature 2. offset of 20. a value of 20 = 0°C int8_t fuel_procent; //#19 Fuel capacity in %. Values 0--100 - // graphical display ranges: 0-25% 50% 75% 100% + // graphical display ranges: 0-25% 50% 75% 100% int8_t fuel_ml_L; //#20 Fuel in ml scale. Full = 65535! int8_t fuel_ml_H; //#21 int8_t rpm_L; //#22 RPM in 10 RPM steps. 300 = 3000rpm @@ -119,10 +119,11 @@ struct HOTT_GAM_MSG { int8_t pressure; //#42 Pressure up to 16bar. 0,1bar scale. 20 = 2bar int8_t version; //#43 version number TODO: more info? int8_t stop_byte; //#44 stop int8_t - //#45 CRC/Parity + //#45 CRC/Parity }; -static void hott_init_gam_msg(struct HOTT_GAM_MSG* hott_gam_msg) { +static void hott_init_gam_msg(struct HOTT_GAM_MSG *hott_gam_msg) +{ memset(hott_gam_msg, 0, sizeof(struct HOTT_GAM_MSG)); hott_gam_msg->start_byte = 0x7C; hott_gam_msg->gam_sensor_id = HOTT_TELEMETRY_GAM_SENSOR_ID; @@ -130,7 +131,8 @@ static void hott_init_gam_msg(struct HOTT_GAM_MSG* hott_gam_msg) { hott_gam_msg->stop_byte = 0x7D; } -static void hott_update_gam_msg(struct HOTT_GAM_MSG* hott_gam_msg) { +static void hott_update_gam_msg(struct HOTT_GAM_MSG *hott_gam_msg) +{ hott_gam_msg->temperature1 = 1; hott_gam_msg->temperature2 = 2; // 0°C hott_gam_msg->altitude_L = 3; diff --git a/sw/airborne/modules/ins/ahrs_chimu_spi.c b/sw/airborne/modules/ins/ahrs_chimu_spi.c index f27352bb0a..75b0167b02 100644 --- a/sw/airborne/modules/ins/ahrs_chimu_spi.c +++ b/sw/airborne/modules/ins/ahrs_chimu_spi.c @@ -39,7 +39,7 @@ void ahrs_init(void) ahrs.status = AHRS_UNINIT; // uint8_t ping[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG00_PING, 0x00, 0xE6 }; - uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI + uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI uint8_t quaternions[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x01, 0x39 }; // 25Hz attitude only + SPI // uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI // uint8_t euler[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x00, 0xaf }; // 25Hz attitude only + SPI @@ -53,8 +53,8 @@ void ahrs_init(void) CHIMU_Init(&CHIMU_DATA); // Quat Filter - CHIMU_Checksum(quaternions,7); - InsSend(quaternions,7); + CHIMU_Checksum(quaternions, 7); + InsSend(quaternions, 7); // Wait a bit (SPI send zero) InsSend1(0); @@ -64,8 +64,8 @@ void ahrs_init(void) InsSend1(0); // 50Hz data: attitude only - CHIMU_Checksum(rate,12); - InsSend(rate,12); + CHIMU_Checksum(rate, 12); + InsSend(rate, 12); } void ahrs_align(void) @@ -73,14 +73,14 @@ void ahrs_align(void) ahrs.status = AHRS_RUNNING; } -void parse_ins_msg( void ) +void parse_ins_msg(void) { while (InsLink(ChAvailable())) { uint8_t ch = InsLink(Getch()); if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) { - RunOnceEvery(25, LED_TOGGLE(3) ); - if(CHIMU_DATA.m_MsgID==CHIMU_Msg_3_IMU_Attitude) { + RunOnceEvery(25, LED_TOGGLE(3)); + if (CHIMU_DATA.m_MsgID == CHIMU_Msg_3_IMU_Attitude) { new_ins_attitude = 1; if (CHIMU_DATA.m_attitude.euler.phi > M_PI) { CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI; @@ -98,10 +98,10 @@ void parse_ins_msg( void ) 0. }; // FIXME rate r stateSetBodyRates_f(&rates); - } - else if(CHIMU_DATA.m_MsgID==0x02) { + } else if (CHIMU_DATA.m_MsgID == 0x02) { #if CHIMU_DOWNLINK_IMMEDIATE - RunOnceEvery(25,DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_sensor.rate[0], &CHIMU_DATA.m_sensor.rate[1], &CHIMU_DATA.m_sensor.rate[2])); + RunOnceEvery(25, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_sensor.rate[0], + &CHIMU_DATA.m_sensor.rate[1], &CHIMU_DATA.m_sensor.rate[2])); #endif } } @@ -117,22 +117,24 @@ void ahrs_update_gps(void) float gps_speed = 0; if (gps.fix == GPS_FIX_3D) { - gps_speed = gps.speed_3d/100.; + gps_speed = gps.speed_3d / 100.; } gps_speed = FloatSwap(gps_speed); - memmove (¢ripedal[6], &gps_speed, 4); + memmove(¢ripedal[6], &gps_speed, 4); // Fill X-speed - CHIMU_Checksum(centripedal,19); - InsSend(centripedal,19); + CHIMU_Checksum(centripedal, 19); + InsSend(centripedal, 19); // Downlink Send } -void ahrs_propagate(float dt __attribute__((unused))) { +void ahrs_propagate(float dt __attribute__((unused))) +{ } -void ahrs_update_accel(float dt __attribute__((unused))) { +void ahrs_update_accel(float dt __attribute__((unused))) +{ } diff --git a/sw/airborne/modules/ins/ahrs_chimu_uart.c b/sw/airborne/modules/ins/ahrs_chimu_uart.c index a549e709a9..d45708e0c2 100644 --- a/sw/airborne/modules/ins/ahrs_chimu_uart.c +++ b/sw/airborne/modules/ins/ahrs_chimu_uart.c @@ -35,7 +35,7 @@ void ahrs_init(void) ahrs.status = AHRS_UNINIT; uint8_t ping[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG00_PING, 0x00, 0xE6 }; - uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI + uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI uint8_t quaternions[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x01, 0x39 }; // 25Hz attitude only + SPI // uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI // uint8_t euler[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x00, 0xaf }; // 25Hz attitude only + SPI @@ -48,18 +48,18 @@ void ahrs_init(void) CHIMU_Init(&CHIMU_DATA); // Request Software version - for (int i=0;i<7;i++) { + for (int i = 0; i < 7; i++) { InsUartSend1(ping[i]); } // Quat Filter - for (int i=0;i<7;i++) { + for (int i = 0; i < 7; i++) { InsUartSend1(quaternions[i]); } // 50Hz - CHIMU_Checksum(rate,12); - InsSend(rate,12); + CHIMU_Checksum(rate, 12); + InsSend(rate, 12); } void ahrs_align(void) { @@ -67,15 +67,15 @@ void ahrs_align(void) } -void parse_ins_msg( void ) +void parse_ins_msg(void) { while (InsLink(ChAvailable())) { uint8_t ch = InsLink(Getch()); if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) { - if(CHIMU_DATA.m_MsgID==0x03) { + if (CHIMU_DATA.m_MsgID == 0x03) { new_ins_attitude = 1; - RunOnceEvery(25, LED_TOGGLE(3) ); + RunOnceEvery(25, LED_TOGGLE(3)); if (CHIMU_DATA.m_attitude.euler.phi > M_PI) { CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI; } @@ -87,7 +87,8 @@ void parse_ins_msg( void ) }; stateSetNedToBodyEulers_f(&att); #if CHIMU_DOWNLINK_IMMEDIATE - DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_attitude.euler.phi, &CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi); + DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_attitude.euler.phi, + &CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi); #endif } @@ -95,6 +96,6 @@ void parse_ins_msg( void ) } } -void ahrs_update_gps( void ) +void ahrs_update_gps(void) { } diff --git a/sw/airborne/modules/ins/alt_filter.c b/sw/airborne/modules/ins/alt_filter.c index 9fd05f5109..bafb13e244 100644 --- a/sw/airborne/modules/ins/alt_filter.c +++ b/sw/airborne/modules/ins/alt_filter.c @@ -46,7 +46,8 @@ void kalmanCorrectionAltimetre(TypeKalman *k, float altitude_altimetre); float last_gps_alt; float last_baro_alt; -void alt_filter_init(void) { +void alt_filter_init(void) +{ SigAltiGPS = 5.; SigAltiAltimetre = 5.; MarcheAleaBiaisAltimetre = 0.1; @@ -57,9 +58,10 @@ void alt_filter_init(void) { kalmanInit(&alt_filter); } -void alt_filter_periodic(void) { +void alt_filter_periodic(void) +{ // estimation at each step - kalmanEstimation(&alt_filter,0.); + kalmanEstimation(&alt_filter, 0.); // update on new data float ga = (float)gps.hmsl / 1000.; @@ -72,9 +74,9 @@ void alt_filter_periodic(void) { last_gps_alt = ga; } - RunOnceEvery(6,DOWNLINK_SEND_VFF(DefaultChannel, DefaultDevice, &baro_ets_altitude, - &(alt_filter.X[0]), &(alt_filter.X[1]), &(alt_filter.X[2]), - &(alt_filter.P[0][0]), &(alt_filter.P[1][1]), &(alt_filter.P[2][2]))); + RunOnceEvery(6, DOWNLINK_SEND_VFF(DefaultChannel, DefaultDevice, &baro_ets_altitude, + &(alt_filter.X[0]), &(alt_filter.X[1]), &(alt_filter.X[2]), + &(alt_filter.P[0][0]), &(alt_filter.P[1][1]), &(alt_filter.P[2][2]))); } @@ -86,10 +88,11 @@ void alt_filter_periodic(void) { *************************************************************************/ -void kalmanInit(TypeKalman *k){ +void kalmanInit(TypeKalman *k) +{ - k->W[0][0] = MarcheAleaAccelerometre*MarcheAleaAccelerometre; k->W[0][1] = 0; - k->W[1][0] = 0; k->W[1][1] = MarcheAleaBiaisAltimetre*MarcheAleaBiaisAltimetre; + k->W[0][0] = MarcheAleaAccelerometre * MarcheAleaAccelerometre; k->W[0][1] = 0; + k->W[1][0] = 0; k->W[1][1] = MarcheAleaBiaisAltimetre * MarcheAleaBiaisAltimetre; k->X[0] = 0; k->X[1] = 0; @@ -99,7 +102,7 @@ void kalmanInit(TypeKalman *k){ k->P[1][0] = 0; k->P[1][1] = 1; k->P[1][2] = 0; k->P[2][0] = 0; k->P[2][1] = 0; k->P[2][2] = 0.0001; - k->Te = (1./60.); + k->Te = (1. / 60.); // System dynamic k->Ad[0][0] = 1; k->Ad[0][1] = k->Te; k->Ad[0][2] = 0; @@ -107,11 +110,11 @@ void kalmanInit(TypeKalman *k){ k->Ad[2][0] = 0; k->Ad[2][1] = 0; k->Ad[2][2] = 1; // System command - k->Bd[0] = pow(k->Te,2)/2; + k->Bd[0] = pow(k->Te, 2) / 2; k->Bd[1] = k->Te; k->Bd[2] = 0; - k->Md[0][0] = pow(k->Te, 1.5)/2; k->Md[0][1] = 0; + k->Md[0][0] = pow(k->Te, 1.5) / 2; k->Md[0][1] = 0; k->Md[1][0] = pow(k->Te, 0.5); k->Md[1][1] = 0; k->Md[2][0] = 0; k->Md[2][1] = pow(k->Te, 0.5); } @@ -124,42 +127,43 @@ void kalmanInit(TypeKalman *k){ *************************************************************************/ -void kalmanEstimation(TypeKalman *k, float accVert){ +void kalmanEstimation(TypeKalman *k, float accVert) +{ - int i,j; + int i, j; float I[3][3]; // matrices temporaires float J[3][2]; // Calcul de X // X(k+1) = Ad*X(k) + Bd*U(k) - k->X[0] = k->Ad[0][0]*k->X[0] + k->Ad[0][1]*k->X[1] + k->Ad[0][2]*k->X[2] + k->Bd[0]*accVert; - k->X[1] = k->Ad[1][0]*k->X[0] + k->Ad[1][1]*k->X[1] + k->Ad[1][2]*k->X[2] + k->Bd[1]*accVert; - k->X[2] = k->Ad[2][0]*k->X[0] + k->Ad[2][1]*k->X[1] + k->Ad[2][2]*k->X[2] + k->Bd[2]*accVert; + k->X[0] = k->Ad[0][0] * k->X[0] + k->Ad[0][1] * k->X[1] + k->Ad[0][2] * k->X[2] + k->Bd[0] * accVert; + k->X[1] = k->Ad[1][0] * k->X[0] + k->Ad[1][1] * k->X[1] + k->Ad[1][2] * k->X[2] + k->Bd[1] * accVert; + k->X[2] = k->Ad[2][0] * k->X[0] + k->Ad[2][1] * k->X[1] + k->Ad[2][2] * k->X[2] + k->Bd[2] * accVert; // Calcul de P // P(k+1) = Ad*P(k)*Ad' + Md*W*Md' - for(i=0;i<3;i++){ - for (j=0;j<3;j++){ - I[i][j] = k->Ad[i][0]*k->P[0][j] + k->Ad[i][1]*k->P[1][j] + k->Ad[i][2]*k->P[2][j]; + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + I[i][j] = k->Ad[i][0] * k->P[0][j] + k->Ad[i][1] * k->P[1][j] + k->Ad[i][2] * k->P[2][j]; } } - for(i=0;i<3;i++){ - for (j=0;j<3;j++){ - k->P[i][j] = I[i][0]*k->Ad[j][0] + I[i][1]*k->Ad[j][1] + I[i][2]*k->Ad[j][2]; + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + k->P[i][j] = I[i][0] * k->Ad[j][0] + I[i][1] * k->Ad[j][1] + I[i][2] * k->Ad[j][2]; } } - for(i=0;i<3;i++){ - for (j=0;j<2;j++){ - J[i][j] = k->Md[i][0]*k->W[0][j] + k->Md[i][1]*k->W[1][j]; + for (i = 0; i < 3; i++) { + for (j = 0; j < 2; j++) { + J[i][j] = k->Md[i][0] * k->W[0][j] + k->Md[i][1] * k->W[1][j]; } } - for(i=0;i<3;i++){ - for (j=0;j<3;j++){ - k->P[i][j] = k->P[i][j] + J[i][0]*k->Md[j][0] + J[i][1]*k->Md[j][1]; + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + k->P[i][j] = k->P[i][j] + J[i][0] * k->Md[j][0] + J[i][1] * k->Md[j][1]; } } @@ -171,7 +175,9 @@ void kalmanEstimation(TypeKalman *k, float accVert){ * *************************************************************************/ -void kalmanCorrectionGPS(TypeKalman *k, float altitude_gps){ // altitude_gps est l'altitude telle qu'elle est mesurée par le GPS +void kalmanCorrectionGPS(TypeKalman *k, + float altitude_gps) // altitude_gps est l'altitude telle qu'elle est mesurée par le GPS +{ int i, j, div; float I[3][3]; // matrice temporaire @@ -181,7 +187,7 @@ void kalmanCorrectionGPS(TypeKalman *k, float altitude_gps){ // altitude_gps est // calcul de Kf // C = [1 0 0] // div = C*P*C' + R - div = k->P[0][0] + SigAltiGPS*SigAltiGPS; + div = k->P[0][0] + SigAltiGPS * SigAltiGPS; if (fabs(div) > 1e-5) { // Kf = P*C'*inv(div) @@ -192,8 +198,8 @@ void kalmanCorrectionGPS(TypeKalman *k, float altitude_gps){ // altitude_gps est // calcul de X // X = X + Kf*(meas - C*X) float constante = k->X[0]; - for(i=0;i<3;i++){ - k->X[i] = k->X[i]+Kf[i]*(altitude_gps - constante); + for (i = 0; i < 3; i++) { + k->X[i] = k->X[i] + Kf[i] * (altitude_gps - constante); } // calcul de P @@ -202,9 +208,9 @@ void kalmanCorrectionGPS(TypeKalman *k, float altitude_gps){ // altitude_gps est I[1][0] = Kf[1]; I[1][1] = 0; I[1][2] = 0; I[2][0] = Kf[2]; I[2][1] = 0; I[2][2] = 0; - for(i=0;i<3;i++){ - for (j=0;j<3;j++){ - k->P[i][j] = k->P[i][j] - I[i][0]*k->P[0][j] - I[i][1]*k->P[1][j] - I[i][2]*k->P[2][j]; + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + k->P[i][j] = k->P[i][j] - I[i][0] * k->P[0][j] - I[i][1] * k->P[1][j] - I[i][2] * k->P[2][j]; } } } @@ -217,7 +223,8 @@ void kalmanCorrectionGPS(TypeKalman *k, float altitude_gps){ // altitude_gps est * *************************************************************************/ -void kalmanCorrectionAltimetre(TypeKalman *k, float altitude_altimetre){ +void kalmanCorrectionAltimetre(TypeKalman *k, float altitude_altimetre) +{ int i, j, div; float I[3][3]; // matrice temporaire @@ -227,7 +234,7 @@ void kalmanCorrectionAltimetre(TypeKalman *k, float altitude_altimetre){ // calcul de Kf // C = [1 0 1] // div = C*P*C' + R - div = k->P[0][0] + k->P[2][0] + k->P[0][2] + k->P[2][2] + SigAltiAltimetre*SigAltiAltimetre; + div = k->P[0][0] + k->P[2][0] + k->P[0][2] + k->P[2][2] + SigAltiAltimetre * SigAltiAltimetre; if (fabs(div) > 1e-5) { // Kf = P*C'*inv(div) @@ -238,8 +245,8 @@ void kalmanCorrectionAltimetre(TypeKalman *k, float altitude_altimetre){ // calcul de X // X = X + Kf*(meas - C*X) float constante = k->X[0] + k->X[2]; - for(i=0;i<3;i++){ - k->X[i] = k->X[i]+Kf[i]*(altitude_altimetre - constante); + for (i = 0; i < 3; i++) { + k->X[i] = k->X[i] + Kf[i] * (altitude_altimetre - constante); } // calcul de P @@ -248,9 +255,9 @@ void kalmanCorrectionAltimetre(TypeKalman *k, float altitude_altimetre){ I[1][0] = Kf[1]; I[1][1] = 0; I[1][2] = Kf[1]; I[2][0] = Kf[2]; I[2][1] = 0; I[2][2] = Kf[2]; - for(i=0;i<3;i++){ - for (j=0;j<3;j++){ - k->P[i][j] = k->P[i][j] - I[i][0]*k->P[0][j] - I[i][1]*k->P[1][j] - I[i][2]*k->P[2][j]; + for (i = 0; i < 3; i++) { + for (j = 0; j < 3; j++) { + k->P[i][j] = k->P[i][j] - I[i][0] * k->P[0][j] - I[i][1] * k->P[1][j] - I[i][2] * k->P[2][j]; } } } diff --git a/sw/airborne/modules/ins/alt_filter.h b/sw/airborne/modules/ins/alt_filter.h index b21af56ffa..7ae19a3b84 100644 --- a/sw/airborne/modules/ins/alt_filter.h +++ b/sw/airborne/modules/ins/alt_filter.h @@ -35,15 +35,14 @@ #define KALT_N_ETAT 3 -typedef struct -{ +typedef struct { float Te; float P[KALT_N_ETAT][KALT_N_ETAT]; - float W[KALT_N_ETAT-1][KALT_N_ETAT-1]; + float W[KALT_N_ETAT - 1][KALT_N_ETAT - 1]; float X[KALT_N_ETAT]; float Bd[KALT_N_ETAT]; float Ad[KALT_N_ETAT][KALT_N_ETAT]; - float Md[KALT_N_ETAT][KALT_N_ETAT-1]; + float Md[KALT_N_ETAT][KALT_N_ETAT - 1]; } TypeKalman; extern TypeKalman alt_filter; diff --git a/sw/airborne/modules/ins/imu_chimu.c b/sw/airborne/modules/ins/imu_chimu.c index bc2400c460..bcbb09f530 100644 --- a/sw/airborne/modules/ins/imu_chimu.c +++ b/sw/airborne/modules/ins/imu_chimu.c @@ -54,7 +54,7 @@ static unsigned long UpdateCRC(unsigned long CRC_acc, void *data, unsigned long // Create the CRC "dividend" for polynomial arithmetic (binary arithmetic // with no carries) - unsigned char *CRC_input = (unsigned char*)data; + unsigned char *CRC_input = (unsigned char *)data; for (unsigned long j = data_len; j; --j) { CRC_acc = CRC_acc ^ *CRC_input++; @@ -69,8 +69,7 @@ static unsigned long UpdateCRC(unsigned long CRC_acc, void *data, unsigned long // if so, shift the CRC value, and XOR "subtract" the poly CRC_acc = CRC_acc >> 1; CRC_acc ^= POLY; - } - else { + } else { // if not, just shift the CRC value CRC_acc = CRC_acc >> 1; } @@ -82,25 +81,25 @@ static unsigned long UpdateCRC(unsigned long CRC_acc, void *data, unsigned long void CHIMU_Checksum(unsigned char *data, unsigned char buflen) { - data[buflen-1] = (unsigned char) (UpdateCRC(0xFFFFFFFF , data , (unsigned long) (buflen - 1) ) & 0xff) ; + data[buflen - 1] = (unsigned char)(UpdateCRC(0xFFFFFFFF , data , (unsigned long)(buflen - 1)) & 0xff) ; } /*************************************************************************** - * CHIMU Protocol Definition + * CHIMU Protocol Definition */ // Lowlevel Protocol Decoding #define CHIMU_STATE_MACHINE_START 0 #define CHIMU_STATE_MACHINE_HEADER2 1 -#define CHIMU_STATE_MACHINE_LEN 2 -#define CHIMU_STATE_MACHINE_DEVICE 3 -#define CHIMU_STATE_MACHINE_ID 4 -#define CHIMU_STATE_MACHINE_PAYLOAD 5 -#define CHIMU_STATE_MACHINE_XSUM 6 +#define CHIMU_STATE_MACHINE_LEN 2 +#define CHIMU_STATE_MACHINE_DEVICE 3 +#define CHIMU_STATE_MACHINE_ID 4 +#define CHIMU_STATE_MACHINE_PAYLOAD 5 +#define CHIMU_STATE_MACHINE_XSUM 6 // Communication Definitions -#define CHIMU_COM_ID_HIGH 0x1F //Must set this to the max ID expected above +#define CHIMU_COM_ID_HIGH 0x1F //Must set this to the max ID expected above /*--------------------------------------------------------------------------- Name: CHIMU_Init @@ -116,13 +115,13 @@ void CHIMU_Init(CHIMU_PARSER_DATA *pstData) pstData->m_PayloadIndex = 0; //Sensor data holder - pstData->m_sensor.cputemp=0.0; - for (i=0;i<3;i++) { - pstData->m_sensor.acc[i]=0.0; - pstData->m_sensor.rate[i]=0.0; - pstData->m_sensor.mag[i]=0.0; + pstData->m_sensor.cputemp = 0.0; + for (i = 0; i < 3; i++) { + pstData->m_sensor.acc[i] = 0.0; + pstData->m_sensor.rate[i] = 0.0; + pstData->m_sensor.mag[i] = 0.0; } - pstData->m_sensor.spare1=0.0; + pstData->m_sensor.spare1 = 0.0; //Attitude data pstData->m_attitude.euler.phi = 0.0; pstData->m_attitude.euler.theta = 0.0; @@ -132,13 +131,13 @@ void CHIMU_Init(CHIMU_PARSER_DATA *pstData) pstData->m_attrates.euler.theta = 0.0; pstData->m_attrates.euler.psi = 0.0; - for (i=0; im_Payload[i]= 0x00; - pstData->m_FullMessage[i]= 0x00; + for (i = 0; i < CHIMU_RX_BUFFERSIZE; i++) { + pstData->m_Payload[i] = 0x00; + pstData->m_FullMessage[i] = 0x00; } pstData->m_MsgLen = 0; pstData->m_MsgID = 0; - pstData->m_TempDeviceID =0; + pstData->m_TempDeviceID = 0; pstData->m_DeviceID = 0x01; //look at this later } @@ -148,89 +147,91 @@ void CHIMU_Init(CHIMU_PARSER_DATA *pstData) ---------------------------------------------------------------------------*/ unsigned char CHIMU_Parse( - unsigned char btData, /* input byte stream buffer */ - unsigned char bInputType __attribute__((unused)), /* for future use if special builds of CHIMU data are performed */ - CHIMU_PARSER_DATA *pstData) /* resulting data */ + unsigned char btData, /* input byte stream buffer */ + unsigned char bInputType __attribute__(( + unused)), /* for future use if special builds of CHIMU data are performed */ + CHIMU_PARSER_DATA *pstData) /* resulting data */ { char bUpdate = FALSE; switch (pstData->m_State) { - case CHIMU_STATE_MACHINE_START: // Waiting for start character 0xAE - if(btData == 0xAE) { - pstData->m_State = CHIMU_STATE_MACHINE_HEADER2; - pstData->m_Index = 0; - pstData->m_FullMessage[pstData->m_Index++] = btData; - } else { - ;; - } - bUpdate = FALSE; - break; - case CHIMU_STATE_MACHINE_HEADER2: // Waiting for second header character 0xAE - if(btData == 0xAE) { - pstData->m_State = CHIMU_STATE_MACHINE_LEN; - pstData->m_FullMessage[pstData->m_Index++]=btData; - } else { - pstData->m_State = CHIMU_STATE_MACHINE_START; - } //Fail to see header. Restart. - break; - case CHIMU_STATE_MACHINE_LEN: // Get chars to read - if( btData <= CHIMU_RX_BUFFERSIZE) { - pstData->m_MsgLen = btData ; // It might be invalid, but we do a check on buffer size - pstData->m_FullMessage[pstData->m_Index++]=btData; - pstData->m_State = CHIMU_STATE_MACHINE_DEVICE; - } else { - pstData->m_State = CHIMU_STATE_MACHINE_START; //Length byte exceeds buffer. Signal a fail and restart - //BuiltInTest(BIT_COM_UART_RECEIPTFAIL, BIT_FAIL); - } - break; - case CHIMU_STATE_MACHINE_DEVICE: // Get device. If not us, ignore and move on. Allows common com with Monkey / Chipmunk - if( (btData == pstData->m_DeviceID) || (btData == 0xAA)) { - //0xAA is global message - pstData->m_TempDeviceID = btData; - pstData->m_FullMessage[pstData->m_Index++]=btData; - pstData->m_State = CHIMU_STATE_MACHINE_ID; - } else { - pstData->m_State = CHIMU_STATE_MACHINE_START; - } //Fail to see correct device ID. Restart. - break; - case CHIMU_STATE_MACHINE_ID: // Get ID - pstData->m_MsgID = btData; // might be invalid, chgeck it out here: - if ( pstData->m_MsgID>CHIMU_COM_ID_HIGH) { - pstData->m_State = CHIMU_STATE_MACHINE_START; - //BuiltInTest(BIT_COM_UART_RECEIPTFAIL, BIT_FAIL); - } else { - pstData->m_FullMessage[pstData->m_Index++]=btData; - pstData->m_PayloadIndex = 0; - pstData->m_State = CHIMU_STATE_MACHINE_PAYLOAD; //Finally....Good to go... - } - break; - case CHIMU_STATE_MACHINE_PAYLOAD: // Waiting for number of bytes in payload - pstData->m_Payload[pstData->m_PayloadIndex++]= btData; - pstData->m_FullMessage[pstData->m_Index++]=btData; - if ((pstData->m_Index) >= (pstData->m_MsgLen + 5)) { - //Now we have the payload. Verify XSUM and then parse it next - pstData->m_Checksum = (unsigned char) ((UpdateCRC(0xFFFFFFFF , pstData->m_FullMessage , (unsigned long) (pstData->m_MsgLen)+5)) & 0xFF); - pstData->m_State = CHIMU_STATE_MACHINE_XSUM; - } else { - return FALSE; - } - break; - case CHIMU_STATE_MACHINE_XSUM: // Verify - pstData->m_ReceivedChecksum = btData; - pstData->m_FullMessage[pstData->m_Index++]=btData; - if (pstData->m_Checksum!=pstData->m_ReceivedChecksum) { + case CHIMU_STATE_MACHINE_START: // Waiting for start character 0xAE + if (btData == 0xAE) { + pstData->m_State = CHIMU_STATE_MACHINE_HEADER2; + pstData->m_Index = 0; + pstData->m_FullMessage[pstData->m_Index++] = btData; + } else { + ;; + } bUpdate = FALSE; - //BuiltInTest(BIT_COM_UART_RECEIPTFAIL, BIT_FAIL); - } else { - //Xsum passed, go parse it. - // We have pstData->m_MsgID to parse off of, pstData->m_pstData->m_Payload as the data. - bUpdate = CHIMU_ProcessMessage(&pstData->m_MsgID, pstData->m_Payload, pstData); - } - pstData->m_State = CHIMU_STATE_MACHINE_START; - break; - default: - pstData->m_State = CHIMU_STATE_MACHINE_START; + break; + case CHIMU_STATE_MACHINE_HEADER2: // Waiting for second header character 0xAE + if (btData == 0xAE) { + pstData->m_State = CHIMU_STATE_MACHINE_LEN; + pstData->m_FullMessage[pstData->m_Index++] = btData; + } else { + pstData->m_State = CHIMU_STATE_MACHINE_START; + } //Fail to see header. Restart. + break; + case CHIMU_STATE_MACHINE_LEN: // Get chars to read + if (btData <= CHIMU_RX_BUFFERSIZE) { + pstData->m_MsgLen = btData ; // It might be invalid, but we do a check on buffer size + pstData->m_FullMessage[pstData->m_Index++] = btData; + pstData->m_State = CHIMU_STATE_MACHINE_DEVICE; + } else { + pstData->m_State = CHIMU_STATE_MACHINE_START; //Length byte exceeds buffer. Signal a fail and restart + //BuiltInTest(BIT_COM_UART_RECEIPTFAIL, BIT_FAIL); + } + break; + case CHIMU_STATE_MACHINE_DEVICE: // Get device. If not us, ignore and move on. Allows common com with Monkey / Chipmunk + if ((btData == pstData->m_DeviceID) || (btData == 0xAA)) { + //0xAA is global message + pstData->m_TempDeviceID = btData; + pstData->m_FullMessage[pstData->m_Index++] = btData; + pstData->m_State = CHIMU_STATE_MACHINE_ID; + } else { + pstData->m_State = CHIMU_STATE_MACHINE_START; + } //Fail to see correct device ID. Restart. + break; + case CHIMU_STATE_MACHINE_ID: // Get ID + pstData->m_MsgID = btData; // might be invalid, chgeck it out here: + if (pstData->m_MsgID > CHIMU_COM_ID_HIGH) { + pstData->m_State = CHIMU_STATE_MACHINE_START; + //BuiltInTest(BIT_COM_UART_RECEIPTFAIL, BIT_FAIL); + } else { + pstData->m_FullMessage[pstData->m_Index++] = btData; + pstData->m_PayloadIndex = 0; + pstData->m_State = CHIMU_STATE_MACHINE_PAYLOAD; //Finally....Good to go... + } + break; + case CHIMU_STATE_MACHINE_PAYLOAD: // Waiting for number of bytes in payload + pstData->m_Payload[pstData->m_PayloadIndex++] = btData; + pstData->m_FullMessage[pstData->m_Index++] = btData; + if ((pstData->m_Index) >= (pstData->m_MsgLen + 5)) { + //Now we have the payload. Verify XSUM and then parse it next + pstData->m_Checksum = (unsigned char)((UpdateCRC(0xFFFFFFFF , pstData->m_FullMessage , + (unsigned long)(pstData->m_MsgLen) + 5)) & 0xFF); + pstData->m_State = CHIMU_STATE_MACHINE_XSUM; + } else { + return FALSE; + } + break; + case CHIMU_STATE_MACHINE_XSUM: // Verify + pstData->m_ReceivedChecksum = btData; + pstData->m_FullMessage[pstData->m_Index++] = btData; + if (pstData->m_Checksum != pstData->m_ReceivedChecksum) { + bUpdate = FALSE; + //BuiltInTest(BIT_COM_UART_RECEIPTFAIL, BIT_FAIL); + } else { + //Xsum passed, go parse it. + // We have pstData->m_MsgID to parse off of, pstData->m_pstData->m_Payload as the data. + bUpdate = CHIMU_ProcessMessage(&pstData->m_MsgID, pstData->m_Payload, pstData); + } + pstData->m_State = CHIMU_STATE_MACHINE_START; + break; + default: + pstData->m_State = CHIMU_STATE_MACHINE_START; } /* End of SWITCH */ return (bUpdate); } @@ -245,7 +246,7 @@ static CHIMU_attitude_data GetEulersFromQuat(CHIMU_attitude_data attitude) { CHIMU_attitude_data ps; ps = attitude; - float x, sqw,sqx,sqy,sqz,norm; + float x, sqw, sqx, sqy, sqz, norm; sqw = ps.q.s * ps.q.s; sqx = ps.q.v.x * ps.q.v.x; sqy = ps.q.v.y * ps.q.v.y; @@ -256,38 +257,36 @@ static CHIMU_attitude_data GetEulersFromQuat(CHIMU_attitude_data attitude) ps.q.v.x = ps.q.v.x / norm; ps.q.v.y = ps.q.v.y / norm; ps.q.v.z = ps.q.v.z / norm; - ps.euler.phi =atan2(2.0 * (ps.q.s * ps.q.v.x + ps.q.v.y * ps.q.v.z), (1 - 2 * (sqx + sqy))); - if (ps.euler.phi < 0) ps.euler.phi = ps.euler.phi + 2 *M_PI; + ps.euler.phi = atan2(2.0 * (ps.q.s * ps.q.v.x + ps.q.v.y * ps.q.v.z), (1 - 2 * (sqx + sqy))); + if (ps.euler.phi < 0) { ps.euler.phi = ps.euler.phi + 2 * M_PI; } x = ((2.0 * (ps.q.s * ps.q.v.y - ps.q.v.z * ps.q.v.x))); //Below needed in event normalization not done - if (x > 1.0) x = 1.0; - if (x < -1.0) x = -1.0; + if (x > 1.0) { x = 1.0; } + if (x < -1.0) { x = -1.0; } // if ((ps.q.v.x * ps.q.v.y + ps.q.v.z * ps.q.s) == 0.5) { - ps.euler.theta = 2 *atan2(ps.q.v.x, ps.q.s); + ps.euler.theta = 2 * atan2(ps.q.v.x, ps.q.s); + } else if ((ps.q.v.x * ps.q.v.y + ps.q.v.z * ps.q.s) == -0.5) { + ps.euler.theta = -2 * atan2(ps.q.v.x, ps.q.s); + } else { + ps.euler.theta = asin(x); } - else - if ((ps.q.v.x * ps.q.v.y + ps.q.v.z * ps.q.s) == -0.5) { - ps.euler.theta = -2 *atan2(ps.q.v.x, ps.q.s); - } - else{ - ps.euler.theta = asin(x); - } ps.euler.psi = atan2(2.0 * (ps.q.s * ps.q.v.z + ps.q.v.x * ps.q.v.y), (1 - 2 * (sqy + sqz))); if (ps.euler.psi < 0) { ps.euler.psi = ps.euler.psi + (2 * M_PI); } - return(ps); + return (ps); } -static unsigned char BitTest (unsigned char input, unsigned char n) +static unsigned char BitTest(unsigned char input, unsigned char n) { //Test a bit in n and return TRUE or FALSE - if ( input & (1 << n)) return TRUE; else return FALSE; + if (input & (1 << n)) { return TRUE; } else { return FALSE; } } -unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID __attribute__((unused)), unsigned char *pPayloadData, CHIMU_PARSER_DATA *pstData) +unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID __attribute__((unused)), unsigned char *pPayloadData, + CHIMU_PARSER_DATA *pstData) { //Msgs from CHIMU are off limits (i.e.any CHIMU messages sent up the uplink should go to //CHIMU). @@ -296,131 +295,139 @@ unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID __attribute__((unused)) // by itself. However, here we should decode CHIMU messages being received and // a) pass them down to ground // b) grab the data from the CHIMU for our own needs / purposes - int CHIMU_index =0; - float sanity_check=0.0; + int CHIMU_index = 0; + float sanity_check = 0.0; - switch (pstData->m_MsgID){ - case CHIMU_Msg_0_Ping: - CHIMU_index = 0; - pstData->gCHIMU_SW_Exclaim = pPayloadData[CHIMU_index]; CHIMU_index++; - pstData->gCHIMU_SW_Major = pPayloadData[CHIMU_index]; CHIMU_index++; - pstData->gCHIMU_SW_Minor = pPayloadData[CHIMU_index]; CHIMU_index++; - pstData->gCHIMU_SW_SerialNumber = (pPayloadData[CHIMU_index]<<8) & (0x0000FF00); CHIMU_index++; - pstData->gCHIMU_SW_SerialNumber += pPayloadData[CHIMU_index]; CHIMU_index++; + switch (pstData->m_MsgID) { + case CHIMU_Msg_0_Ping: + CHIMU_index = 0; + pstData->gCHIMU_SW_Exclaim = pPayloadData[CHIMU_index]; CHIMU_index++; + pstData->gCHIMU_SW_Major = pPayloadData[CHIMU_index]; CHIMU_index++; + pstData->gCHIMU_SW_Minor = pPayloadData[CHIMU_index]; CHIMU_index++; + pstData->gCHIMU_SW_SerialNumber = (pPayloadData[CHIMU_index] << 8) & (0x0000FF00); CHIMU_index++; + pstData->gCHIMU_SW_SerialNumber += pPayloadData[CHIMU_index]; CHIMU_index++; - return TRUE; - break; - case CHIMU_Msg_1_IMU_Raw: - break; - case CHIMU_Msg_2_IMU_FP: - CHIMU_index = 0; - memmove (&pstData->m_sensor.cputemp, &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.cputemp));CHIMU_index += (sizeof(pstData->m_sensor.cputemp)); - pstData->m_sensor.cputemp = FloatSwap(pstData->m_sensor.cputemp); - memmove (&pstData->m_sensor.acc[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.acc));CHIMU_index += (sizeof(pstData->m_sensor.acc)); - pstData->m_sensor.acc[0] = FloatSwap(pstData->m_sensor.acc[0]); - pstData->m_sensor.acc[1] = FloatSwap(pstData->m_sensor.acc[1]); - pstData->m_sensor.acc[2] = FloatSwap(pstData->m_sensor.acc[2]); - memmove (&pstData->m_sensor.rate[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.rate));CHIMU_index += (sizeof(pstData->m_sensor.rate)); - pstData->m_sensor.rate[0] = FloatSwap(pstData->m_sensor.rate[0]); - pstData->m_sensor.rate[1] = FloatSwap(pstData->m_sensor.rate[1]); - pstData->m_sensor.rate[2] = FloatSwap(pstData->m_sensor.rate[2]); - memmove (&pstData->m_sensor.mag[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.mag));CHIMU_index += (sizeof(pstData->m_sensor.mag)); - pstData->m_sensor.mag[0] = FloatSwap(pstData->m_sensor.mag[0]); - pstData->m_sensor.mag[1] = FloatSwap(pstData->m_sensor.mag[1]); - pstData->m_sensor.mag[2] = FloatSwap(pstData->m_sensor.mag[2]); - memmove (&pstData->m_sensor.spare1, &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.spare1));CHIMU_index += (sizeof(pstData->m_sensor.spare1)); - pstData->m_sensor.spare1 = FloatSwap(pstData->m_sensor.spare1); - return TRUE; - break; - case CHIMU_Msg_3_IMU_Attitude: - //Attitude message data from CHIMU - // Includes attitude and rates only, along with configuration bits - // All you need for control + return TRUE; + break; + case CHIMU_Msg_1_IMU_Raw: + break; + case CHIMU_Msg_2_IMU_FP: + CHIMU_index = 0; + memmove(&pstData->m_sensor.cputemp, &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.cputemp)); + CHIMU_index += (sizeof(pstData->m_sensor.cputemp)); + pstData->m_sensor.cputemp = FloatSwap(pstData->m_sensor.cputemp); + memmove(&pstData->m_sensor.acc[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.acc)); + CHIMU_index += (sizeof(pstData->m_sensor.acc)); + pstData->m_sensor.acc[0] = FloatSwap(pstData->m_sensor.acc[0]); + pstData->m_sensor.acc[1] = FloatSwap(pstData->m_sensor.acc[1]); + pstData->m_sensor.acc[2] = FloatSwap(pstData->m_sensor.acc[2]); + memmove(&pstData->m_sensor.rate[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.rate)); + CHIMU_index += (sizeof(pstData->m_sensor.rate)); + pstData->m_sensor.rate[0] = FloatSwap(pstData->m_sensor.rate[0]); + pstData->m_sensor.rate[1] = FloatSwap(pstData->m_sensor.rate[1]); + pstData->m_sensor.rate[2] = FloatSwap(pstData->m_sensor.rate[2]); + memmove(&pstData->m_sensor.mag[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.mag)); + CHIMU_index += (sizeof(pstData->m_sensor.mag)); + pstData->m_sensor.mag[0] = FloatSwap(pstData->m_sensor.mag[0]); + pstData->m_sensor.mag[1] = FloatSwap(pstData->m_sensor.mag[1]); + pstData->m_sensor.mag[2] = FloatSwap(pstData->m_sensor.mag[2]); + memmove(&pstData->m_sensor.spare1, &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.spare1)); + CHIMU_index += (sizeof(pstData->m_sensor.spare1)); + pstData->m_sensor.spare1 = FloatSwap(pstData->m_sensor.spare1); + return TRUE; + break; + case CHIMU_Msg_3_IMU_Attitude: + //Attitude message data from CHIMU + // Includes attitude and rates only, along with configuration bits + // All you need for control - //Led_On(LED_RED); + //Led_On(LED_RED); - CHIMU_index = 0; - memmove (&pstData->m_attitude.euler, &pPayloadData[CHIMU_index], sizeof(pstData->m_attitude.euler));CHIMU_index += sizeof(pstData->m_attitude.euler); - pstData->m_attitude.euler.phi = FloatSwap(pstData->m_attitude.euler.phi); - pstData->m_attitude.euler.theta = FloatSwap(pstData->m_attitude.euler.theta); - pstData->m_attitude.euler.psi = FloatSwap(pstData->m_attitude.euler.psi); - memmove (&pstData->m_sensor.rate[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.rate)); CHIMU_index += (sizeof(pstData->m_sensor.rate)); - pstData->m_sensor.rate[0] = FloatSwap(pstData->m_sensor.rate[0]); - pstData->m_sensor.rate[1] = FloatSwap(pstData->m_sensor.rate[1]); - pstData->m_sensor.rate[2] = FloatSwap(pstData->m_sensor.rate[2]); + CHIMU_index = 0; + memmove(&pstData->m_attitude.euler, &pPayloadData[CHIMU_index], sizeof(pstData->m_attitude.euler)); + CHIMU_index += sizeof(pstData->m_attitude.euler); + pstData->m_attitude.euler.phi = FloatSwap(pstData->m_attitude.euler.phi); + pstData->m_attitude.euler.theta = FloatSwap(pstData->m_attitude.euler.theta); + pstData->m_attitude.euler.psi = FloatSwap(pstData->m_attitude.euler.psi); + memmove(&pstData->m_sensor.rate[0], &pPayloadData[CHIMU_index], sizeof(pstData->m_sensor.rate)); + CHIMU_index += (sizeof(pstData->m_sensor.rate)); + pstData->m_sensor.rate[0] = FloatSwap(pstData->m_sensor.rate[0]); + pstData->m_sensor.rate[1] = FloatSwap(pstData->m_sensor.rate[1]); + pstData->m_sensor.rate[2] = FloatSwap(pstData->m_sensor.rate[2]); - memmove (&pstData->m_attitude.q, &pPayloadData[CHIMU_index], sizeof(pstData->m_attitude.q));CHIMU_index += sizeof(pstData->m_attitude.q); - pstData->m_attitude.q.s = FloatSwap(pstData->m_attitude.q.s); - pstData->m_attitude.q.v.x = FloatSwap(pstData->m_attitude.q.v.x); - pstData->m_attitude.q.v.y = FloatSwap(pstData->m_attitude.q.v.y); - pstData->m_attitude.q.v.z = FloatSwap(pstData->m_attitude.q.v.z); + memmove(&pstData->m_attitude.q, &pPayloadData[CHIMU_index], sizeof(pstData->m_attitude.q)); + CHIMU_index += sizeof(pstData->m_attitude.q); + pstData->m_attitude.q.s = FloatSwap(pstData->m_attitude.q.s); + pstData->m_attitude.q.v.x = FloatSwap(pstData->m_attitude.q.v.x); + pstData->m_attitude.q.v.y = FloatSwap(pstData->m_attitude.q.v.y); + pstData->m_attitude.q.v.z = FloatSwap(pstData->m_attitude.q.v.z); - memmove (&pstData->m_attrates.q, &pPayloadData[CHIMU_index], sizeof(pstData->m_attrates.q));CHIMU_index += sizeof(pstData->m_attitude.q); - pstData->m_attrates.q.s = FloatSwap(pstData->m_attrates.q.s); - pstData->m_attrates.q.v.x = FloatSwap(pstData->m_attrates.q.v.x); - pstData->m_attrates.q.v.y = FloatSwap(pstData->m_attrates.q.v.y); - pstData->m_attrates.q.v.z = FloatSwap(pstData->m_attrates.q.v.z); + memmove(&pstData->m_attrates.q, &pPayloadData[CHIMU_index], sizeof(pstData->m_attrates.q)); + CHIMU_index += sizeof(pstData->m_attitude.q); + pstData->m_attrates.q.s = FloatSwap(pstData->m_attrates.q.s); + pstData->m_attrates.q.v.x = FloatSwap(pstData->m_attrates.q.v.x); + pstData->m_attrates.q.v.y = FloatSwap(pstData->m_attrates.q.v.y); + pstData->m_attrates.q.v.z = FloatSwap(pstData->m_attrates.q.v.z); - //Now put the rates into the Euler section as well. User can use pstData->m_attitude and pstData->m_attrates structures for control - pstData->m_attrates.euler.phi = pstData->m_sensor.rate[0]; - pstData->m_attrates.euler.theta = pstData->m_sensor.rate[1]; - pstData->m_attrates.euler.psi = pstData->m_sensor.rate[2]; + //Now put the rates into the Euler section as well. User can use pstData->m_attitude and pstData->m_attrates structures for control + pstData->m_attrates.euler.phi = pstData->m_sensor.rate[0]; + pstData->m_attrates.euler.theta = pstData->m_sensor.rate[1]; + pstData->m_attrates.euler.psi = pstData->m_sensor.rate[2]; - pstData->gCalStatus = pPayloadData[CHIMU_index]; CHIMU_index ++; - pstData->gCHIMU_BIT = pPayloadData[CHIMU_index]; CHIMU_index ++; - pstData->gConfigInfo = pPayloadData[CHIMU_index]; CHIMU_index ++; + pstData->gCalStatus = pPayloadData[CHIMU_index]; CHIMU_index ++; + pstData->gCHIMU_BIT = pPayloadData[CHIMU_index]; CHIMU_index ++; + pstData->gConfigInfo = pPayloadData[CHIMU_index]; CHIMU_index ++; - // TODO: Read configuration bits + // TODO: Read configuration bits - /* bC0_SPI_En = BitTest (gConfigInfo, 0); - bC1_HWCentrip_En = BitTest (gConfigInfo, 1); - bC2_TempCal_En = BitTest (gConfigInfo, 2); - bC3_RateOut_En = BitTest (gConfigInfo, 3); - bC4_TBD = BitTest (gConfigInfo, 4); - bC5_Quat_Est = BitTest (gConfigInfo, 5); - bC6_SWCentrip_En = BitTest (gConfigInfo, 6); - bC7_AllowHW_Override = BitTest (gConfigInfo, 7); - */ - //CHIMU currently (v 1.3) does not compute Eulers if quaternion estimator is selected - if(BitTest (pstData->gConfigInfo, 5) == TRUE) - { + /* bC0_SPI_En = BitTest (gConfigInfo, 0); + bC1_HWCentrip_En = BitTest (gConfigInfo, 1); + bC2_TempCal_En = BitTest (gConfigInfo, 2); + bC3_RateOut_En = BitTest (gConfigInfo, 3); + bC4_TBD = BitTest (gConfigInfo, 4); + bC5_Quat_Est = BitTest (gConfigInfo, 5); + bC6_SWCentrip_En = BitTest (gConfigInfo, 6); + bC7_AllowHW_Override = BitTest (gConfigInfo, 7); + */ + //CHIMU currently (v 1.3) does not compute Eulers if quaternion estimator is selected + if (BitTest(pstData->gConfigInfo, 5) == TRUE) { pstData->m_attitude = GetEulersFromQuat((pstData->m_attitude)); } - //NEW: Checks for bad attitude data (bad SPI maybe?) - // Only allow globals to contain updated data if it makes sense - sanity_check = (pstData->m_attitude.q.s * pstData->m_attitude.q.s); - sanity_check += (pstData->m_attitude.q.v.x * pstData->m_attitude.q.v.x); - sanity_check += (pstData->m_attitude.q.v.y * pstData->m_attitude.q.v.y); - sanity_check += (pstData->m_attitude.q.v.z * pstData->m_attitude.q.v.z); + //NEW: Checks for bad attitude data (bad SPI maybe?) + // Only allow globals to contain updated data if it makes sense + sanity_check = (pstData->m_attitude.q.s * pstData->m_attitude.q.s); + sanity_check += (pstData->m_attitude.q.v.x * pstData->m_attitude.q.v.x); + sanity_check += (pstData->m_attitude.q.v.y * pstData->m_attitude.q.v.y); + sanity_check += (pstData->m_attitude.q.v.z * pstData->m_attitude.q.v.z); - //Should be 1.0 (normalized quaternion) - if ((sanity_check > 0.8) && (sanity_check < 1.2)) { - // gAttitude = pstData->m_attitude; - // gAttRates = pstData->m_attrates; - // gSensor = pstData->m_sensor; - } else { - //TODO: Log BIT that indicates IMU message incoming failed (maybe SPI error?) - } + //Should be 1.0 (normalized quaternion) + if ((sanity_check > 0.8) && (sanity_check < 1.2)) { + // gAttitude = pstData->m_attitude; + // gAttRates = pstData->m_attrates; + // gSensor = pstData->m_sensor; + } else { + //TODO: Log BIT that indicates IMU message incoming failed (maybe SPI error?) + } - return TRUE; - break; - case CHIMU_Msg_4_BiasSF: - case CHIMU_Msg_5_BIT: - case CHIMU_Msg_6_MagCal: - case CHIMU_Msg_7_GyroBias: - case CHIMU_Msg_8_TempCal: - case CHIMU_Msg_9_DAC_Offsets: - case CHIMU_Msg_10_Res: - case CHIMU_Msg_11_Res: - case CHIMU_Msg_12_Res: - case CHIMU_Msg_13_Res: - case CHIMU_Msg_14_RefVector: - case CHIMU_Msg_15_SFCheck: - break; - default: - return FALSE; - break; + return TRUE; + break; + case CHIMU_Msg_4_BiasSF: + case CHIMU_Msg_5_BIT: + case CHIMU_Msg_6_MagCal: + case CHIMU_Msg_7_GyroBias: + case CHIMU_Msg_8_TempCal: + case CHIMU_Msg_9_DAC_Offsets: + case CHIMU_Msg_10_Res: + case CHIMU_Msg_11_Res: + case CHIMU_Msg_12_Res: + case CHIMU_Msg_13_Res: + case CHIMU_Msg_14_RefVector: + case CHIMU_Msg_15_SFCheck: + break; + default: + return FALSE; + break; } return FALSE; } diff --git a/sw/airborne/modules/ins/imu_chimu.h b/sw/airborne/modules/ins/imu_chimu.h index 328a2c1e68..539be718ac 100644 --- a/sw/airborne/modules/ins/imu_chimu.h +++ b/sw/airborne/modules/ins/imu_chimu.h @@ -44,44 +44,44 @@ #ifndef CHIMU_DEFINED_H #define CHIMU_DEFINED_H -#define CHIMU_STX 0xae -#define CHIMU_BROADCAST 0xaa +#define CHIMU_STX 0xae +#define CHIMU_BROADCAST 0xaa // Message ID's that go TO the CHIMU -#define MSG00_PING 0x00 -#define MSG01_BIAS 0x01 -#define MSG02_DACMODE 0x02 -#define MSG03_CALACC 0x03 -#define MSG04_CALMAG 0x04 -#define MSG05_CALRATE 0x05 -#define MSG06_CONFIGCLR 0x06 -#define MSG07_CONFIGSET 0x07 -#define MSG08_SAVEGYROBIAS 0x08 -#define MSG09_ESTIMATOR 0x09 -#define MSG0A_SFCHECK 0x0A -#define MSG0B_CENTRIP 0x0B -#define MSG0C_INITGYROS 0x0C -#define MSG0D_DEVICEID 0x0D -#define MSG0E_REFVECTOR 0x0E -#define MSG0F_RESET 0x0F -#define MSG10_UARTSETTINGS 0x10 -#define MSG11_SERIALNUMBER 0x11 +#define MSG00_PING 0x00 +#define MSG01_BIAS 0x01 +#define MSG02_DACMODE 0x02 +#define MSG03_CALACC 0x03 +#define MSG04_CALMAG 0x04 +#define MSG05_CALRATE 0x05 +#define MSG06_CONFIGCLR 0x06 +#define MSG07_CONFIGSET 0x07 +#define MSG08_SAVEGYROBIAS 0x08 +#define MSG09_ESTIMATOR 0x09 +#define MSG0A_SFCHECK 0x0A +#define MSG0B_CENTRIP 0x0B +#define MSG0C_INITGYROS 0x0C +#define MSG0D_DEVICEID 0x0D +#define MSG0E_REFVECTOR 0x0E +#define MSG0F_RESET 0x0F +#define MSG10_UARTSETTINGS 0x10 +#define MSG11_SERIALNUMBER 0x11 // Output message identifiers from the CHIMU unit -#define CHIMU_Msg_0_Ping 0 +#define CHIMU_Msg_0_Ping 0 #define CHIMU_Msg_1_IMU_Raw 1 -#define CHIMU_Msg_2_IMU_FP 2 +#define CHIMU_Msg_2_IMU_FP 2 #define CHIMU_Msg_3_IMU_Attitude 3 -#define CHIMU_Msg_4_BiasSF 4 +#define CHIMU_Msg_4_BiasSF 4 #define CHIMU_Msg_5_BIT 5 -#define CHIMU_Msg_6_MagCal 6 +#define CHIMU_Msg_6_MagCal 6 #define CHIMU_Msg_7_GyroBias 7 #define CHIMU_Msg_8_TempCal 8 #define CHIMU_Msg_9_DAC_Offsets 9 -#define CHIMU_Msg_10_Res 10 -#define CHIMU_Msg_11_Res 11 -#define CHIMU_Msg_12_Res 12 -#define CHIMU_Msg_13_Res 13 +#define CHIMU_Msg_10_Res 10 +#define CHIMU_Msg_11_Res 11 +#define CHIMU_Msg_12_Res 12 +#define CHIMU_Msg_13_Res 13 #define CHIMU_Msg_14_RefVector 14 #define CHIMU_Msg_15_SFCheck 15 @@ -92,10 +92,9 @@ #ifdef CHIMU_BIG_ENDIAN -static inline float FloatSwap( float f ) +static inline float FloatSwap(float f) { - union - { + union { float f; unsigned char b[4]; } dat1, dat2; @@ -155,17 +154,17 @@ typedef struct { #define CHIMU_RX_BUFFERSIZE 128 typedef struct { - unsigned char m_State; // Current state protocol parser is in - unsigned char m_Checksum; // Calculated CHIMU sentence checksum - unsigned char m_ReceivedChecksum; // Received CHIMU sentence checksum (if exists) - unsigned char m_Index; // Index used for command and data + unsigned char m_State; // Current state protocol parser is in + unsigned char m_Checksum; // Calculated CHIMU sentence checksum + unsigned char m_ReceivedChecksum; // Received CHIMU sentence checksum (if exists) + unsigned char m_Index; // Index used for command and data unsigned char m_PayloadIndex; unsigned char m_MsgID; unsigned char m_MsgLen; unsigned char m_TempDeviceID; unsigned char m_DeviceID; unsigned char m_Payload[CHIMU_RX_BUFFERSIZE]; // CHIMU data - unsigned char m_FullMessage[CHIMU_RX_BUFFERSIZE]; // CHIMU data + unsigned char m_FullMessage[CHIMU_RX_BUFFERSIZE]; // CHIMU data CHIMU_attitude_data m_attitude; CHIMU_attitude_data m_attrates; CHIMU_sensor_data m_sensor; diff --git a/sw/airborne/modules/ins/ins_arduimu.c b/sw/airborne/modules/ins/ins_arduimu.c index a441613e0e..d5eb2bc12f 100644 --- a/sw/airborne/modules/ins/ins_arduimu.c +++ b/sw/airborne/modules/ins/ins_arduimu.c @@ -24,7 +24,7 @@ int32_t GPS_Data[14]; #define ARDUIMU_I2C_DEV i2c0 #endif -// Adresse des I2C Slaves: 0001 0110 letztes Bit ist für Read/Write +// Adresse des I2C Slaves: 0001 0110 letztes Bit ist für Read/Write // einzugebende Adresse im ArduIMU ist 0000 1011 //da ArduIMU das Read/Write Bit selber anfügt. #define ArduIMU_SLAVE_ADDR 0x22 @@ -50,7 +50,8 @@ float ins_pitch_neutral; //float pitch_of_throttle_gain; float throttle_slew; -void ArduIMU_init( void ) { +void ArduIMU_init(void) +{ ardu_gps_trans.buf[0] = 0; ardu_gps_trans.buf[1] = 0; messageNr = 0; @@ -65,153 +66,155 @@ void ArduIMU_init( void ) { } -void ArduIMU_periodicGPS( void ) { +void ArduIMU_periodicGPS(void) +{ - if ( gps_daten_versendet_msg1 == TRUE && gps_daten_versendet_msg2 == TRUE ) { - gps_daten_abgespeichert = FALSE; - } + if (gps_daten_versendet_msg1 == TRUE && gps_daten_versendet_msg2 == TRUE) { + gps_daten_abgespeichert = FALSE; + } - if( imu_daten_angefordert == TRUE ){ - IMU_Daten_verarbeiten(); - } + if (imu_daten_angefordert == TRUE) { + IMU_Daten_verarbeiten(); + } - if ( gps_daten_abgespeichert == FALSE ) { - //posllh - GPS_Data [0] = gps.tow; - GPS_Data [1] = gps.lla_pos.lon; - GPS_Data [2] = gps.lla_pos.lat; - GPS_Data [3] = gps.lla_pos.alt; //höhe über elipsoid - GPS_Data [4] = gps.hmsl; //höhe über sea level - //velend - GPS_Data [5] = gps.speed_3d; //speed 3D - GPS_Data [6] = gps.gspeed; //ground speed - GPS_Data [7] = DegOfRad(gps.course / 1e6); //Kurs - //status - GPS_Data [8] = gps.fix; //fix - GPS_Data [9] = gps_ubx.status_flags; //flags - //sol - GPS_Data [10] = gps.fix; //fix - //GPS_Data [11] = gps_ubx.sol_flags; //flags - GPS_Data [12] = -gps.ned_vel.z; - GPS_Data [13] = gps.num_sv; + if (gps_daten_abgespeichert == FALSE) { + //posllh + GPS_Data [0] = gps.tow; + GPS_Data [1] = gps.lla_pos.lon; + GPS_Data [2] = gps.lla_pos.lat; + GPS_Data [3] = gps.lla_pos.alt; //höhe über elipsoid + GPS_Data [4] = gps.hmsl; //höhe über sea level + //velend + GPS_Data [5] = gps.speed_3d; //speed 3D + GPS_Data [6] = gps.gspeed; //ground speed + GPS_Data [7] = DegOfRad(gps.course / 1e6); //Kurs + //status + GPS_Data [8] = gps.fix; //fix + GPS_Data [9] = gps_ubx.status_flags; //flags + //sol + GPS_Data [10] = gps.fix; //fix + //GPS_Data [11] = gps_ubx.sol_flags; //flags + GPS_Data [12] = -gps.ned_vel.z; + GPS_Data [13] = gps.num_sv; - gps_daten_abgespeichert = TRUE; - } + gps_daten_abgespeichert = TRUE; + } - if(messageNr == 0) { + if (messageNr == 0) { - //test für 32bit in byte packete abzupacken: - //GPS_Data [0] = -1550138773; + //test für 32bit in byte packete abzupacken: + //GPS_Data [0] = -1550138773; - ardu_gps_trans.buf[0] = 0; //message Nr = 0 --> itow bis ground speed - ardu_gps_trans.buf[1] = (uint8_t) GPS_Data[0]; //itow - ardu_gps_trans.buf[2] = (uint8_t) (GPS_Data[0] >>8); - ardu_gps_trans.buf[3] = (uint8_t) (GPS_Data[0] >>16); - ardu_gps_trans.buf[4] = (uint8_t) (GPS_Data[0] >>24); - ardu_gps_trans.buf[5] = (uint8_t) GPS_Data[1]; //lon - ardu_gps_trans.buf[6] = (uint8_t) (GPS_Data[1] >>8); - ardu_gps_trans.buf[7] = (uint8_t) (GPS_Data[1] >>16); - ardu_gps_trans.buf[8] = (uint8_t) (GPS_Data[1] >>24); - ardu_gps_trans.buf[9] = (uint8_t) GPS_Data[2]; //lat - ardu_gps_trans.buf[10] = (uint8_t) (GPS_Data[2] >>8); - ardu_gps_trans.buf[11] = (uint8_t) (GPS_Data[2] >>16); - ardu_gps_trans.buf[12] = (uint8_t) (GPS_Data[2] >>24); - ardu_gps_trans.buf[13] = (uint8_t) GPS_Data[3]; //height - ardu_gps_trans.buf[14] = (uint8_t) (GPS_Data[3] >>8); - ardu_gps_trans.buf[15] = (uint8_t) (GPS_Data[3] >>16); - ardu_gps_trans.buf[16] = (uint8_t) (GPS_Data[3] >>24); - ardu_gps_trans.buf[17] = (uint8_t) GPS_Data[4]; //hmsl - ardu_gps_trans.buf[18] = (uint8_t) (GPS_Data[4] >>8); - ardu_gps_trans.buf[19] = (uint8_t) (GPS_Data[4] >>16); - ardu_gps_trans.buf[20] = (uint8_t) (GPS_Data[4] >>24); - ardu_gps_trans.buf[21] = (uint8_t) GPS_Data[5]; //speed - ardu_gps_trans.buf[22] = (uint8_t) (GPS_Data[5] >>8); - ardu_gps_trans.buf[23] = (uint8_t) (GPS_Data[5] >>16); - ardu_gps_trans.buf[24] = (uint8_t) (GPS_Data[5] >>24); - ardu_gps_trans.buf[25] = (uint8_t) GPS_Data[6]; //gspeed - ardu_gps_trans.buf[26] = (uint8_t) (GPS_Data[6] >>8); - ardu_gps_trans.buf[27] = (uint8_t) (GPS_Data[6] >>16); - ardu_gps_trans.buf[28] = (uint8_t) (GPS_Data[6] >>24); - i2c_transmit(&ARDUIMU_I2C_DEV, &ardu_gps_trans, ArduIMU_SLAVE_ADDR, 28); + ardu_gps_trans.buf[0] = 0; //message Nr = 0 --> itow bis ground speed + ardu_gps_trans.buf[1] = (uint8_t) GPS_Data[0]; //itow + ardu_gps_trans.buf[2] = (uint8_t)(GPS_Data[0] >> 8); + ardu_gps_trans.buf[3] = (uint8_t)(GPS_Data[0] >> 16); + ardu_gps_trans.buf[4] = (uint8_t)(GPS_Data[0] >> 24); + ardu_gps_trans.buf[5] = (uint8_t) GPS_Data[1]; //lon + ardu_gps_trans.buf[6] = (uint8_t)(GPS_Data[1] >> 8); + ardu_gps_trans.buf[7] = (uint8_t)(GPS_Data[1] >> 16); + ardu_gps_trans.buf[8] = (uint8_t)(GPS_Data[1] >> 24); + ardu_gps_trans.buf[9] = (uint8_t) GPS_Data[2]; //lat + ardu_gps_trans.buf[10] = (uint8_t)(GPS_Data[2] >> 8); + ardu_gps_trans.buf[11] = (uint8_t)(GPS_Data[2] >> 16); + ardu_gps_trans.buf[12] = (uint8_t)(GPS_Data[2] >> 24); + ardu_gps_trans.buf[13] = (uint8_t) GPS_Data[3]; //height + ardu_gps_trans.buf[14] = (uint8_t)(GPS_Data[3] >> 8); + ardu_gps_trans.buf[15] = (uint8_t)(GPS_Data[3] >> 16); + ardu_gps_trans.buf[16] = (uint8_t)(GPS_Data[3] >> 24); + ardu_gps_trans.buf[17] = (uint8_t) GPS_Data[4]; //hmsl + ardu_gps_trans.buf[18] = (uint8_t)(GPS_Data[4] >> 8); + ardu_gps_trans.buf[19] = (uint8_t)(GPS_Data[4] >> 16); + ardu_gps_trans.buf[20] = (uint8_t)(GPS_Data[4] >> 24); + ardu_gps_trans.buf[21] = (uint8_t) GPS_Data[5]; //speed + ardu_gps_trans.buf[22] = (uint8_t)(GPS_Data[5] >> 8); + ardu_gps_trans.buf[23] = (uint8_t)(GPS_Data[5] >> 16); + ardu_gps_trans.buf[24] = (uint8_t)(GPS_Data[5] >> 24); + ardu_gps_trans.buf[25] = (uint8_t) GPS_Data[6]; //gspeed + ardu_gps_trans.buf[26] = (uint8_t)(GPS_Data[6] >> 8); + ardu_gps_trans.buf[27] = (uint8_t)(GPS_Data[6] >> 16); + ardu_gps_trans.buf[28] = (uint8_t)(GPS_Data[6] >> 24); + i2c_transmit(&ARDUIMU_I2C_DEV, &ardu_gps_trans, ArduIMU_SLAVE_ADDR, 28); - gps_daten_versendet_msg1 = TRUE; - messageNr =1; - } - else { + gps_daten_versendet_msg1 = TRUE; + messageNr = 1; + } else { - ardu_gps_trans.buf[0] = 1; //message Nr = 1 --> ground course, ecefVZ, numSV, Fix, flags, fix, flags - ardu_gps_trans.buf[1] = GPS_Data[7]; //ground course - ardu_gps_trans.buf[2] = (GPS_Data[7] >>8); - ardu_gps_trans.buf[3] = (GPS_Data[7] >>16); - ardu_gps_trans.buf[4] = (GPS_Data[7] >>24); - ardu_gps_trans.buf[5] = GPS_Data[12]; //ecefVZ - ardu_gps_trans.buf[6] = (GPS_Data[12] >>8); - ardu_gps_trans.buf[7] = (GPS_Data[12] >>16); - ardu_gps_trans.buf[8] = (GPS_Data[12] >>24); - ardu_gps_trans.buf[9] = GPS_Data[13]; //numSV - ardu_gps_trans.buf[10] = GPS_Data[8]; //status gps fix - ardu_gps_trans.buf[11] = GPS_Data[9]; //status flags - ardu_gps_trans.buf[12] = GPS_Data[10]; //sol gps fix - ardu_gps_trans.buf[13] = GPS_Data[11]; //sol flags - i2c_transmit(&ARDUIMU_I2C_DEV, &ardu_gps_trans, ArduIMU_SLAVE_ADDR, 13); + ardu_gps_trans.buf[0] = 1; //message Nr = 1 --> ground course, ecefVZ, numSV, Fix, flags, fix, flags + ardu_gps_trans.buf[1] = GPS_Data[7]; //ground course + ardu_gps_trans.buf[2] = (GPS_Data[7] >> 8); + ardu_gps_trans.buf[3] = (GPS_Data[7] >> 16); + ardu_gps_trans.buf[4] = (GPS_Data[7] >> 24); + ardu_gps_trans.buf[5] = GPS_Data[12]; //ecefVZ + ardu_gps_trans.buf[6] = (GPS_Data[12] >> 8); + ardu_gps_trans.buf[7] = (GPS_Data[12] >> 16); + ardu_gps_trans.buf[8] = (GPS_Data[12] >> 24); + ardu_gps_trans.buf[9] = GPS_Data[13]; //numSV + ardu_gps_trans.buf[10] = GPS_Data[8]; //status gps fix + ardu_gps_trans.buf[11] = GPS_Data[9]; //status flags + ardu_gps_trans.buf[12] = GPS_Data[10]; //sol gps fix + ardu_gps_trans.buf[13] = GPS_Data[11]; //sol flags + i2c_transmit(&ARDUIMU_I2C_DEV, &ardu_gps_trans, ArduIMU_SLAVE_ADDR, 13); - gps_daten_versendet_msg2 = TRUE; - messageNr = 0; - } + gps_daten_versendet_msg2 = TRUE; + messageNr = 0; + } - //DOWNLINK_SEND_DEBUG_ZHAW(DefaultChannel, DefaultDevice, &gps_mode , &gps_numSV ,&gps_alt , &gps_hmsl , &gps.tow, &gps_speed_3d); + //DOWNLINK_SEND_DEBUG_ZHAW(DefaultChannel, DefaultDevice, &gps_mode , &gps_numSV ,&gps_alt , &gps_hmsl , &gps.tow, &gps_speed_3d); } -void ArduIMU_periodic( void ) { +void ArduIMU_periodic(void) +{ //Frequenz des Aufrufs wird in conf/modules/ArduIMU.xml festgelegt. - //I2C-Nachricht anfordern an Slave Adresse, erwartete Anzahl Byte, Status - if (imu_daten_angefordert == TRUE) { - IMU_Daten_verarbeiten(); - } - i2c_receive(&ARDUIMU_I2C_DEV, &ardu_ins_trans, ArduIMU_SLAVE_ADDR, 12); + //I2C-Nachricht anfordern an Slave Adresse, erwartete Anzahl Byte, Status + if (imu_daten_angefordert == TRUE) { + IMU_Daten_verarbeiten(); + } + i2c_receive(&ARDUIMU_I2C_DEV, &ardu_ins_trans, ArduIMU_SLAVE_ADDR, 12); - imu_daten_angefordert = TRUE; - /* - Buffer O: Roll - Buffer 1: Pitch - Buffer 2: Yaw - Buffer 3: Beschleunigung X-Achse - Buffer 4: Beschleunigung Y-Achse - Buffer 5: Beschleunigung Z-Achse - */ + imu_daten_angefordert = TRUE; + /* + Buffer O: Roll + Buffer 1: Pitch + Buffer 2: Yaw + Buffer 3: Beschleunigung X-Achse + Buffer 4: Beschleunigung Y-Achse + Buffer 5: Beschleunigung Z-Achse + */ - //Nachricht zum GCS senden - // DOWNLINK_SEND_ArduIMU(DefaultChannel, DefaultDevice, &ArduIMU_data[0], &ArduIMU_data[1], &ArduIMU_data[2], &ArduIMU_data[3], &ArduIMU_data[4], &ArduIMU_data[5]); + //Nachricht zum GCS senden + // DOWNLINK_SEND_ArduIMU(DefaultChannel, DefaultDevice, &ArduIMU_data[0], &ArduIMU_data[1], &ArduIMU_data[2], &ArduIMU_data[3], &ArduIMU_data[4], &ArduIMU_data[5]); - // DOWNLINK_SEND_DEBUG_ZHAW(DefaultChannel, DefaultDevice, &airspeed_mode , &altitude_mode ,&amsys_baro, &amsys_baro, &amsys_airspeed_scaliert, &amsys_baro_scaliert); + // DOWNLINK_SEND_DEBUG_ZHAW(DefaultChannel, DefaultDevice, &airspeed_mode , &altitude_mode ,&amsys_baro, &amsys_baro, &amsys_airspeed_scaliert, &amsys_baro_scaliert); } -void IMU_Daten_verarbeiten( void ) { - //Empfangene Byts zusammenfügen und bereitstellen - recievedData[0] = (ardu_ins_trans.buf[1]<<8) | ardu_ins_trans.buf[0]; - recievedData[1] = (ardu_ins_trans.buf[3]<<8) | ardu_ins_trans.buf[2]; - recievedData[2] = (ardu_ins_trans.buf[5]<<8) | ardu_ins_trans.buf[4]; - recievedData[3] = (ardu_ins_trans.buf[7]<<8) | ardu_ins_trans.buf[6]; - recievedData[4] = (ardu_ins_trans.buf[9]<<8) | ardu_ins_trans.buf[8]; - recievedData[5] = (ardu_ins_trans.buf[11]<<8) | ardu_ins_trans.buf[10]; +void IMU_Daten_verarbeiten(void) +{ + //Empfangene Byts zusammenfügen und bereitstellen + recievedData[0] = (ardu_ins_trans.buf[1] << 8) | ardu_ins_trans.buf[0]; + recievedData[1] = (ardu_ins_trans.buf[3] << 8) | ardu_ins_trans.buf[2]; + recievedData[2] = (ardu_ins_trans.buf[5] << 8) | ardu_ins_trans.buf[4]; + recievedData[3] = (ardu_ins_trans.buf[7] << 8) | ardu_ins_trans.buf[6]; + recievedData[4] = (ardu_ins_trans.buf[9] << 8) | ardu_ins_trans.buf[8]; + recievedData[5] = (ardu_ins_trans.buf[11] << 8) | ardu_ins_trans.buf[10]; - //Floats zurück transformieren. Transformation ist auf ArduIMU programmiert. - ArduIMU_data[0] = (float) (recievedData[0] / (float)100); - ArduIMU_data[1] = (float) (recievedData[1] / (float)100); - ArduIMU_data[2] = (float) (recievedData[2] / (float)100); - ArduIMU_data[3] = (float) (recievedData[3] / (float)100); - ArduIMU_data[4] = (float) (recievedData[4] / (float)100); - ArduIMU_data[5] = (float) (recievedData[5] / (float)100); + //Floats zurück transformieren. Transformation ist auf ArduIMU programmiert. + ArduIMU_data[0] = (float)(recievedData[0] / (float)100); + ArduIMU_data[1] = (float)(recievedData[1] / (float)100); + ArduIMU_data[2] = (float)(recievedData[2] / (float)100); + ArduIMU_data[3] = (float)(recievedData[3] / (float)100); + ArduIMU_data[4] = (float)(recievedData[4] / (float)100); + ArduIMU_data[5] = (float)(recievedData[5] / (float)100); - // test - struct FloatEulers att; - att.phi = (float)ArduIMU_data[0]*0.01745329252 - ins_roll_neutral; - att.theta = (float)ArduIMU_data[1]*0.01745329252 - ins_pitch_neutral; - att.psi = 0.; - imu_daten_angefordert = FALSE; - stateSetNedToBodyEulers_f(&att); + // test + struct FloatEulers att; + att.phi = (float)ArduIMU_data[0] * 0.01745329252 - ins_roll_neutral; + att.theta = (float)ArduIMU_data[1] * 0.01745329252 - ins_pitch_neutral; + att.psi = 0.; + imu_daten_angefordert = FALSE; + stateSetNedToBodyEulers_f(&att); - RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &att->phi, &att->theta, &att->psi)); + RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &att->phi, &att->theta, &att->psi)); } diff --git a/sw/airborne/modules/ins/ins_arduimu.h b/sw/airborne/modules/ins/ins_arduimu.h index f85b7c66af..056207206d 100644 --- a/sw/airborne/modules/ins/ins_arduimu.h +++ b/sw/airborne/modules/ins/ins_arduimu.h @@ -14,9 +14,9 @@ extern float ins_pitch_neutral; extern float pitch_of_throttle_gain; extern float throttle_slew; -void ArduIMU_init( void ); -void ArduIMU_periodic( void ); -void ArduIMU_periodicGPS( void ); -void IMU_Daten_verarbeiten( void ); +void ArduIMU_init(void); +void ArduIMU_periodic(void); +void ArduIMU_periodicGPS(void); +void IMU_Daten_verarbeiten(void); #endif // ArduIMU_H diff --git a/sw/airborne/modules/ins/ins_arduimu_basic.c b/sw/airborne/modules/ins/ins_arduimu_basic.c index 6b66a7e6d3..68da7f92a6 100644 --- a/sw/airborne/modules/ins/ins_arduimu_basic.c +++ b/sw/airborne/modules/ins/ins_arduimu_basic.c @@ -43,7 +43,7 @@ #define ARDUIMU_I2C_DEV i2c0 #endif -// Adresse des I2C Slaves: 0001 0110 letztes Bit ist für Read/Write +// Adresse des I2C Slaves: 0001 0110 letztes Bit ist für Read/Write // einzugebende Adresse im ArduIMU ist 0000 1011 // da ArduIMU das Read/Write Bit selber anfügt. #define ArduIMU_SLAVE_ADDR 0x22 @@ -78,7 +78,8 @@ bool_t arduimu_calibrate_neutrals; bool_t high_accel_done; bool_t high_accel_flag; -void ArduIMU_init( void ) { +void ArduIMU_init(void) +{ FLOAT_EULERS_ZERO(arduimu_eulers); FLOAT_RATES_ZERO(arduimu_rates); FLOAT_VECT3_ZERO(arduimu_accel); @@ -95,13 +96,14 @@ void ArduIMU_init( void ) { } #define FillBufWith32bit(_buf, _index, _value) { \ - _buf[_index] = (uint8_t) (_value); \ - _buf[_index+1] = (uint8_t) ((_value) >> 8); \ - _buf[_index+2] = (uint8_t) ((_value) >> 16); \ - _buf[_index+3] = (uint8_t) ((_value) >> 24); \ -} + _buf[_index] = (uint8_t) (_value); \ + _buf[_index+1] = (uint8_t) ((_value) >> 8); \ + _buf[_index+2] = (uint8_t) ((_value) >> 16); \ + _buf[_index+3] = (uint8_t) ((_value) >> 24); \ + } -void ArduIMU_periodicGPS( void ) { +void ArduIMU_periodicGPS(void) +{ if (ardu_gps_trans.status != I2CTransDone) { return; } @@ -128,48 +130,51 @@ void ArduIMU_periodicGPS( void ) { FillBufWith32bit(ardu_gps_trans.buf, 8, (int32_t)gps.course); // course ardu_gps_trans.buf[12] = gps.fix; // status gps fix ardu_gps_trans.buf[13] = (uint8_t)arduimu_calibrate_neutrals; // calibration flag - ardu_gps_trans.buf[14] = (uint8_t)high_accel_flag; // high acceleration flag (disable accelerometers in the arduimu filter) + ardu_gps_trans.buf[14] = (uint8_t) + high_accel_flag; // high acceleration flag (disable accelerometers in the arduimu filter) i2c_transmit(&ARDUIMU_I2C_DEV, &ardu_gps_trans, ArduIMU_SLAVE_ADDR, 15); // Reset calibration flag - if (arduimu_calibrate_neutrals) arduimu_calibrate_neutrals = FALSE; + if (arduimu_calibrate_neutrals) { arduimu_calibrate_neutrals = FALSE; } } -void ArduIMU_periodic( void ) { +void ArduIMU_periodic(void) +{ //Frequence defined in conf/modules/ins_arduimu.xml if (ardu_ins_trans.status == I2CTransDone) { - i2c_receive(&ARDUIMU_I2C_DEV, &ardu_ins_trans, ArduIMU_SLAVE_ADDR, NB_DATA*2); + i2c_receive(&ARDUIMU_I2C_DEV, &ardu_ins_trans, ArduIMU_SLAVE_ADDR, NB_DATA * 2); } } #include "math/pprz_algebra_int.h" /* - Buffer O: Roll - Buffer 1: Pitch - Buffer 2: Yaw - Buffer 3: Gyro X - Buffer 4: Gyro Y - Buffer 5: Gyro Z - Buffer 6: Accel X - Buffer 7: Accel Y - Buffer 8: Accel Z + Buffer O: Roll + Buffer 1: Pitch + Buffer 2: Yaw + Buffer 3: Gyro X + Buffer 4: Gyro Y + Buffer 5: Gyro Z + Buffer 6: Accel X + Buffer 7: Accel Y + Buffer 8: Accel Z */ -void ArduIMU_event( void ) { +void ArduIMU_event(void) +{ // Handle INS I2C event if (ardu_ins_trans.status == I2CTransSuccess) { // received data from I2C transaction - recievedData[0] = (ardu_ins_trans.buf[1]<<8) | ardu_ins_trans.buf[0]; - recievedData[1] = (ardu_ins_trans.buf[3]<<8) | ardu_ins_trans.buf[2]; - recievedData[2] = (ardu_ins_trans.buf[5]<<8) | ardu_ins_trans.buf[4]; - recievedData[3] = (ardu_ins_trans.buf[7]<<8) | ardu_ins_trans.buf[6]; - recievedData[4] = (ardu_ins_trans.buf[9]<<8) | ardu_ins_trans.buf[8]; - recievedData[5] = (ardu_ins_trans.buf[11]<<8) | ardu_ins_trans.buf[10]; - recievedData[6] = (ardu_ins_trans.buf[13]<<8) | ardu_ins_trans.buf[12]; - recievedData[7] = (ardu_ins_trans.buf[15]<<8) | ardu_ins_trans.buf[14]; - recievedData[8] = (ardu_ins_trans.buf[17]<<8) | ardu_ins_trans.buf[16]; + recievedData[0] = (ardu_ins_trans.buf[1] << 8) | ardu_ins_trans.buf[0]; + recievedData[1] = (ardu_ins_trans.buf[3] << 8) | ardu_ins_trans.buf[2]; + recievedData[2] = (ardu_ins_trans.buf[5] << 8) | ardu_ins_trans.buf[4]; + recievedData[3] = (ardu_ins_trans.buf[7] << 8) | ardu_ins_trans.buf[6]; + recievedData[4] = (ardu_ins_trans.buf[9] << 8) | ardu_ins_trans.buf[8]; + recievedData[5] = (ardu_ins_trans.buf[11] << 8) | ardu_ins_trans.buf[10]; + recievedData[6] = (ardu_ins_trans.buf[13] << 8) | ardu_ins_trans.buf[12]; + recievedData[7] = (ardu_ins_trans.buf[15] << 8) | ardu_ins_trans.buf[14]; + recievedData[8] = (ardu_ins_trans.buf[17] << 8) | ardu_ins_trans.buf[16]; // Update ArduIMU data arduimu_eulers.phi = ANGLE_FLOAT_OF_BFP(recievedData[0]) - ins_roll_neutral; @@ -190,11 +195,12 @@ void ArduIMU_event( void ) { #ifdef ARDUIMU_SYNC_SEND //RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi)); - RunOnceEvery(15, DOWNLINK_SEND_IMU_GYRO(DefaultChannel, DefaultDevice, &arduimu_rates.p, &arduimu_rates.q, &arduimu_rates.r)); - RunOnceEvery(15, DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, DefaultDevice, &arduimu_accel.x, &arduimu_accel.y, &arduimu_accel.z)); + RunOnceEvery(15, DOWNLINK_SEND_IMU_GYRO(DefaultChannel, DefaultDevice, &arduimu_rates.p, &arduimu_rates.q, + &arduimu_rates.r)); + RunOnceEvery(15, DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, DefaultDevice, &arduimu_accel.x, &arduimu_accel.y, + &arduimu_accel.z)); #endif - } - else if (ardu_ins_trans.status == I2CTransFailed) { + } else if (ardu_ins_trans.status == I2CTransFailed) { ardu_ins_trans.status = I2CTransDone; } // Handle GPS I2C event @@ -203,7 +209,7 @@ void ArduIMU_event( void ) { } } -void ahrs_update_gps( void ) +void ahrs_update_gps(void) { } diff --git a/sw/airborne/modules/ins/ins_arduimu_basic.h b/sw/airborne/modules/ins/ins_arduimu_basic.h index f21b12b160..ddb207539c 100644 --- a/sw/airborne/modules/ins/ins_arduimu_basic.h +++ b/sw/airborne/modules/ins/ins_arduimu_basic.h @@ -37,9 +37,9 @@ extern float ins_roll_neutral; extern float ins_pitch_neutral; extern bool_t arduimu_calibrate_neutrals; -void ArduIMU_init( void ); -void ArduIMU_periodic( void ); -void ArduIMU_periodicGPS( void ); -void ArduIMU_event( void ); +void ArduIMU_init(void); +void ArduIMU_periodic(void); +void ArduIMU_periodicGPS(void); +void ArduIMU_event(void); #endif // ArduIMU_H diff --git a/sw/airborne/modules/ins/ins_module.h b/sw/airborne/modules/ins/ins_module.h index e484fa1240..ac71638289 100644 --- a/sw/airborne/modules/ins/ins_module.h +++ b/sw/airborne/modules/ins/ins_module.h @@ -88,15 +88,15 @@ void parse_ins_buffer(uint8_t); #endif /** !SITL */ -#define InsEventCheckAndHandle(handler) { \ - if (InsBuffer()) { \ - ReadInsBuffer(); \ - } \ - if (ins_msg_received) { \ - parse_ins_msg(); \ - handler; \ - ins_msg_received = FALSE; \ - } \ +#define InsEventCheckAndHandle(handler) { \ + if (InsBuffer()) { \ + ReadInsBuffer(); \ + } \ + if (ins_msg_received) { \ + parse_ins_msg(); \ + handler; \ + ins_msg_received = FALSE; \ + } \ } diff --git a/sw/airborne/modules/ins/ins_vn100.c b/sw/airborne/modules/ins/ins_vn100.c index 001f4e759e..f35c2c5e78 100644 --- a/sw/airborne/modules/ins/ins_vn100.c +++ b/sw/airborne/modules/ins/ins_vn100.c @@ -76,13 +76,14 @@ uint32_t ins_baud; uint8_t ins_init_status; // parsing function -static inline void parse_ins_msg( void ); +static inline void parse_ins_msg(void); /* spi transaction */ struct spi_transaction vn100_trans; /* init vn100 */ -void vn100_init( void ) { +void vn100_init(void) +{ //ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; //ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT; @@ -93,8 +94,8 @@ void vn100_init( void ) { vn100_trans.cpha = SPICphaEdge2; vn100_trans.dss = SPIDss8bit; vn100_trans.select = SPISelectUnselect; - vn100_trans.output_buf = (uint8_t*)&last_send_packet; - vn100_trans.input_buf = (uint8_t*)&last_received_packet; + vn100_trans.output_buf = (uint8_t *)&last_send_packet; + vn100_trans.input_buf = (uint8_t *)&last_received_packet; vn100_trans.status = SPITransDone; ins_ador = VN100_ADOR; @@ -105,24 +106,25 @@ void vn100_init( void ) { } -static inline bool_t ins_configure( void ) { +static inline bool_t ins_configure(void) +{ // nothing to receive during conf vn100_trans.input_length = 0; switch (ins_init_status) { case INS_VN100_SET_BAUD : last_send_packet.RegID = VN100_REG_SBAUD; - vn100_trans.output_length = 4+VN100_REG_SBAUD_SIZE; + vn100_trans.output_length = 4 + VN100_REG_SBAUD_SIZE; ins_init_status++; break; case INS_VN100_SET_ADOR : last_send_packet.RegID = VN100_REG_ADOR; - vn100_trans.output_length = 4+VN100_REG_ADOR_SIZE; + vn100_trans.output_length = 4 + VN100_REG_ADOR_SIZE; ins_init_status++; break; case INS_VN100_SET_ADOF : last_send_packet.RegID = VN100_REG_ADOF; - vn100_trans.output_length = 4+VN100_REG_ADOF_SIZE; + vn100_trans.output_length = 4 + VN100_REG_ADOF_SIZE; ins_init_status++; break; case INS_VN100_READY : @@ -130,12 +132,13 @@ static inline bool_t ins_configure( void ) { } last_send_packet.CmdID = VN100_CmdID_WriteRegister; - spi_submit(&(VN100_SPI_DEV),&vn100_trans); + spi_submit(&(VN100_SPI_DEV), &vn100_trans); return FALSE; } -void vn100_periodic_task( void ) { +void vn100_periodic_task(void) +{ // only send config or request when last transaction is done if (vn100_trans.status != SPITransDone) { return; } @@ -147,14 +150,15 @@ void vn100_periodic_task( void ) { last_send_packet.RegID = VN100_REG_YMR; // Set IO length vn100_trans.output_length = 2; // Only 2 ? - vn100_trans.input_length = 4+VN100_REG_YMR_SIZE; + vn100_trans.input_length = 4 + VN100_REG_YMR_SIZE; // submit - spi_submit(&(VN100_SPI_DEV),&vn100_trans); + spi_submit(&(VN100_SPI_DEV), &vn100_trans); } } -void vn100_event_task( void ) { +void vn100_event_task(void) +{ if (vn100_trans.status == SPITransSuccess) { parse_ins_msg(); #ifndef INS_VN100_READ_ONLY @@ -178,7 +182,8 @@ void vn100_event_task( void ) { } } -static inline void parse_ins_msg( void ) { +static inline void parse_ins_msg(void) +{ if (last_received_packet.ErrID != VN100_Error_None) { //TODO send error return; @@ -303,12 +308,13 @@ static inline void parse_ins_msg( void ) { #include "messages.h" #include "subsystems/datalink/downlink.h" -extern void vn100_report_task( void ) { +extern void vn100_report_task(void) +{ DOWNLINK_SEND_AHRS_LKF(DefaultChannel, DefaultDevice, - &ins_eulers.phi, &ins_eulers.theta, &ins_eulers.psi, - &ins_quat.qi, &ins_quat.qx, &ins_quat.qy, &ins_quat.qz, - &ins_rates.p, &ins_rates.q, &ins_rates.r, - &ins_accel.x, &ins_accel.y, &ins_accel.z, - &ins_mag.x, &ins_mag.y, &ins_mag.z); + &ins_eulers.phi, &ins_eulers.theta, &ins_eulers.psi, + &ins_quat.qi, &ins_quat.qx, &ins_quat.qy, &ins_quat.qz, + &ins_rates.p, &ins_rates.q, &ins_rates.r, + &ins_accel.x, &ins_accel.y, &ins_accel.z, + &ins_mag.x, &ins_mag.y, &ins_mag.z); } diff --git a/sw/airborne/modules/ins/ins_vn100.h b/sw/airborne/modules/ins/ins_vn100.h index c11a4d2351..38ea5c5a5a 100644 --- a/sw/airborne/modules/ins/ins_vn100.h +++ b/sw/airborne/modules/ins/ins_vn100.h @@ -48,10 +48,10 @@ extern struct FloatVect3 ins_mag; extern volatile uint8_t ins_msg_received; -extern void vn100_init( void ); -extern void vn100_periodic_task( void ); -extern void vn100_event_task( void ); -extern void vn100_report_task( void ); +extern void vn100_init(void); +extern void vn100_periodic_task(void); +extern void vn100_event_task(void); +extern void vn100_report_task(void); /* last received SPI packet */ extern VN100_Res_Packet last_received_packet; diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c index e6833bbe3b..e0c48687d6 100644 --- a/sw/airborne/modules/ins/ins_xsens.c +++ b/sw/airborne/modules/ins/ins_xsens.c @@ -84,7 +84,7 @@ float ins_roll_neutral; ////////////////////////////////////////////////////////////////////////////////////////// // -// XSens Specific +// XSens Specific // volatile uint8_t ins_msg_received; @@ -98,12 +98,12 @@ volatile uint8_t ins_msg_received; #define XsensSend4ByAddr(x) { XsensSend1(*(x+3)); XsensSend1(*(x+2)); XsensSend1(*(x+1)); XsensSend1(*x); } #define XsensHeader(msg_id, len) { \ - InsUartSend1(XSENS_START); \ - XsensInitCheksum(); \ - XsensSend1(XSENS_BID); \ - XsensSend1(msg_id); \ - XsensSend1(len); \ -} + InsUartSend1(XSENS_START); \ + XsensInitCheksum(); \ + XsensSend1(XSENS_BID); \ + XsensSend1(msg_id); \ + XsensSend1(len); \ + } #define XsensTrailer() { uint8_t i8=0x100-send_ck; InsUartSend1(i8); } /** Includes macros generated from xsens_MTi-G.xml */ @@ -116,22 +116,22 @@ uint8_t xsens_msg_buf[XSENS_MAX_PAYLOAD]; /* output mode : calibrated, orientation, position, velocity, status * ----------- * - * bit 0 temp - * bit 1 calibrated - * bit 2 orentation - * bit 3 aux + * bit 0 temp + * bit 1 calibrated + * bit 2 orentation + * bit 3 aux * - * bit 4 position - * bit 5 velocity - * bit 6-7 Reserved + * bit 4 position + * bit 5 velocity + * bit 6-7 Reserved * - * bit 8-10 Reserved - * bit 11 status + * bit 8-10 Reserved + * bit 11 status * - * bit 12 GPS PVT+baro - * bit 13 Reserved - * bit 14 Raw - * bit 15 Reserved + * bit 12 GPS PVT+baro + * bit 13 Reserved + * bit 14 Raw + * bit 15 Reserved */ #ifndef XSENS_OUTPUT_MODE @@ -140,28 +140,28 @@ uint8_t xsens_msg_buf[XSENS_MAX_PAYLOAD]; /* output settings : timestamp, euler, acc, rate, mag, float, no aux, lla, m/s, NED * ----------- * - * bit 01 0=none, 1=sample counter, 2=utc, 3=sample+utc - * bit 23 0=quaternion, 1=euler, 2=DCM + * bit 01 0=none, 1=sample counter, 2=utc, 3=sample+utc + * bit 23 0=quaternion, 1=euler, 2=DCM * - * bit 4 1=disable acc output - * bit 5 1=disable gyro output - * bit 6 1=disable magneto output - * bit 7 Reserved + * bit 4 1=disable acc output + * bit 5 1=disable gyro output + * bit 6 1=disable magneto output + * bit 7 Reserved * - * bit 89 0=float, 1=fixedpoint12.20, 2=fixedpoint16.32 - * bit 10 1=disable aux analog 1 - * bit 11 1=disable aux analog 2 + * bit 89 0=float, 1=fixedpoint12.20, 2=fixedpoint16.32 + * bit 10 1=disable aux analog 1 + * bit 11 1=disable aux analog 2 * - * bit 12-15 0-only: 14-16 WGS84 + * bit 12-15 0-only: 14-16 WGS84 * - * bit 16-19 0-only: 16-18 m/s XYZ + * bit 16-19 0-only: 16-18 m/s XYZ * - * bit 20-23 Reserved + * bit 20-23 Reserved * - * bit 24-27 Reseverd + * bit 24-27 Reseverd * - * bit 28-30 Reseverd - * bit 31 0=X-North-Z-Up, 1=North-East-Down + * bit 28-30 Reseverd + * bit 31 0=X-North-Z-Up, 1=North-East-Down */ #ifndef XSENS_OUTPUT_SETTINGS #define XSENS_OUTPUT_SETTINGS 0x80000C05 @@ -212,7 +212,8 @@ volatile int xsens_configured = 0; void xsens_init(void); void xsens_periodic(void); -void xsens_init(void) { +void xsens_init(void) +{ xsens_status = UNINIT; xsens_configured = 20; @@ -231,20 +232,23 @@ void xsens_init(void) { #if USE_IMU struct ImuXsens imu_xsens; -void imu_impl_init(void) { +void imu_impl_init(void) +{ xsens_init(); imu_xsens.gyro_available = FALSE; imu_xsens.accel_available = FALSE; imu_xsens.mag_available = FALSE; } -void imu_periodic(void) { +void imu_periodic(void) +{ xsens_periodic(); } #endif /* USE_IMU */ #if USE_INS_MODULE -void ins_init(void) { +void ins_init(void) +{ struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; stateSetLocalUtmOrigin_f(&utm0); stateSetPositionUtm_f(&utm0); @@ -252,11 +256,13 @@ void ins_init(void) { xsens_init(); } -void ins_periodic(void) { +void ins_periodic(void) +{ xsens_periodic(); } -void ins_update_gps(void) { +void ins_update_gps(void) +{ struct UtmCoor_f utm; utm.east = gps.utm_pos.east / 100.; utm.north = gps.utm_pos.north / 100.; @@ -277,82 +283,82 @@ void ins_update_gps(void) { #endif #if USE_GPS_XSENS -void gps_impl_init(void) { +void gps_impl_init(void) +{ gps.nb_channels = 0; gps_xsens_msg_available = FALSE; } #endif -void xsens_periodic(void) { - if (xsens_configured > 0) - { - switch (xsens_configured) - { - case 20: - /* 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: - // 1 pulse every 100 samples - XSENS_SetSyncOutSettings(1,100); - break; - case 2: - XSENS_ReqLeverArmGps(); - break; +void xsens_periodic(void) +{ + if (xsens_configured > 0) { + switch (xsens_configured) { + case 20: + /* 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: + // 1 pulse every 100 samples + XSENS_SetSyncOutSettings(1, 100); + break; + case 2: + XSENS_ReqLeverArmGps(); + break; - case 6: - XSENS_ReqMagneticDeclination(); - break; + case 6: + XSENS_ReqMagneticDeclination(); + break; - case 13: + 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); + xsens_declination = atan2(AHRS_H_Y, AHRS_H_X); + XSENS_SetMagneticDeclination(xsens_declination); #endif - break; - case 12: + 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); + 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; + break; + case 10: { + uint8_t baud = 1; + XSENS_SetBaudrate(baud); + } + break; - case 1: - XSENS_GoToMeasurment(); - break; + case 1: + XSENS_GoToMeasurment(); + break; - default: - break; - } - xsens_configured--; - return; + default: + break; } + xsens_configured--; + return; + } - RunOnceEvery(100,XSENS_ReqGPSStatus()); + RunOnceEvery(100, XSENS_ReqGPSStatus()); } #if USE_INS_MODULE #include "state.h" -static inline void update_fw_estimator(void) { +static inline void update_fw_estimator(void) +{ // Send to Estimator (Control) #ifdef XSENS_BACKWARDS struct FloatEulers att = { - -ins_phi+ins_roll_neutral, - -ins_theta+ins_pitch_neutral, + -ins_phi + ins_roll_neutral, + -ins_theta + ins_pitch_neutral, ins_psi + RadOfDeg(180) }; struct FloatRates rates = { @@ -362,8 +368,8 @@ static inline void update_fw_estimator(void) { }; #else struct FloatEulers att = { - ins_phi+ins_roll_neutral, - ins_theta+ins_pitch_neutral, + ins_phi + ins_roll_neutral, + ins_theta + ins_pitch_neutral, ins_psi }; struct FloatRates rates = { @@ -377,7 +383,8 @@ static inline void update_fw_estimator(void) { } #endif /* USE_INS_MODULE */ -void handle_ins_msg(void) { +void handle_ins_msg(void) +{ #if USE_INS_MODULE update_fw_estimator(); @@ -410,39 +417,38 @@ void handle_ins_msg(void) { #if USE_GPS_XSENS // Horizontal speed - float fspeed = sqrt(ins_vx*ins_vx + ins_vy*ins_vy); + float fspeed = sqrt(ins_vx * ins_vx + ins_vy * ins_vy); if (gps.fix != GPS_FIX_3D) { fspeed = 0; } gps.gspeed = fspeed * 100.; - gps.speed_3d = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy + ins_vz*ins_vz) * 100); + gps.speed_3d = (uint16_t)(sqrt(ins_vx * ins_vx + ins_vy * ins_vy + ins_vz * ins_vz) * 100); float fcourse = atan2f((float)ins_vy, (float)ins_vx); gps.course = fcourse * 1e7; #endif // USE_GPS_XSENS } -void parse_ins_msg(void) { +void parse_ins_msg(void) +{ uint8_t offset = 0; if (xsens_id == XSENS_ReqOutputModeAck_ID) { xsens_output_mode = XSENS_ReqOutputModeAck_mode(xsens_msg_buf); - } - else if (xsens_id == XSENS_ReqOutputSettings_ID) { + } else if (xsens_id == XSENS_ReqOutputSettings_ID) { xsens_output_settings = XSENS_ReqOutputSettingsAck_settings(xsens_msg_buf); - } - else if (xsens_id == XSENS_ReqMagneticDeclinationAck_ID) { - xsens_declination = DegOfRad (XSENS_ReqMagneticDeclinationAck_declination(xsens_msg_buf) ); + } else if (xsens_id == XSENS_ReqMagneticDeclinationAck_ID) { + xsens_declination = DegOfRad(XSENS_ReqMagneticDeclinationAck_declination(xsens_msg_buf)); - DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel, DefaultDevice,&xsens_declination,&xsens_declination,&xsens_gps_arm_x,&xsens_gps_arm_y,&xsens_gps_arm_z); - } - else if (xsens_id == XSENS_ReqLeverArmGpsAck_ID) { + DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel, DefaultDevice, &xsens_declination, &xsens_declination, &xsens_gps_arm_x, + &xsens_gps_arm_y, &xsens_gps_arm_z); + } else if (xsens_id == XSENS_ReqLeverArmGpsAck_ID) { xsens_gps_arm_x = XSENS_ReqLeverArmGpsAck_x(xsens_msg_buf); xsens_gps_arm_y = XSENS_ReqLeverArmGpsAck_y(xsens_msg_buf); xsens_gps_arm_z = XSENS_ReqLeverArmGpsAck_z(xsens_msg_buf); - DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel, DefaultDevice,&xsens_declination,&xsens_declination,&xsens_gps_arm_x,&xsens_gps_arm_y,&xsens_gps_arm_z); - } - else if (xsens_id == XSENS_Error_ID) { + DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel, DefaultDevice, &xsens_declination, &xsens_declination, &xsens_gps_arm_x, + &xsens_gps_arm_y, &xsens_gps_arm_z); + } else if (xsens_id == XSENS_Error_ID) { xsens_errorcode = XSENS_Error_errorcode(xsens_msg_buf); } #if USE_GPS_XSENS @@ -452,15 +458,14 @@ void parse_ins_msg(void) { uint8_t i; // Do not write outside buffer - for(i = 0; i < Min(gps.nb_channels, GPS_NB_CHANNELS); i++) { - uint8_t ch = XSENS_GPSStatus_chn(xsens_msg_buf,i); - if (ch > gps.nb_channels) continue; + for (i = 0; i < Min(gps.nb_channels, GPS_NB_CHANNELS); i++) { + uint8_t ch = XSENS_GPSStatus_chn(xsens_msg_buf, i); + if (ch > gps.nb_channels) { continue; } gps.svinfos[ch].svid = XSENS_GPSStatus_svid(xsens_msg_buf, i); gps.svinfos[ch].flags = XSENS_GPSStatus_bitmask(xsens_msg_buf, i); gps.svinfos[ch].qi = XSENS_GPSStatus_qi(xsens_msg_buf, i); gps.svinfos[ch].cno = XSENS_GPSStatus_cnr(xsens_msg_buf, i); - if (gps.svinfos[ch].flags > 0) - { + if (gps.svinfos[ch].flags > 0) { gps.num_sv++; } } @@ -469,244 +474,249 @@ void parse_ins_msg(void) { else if (xsens_id == XSENS_MTData_ID) { /* test RAW modes else calibrated modes */ //if ((XSENS_MASK_RAWInertial(xsens_output_mode)) || (XSENS_MASK_RAWGPS(xsens_output_mode))) { - if (XSENS_MASK_RAWInertial(xsens_output_mode)) { - ins_p = XSENS_DATA_RAWInertial_gyrX(xsens_msg_buf,offset); - ins_q = XSENS_DATA_RAWInertial_gyrY(xsens_msg_buf,offset); - ins_r = XSENS_DATA_RAWInertial_gyrZ(xsens_msg_buf,offset); + if (XSENS_MASK_RAWInertial(xsens_output_mode)) { + ins_p = XSENS_DATA_RAWInertial_gyrX(xsens_msg_buf, offset); + ins_q = XSENS_DATA_RAWInertial_gyrY(xsens_msg_buf, offset); + ins_r = XSENS_DATA_RAWInertial_gyrZ(xsens_msg_buf, offset); +#if USE_IMU + imu_xsens.gyro_available = TRUE; +#endif + offset += XSENS_DATA_RAWInertial_LENGTH; + } + if (XSENS_MASK_RAWGPS(xsens_output_mode)) { +#if USE_GPS_XSENS_RAW_DATA && USE_GPS_XSENS + + gps.week = 0; // FIXME + gps.tow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf, offset) * 10; + gps.lla_pos.lat = XSENS_DATA_RAWGPS_lat(xsens_msg_buf, offset); + gps.lla_pos.lon = XSENS_DATA_RAWGPS_lon(xsens_msg_buf, offset); + gps.lla_pos.alt = XSENS_DATA_RAWGPS_alt(xsens_msg_buf, offset); + + /* Set the real UTM zone */ + gps.utm_pos.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1; + LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos); + utm_f.zone = nav_utm_zone0; + /* convert to utm */ + utm_of_lla_f(&utm_f, &lla_f); + /* copy results of utm conversion */ + gps.utm_pos.east = utm_f.east * 100; + gps.utm_pos.north = utm_f.north * 100; + gps.utm_pos.alt = gps.lla_pos.alt; + + ins_x = utm_f.east; + ins_y = utm_f.north; + // Altitude: Xsens LLH gives ellipsoid height + ins_z = -(INS_FORMAT)XSENS_DATA_RAWGPS_alt(xsens_msg_buf, offset) / 1000.; + + // Compute geoid (MSL) height + float hmsl = wgs84_ellipsoid_to_geoid(lla_f.lat, lla_f.lon); + gps.hmsl = XSENS_DATA_RAWGPS_alt(xsens_msg_buf, offset) - (hmsl * 1000.0f); + + ins_vx = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf, offset)) / 100.; + ins_vy = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf, offset)) / 100.; + ins_vz = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf, offset)) / 100.; + gps.ned_vel.x = XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf, offset); + gps.ned_vel.y = XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf, offset); + gps.ned_vel.z = XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf, offset); + gps.pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf, offset) / 100; + gps.sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf, offset) / 100; + gps.pdop = 5; // FIXME Not output by XSens + + gps_xsens_msg_available = TRUE; +#endif + offset += XSENS_DATA_RAWGPS_LENGTH; + } + //} else { + if (XSENS_MASK_Temp(xsens_output_mode)) { + offset += XSENS_DATA_Temp_LENGTH; + } + if (XSENS_MASK_Calibrated(xsens_output_mode)) { + uint8_t l = 0; + if (!XSENS_MASK_AccOut(xsens_output_settings)) { + ins_ax = XSENS_DATA_Calibrated_accX(xsens_msg_buf, offset); + ins_ay = XSENS_DATA_Calibrated_accY(xsens_msg_buf, offset); + ins_az = XSENS_DATA_Calibrated_accZ(xsens_msg_buf, offset); +#if USE_IMU + imu_xsens.accel_available = TRUE; +#endif + l++; + } + if (!XSENS_MASK_GyrOut(xsens_output_settings)) { + ins_p = XSENS_DATA_Calibrated_gyrX(xsens_msg_buf, offset); + ins_q = XSENS_DATA_Calibrated_gyrY(xsens_msg_buf, offset); + ins_r = XSENS_DATA_Calibrated_gyrZ(xsens_msg_buf, offset); #if USE_IMU imu_xsens.gyro_available = TRUE; #endif - offset += XSENS_DATA_RAWInertial_LENGTH; + l++; } - if (XSENS_MASK_RAWGPS(xsens_output_mode)) { -#if USE_GPS_XSENS_RAW_DATA && USE_GPS_XSENS - - gps.week = 0; // FIXME - gps.tow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf,offset) * 10; - gps.lla_pos.lat = XSENS_DATA_RAWGPS_lat(xsens_msg_buf,offset); - gps.lla_pos.lon = XSENS_DATA_RAWGPS_lon(xsens_msg_buf,offset); - gps.lla_pos.alt = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset); - - /* Set the real UTM zone */ - gps.utm_pos.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1; - LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos); - utm_f.zone = nav_utm_zone0; - /* convert to utm */ - utm_of_lla_f(&utm_f, &lla_f); - /* copy results of utm conversion */ - gps.utm_pos.east = utm_f.east*100; - gps.utm_pos.north = utm_f.north*100; - gps.utm_pos.alt = gps.lla_pos.alt; - - ins_x = utm_f.east; - ins_y = utm_f.north; - // Altitude: Xsens LLH gives ellipsoid height - ins_z = -(INS_FORMAT)XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) / 1000.; - - // Compute geoid (MSL) height - float hmsl = wgs84_ellipsoid_to_geoid(lla_f.lat, lla_f.lon); - gps.hmsl = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset) - (hmsl * 1000.0f); - - ins_vx = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset)) / 100.; - ins_vy = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset)) / 100.; - ins_vz = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset)) / 100.; - gps.ned_vel.x = XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf,offset); - gps.ned_vel.y = XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf,offset); - gps.ned_vel.z = XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf,offset); - gps.pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf,offset) / 100; - gps.sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf,offset) / 100; - gps.pdop = 5; // FIXME Not output by XSens - - gps_xsens_msg_available = TRUE; -#endif - offset += XSENS_DATA_RAWGPS_LENGTH; - } - //} else { - if (XSENS_MASK_Temp(xsens_output_mode)) { - offset += XSENS_DATA_Temp_LENGTH; - } - if (XSENS_MASK_Calibrated(xsens_output_mode)) { - uint8_t l = 0; - if (!XSENS_MASK_AccOut(xsens_output_settings)) { - ins_ax = XSENS_DATA_Calibrated_accX(xsens_msg_buf,offset); - ins_ay = XSENS_DATA_Calibrated_accY(xsens_msg_buf,offset); - ins_az = XSENS_DATA_Calibrated_accZ(xsens_msg_buf,offset); + if (!XSENS_MASK_MagOut(xsens_output_settings)) { + ins_mx = XSENS_DATA_Calibrated_magX(xsens_msg_buf, offset); + ins_my = XSENS_DATA_Calibrated_magY(xsens_msg_buf, offset); + ins_mz = XSENS_DATA_Calibrated_magZ(xsens_msg_buf, offset); #if USE_IMU - imu_xsens.accel_available = TRUE; + imu_xsens.mag_available = TRUE; #endif - l++; - } - if (!XSENS_MASK_GyrOut(xsens_output_settings)) { - ins_p = XSENS_DATA_Calibrated_gyrX(xsens_msg_buf,offset); - ins_q = XSENS_DATA_Calibrated_gyrY(xsens_msg_buf,offset); - ins_r = XSENS_DATA_Calibrated_gyrZ(xsens_msg_buf,offset); -#if USE_IMU - imu_xsens.gyro_available = TRUE; -#endif - l++; - } - if (!XSENS_MASK_MagOut(xsens_output_settings)) { - ins_mx = XSENS_DATA_Calibrated_magX(xsens_msg_buf,offset); - ins_my = XSENS_DATA_Calibrated_magY(xsens_msg_buf,offset); - ins_mz = XSENS_DATA_Calibrated_magZ(xsens_msg_buf,offset); -#if USE_IMU - imu_xsens.mag_available = TRUE; -#endif - l++; - } - offset += l * XSENS_DATA_Calibrated_LENGTH / 3; + l++; } - if (XSENS_MASK_Orientation(xsens_output_mode)) { - if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x00) { - float q0 = XSENS_DATA_Quaternion_q0(xsens_msg_buf,offset); - float q1 = XSENS_DATA_Quaternion_q1(xsens_msg_buf,offset); - float q2 = XSENS_DATA_Quaternion_q2(xsens_msg_buf,offset); - float q3 = XSENS_DATA_Quaternion_q3(xsens_msg_buf,offset); - float dcm00 = 1.0 - 2 * (q2*q2 + q3*q3); - float dcm01 = 2 * (q1*q2 + q0*q3); - float dcm02 = 2 * (q1*q3 - q0*q2); - float dcm12 = 2 * (q2*q3 + q0*q1); - float dcm22 = 1.0 - 2 * (q1*q1 + q2*q2); - ins_phi = atan2(dcm12, dcm22); - ins_theta = -asin(dcm02); - ins_psi = atan2(dcm01, dcm00); - offset += XSENS_DATA_Quaternion_LENGTH; - } - if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x01) { - ins_phi = XSENS_DATA_Euler_roll(xsens_msg_buf,offset) * M_PI / 180; - ins_theta = XSENS_DATA_Euler_pitch(xsens_msg_buf,offset) * M_PI / 180; - ins_psi = XSENS_DATA_Euler_yaw(xsens_msg_buf,offset) * M_PI / 180; - offset += XSENS_DATA_Euler_LENGTH; - } - if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x10) { - offset += XSENS_DATA_Matrix_LENGTH; - } - new_ins_attitude = 1; + offset += l * XSENS_DATA_Calibrated_LENGTH / 3; + } + if (XSENS_MASK_Orientation(xsens_output_mode)) { + if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x00) { + float q0 = XSENS_DATA_Quaternion_q0(xsens_msg_buf, offset); + float q1 = XSENS_DATA_Quaternion_q1(xsens_msg_buf, offset); + float q2 = XSENS_DATA_Quaternion_q2(xsens_msg_buf, offset); + float q3 = XSENS_DATA_Quaternion_q3(xsens_msg_buf, offset); + float dcm00 = 1.0 - 2 * (q2 * q2 + q3 * q3); + float dcm01 = 2 * (q1 * q2 + q0 * q3); + float dcm02 = 2 * (q1 * q3 - q0 * q2); + float dcm12 = 2 * (q2 * q3 + q0 * q1); + float dcm22 = 1.0 - 2 * (q1 * q1 + q2 * q2); + ins_phi = atan2(dcm12, dcm22); + ins_theta = -asin(dcm02); + ins_psi = atan2(dcm01, dcm00); + offset += XSENS_DATA_Quaternion_LENGTH; } - if (XSENS_MASK_Auxiliary(xsens_output_mode)) { - uint8_t l = 0; - if (!XSENS_MASK_Aux1Out(xsens_output_settings)) { - l++; - } - if (!XSENS_MASK_Aux2Out(xsens_output_settings)) { - l++; - } - offset += l * XSENS_DATA_Auxiliary_LENGTH / 2; + if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x01) { + ins_phi = XSENS_DATA_Euler_roll(xsens_msg_buf, offset) * M_PI / 180; + ins_theta = XSENS_DATA_Euler_pitch(xsens_msg_buf, offset) * M_PI / 180; + ins_psi = XSENS_DATA_Euler_yaw(xsens_msg_buf, offset) * M_PI / 180; + offset += XSENS_DATA_Euler_LENGTH; } - if (XSENS_MASK_Position(xsens_output_mode)) { + if (XSENS_MASK_OrientationMode(xsens_output_settings) == 0x10) { + offset += XSENS_DATA_Matrix_LENGTH; + } + new_ins_attitude = 1; + } + if (XSENS_MASK_Auxiliary(xsens_output_mode)) { + uint8_t l = 0; + if (!XSENS_MASK_Aux1Out(xsens_output_settings)) { + l++; + } + if (!XSENS_MASK_Aux2Out(xsens_output_settings)) { + l++; + } + offset += l * XSENS_DATA_Auxiliary_LENGTH / 2; + } + if (XSENS_MASK_Position(xsens_output_mode)) { #if (! USE_GPS_XSENS_RAW_DATA) && USE_GPS_XSENS - lla_f.lat = RadOfDeg(XSENS_DATA_Position_lat(xsens_msg_buf,offset)); - lla_f.lon = RadOfDeg(XSENS_DATA_Position_lon(xsens_msg_buf,offset)); - gps.lla_pos.lat = (int32_t)(DegOfRad(lla_f.lat) * 1e7); - gps.lla_pos.lon = (int32_t)(DegOfRad(lla_f.lon) * 1e7); - gps.utm_pos.zone = (DegOfRad(lla_f.lon)+180) / 6 + 1; - /* convert to utm */ - utm_of_lla_f(&utm_f, &lla_f); - /* copy results of utm conversion */ - gps.utm_pos.east = utm_f.east*100; - gps.utm_pos.north = utm_f.north*100; - ins_x = utm_f.east; - ins_y = utm_f.north; - ins_z = XSENS_DATA_Position_alt(xsens_msg_buf,offset);//TODO is this hms or above ellipsoid? - gps.hmsl = ins_z * 1000; - // what about gps.lla_pos.alt and gps.utm_pos.alt ? + lla_f.lat = RadOfDeg(XSENS_DATA_Position_lat(xsens_msg_buf, offset)); + lla_f.lon = RadOfDeg(XSENS_DATA_Position_lon(xsens_msg_buf, offset)); + gps.lla_pos.lat = (int32_t)(DegOfRad(lla_f.lat) * 1e7); + gps.lla_pos.lon = (int32_t)(DegOfRad(lla_f.lon) * 1e7); + gps.utm_pos.zone = (DegOfRad(lla_f.lon) + 180) / 6 + 1; + /* convert to utm */ + utm_of_lla_f(&utm_f, &lla_f); + /* copy results of utm conversion */ + gps.utm_pos.east = utm_f.east * 100; + gps.utm_pos.north = utm_f.north * 100; + ins_x = utm_f.east; + ins_y = utm_f.north; + ins_z = XSENS_DATA_Position_alt(xsens_msg_buf, offset); //TODO is this hms or above ellipsoid? + gps.hmsl = ins_z * 1000; + // what about gps.lla_pos.alt and gps.utm_pos.alt ? - gps_xsens_msg_available = TRUE; + gps_xsens_msg_available = TRUE; #endif - offset += XSENS_DATA_Position_LENGTH; - } - if (XSENS_MASK_Velocity(xsens_output_mode)) { + offset += XSENS_DATA_Position_LENGTH; + } + if (XSENS_MASK_Velocity(xsens_output_mode)) { #if (! USE_GPS_XSENS_RAW_DATA) && USE_GPS_XSENS - ins_vx = XSENS_DATA_Velocity_vx(xsens_msg_buf,offset); - ins_vy = XSENS_DATA_Velocity_vy(xsens_msg_buf,offset); - ins_vz = XSENS_DATA_Velocity_vz(xsens_msg_buf,offset); + ins_vx = XSENS_DATA_Velocity_vx(xsens_msg_buf, offset); + ins_vy = XSENS_DATA_Velocity_vy(xsens_msg_buf, offset); + ins_vz = XSENS_DATA_Velocity_vz(xsens_msg_buf, offset); #endif - offset += XSENS_DATA_Velocity_LENGTH; - } - if (XSENS_MASK_Status(xsens_output_mode)) { - xsens_msg_status = XSENS_DATA_Status_status(xsens_msg_buf,offset); + offset += XSENS_DATA_Velocity_LENGTH; + } + if (XSENS_MASK_Status(xsens_output_mode)) { + xsens_msg_status = XSENS_DATA_Status_status(xsens_msg_buf, offset); #if USE_GPS_XSENS - if (bit_is_set(xsens_msg_status,2)) gps.fix = GPS_FIX_3D; // gps fix - else if (bit_is_set(xsens_msg_status,1)) gps.fix = 0x01; // efk valid - else gps.fix = GPS_FIX_NONE; + if (bit_is_set(xsens_msg_status, 2)) { gps.fix = GPS_FIX_3D; } // gps fix + else if (bit_is_set(xsens_msg_status, 1)) { gps.fix = 0x01; } // efk valid + else { gps.fix = GPS_FIX_NONE; } #ifdef GPS_LED - if (gps.fix == GPS_FIX_3D) { - LED_ON(GPS_LED); - } - else { - LED_TOGGLE(GPS_LED); - } + if (gps.fix == GPS_FIX_3D) { + LED_ON(GPS_LED); + } else { + LED_TOGGLE(GPS_LED); + } #endif // GPS_LED #endif // USE_GPS_XSENS - offset += XSENS_DATA_Status_LENGTH; - } - if (XSENS_MASK_TimeStamp(xsens_output_settings)) { - xsens_time_stamp = XSENS_DATA_TimeStamp_ts(xsens_msg_buf,offset); + offset += XSENS_DATA_Status_LENGTH; + } + if (XSENS_MASK_TimeStamp(xsens_output_settings)) { + xsens_time_stamp = XSENS_DATA_TimeStamp_ts(xsens_msg_buf, offset); #if USE_GPS_XSENS - gps.tow = xsens_time_stamp; + gps.tow = xsens_time_stamp; #endif - offset += XSENS_DATA_TimeStamp_LENGTH; - } - if (XSENS_MASK_UTC(xsens_output_settings)) { - xsens_time.hour = XSENS_DATA_UTC_hour(xsens_msg_buf,offset); - xsens_time.min = XSENS_DATA_UTC_min(xsens_msg_buf,offset); - xsens_time.sec = XSENS_DATA_UTC_sec(xsens_msg_buf,offset); - xsens_time.nanosec = XSENS_DATA_UTC_nanosec(xsens_msg_buf,offset); - xsens_time.year = XSENS_DATA_UTC_year(xsens_msg_buf,offset); - xsens_time.month = XSENS_DATA_UTC_month(xsens_msg_buf,offset); - xsens_time.day = XSENS_DATA_UTC_day(xsens_msg_buf,offset); + offset += XSENS_DATA_TimeStamp_LENGTH; + } + if (XSENS_MASK_UTC(xsens_output_settings)) { + xsens_time.hour = XSENS_DATA_UTC_hour(xsens_msg_buf, offset); + xsens_time.min = XSENS_DATA_UTC_min(xsens_msg_buf, offset); + xsens_time.sec = XSENS_DATA_UTC_sec(xsens_msg_buf, offset); + xsens_time.nanosec = XSENS_DATA_UTC_nanosec(xsens_msg_buf, offset); + xsens_time.year = XSENS_DATA_UTC_year(xsens_msg_buf, offset); + xsens_time.month = XSENS_DATA_UTC_month(xsens_msg_buf, offset); + xsens_time.day = XSENS_DATA_UTC_day(xsens_msg_buf, offset); - offset += XSENS_DATA_UTC_LENGTH; - } + offset += XSENS_DATA_UTC_LENGTH; + } //} } } -void parse_ins_buffer(uint8_t c) { +void parse_ins_buffer(uint8_t c) +{ ck += c; switch (xsens_status) { - case UNINIT: - if (c != XSENS_START) - goto error; - xsens_status++; - ck = 0; - break; - case GOT_START: - if (c != XSENS_BID) - goto error; - xsens_status++; - break; - case GOT_BID: - xsens_id = c; - xsens_status++; - break; - case GOT_MID: - xsens_len = c; - if (xsens_len > XSENS_MAX_PAYLOAD) - goto error; - xsens_msg_idx = 0; - xsens_status++; - break; - case GOT_LEN: - xsens_msg_buf[xsens_msg_idx] = c; - xsens_msg_idx++; - if (xsens_msg_idx >= xsens_len) + case UNINIT: + if (c != XSENS_START) { + goto error; + } xsens_status++; - break; - case GOT_DATA: - if (ck != 0) - goto error; - ins_msg_received = TRUE; - goto restart; - break; - default: - break; + ck = 0; + break; + case GOT_START: + if (c != XSENS_BID) { + goto error; + } + xsens_status++; + break; + case GOT_BID: + xsens_id = c; + xsens_status++; + break; + case GOT_MID: + xsens_len = c; + if (xsens_len > XSENS_MAX_PAYLOAD) { + goto error; + } + xsens_msg_idx = 0; + xsens_status++; + break; + case GOT_LEN: + xsens_msg_buf[xsens_msg_idx] = c; + xsens_msg_idx++; + if (xsens_msg_idx >= xsens_len) { + xsens_status++; + } + break; + case GOT_DATA: + if (ck != 0) { + goto error; + } + ins_msg_received = TRUE; + goto restart; + break; + default: + break; } return; - error: - restart: +error: +restart: xsens_status = UNINIT; return; } diff --git a/sw/airborne/modules/ins/ins_xsens.h b/sw/airborne/modules/ins/ins_xsens.h index 379706205f..b662360ae0 100644 --- a/sw/airborne/modules/ins/ins_xsens.h +++ b/sw/airborne/modules/ins/ins_xsens.h @@ -79,9 +79,9 @@ extern struct ImuXsens imu_xsens; /* use Xsens as a full INS solution */ #if USE_INS_MODULE -#define InsEvent(_ins_handler) { \ - InsEventCheckAndHandle(handle_ins_msg()) \ -} +#define InsEvent(_ins_handler) { \ + InsEventCheckAndHandle(handle_ins_msg()) \ + } #endif diff --git a/sw/airborne/modules/ins/ins_xsens700.c b/sw/airborne/modules/ins/ins_xsens700.c index b2961efa35..ffe462ded9 100644 --- a/sw/airborne/modules/ins/ins_xsens700.c +++ b/sw/airborne/modules/ins/ins_xsens700.c @@ -86,7 +86,7 @@ float ins_roll_neutral; ////////////////////////////////////////////////////////////////////////////////////////// // -// XSens Specific +// XSens Specific // volatile uint8_t ins_msg_received; @@ -164,7 +164,8 @@ volatile int xsens_configured = 0; void xsens_init(void); void xsens_periodic(void); -void xsens_init(void) { +void xsens_init(void) +{ xsens_status = UNINIT; xsens_configured = 30; @@ -175,7 +176,8 @@ void xsens_init(void) { } #if USE_INS_MODULE -void ins_init(void) { +void ins_init(void) +{ struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; stateSetLocalUtmOrigin_f(&utm0); stateSetPositionUtm_f(&utm0); @@ -186,11 +188,13 @@ void ins_init(void) { xsens_init(); } -void ins_periodic(void) { +void ins_periodic(void) +{ xsens_periodic(); } -void ins_update_gps(void) { +void ins_update_gps(void) +{ struct UtmCoor_f utm; utm.east = gps.utm_pos.east / 100.; utm.north = gps.utm_pos.north / 100.; @@ -211,7 +215,8 @@ void ins_update_gps(void) { #endif #if USE_GPS_XSENS -void gps_impl_init(void) { +void gps_impl_init(void) +{ gps.nb_channels = 0; gps_xsens_msg_available = FALSE; } @@ -226,11 +231,10 @@ static void xsens_ask_message_rate(uint8_t c1, uint8_t c2, uint8_t freq) XsensSend1ByAddr(&freq); } -void xsens_periodic(void) { - if (xsens_configured > 0) - { - switch (xsens_configured) - { +void xsens_periodic(void) +{ + if (xsens_configured > 0) { + switch (xsens_configured) { case 25: /* send mode and settings to MT */ XSENS_GoToConfig(); @@ -244,17 +248,17 @@ void xsens_periodic(void) { 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 + 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: @@ -275,15 +279,14 @@ void xsens_periodic(void) { 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); + 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 10: { + uint8_t baud = 1; + XSENS_SetBaudrate(baud); + } + break; case 1: XSENS_GoToMeasurment(); @@ -301,12 +304,13 @@ void xsens_periodic(void) { #if USE_INS_MODULE #include "state.h" -static inline void update_fw_estimator(void) { +static inline void update_fw_estimator(void) +{ // Send to Estimator (Control) #if XSENS_BACKWARDS struct FloatEulers att = { - ins_phi+ins_roll_neutral, - -ins_theta+ins_pitch_neutral, + ins_phi + ins_roll_neutral, + -ins_theta + ins_pitch_neutral, -ins_psi + RadOfDeg(180) }; struct FloatRates rates = { @@ -316,8 +320,8 @@ static inline void update_fw_estimator(void) { }; #else struct FloatEulers att = { - -ins_phi+ins_roll_neutral, - ins_theta+ins_pitch_neutral, + -ins_phi + ins_roll_neutral, + ins_theta + ins_pitch_neutral, -ins_psi }; struct FloatRates rates = { @@ -331,7 +335,8 @@ static inline void update_fw_estimator(void) { } #endif /* USE_INS_MODULE */ -void handle_ins_msg( void) { +void handle_ins_msg(void) +{ #if USE_INS_MODULE update_fw_estimator(); @@ -339,165 +344,132 @@ void handle_ins_msg( void) { #if USE_GPS_XSENS // Horizontal speed - float fspeed = sqrt(ins_vx*ins_vx + ins_vy*ins_vy); + float fspeed = sqrt(ins_vx * ins_vx + ins_vy * ins_vy); if (gps.fix != GPS_FIX_3D) { fspeed = 0; } gps.gspeed = fspeed * 100.; - gps.speed_3d = (uint16_t)(sqrt(ins_vx*ins_vx + ins_vy*ins_vy + ins_vz*ins_vz) * 100); + gps.speed_3d = (uint16_t)(sqrt(ins_vx * ins_vx + ins_vy * ins_vy + ins_vz * ins_vz) * 100); float fcourse = atan2f((float)ins_vy, (float)ins_vx); gps.course = fcourse * 1e7; #endif // USE_GPS_XSENS } -void parse_ins_msg( void ) { +void parse_ins_msg(void) +{ uint8_t offset = 0; if (xsens_id == XSENS_ReqLeverArmGpsAck_ID) { xsens_gps_arm_x = XSENS_ReqLeverArmGpsAck_x(xsens_msg_buf); xsens_gps_arm_y = XSENS_ReqLeverArmGpsAck_y(xsens_msg_buf); xsens_gps_arm_z = XSENS_ReqLeverArmGpsAck_z(xsens_msg_buf); - DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel, DefaultDevice,&xsens_declination,&xsens_declination,&xsens_gps_arm_x,&xsens_gps_arm_y,&xsens_gps_arm_z); - } - else if (xsens_id == XSENS_Error_ID) { + DOWNLINK_SEND_IMU_MAG_SETTINGS(DefaultChannel, DefaultDevice, &xsens_declination, &xsens_declination, &xsens_gps_arm_x, + &xsens_gps_arm_y, &xsens_gps_arm_z); + } else if (xsens_id == XSENS_Error_ID) { xsens_errorcode = XSENS_Error_errorcode(xsens_msg_buf); - } - else if (xsens_id == XSENS_MTData2_ID) { - for (offset=0;offset gps.nb_channels) continue; - gps.svinfos[ch].svid = XSENS_XDI_GpsSvInfo_svid(xsens_msg_buf+offset, i); - gps.svinfos[ch].flags = XSENS_XDI_GpsSvInfo_bitmask(xsens_msg_buf+offset, i); - gps.svinfos[ch].qi = XSENS_XDI_GpsSvInfo_qi(xsens_msg_buf+offset, i); - gps.svinfos[ch].cno = XSENS_XDI_GpsSvInfo_cnr(xsens_msg_buf+offset, i); + for (i = 0; i < Min(gps.nb_channels, GPS_NB_CHANNELS); i++) { + uint8_t ch = XSENS_XDI_GpsSvInfo_chn(xsens_msg_buf + offset, i); + if (ch > gps.nb_channels) { continue; } + gps.svinfos[ch].svid = XSENS_XDI_GpsSvInfo_svid(xsens_msg_buf + offset, i); + gps.svinfos[ch].flags = XSENS_XDI_GpsSvInfo_bitmask(xsens_msg_buf + offset, i); + gps.svinfos[ch].qi = XSENS_XDI_GpsSvInfo_qi(xsens_msg_buf + offset, i); + gps.svinfos[ch].cno = XSENS_XDI_GpsSvInfo_cnr(xsens_msg_buf + offset, i); } #endif } - } - else if (code1 == 0x50) - { - if (code2 == 0x10) - { + } else if (code1 == 0x50) { + if (code2 == 0x10) { //gps.hmsl = XSENS_DATA_Altitude_h(xsens_msg_buf,offset)* 1000.0f; - } - else if (code2 == 0x20) - { + } else if (code2 == 0x20) { // Altitude Elipsoid - gps.utm_pos.alt = XSENS_DATA_Altitude_h(xsens_msg_buf,offset)* 1000.0f; + gps.utm_pos.alt = XSENS_DATA_Altitude_h(xsens_msg_buf, offset) * 1000.0f; // Compute geoid (MSL) height float geoid_h = wgs84_ellipsoid_to_geoid(lla_f.lat, lla_f.lon); gps.hmsl = gps.utm_pos.alt - (geoid_h * 1000.0f); //gps.tow = geoid_h * 1000.0f; //gps.utm_pos.alt; - } - else if (code2 == 0x40) - { + } else if (code2 == 0x40) { // LatLong #ifdef GPS_LED LED_TOGGLE(GPS_LED); @@ -505,38 +477,36 @@ void parse_ins_msg( void ) { gps.last_3dfix_ticks = sys_time.nb_sec_rem; gps.last_3dfix_time = sys_time.nb_sec; gps.week = 0; // FIXME - lla_f.lat = RadOfDeg(XSENS_DATA_LatLon_lat(xsens_msg_buf,offset)); - lla_f.lon = RadOfDeg(XSENS_DATA_LatLon_lon(xsens_msg_buf,offset)); + lla_f.lat = RadOfDeg(XSENS_DATA_LatLon_lat(xsens_msg_buf, offset)); + lla_f.lon = RadOfDeg(XSENS_DATA_LatLon_lon(xsens_msg_buf, offset)); // Set the real UTM zone - gps.utm_pos.zone = (DegOfRad(lla_f.lon)+180) / 6 + 1; + gps.utm_pos.zone = (DegOfRad(lla_f.lon) + 180) / 6 + 1; utm_f.zone = nav_utm_zone0; // convert to utm utm_of_lla_f(&utm_f, &lla_f); // copy results of utm conversion - gps.utm_pos.east = utm_f.east*100; - gps.utm_pos.north = utm_f.north*100; + gps.utm_pos.east = utm_f.east * 100; + gps.utm_pos.north = utm_f.north * 100; gps_xsens_msg_available = TRUE; } - } - else if (code1 == 0xD0) - { - if (code2 == 0x10) - { + } else if (code1 == 0xD0) { + if (code2 == 0x10) { // Velocity - ins_vx = ((INS_FORMAT)XSENS_DATA_VelocityXYZ_x(xsens_msg_buf,offset)); - ins_vy = ((INS_FORMAT)XSENS_DATA_VelocityXYZ_y(xsens_msg_buf,offset)); - ins_vz = ((INS_FORMAT)XSENS_DATA_VelocityXYZ_z(xsens_msg_buf,offset)); + ins_vx = ((INS_FORMAT)XSENS_DATA_VelocityXYZ_x(xsens_msg_buf, offset)); + ins_vy = ((INS_FORMAT)XSENS_DATA_VelocityXYZ_y(xsens_msg_buf, offset)); + ins_vz = ((INS_FORMAT)XSENS_DATA_VelocityXYZ_z(xsens_msg_buf, offset)); gps.ned_vel.x = ins_vx; gps.ned_vel.y = ins_vy; gps.ned_vel.z = ins_vx; } } - if (subpacklen < 0) + if (subpacklen < 0) { subpacklen = 0; + } offset += subpacklen; } @@ -571,18 +541,21 @@ void parse_ins_msg( void ) { } -void parse_ins_buffer( uint8_t c ) { +void parse_ins_buffer(uint8_t c) +{ ck += c; switch (xsens_status) { case UNINIT: - if (c != XSENS_START) + if (c != XSENS_START) { goto error; + } xsens_status++; ck = 0; break; case GOT_START: - if (c != XSENS_BID) + if (c != XSENS_BID) { goto error; + } xsens_status++; break; case GOT_BID: @@ -591,20 +564,23 @@ void parse_ins_buffer( uint8_t c ) { break; case GOT_MID: xsens_len = c; - if (xsens_len > XSENS_MAX_PAYLOAD) + if (xsens_len > XSENS_MAX_PAYLOAD) { goto error; + } xsens_msg_idx = 0; xsens_status++; break; case GOT_LEN: xsens_msg_buf[xsens_msg_idx] = c; xsens_msg_idx++; - if (xsens_msg_idx >= xsens_len) + if (xsens_msg_idx >= xsens_len) { xsens_status++; + } break; case GOT_DATA: - if (ck != 0) + if (ck != 0) { goto error; + } ins_msg_received = TRUE; goto restart; break; @@ -612,8 +588,8 @@ void parse_ins_buffer( uint8_t c ) { break; } return; - error: - restart: +error: +restart: xsens_status = UNINIT; return; } diff --git a/sw/airborne/modules/led_safety_status/led_safety_status.c b/sw/airborne/modules/led_safety_status/led_safety_status.c index 15361fd1a0..c034d1e7f8 100644 --- a/sw/airborne/modules/led_safety_status/led_safety_status.c +++ b/sw/airborne/modules/led_safety_status/led_safety_status.c @@ -33,41 +33,37 @@ #error You must define SAFETY_WARNING_LED to use this module! #else -void led_safety_status_init(void) { +void led_safety_status_init(void) +{ LED_ON(SAFETY_WARNING_LED); led_safety_status_periodic(); } -void led_safety_status_periodic(void) { - if (radio_control.status == RC_LOST || radio_control.status == RC_REALLY_LOST){ +void led_safety_status_periodic(void) +{ + if (radio_control.status == RC_LOST || radio_control.status == RC_REALLY_LOST) { RunXTimesEvery(0, 60, 5, 7, {LED_TOGGLE(SAFETY_WARNING_LED);}); RunXTimesEvery(130, 130, 10, 6, {LED_TOGGLE(SAFETY_WARNING_LED);}); - } - else if (!(autopilot_mode == MODE_MANUAL) && !autopilot_motors_on){ + } else if (!(autopilot_mode == MODE_MANUAL) && !autopilot_motors_on) { RunXTimesEvery(20, 240, 40, 1, {LED_ON(SAFETY_WARNING_LED);}); RunXTimesEvery(0, 240, 40, 1, {LED_OFF(SAFETY_WARNING_LED);}); - } - else if (!THROTTLE_STICK_DOWN() && !autopilot_motors_on){ + } else if (!THROTTLE_STICK_DOWN() && !autopilot_motors_on) { RunXTimesEvery(20, 240, 40, 2, {LED_ON(SAFETY_WARNING_LED);}); RunXTimesEvery(0, 240, 40, 2, {LED_OFF(SAFETY_WARNING_LED);}); - } - else if (!ROLL_STICK_CENTERED() && !autopilot_motors_on){ + } else if (!ROLL_STICK_CENTERED() && !autopilot_motors_on) { RunXTimesEvery(20, 240, 40, 3, {LED_ON(SAFETY_WARNING_LED);}); RunXTimesEvery(0, 240, 40, 3, {LED_OFF(SAFETY_WARNING_LED);}); - } - else if (!PITCH_STICK_CENTERED() && !autopilot_motors_on){ + } else if (!PITCH_STICK_CENTERED() && !autopilot_motors_on) { RunXTimesEvery(20, 240, 40, 4, {LED_ON(SAFETY_WARNING_LED);}); RunXTimesEvery(0, 240, 40, 4, {LED_OFF(SAFETY_WARNING_LED);}); - } - else if (!YAW_STICK_CENTERED() && !autopilot_motors_on){ + } else if (!YAW_STICK_CENTERED() && !autopilot_motors_on) { RunXTimesEvery(20, 240, 40, 5, {LED_ON(SAFETY_WARNING_LED);}); RunXTimesEvery(0, 240, 40, 5, {LED_OFF(SAFETY_WARNING_LED);}); } #ifdef MIN_BAT_LEVEL - else if (electrical.vsupply < (MIN_BAT_LEVEL * 10)){ + else if (electrical.vsupply < (MIN_BAT_LEVEL * 10)) { RunOnceEvery(20, {LED_TOGGLE(SAFETY_WARNING_LED);}); - } - else if (electrical.vsupply < ((MIN_BAT_LEVEL + 0.5) * 10)){ + } else if (electrical.vsupply < ((MIN_BAT_LEVEL + 0.5) * 10)) { RunXTimesEvery(0, 300, 10, 10, {LED_TOGGLE(SAFETY_WARNING_LED);}); } #endif diff --git a/sw/airborne/modules/light/light.c b/sw/airborne/modules/light/light.c index 9f4136f76f..59aab1f4d7 100644 --- a/sw/airborne/modules/light/light.c +++ b/sw/airborne/modules/light/light.c @@ -40,7 +40,8 @@ uint8_t nav_light_mode; #endif -void init_light(void) { +void init_light(void) +{ // this part is already done by led_init in fact LED_INIT(LIGHT_LED_STROBE); LED_OFF(LIGHT_LED_STROBE); @@ -59,91 +60,66 @@ void periodic_light(void) static uint8_t counter_nav = 0; #endif - switch (strobe_light_mode) - { - default: // Always off + switch (strobe_light_mode) { + default: // Always off LED_OFF(LIGHT_LED_STROBE); break; - case 1: // Always on + case 1: // Always on LED_ON(LIGHT_LED_STROBE); break; - case 2: // Blink + case 2: // Blink case 3: case 4: - if (counter == (strobe_light_mode*5 - 4)) - { + if (counter == (strobe_light_mode * 5 - 4)) { LED_OFF(LIGHT_LED_STROBE); - } - else if (counter >= 20) - { + } else if (counter >= 20) { LED_ON(LIGHT_LED_STROBE); counter = 0; } break; - case 5: // Complex Blinking - if (counter == 3) - { + case 5: // Complex Blinking + if (counter == 3) { LED_OFF(LIGHT_LED_STROBE); - } - else if (counter == 4) - { + } else if (counter == 4) { LED_ON(LIGHT_LED_STROBE); - } - else if (counter == 6) - { + } else if (counter == 6) { LED_OFF(LIGHT_LED_STROBE); - } - else if (counter == 7) - { + } else if (counter == 7) { LED_ON(LIGHT_LED_STROBE); - } - else if (counter == 8) - { + } else if (counter == 8) { LED_OFF(LIGHT_LED_STROBE); - } - else if (counter >= 25) - { + } else if (counter >= 25) { LED_ON(LIGHT_LED_STROBE); counter = 0; } break; case 6: - if (counter <= 18) - { - if ((counter % 2) == 0) - { + if (counter <= 18) { + if ((counter % 2) == 0) { LED_ON(LIGHT_LED_STROBE); - } - else - { + } else { LED_OFF(LIGHT_LED_STROBE); } - } - else if (counter == 35) - { + } else if (counter == 35) { counter = 0; } break; } #ifdef LIGHT_LED_NAV - switch (nav_light_mode) - { - default: // Always off + switch (nav_light_mode) { + default: // Always off LED_OFF(LIGHT_LED_NAV); break; - case 1: // Always on + case 1: // Always on LED_ON(LIGHT_LED_NAV); break; - case 2: // Blink + case 2: // Blink case 3: case 4: - if (counter_nav == (nav_light_mode*5 - 4)) - { + if (counter_nav == (nav_light_mode * 5 - 4)) { LED_OFF(LIGHT_LED_NAV); - } - else if (counter_nav >= 20) - { + } else if (counter_nav >= 20) { LED_ON(LIGHT_LED_NAV); counter_nav = 0; } diff --git a/sw/airborne/modules/loggers/direct_memory_logger.c b/sw/airborne/modules/loggers/direct_memory_logger.c index e2104caab1..3d0dbcc6b8 100644 --- a/sw/airborne/modules/loggers/direct_memory_logger.c +++ b/sw/airborne/modules/loggers/direct_memory_logger.c @@ -31,8 +31,8 @@ #include "stabilization.h" struct DirectMemoryLogger dml; -static void direct_memory_spi_cb(struct spi_transaction* trans); -static int32_t seq_in_array(uint8_t* array, uint16_t array_size, uint8_t* sequence, uint16_t sequence_size); +static void direct_memory_spi_cb(struct spi_transaction *trans); +static int32_t seq_in_array(uint8_t *array, uint16_t array_size, uint8_t *sequence, uint16_t sequence_size); // Different sequences static uint8_t start_log_sequence[6] = {0xAA, 0x55, 0xFF, 0x00, 0x55, 0xAA}; @@ -55,7 +55,7 @@ struct LogStruct { static struct LogStruct log_struct; static uint32_t dm_counter = 0; -static int32_t seq_in_array(uint8_t* array, uint16_t array_size, uint8_t* sequence, uint16_t sequence_size) +static int32_t seq_in_array(uint8_t *array, uint16_t array_size, uint8_t *sequence, uint16_t sequence_size) { uint16_t i; static uint16_t current_sequence_id = 0; @@ -141,7 +141,7 @@ void direct_memory_logger_periodic(void) log_struct.gyro_r = imu.gyro.r; log_struct.thrust = stabilization_cmd[COMMAND_THRUST]; - sst25vfxxxx_write(&dml.sst, (uint8_t*) &log_struct, sizeof(struct LogStruct)); + sst25vfxxxx_write(&dml.sst, (uint8_t *) &log_struct, sizeof(struct LogStruct)); break; // Reading @@ -215,7 +215,7 @@ void direct_memory_logger_set(uint8_t val) } } -static void direct_memory_spi_cb(__attribute__((unused)) struct spi_transaction* trans) +static void direct_memory_spi_cb(__attribute__((unused)) struct spi_transaction *trans) { sst25vfxxxx_after_cb(&dml.sst); } diff --git a/sw/airborne/modules/loggers/file_logger.c b/sw/airborne/modules/loggers/file_logger.c index b779954353..94fcc36ebe 100644 --- a/sw/airborne/modules/loggers/file_logger.c +++ b/sw/airborne/modules/loggers/file_logger.c @@ -37,10 +37,11 @@ #endif /** The file pointer */ -static FILE* file_logger; +static FILE *file_logger; /** Start the file logger and open a new file */ -void file_logger_start(void) { +void file_logger_start(void) +{ uint32_t counter = 0; char filename[512]; @@ -64,7 +65,8 @@ void file_logger_start(void) { } /** Stop the logger an nicely close the file */ -void file_logger_stop(void) { +void file_logger_stop(void) +{ fclose(file_logger); file_logger = NULL; } @@ -76,27 +78,27 @@ void file_logger_periodic(void) return; } static uint32_t counter; - struct Int32Quat* quat = stateGetNedToBodyQuat_i(); + struct Int32Quat *quat = stateGetNedToBodyQuat_i(); fprintf(file_logger, "%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\n", - counter, - imu.gyro_unscaled.p, - imu.gyro_unscaled.q, - imu.gyro_unscaled.r, - imu.accel_unscaled.x, - imu.accel_unscaled.y, - imu.accel_unscaled.z, - imu.mag_unscaled.x, - imu.mag_unscaled.y, - imu.mag_unscaled.z, - stabilization_cmd[COMMAND_THRUST], - stabilization_cmd[COMMAND_ROLL], - stabilization_cmd[COMMAND_PITCH], - stabilization_cmd[COMMAND_YAW], - quat->qi, - quat->qx, - quat->qy, - quat->qz - ); + counter, + imu.gyro_unscaled.p, + imu.gyro_unscaled.q, + imu.gyro_unscaled.r, + imu.accel_unscaled.x, + imu.accel_unscaled.y, + imu.accel_unscaled.z, + imu.mag_unscaled.x, + imu.mag_unscaled.y, + imu.mag_unscaled.z, + stabilization_cmd[COMMAND_THRUST], + stabilization_cmd[COMMAND_ROLL], + stabilization_cmd[COMMAND_PITCH], + stabilization_cmd[COMMAND_YAW], + quat->qi, + quat->qx, + quat->qy, + quat->qz + ); counter++; } diff --git a/sw/airborne/modules/loggers/high_speed_logger_direct_memory.c b/sw/airborne/modules/loggers/high_speed_logger_direct_memory.c index d630866a97..10d05186ce 100644 --- a/sw/airborne/modules/loggers/high_speed_logger_direct_memory.c +++ b/sw/airborne/modules/loggers/high_speed_logger_direct_memory.c @@ -390,7 +390,8 @@ void memory_read_values(uint32_t mem_addr, uint8_t size) memory_transaction.output_length = 5; memory_transaction.input_buf = (uint8_t *) uart_read_buff; - memory_transaction.input_length = size + MEMORY_READ_LATTENCY; //the first MEMORY_READ_LATTENCY Bytes are lost because of reading to soon + memory_transaction.input_length = size + + MEMORY_READ_LATTENCY; //the first MEMORY_READ_LATTENCY Bytes are lost because of reading to soon memory_transaction.after_cb = memory_read_values_cb; diff --git a/sw/airborne/modules/loggers/high_speed_logger_spi_link.c b/sw/airborne/modules/loggers/high_speed_logger_spi_link.c index 81c2e734f9..fd8993d0d8 100644 --- a/sw/airborne/modules/loggers/high_speed_logger_spi_link.c +++ b/sw/airborne/modules/loggers/high_speed_logger_spi_link.c @@ -30,9 +30,10 @@ struct spi_transaction high_speed_logger_spi_link_transaction; static volatile bool_t high_speed_logger_spi_link_ready = TRUE; -static void high_speed_logger_spi_link_trans_cb( struct spi_transaction *trans ); +static void high_speed_logger_spi_link_trans_cb(struct spi_transaction *trans); -void high_speed_logger_spi_link_init(void) { +void high_speed_logger_spi_link_init(void) +{ high_speed_logger_spi_link_data.id = 0; high_speed_logger_spi_link_transaction.select = SPISelectUnselect; @@ -43,7 +44,7 @@ void high_speed_logger_spi_link_init(void) { high_speed_logger_spi_link_transaction.cdiv = SPIDiv64; high_speed_logger_spi_link_transaction.slave_idx = HIGH_SPEED_LOGGER_SPI_LINK_SLAVE_NUMBER; high_speed_logger_spi_link_transaction.output_length = sizeof(high_speed_logger_spi_link_data); - high_speed_logger_spi_link_transaction.output_buf = (uint8_t*) &high_speed_logger_spi_link_data; + high_speed_logger_spi_link_transaction.output_buf = (uint8_t *) &high_speed_logger_spi_link_data; high_speed_logger_spi_link_transaction.input_length = 0; high_speed_logger_spi_link_transaction.input_buf = NULL; high_speed_logger_spi_link_transaction.after_cb = high_speed_logger_spi_link_trans_cb; @@ -52,8 +53,7 @@ void high_speed_logger_spi_link_init(void) { void high_speed_logger_spi_link_periodic(void) { - if (high_speed_logger_spi_link_ready) - { + if (high_speed_logger_spi_link_ready) { high_speed_logger_spi_link_ready = FALSE; high_speed_logger_spi_link_data.gyro_p = imu.gyro_unscaled.p; high_speed_logger_spi_link_data.gyro_q = imu.gyro_unscaled.q; @@ -71,7 +71,8 @@ void high_speed_logger_spi_link_periodic(void) high_speed_logger_spi_link_data.id++; } -static void high_speed_logger_spi_link_trans_cb( struct spi_transaction *trans __attribute__ ((unused)) ) { +static void high_speed_logger_spi_link_trans_cb(struct spi_transaction *trans __attribute__((unused))) +{ high_speed_logger_spi_link_ready = TRUE; } diff --git a/sw/airborne/modules/loggers/high_speed_logger_spi_link.h b/sw/airborne/modules/loggers/high_speed_logger_spi_link.h index f4677552a2..1bf0475786 100644 --- a/sw/airborne/modules/loggers/high_speed_logger_spi_link.h +++ b/sw/airborne/modules/loggers/high_speed_logger_spi_link.h @@ -31,22 +31,22 @@ extern void high_speed_logger_spi_link_periodic(void); #define PACKED __attribute__((__packed__)) struct PACKED high_speed_logger_spi_link_data { - int32_t id; // 1 - int32_t gyro_p; // 2 - int32_t gyro_q; - int32_t gyro_r; - int32_t acc_x; // 5 - int32_t acc_y; - int32_t acc_z; - int32_t mag_x; // 8 - int32_t mag_y; - int32_t mag_z; - int32_t phi; // 11 - int32_t theta; - int32_t psi; - int32_t extra1; // 14 - int32_t extra2; // 15 - int32_t extra3; // 16 + int32_t id; // 1 + int32_t gyro_p; // 2 + int32_t gyro_q; + int32_t gyro_r; + int32_t acc_x; // 5 + int32_t acc_y; + int32_t acc_z; + int32_t mag_x; // 8 + int32_t mag_y; + int32_t mag_z; + int32_t phi; // 11 + int32_t theta; + int32_t psi; + int32_t extra1; // 14 + int32_t extra2; // 15 + int32_t extra3; // 16 }; #endif /* HIGH_SPEED_LOGGER_SPI_LINK_H_ */ diff --git a/sw/airborne/modules/max3100/max3100_hw.c b/sw/airborne/modules/max3100/max3100_hw.c index 2cf0179bdd..3b66a40efe 100644 --- a/sw/airborne/modules/max3100/max3100_hw.c +++ b/sw/airborne/modules/max3100/max3100_hw.c @@ -75,7 +75,8 @@ static void SPI1_ISR(void) __attribute__((naked)); #endif -void max3100_init( void ) { +void max3100_init(void) +{ max3100_status = MAX3100_STATUS_IDLE; max3100_data_available = false; max3100_transmit_buffer_empty = true; @@ -110,8 +111,8 @@ void max3100_init( void ) { SetBit(EXTINT, MAX3100_IRQ_EINT); /* Configure interrupt vector for external pin interrupt */ - VICIntSelect &= ~VIC_BIT( MAX3100_VIC_EINT ); // EXTINT selected as IRQ - VICIntEnable = VIC_BIT( MAX3100_VIC_EINT ); // EXTINT interrupt enabled + VICIntSelect &= ~VIC_BIT(MAX3100_VIC_EINT); // EXTINT selected as IRQ + VICIntEnable = VIC_BIT(MAX3100_VIC_EINT); // EXTINT interrupt enabled VICVectCntl8 = VIC_ENABLE | MAX3100_VIC_EINT; VICVectAddr8 = (uint32_t)EXTINT_ISR; // address of the ISR @@ -128,7 +129,8 @@ void max3100_init( void ) { /******* External interrupt: Data input available ***********/ -void EXTINT_ISR(void) { +void EXTINT_ISR(void) +{ ISR_ENTRY(); max3100_data_available = true; @@ -139,7 +141,8 @@ void EXTINT_ISR(void) { ISR_EXIT(); } -void SPI1_ISR(void) { +void SPI1_ISR(void) +{ ISR_ENTRY(); while (bit_is_set(SSPSR, RNE)) { @@ -156,7 +159,7 @@ void SPI1_ISR(void) { } SpiClearRti(); /* clear interrupt */ SpiDisableRti(); - SpiDisable (); + SpiDisable(); Max3100Unselect(); max3100_status = MAX3100_STATUS_IDLE; @@ -164,6 +167,7 @@ void SPI1_ISR(void) { ISR_EXIT(); } -void max3100_debug(void) { +void max3100_debug(void) +{ /*** DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice, 16, max3100_rx_buf); ***/ } diff --git a/sw/airborne/modules/max3100/max3100_hw.h b/sw/airborne/modules/max3100/max3100_hw.h index 2126f0b964..bd73b15ecd 100644 --- a/sw/airborne/modules/max3100/max3100_hw.h +++ b/sw/airborne/modules/max3100/max3100_hw.h @@ -56,12 +56,12 @@ extern volatile uint8_t max3100_rx_buf[MAX3100_RX_BUF_LEN]; extern volatile uint8_t read_byte1, read_byte2; extern bool read_bytes; -#define Max3100Select() { \ - SetBit(MAX3100_SS_IOCLR, MAX3100_SS_PIN); \ +#define Max3100Select() { \ + SetBit(MAX3100_SS_IOCLR, MAX3100_SS_PIN); \ } #define Max3100Unselect() { \ - SetBit(MAX3100_SS_IOSET, MAX3100_SS_PIN); \ + SetBit(MAX3100_SS_IOSET, MAX3100_SS_PIN); \ } #define MAX3100_WRITE_CONF ((1U<<15) | (1U<<14)) @@ -94,14 +94,15 @@ extern bool read_bytes; #define UART3100Transmit(_x) { max3100_putchar(_x); } #define UART3100SendMessage() {} #define UART3100Getch() ({\ - uint8_t ret = max3100_rx_buf[max3100_rx_extract_idx]; \ - max3100_rx_extract_idx++; /* Since size=256 */ \ - ret; \ -}) + uint8_t ret = max3100_rx_buf[max3100_rx_extract_idx]; \ + max3100_rx_extract_idx++; /* Since size=256 */ \ + ret; \ + }) #define UART3100ChAvailable() (max3100_rx_extract_idx != max3100_rx_insert_idx) -static inline void max3100_transmit(uint16_t data) { +static inline void max3100_transmit(uint16_t data) +{ Max3100Select(); SpiClearRti(); SpiEnableRti(); /* enable rx fifo time out */ @@ -116,12 +117,14 @@ static inline void max3100_transmit(uint16_t data) { #define Max3100ReadData() max3100_transmit(MAX3100_READ_DATA) -static inline void max3100_read_data(void) { +static inline void max3100_read_data(void) +{ Max3100ReadData(); max3100_status = MAX3100_STATUS_READING; } -static inline void max3100_flush( void ) { +static inline void max3100_flush(void) +{ if (max3100_status == MAX3100_STATUS_IDLE && max3100_tx_extract_idx != max3100_tx_insert_idx && max3100_transmit_buffer_empty) { @@ -133,21 +136,24 @@ static inline void max3100_flush( void ) { } /** Warning: No bufferring; SPI must be available */ -static inline void max3100_putconfchar(char c) { +static inline void max3100_putconfchar(char c) +{ Max3100TransmitConf(c); max3100_status = MAX3100_STATUS_WRITING; } -static inline void max3100_putchar(char c) { +static inline void max3100_putchar(char c) +{ max3100_tx_buf[max3100_tx_insert_idx] = c; max3100_tx_insert_idx++; /* automatic overflow since len=256 */ /* flushed in the next event */ } -extern void max3100_init( void ); -extern void max3100_debug( void ); +extern void max3100_init(void); +extern void max3100_debug(void); -static inline void max3100_event( void ) { +static inline void max3100_event(void) +{ if (read_bytes) { read_bytes = false; max3100_debug(); @@ -156,8 +162,9 @@ static inline void max3100_event( void ) { if (max3100_data_available) { max3100_data_available = false; max3100_read_data(); - } else + } else { max3100_flush(); + } } } diff --git a/sw/airborne/modules/max7456/max7456.c b/sw/airborne/modules/max7456/max7456.c index 346102c06d..13919ca2cf 100644 --- a/sw/airborne/modules/max7456/max7456.c +++ b/sw/airborne/modules/max7456/max7456.c @@ -51,9 +51,9 @@ #define OSD_STRING_SIZE 31 #define osd_sprintf _osd_sprintf -static char ascii_to_osd_c( char c); +static char ascii_to_osd_c(char c); static void osd_put_s(char *string, uint8_t attributes, uint8_t char_nb, uint8_t row, uint8_t column); -static bool_t _osd_sprintf(char* buffer, char* string, float value); +static bool_t _osd_sprintf(char *buffer, char *string, float value); struct spi_transaction max7456_trans; @@ -80,7 +80,7 @@ enum max7456_osd_status_codes { OSD_FINISHED, }; -enum osd_attributes{ +enum osd_attributes { BLINK = OSD_BLINK_CHAR, INVERT = OSD_INVERT_PIXELS, }; @@ -91,7 +91,8 @@ uint8_t osd_enable_val = OSD_IMAGE_ENABLE; uint8_t osd_stat_reg = 0; bool_t osd_stat_reg_valid = FALSE; -void max7456_init(void) { +void max7456_init(void) +{ max7456_trans.slave_idx = MAX7456_SLAVE_IDX; max7456_trans.select = SPISelectUnselect; @@ -101,9 +102,9 @@ void max7456_init(void) { max7456_trans.bitorder = SPIMSBFirst; max7456_trans.cdiv = SPIDiv64; max7456_trans.output_length = sizeof(osd_spi_tx_buffer); - max7456_trans.output_buf = (uint8_t*) osd_spi_tx_buffer; + max7456_trans.output_buf = (uint8_t *) osd_spi_tx_buffer; max7456_trans.input_length = 0; - max7456_trans.input_buf = (uint8_t*)osd_spi_rx_buffer; + max7456_trans.input_buf = (uint8_t *)osd_spi_rx_buffer; max7456_trans.before_cb = NULL; max7456_trans.after_cb = NULL; @@ -114,16 +115,18 @@ void max7456_init(void) { return; } -void max7456_periodic(void) { +void max7456_periodic(void) +{ float temp = 0; //This code is executed always and checks if the "osd_enable" var has been changed by telemetry. //If yes then it commands a reset but this time turns on or off the osd overlay, not the video. if (max7456_osd_status == OSD_IDLE) { - if(osd_enable > 1) + if (osd_enable > 1) { osd_enable = 1; - if ((osd_enable<<3) != osd_enable_val) { - osd_enable_val = (osd_enable<<3); + } + if ((osd_enable << 3) != osd_enable_val) { + osd_enable_val = (osd_enable << 3); max7456_osd_status = OSD_UNINIT; } } @@ -138,108 +141,108 @@ void max7456_periodic(void) { max7456_trans.output_buf[1] = OSD_RESET; max7456_osd_status = OSD_INIT1; spi_submit(&(MAX7456_SPI_DEV), &max7456_trans); - } - else - if (max7456_osd_status == OSD_INIT2) { + } else if (max7456_osd_status == OSD_INIT2) { max7456_trans.output_length = 1; max7456_trans.input_length = 1; max7456_trans.output_buf[0] = OSD_OSDBL_REG_R; max7456_osd_status = OSD_INIT3; spi_submit(&(MAX7456_SPI_DEV), &max7456_trans); - } - else - if (max7456_osd_status == OSD_IDLE && osd_enable > 0) { // DRAW THE OSD SCREEN - //draw_osd(); + } else if (max7456_osd_status == OSD_IDLE && osd_enable > 0) { // DRAW THE OSD SCREEN + //draw_osd(); switch (step) { case (0): - osd_put_s("HDG", FALSE, 3, 0, 13); - step = 10; - break; + osd_put_s("HDG", FALSE, 3, 0, 13); + step = 10; + break; case (10): - temp = ((float)electrical.vsupply)/10; - osd_sprintf(osd_string, "%.1fV", temp ); - if (temp > LOW_BAT_LEVEL) + temp = ((float)electrical.vsupply) / 10; + osd_sprintf(osd_string, "%.1fV", temp); + if (temp > LOW_BAT_LEVEL) { osd_put_s(osd_string, FALSE, 8, 0, 2); - else - osd_put_s(osd_string, BLINK|INVERT, 8, 0, 2); + } else { + osd_put_s(osd_string, BLINK | INVERT, 8, 0, 2); + } step = 20; - break; + break; case (20): - #if MAG_HEADING_AVAILABLE && !defined(SITL) +#if MAG_HEADING_AVAILABLE && !defined(SITL) temp = DegOfRad(MAG_Heading); - if (temp < 0) + if (temp < 0) { temp += 360; - #else + } +#else temp = DegOfRad(state.h_speed_dir_f); - if (temp < 0) + if (temp < 0) { temp += 360; - #endif + } +#endif osd_sprintf(osd_string, "%.0f", temp); osd_put_s(osd_string, FALSE, 8, 1, 13); step = 30; - break; + break; case (30): - osd_sprintf(osd_string, "%.0fKm", (state.h_speed_norm_f*3.6)); + osd_sprintf(osd_string, "%.0fKm", (state.h_speed_norm_f * 3.6)); osd_put_s(osd_string, FALSE, 8, 0, 24); step = 40; - break; + break; case (40): - osd_sprintf(osd_string, "%.0fm", GetPosAlt() ); + osd_sprintf(osd_string, "%.0fm", GetPosAlt()); osd_put_s(osd_string, FALSE, 10, 13, 2); step = 50; - break; + break; case (50): osd_sprintf(osd_string, "%.1fVZ", stateGetSpeedEnu_f()->z); osd_put_s(osd_string, FALSE, 7, 13, 24); step = 10; - break; + break; default: break; } } return; } -void max7456_event(void) { +void max7456_event(void) +{ static uint8_t x = 0; if (max7456_trans.status == SPITransSuccess) { - max7456_trans.status = SPITransDone; + max7456_trans.status = SPITransDone; switch (max7456_osd_status) { case (OSD_INIT1): max7456_osd_status = OSD_INIT2; - break; + break; case (OSD_INIT3): max7456_trans.output_length = 2; max7456_trans.input_length = 0; max7456_trans.output_buf[0] = OSD_OSDBL_REG; - max7456_trans.output_buf[1] = max7456_trans.input_buf[0] & (~(1<<4)); + max7456_trans.output_buf[1] = max7456_trans.input_buf[0] & (~(1 << 4)); max7456_osd_status = OSD_INIT4; spi_submit(&(MAX7456_SPI_DEV), &max7456_trans); - break; + break; case (OSD_INIT4): max7456_trans.output_buf[0] = OSD_VM0_REG; #if USE_PAL_FOR_OSD_VIDEO - max7456_trans.output_buf[1] = OSD_VIDEO_MODE_PAL|osd_enable_val; + max7456_trans.output_buf[1] = OSD_VIDEO_MODE_PAL | osd_enable_val; #else max7456_trans.output_buf[1] = osd_enable_val; #endif max7456_osd_status = OSD_FINISHED; spi_submit(&(MAX7456_SPI_DEV), &max7456_trans); - break; + break; case (OSD_READ_STATUS): osd_stat_reg = max7456_trans.input_buf[0]; osd_stat_reg_valid = TRUE; max7456_osd_status = OSD_FINISHED; - break; + break; case (OSD_S_STEP1): max7456_trans.output_length = 2; max7456_trans.output_buf[0] = OSD_DMAL_REG; max7456_trans.output_buf[1] = (uint8_t)(osd_char_address); max7456_osd_status = OSD_S_STEP2; spi_submit(&(MAX7456_SPI_DEV), &max7456_trans); - break; + break; case (OSD_S_STEP2): max7456_trans.output_length = 2; max7456_trans.output_buf[0] = OSD_DMM_REG; @@ -247,95 +250,99 @@ void max7456_event(void) { max7456_osd_status = OSD_S_STEP3; spi_submit(&(MAX7456_SPI_DEV), &max7456_trans); x = 0; - break; + break; case (OSD_S_STEP3): max7456_trans.output_length = 1; //1 byte tranfers, auto address incrementing. if (osd_string[x] != 0XFF) { max7456_trans.output_buf[0] = osd_string[x++]; spi_submit(&(MAX7456_SPI_DEV), &max7456_trans); - } - else { + } else { max7456_trans.output_buf[0] = 0xFF; //Exit the auto increment mode max7456_osd_status = OSD_FINISHED; spi_submit(&(MAX7456_SPI_DEV), &max7456_trans); } - break; + break; case (OSD_FINISHED): osd_attr = 0; max7456_trans.status = SPITransDone; max7456_osd_status = OSD_IDLE; - break; + break; default: break; - } + } } return; } -static char ascii_to_osd_c(char c) { +static char ascii_to_osd_c(char c) +{ if (c >= '0' && c <= '9') { - if (c == '0') + if (c == '0') { c -= 38; - else + } else { c -= 48; - } - else { - if (c >= 'A' && c <= 'Z') + } + } else { + if (c >= 'A' && c <= 'Z') { c -= 54; - else { - if (c >= 'a' && c <= 'z') - c -= 60; - else { + } else { + if (c >= 'a' && c <= 'z') { + c -= 60; + } else { switch (c) { - case('('): c = 0x3f; break; - case(')'): c = 0x40; break; - case('.'): c = 0x41; break; - case('?'): c = 0x42; break; - case(';'): c = 0x43; break; - case(':'): c = 0x44; break; - case(','): c = 0x45; break; - //case('''): c = 0x46; break; - case('/'): c = 0x47; break; - case('"'): c = 0x48; break; - case('-'): c = 0x49; break; - case('<'): c = 0x4A; break; - case('>'): c = 0x4B; break; - case('@'): c = 0x4C; break; - case(' '): c = 0x00; break; - case('\0'): c = 0xFF; break; + case ('('): c = 0x3f; break; + case (')'): c = 0x40; break; + case ('.'): c = 0x41; break; + case ('?'): c = 0x42; break; + case (';'): c = 0x43; break; + case (':'): c = 0x44; break; + case (','): c = 0x45; break; + //case('''): c = 0x46; break; + case ('/'): c = 0x47; break; + case ('"'): c = 0x48; break; + case ('-'): c = 0x49; break; + case ('<'): c = 0x4A; break; + case ('>'): c = 0x4B; break; + case ('@'): c = 0x4C; break; + case (' '): c = 0x00; break; + case ('\0'): c = 0xFF; break; default : break; } } } } - return(c); + return (c); } -static void osd_put_s(char *string, uint8_t attributes, uint8_t char_nb, uint8_t row, uint8_t column){ +static void osd_put_s(char *string, uint8_t attributes, uint8_t char_nb, uint8_t row, uint8_t column) +{ - uint8_t x=0; + uint8_t x = 0; - if (row > 15) + if (row > 15) { column = 15; - if (column > 29) + } + if (column > 29) { column = 29; - osd_char_address = ((uint16_t)row*30) + column; + } + osd_char_address = ((uint16_t)row * 30) + column; // translate the string and put it to the "osd_string" '\0' = 0xff x = 0; - while (*(string+x) != '\0') { - osd_string[x] = ascii_to_osd_c(*(string+x)); + while (*(string + x) != '\0') { + osd_string[x] = ascii_to_osd_c(*(string + x)); x++; } - osd_string[x] = ascii_to_osd_c(*(string+x)); + osd_string[x] = ascii_to_osd_c(*(string + x)); - for (x=0; x < sizeof(osd_string); x++) { - if(osd_string[x] == 0xff) + for (x = 0; x < sizeof(osd_string); x++) { + if (osd_string[x] == 0xff) { break; + } } //Adjust for the reserved character number. - for (; x< char_nb; x++) { + for (; x < char_nb; x++) { osd_string[x] = 0; } osd_string[x] = 0xff; @@ -343,10 +350,10 @@ static void osd_put_s(char *string, uint8_t attributes, uint8_t char_nb, uint8_t osd_attr = attributes; //TRIGGER THE SPI TRANSFERS. The rest of the spi transfers occur in the "max7456_event" function. - if (max7456_osd_status == OSD_IDLE){ + if (max7456_osd_status == OSD_IDLE) { max7456_trans.output_length = 2; max7456_trans.output_buf[0] = OSD_DMAH_REG; - max7456_trans.output_buf[1] = (uint8_t)((osd_char_address>>8) & 0x0001); + max7456_trans.output_buf[1] = (uint8_t)((osd_char_address >> 8) & 0x0001); max7456_osd_status = OSD_S_STEP1; spi_submit(&(MAX7456_SPI_DEV), &max7456_trans); } @@ -354,7 +361,8 @@ static void osd_put_s(char *string, uint8_t attributes, uint8_t char_nb, uint8_t } // A VERY VERY STRIPED DOWN sprintf function suitable only for the paparazzi OSD. -static bool_t _osd_sprintf(char* buffer, char* string, float value) { +static bool_t _osd_sprintf(char *buffer, char *string, float value) +{ uint8_t param_start = 0; uint8_t param_end = 0; @@ -365,31 +373,29 @@ static bool_t _osd_sprintf(char* buffer, char* string, float value) { uint16_t i_dec = 0; uint16_t i_frac = 0; - char to_asc[10] = {48,48,48,48,48,48,48,48,48,48}; + char to_asc[10] = {48, 48, 48, 48, 48, 48, 48, 48, 48, 48}; // Clear the osd string. - for (x=0; x < sizeof(osd_string); x++) { + for (x = 0; x < sizeof(osd_string); x++) { osd_string[x] = 0; } x = 0; // Search for the prameter start and stop positions. - while (*(string+x) != '\0'){ - if (*(string+x) == '%'){ - param_start = x; + while (*(string + x) != '\0') { + if (*(string + x) == '%') { + param_start = x; + } else if (*(string + x) == 'f') { + param_end = x; + break; } - else - if (*(string+x) == 'f') { - param_end = x; - break; - } x++; } // find and bound the precision specified. - frac_nb = *(string+param_end-1) - 48; // Convert to number, ASCII 48 = '0' - if(frac_nb > 3) { + frac_nb = *(string + param_end - 1) - 48; // Convert to number, ASCII 48 = '0' + if (frac_nb > 3) { frac_nb = 3; // Bound value. } - y = (sizeof(to_asc)- 1); // Point y to the end of the array. + y = (sizeof(to_asc) - 1); // Point y to the end of the array. i_dec = abs((int16_t)value); // Fist we will deal with the fractional part if specified. if (frac_nb > 0 && frac_nb <= 3) { @@ -399,10 +405,10 @@ static bool_t _osd_sprintf(char* buffer, char* string, float value) { do { // Example if frac_nb=2 then 952 will show as .95 z--; digit = (i_frac / x); - to_asc[y+z] = digit + 48; // Convert to ASCII + to_asc[y + z] = digit + 48; // Convert to ASCII i_frac -= digit * x; // Calculate the remainder. x /= 10; // 952-(9*100) = 52, 52-(10*5)=2 etc. - } while(z > 0); + } while (z > 0); y -= frac_nb; // set y to point where the dot must be placed. to_asc[y] = '.'; y--; // Set y to point where the rest of the numbers must be written. @@ -413,18 +419,18 @@ static bool_t _osd_sprintf(char* buffer, char* string, float value) { to_asc[y] = (i_dec % 10) + 48; //Write at least one digit even if value is zero. i_dec /= 10; if (i_dec <= 0) { // This way the leading zero is ommited. - if(value < 0) { + if (value < 0) { y--; to_asc[y] = '-'; // Place the minus sign if needed. } break; - } - else + } else { y--; - } while(1); + } + } while (1); // Fill the buffer with the characters in the beggining of the string if any. - for (x=0; x= CHARGE_NB) { DOWNLINK_SEND_ATMOSPHERE_CHARGE(DefaultChannel, DefaultDevice, - &charge[0], &charge[1], &charge[2], &charge[3], &charge[4], - &charge[5], &charge[6], &charge[7], &charge[8], &charge[9]); + &charge[0], &charge[1], &charge[2], &charge[3], &charge[4], + &charge[5], &charge[6], &charge[7], &charge[8], &charge[9]); charge_cnt = 0; } } diff --git a/sw/airborne/modules/meteo/dust_gp2y.c b/sw/airborne/modules/meteo/dust_gp2y.c index 1fa0b3f6e6..ea6a191c16 100644 --- a/sw/airborne/modules/meteo/dust_gp2y.c +++ b/sw/airborne/modules/meteo/dust_gp2y.c @@ -48,20 +48,22 @@ struct i2c_transaction gp2y_trans; #define GP2Y_SLAVE_ADDR 0xED -void dust_gp2y_init( void ) { +void dust_gp2y_init(void) +{ dust_gp2y_status = DUST_GP2Y_UNINIT; } -void dust_gp2y_periodic( void ) { +void dust_gp2y_periodic(void) +{ if (dust_gp2y_status == DUST_GP2Y_IDLE) { i2c_receive(&GP2Y_I2C_DEV, &gp2y_trans, GP2Y_SLAVE_ADDR, 2); - } - else if (dust_gp2y_status == DUST_GP2Y_UNINIT && sys_time.nb_sec > 1) { + } else if (dust_gp2y_status == DUST_GP2Y_UNINIT && sys_time.nb_sec > 1) { dust_gp2y_status = DUST_GP2Y_IDLE; } } -void dust_gp2y_event( void ) { +void dust_gp2y_event(void) +{ if (gp2y_trans.status == I2CTransSuccess) { /* read two byte particle density */ dust_gp2y_density = gp2y_trans.buf[0] << 8; @@ -69,8 +71,9 @@ void dust_gp2y_event( void ) { /* "just for reference and not for guarantee" */ dust_gp2y_density_f = ((dust_gp2y_density / 1024.) * 3.3 * (51. / 33.) - 0.6) * (0.5 / 3.); - if (dust_gp2y_density_f < 0) + if (dust_gp2y_density_f < 0) { dust_gp2y_density_f = 0; + } DOWNLINK_SEND_GP2Y_STATUS(DefaultChannel, DefaultDevice, &dust_gp2y_density, &dust_gp2y_density_f); diff --git a/sw/airborne/modules/meteo/geiger_counter.c b/sw/airborne/modules/meteo/geiger_counter.c index 6e9a065902..2563a0fa56 100644 --- a/sw/airborne/modules/meteo/geiger_counter.c +++ b/sw/airborne/modules/meteo/geiger_counter.c @@ -42,14 +42,17 @@ struct i2c_transaction geiger_trans; uint32_t count_geiger_1, count_geiger_2; uint16_t volt_geiger; -void geiger_counter_init( void ) { +void geiger_counter_init(void) +{ } -void geiger_counter_periodic( void ) { +void geiger_counter_periodic(void) +{ i2c_receive(&GEIGER_CNT_DEV, &geiger_trans, GEIGER_CNT_I2C_ADDR, 10); } -void geiger_counter_event( void ) { +void geiger_counter_event(void) +{ if (geiger_trans.status == I2CTransSuccess) { count_geiger_1 = (geiger_trans.buf[3] << 24) | (geiger_trans.buf[2] << 16) | @@ -66,7 +69,7 @@ void geiger_counter_event( void ) { if (volt_geiger & 0x8000) { volt_geiger &= 0x7FFF; DOWNLINK_SEND_GEIGER_COUNTER(DefaultChannel, DefaultDevice, - &count_geiger_1, &count_geiger_2, &volt_geiger); + &count_geiger_1, &count_geiger_2, &volt_geiger); } } } diff --git a/sw/airborne/modules/meteo/humid_dpicco.c b/sw/airborne/modules/meteo/humid_dpicco.c index 1048946375..2109358198 100644 --- a/sw/airborne/modules/meteo/humid_dpicco.c +++ b/sw/airborne/modules/meteo/humid_dpicco.c @@ -51,22 +51,25 @@ float dpicco_temp; struct i2c_transaction dpicco_trans; -void dpicco_init( void ) { +void dpicco_init(void) +{ dpicco_trans.status = I2CTransDone; } -void dpicco_periodic( void ) { +void dpicco_periodic(void) +{ /* init read */ i2c_receive(&DPICCO_I2C_DEV, &dpicco_trans, DPICCO_SLAVE_ADDR, 4); } -void dpicco_event( void ) { +void dpicco_event(void) +{ if (dpicco_trans.status == I2CTransSuccess) { //LED_TOGGLE(2); - dpicco_val[0] = (dpicco_trans.buf[0]<<8) | dpicco_trans.buf[1]; - dpicco_val[1] = (dpicco_trans.buf[2]<<8) | dpicco_trans.buf[3]; + dpicco_val[0] = (dpicco_trans.buf[0] << 8) | dpicco_trans.buf[1]; + dpicco_val[1] = (dpicco_trans.buf[2] << 8) | dpicco_trans.buf[3]; dpicco_humid = (dpicco_val[0] * DPICCO_HUMID_RANGE) / DPICCO_HUMID_MAX; dpicco_temp = ((dpicco_val[1] * DPICCO_TEMP_RANGE) / DPICCO_TEMP_MAX) + DPICCO_TEMP_OFFS; diff --git a/sw/airborne/modules/meteo/humid_dpicco.h b/sw/airborne/modules/meteo/humid_dpicco.h index 3d1d6d426a..32f2b6ad4e 100644 --- a/sw/airborne/modules/meteo/humid_dpicco.h +++ b/sw/airborne/modules/meteo/humid_dpicco.h @@ -41,9 +41,9 @@ extern float dpicco_temp; -extern void dpicco_init( void ); -extern void dpicco_periodic( void ); -extern void dpicco_event( void ); +extern void dpicco_init(void); +extern void dpicco_periodic(void); +extern void dpicco_event(void); #endif /* DPICCO_H */ diff --git a/sw/airborne/modules/meteo/humid_hih.c b/sw/airborne/modules/meteo/humid_hih.c index fe6bb9cceb..eb10ae662d 100644 --- a/sw/airborne/modules/meteo/humid_hih.c +++ b/sw/airborne/modules/meteo/humid_hih.c @@ -50,11 +50,13 @@ float fhih_humid; static struct adc_buf buf_humid_hih; -void humid_hih_init( void ) { +void humid_hih_init(void) +{ adc_buf_channel(ADC_CHANNEL_HUMID_HIH, &buf_humid_hih, ADC_CHANNEL_HUMID_NB_SAMPLES); } -void humid_hih_periodic( void ) { +void humid_hih_periodic(void) +{ float fvout, fhih_temp; /* get temperature from external source */ @@ -64,13 +66,13 @@ void humid_hih_periodic( void ) { adc_humid_hih = buf_humid_hih.sum / buf_humid_hih.av_nb_sample; /* 36k/68k voltage divider, 3.3V full sweep, 10 bits adc */ - fvout = (adc_humid_hih / 1024.) * 3.3 * (104./68.); + fvout = (adc_humid_hih / 1024.) * 3.3 * (104. / 68.); /* 5V supply, 1st order curve fit */ - fhih_humid = ((fvout / 5.0)-0.16)/0.0062; + fhih_humid = ((fvout / 5.0) - 0.16) / 0.0062; /* temperature compensation */ - fhih_humid = fhih_humid/(1.0546 - (0.00216 * fhih_temp)); + fhih_humid = fhih_humid / (1.0546 - (0.00216 * fhih_temp)); DOWNLINK_SEND_HIH_STATUS(DefaultChannel, DefaultDevice, &adc_humid_hih, &fhih_humid, &fhih_temp); } diff --git a/sw/airborne/modules/meteo/humid_hih.h b/sw/airborne/modules/meteo/humid_hih.h index f103ac0beb..2f11e925fb 100644 --- a/sw/airborne/modules/meteo/humid_hih.h +++ b/sw/airborne/modules/meteo/humid_hih.h @@ -4,7 +4,7 @@ #include extern uint16_t adc_humid_hih; -void humid_hih_init( void ); -void humid_hih_periodic( void ); +void humid_hih_init(void); +void humid_hih_periodic(void); #endif diff --git a/sw/airborne/modules/meteo/humid_htm_b71.c b/sw/airborne/modules/meteo/humid_htm_b71.c index 0ccd52ad92..68dfcd2a77 100644 --- a/sw/airborne/modules/meteo/humid_htm_b71.c +++ b/sw/airborne/modules/meteo/humid_htm_b71.c @@ -49,11 +49,13 @@ uint16_t humidhtm, temphtm; float fhumidhtm, ftemphtm; -void humid_htm_init(void) { +void humid_htm_init(void) +{ htm_status = HTM_IDLE; } -void humid_htm_start( void ) { +void humid_htm_start(void) +{ if (sys_time.nb_sec > 1) { /* measurement request: wake up sensor, sample temperature/humidity */ i2c_transmit(&HTM_I2C_DEV, &htm_trans, HTM_SLAVE_ADDR, 0); @@ -62,7 +64,8 @@ void humid_htm_start( void ) { } /* needs 18.5ms delay from measurement request */ -void humid_htm_read( void ) { +void humid_htm_read(void) +{ if (htm_status == HTM_MR_OK) { /* read humid and temp*/ htm_status = HTM_READ_DATA; @@ -70,32 +73,33 @@ void humid_htm_read( void ) { } } -void humid_htm_event( void ) { +void humid_htm_event(void) +{ if (htm_trans.status == I2CTransSuccess) { switch (htm_status) { - case HTM_MR: - htm_status = HTM_MR_OK; - htm_trans.status = I2CTransDone; - break; + case HTM_MR: + htm_status = HTM_MR_OK; + htm_trans.status = I2CTransDone; + break; - case HTM_READ_DATA: - /* check stale status */ - if (((htm_trans.buf[0] >> 6) & 0x3) == 0) { - /* humidity */ - humidhtm = ((htm_trans.buf[0] & 0x3F) << 8) | htm_trans.buf[1]; - fhumidhtm = humidhtm / 163.83; - /* temperature */ - temphtm = (htm_trans.buf[2] << 6) | (htm_trans.buf[3] >> 2); - ftemphtm = -40.00 + 0.01 * temphtm; - DOWNLINK_SEND_HTM_STATUS(DefaultChannel, DefaultDevice, &humidhtm, &temphtm, &fhumidhtm, &ftemphtm); - } - htm_trans.status = I2CTransDone; - break; + case HTM_READ_DATA: + /* check stale status */ + if (((htm_trans.buf[0] >> 6) & 0x3) == 0) { + /* humidity */ + humidhtm = ((htm_trans.buf[0] & 0x3F) << 8) | htm_trans.buf[1]; + fhumidhtm = humidhtm / 163.83; + /* temperature */ + temphtm = (htm_trans.buf[2] << 6) | (htm_trans.buf[3] >> 2); + ftemphtm = -40.00 + 0.01 * temphtm; + DOWNLINK_SEND_HTM_STATUS(DefaultChannel, DefaultDevice, &humidhtm, &temphtm, &fhumidhtm, &ftemphtm); + } + htm_trans.status = I2CTransDone; + break; - default: - htm_trans.status = I2CTransDone; - break; + default: + htm_trans.status = I2CTransDone; + break; } } } diff --git a/sw/airborne/modules/meteo/humid_pcap01.c b/sw/airborne/modules/meteo/humid_pcap01.c index 31815dd2c6..22373602d6 100644 --- a/sw/airborne/modules/meteo/humid_pcap01.c +++ b/sw/airborne/modules/meteo/humid_pcap01.c @@ -52,7 +52,7 @@ void writePCAP01_SRAM(uint8_t data, uint16_t s_add) { while (pcap01_trans.status == I2CTransPending); - pcap01_trans.buf[0] = 0x90+(unsigned char)(s_add>>8); + 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); @@ -62,7 +62,7 @@ uint8_t readPCAP01_SRAM(uint16_t s_add) { while (pcap01_trans.status == I2CTransPending); - pcap01_trans.buf[0] = 0x10+(unsigned char)(s_add>>8); + 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); @@ -75,14 +75,14 @@ uint8_t readPCAP01_SRAM(uint16_t s_add) * Function to send control commands to the PCAP01 * * \param control Control command -* possible commands: -* PCAP01_PU_RESET : Hard reset of the device -* PCAP01_IN_RESET : Software reset -* PCAP01_START : Start measurement -* PCAP01_START : Start measurement -* PCAP01_TERM : Stop measurement +* possible commands: +* PCAP01_PU_RESET : Hard reset of the device +* PCAP01_IN_RESET : Software reset +* PCAP01_START : Start measurement +* PCAP01_START : Start measurement +* PCAP01_TERM : Stop measurement */ - void PCAP01_Control(uint8_t control) +void PCAP01_Control(uint8_t control) { while (pcap01_trans.status == I2CTransPending); @@ -93,16 +93,16 @@ uint8_t readPCAP01_SRAM(uint16_t s_add) i2c_transmit(&PCAP01_I2C_DEV, &pcap01_trans, PCAP01_ADDR, 4); } - void pcap01writeRegister(uint8_t reg,uint32_t value) - { +void pcap01writeRegister(uint8_t reg, uint32_t value) +{ 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); + 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) @@ -117,41 +117,39 @@ void writePCAP01_firmware(void) writePCAP01_SRAM(testbyte, testaddress); //check testbyte - if (readPCAP01_SRAM(testaddress) != testbyte) + if (readPCAP01_SRAM(testaddress) != testbyte) { return; - else - { -LED_ON(3); + } 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); + for (i = 0; i < sizeof(firmware); i++) { + writePCAP01_SRAM(firmware[i], i); } //fill with ffs - for (;i< 4029; i++) - { - writePCAP01_SRAM(0xff,i); + 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++); + 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++); + writePCAP01_SRAM(0x02, i++); + writePCAP01_SRAM(0x01, i++); + writePCAP01_SRAM(0x03, i++); #endif } } #endif // PCAP01_LOAD_FIRMWARE -void pcap01_init(void) { +void pcap01_init(void) +{ pcap01_trans.status = I2CTransDone; pcap01Value.status = PCAP01_IDLE; #ifdef PCAP01_LOAD_FIRMWARE @@ -184,11 +182,11 @@ void pcap01_init(void) { } 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); - } +} /** * \brief pcap01_readData @@ -198,12 +196,12 @@ 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 } @@ -215,37 +213,37 @@ void pcap01_event(void) 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 dcb80f1fa3..361d2cde77 100644 --- a/sw/airborne/modules/meteo/humid_pcap01.h +++ b/sw/airborne/modules/meteo/humid_pcap01.h @@ -45,13 +45,13 @@ typedef struct { uint32_t NV_temp; uint32_t V_rh; uint32_t status; -}PCAP01VALUE; +} PCAP01VALUE; #define PCAP01_ADDR 0xA0 -#define PCAP01_IDLE 0 -#define PCAP01_GET_HUMID 1 -#define PCAP01_GET_TEMP 2 +#define PCAP01_IDLE 0 +#define PCAP01_GET_HUMID 1 +#define PCAP01_GET_TEMP 2 //OpCodes für PCap Programmierung #define PCAP01_PU_RESET 0x88 @@ -141,7 +141,7 @@ void writePCAP01_SRAM(uint8_t data, uint16_t s_add); uint8_t readPCAP01_SRAM(uint16_t s_add); void PCAP01_Control(uint8_t control); void pcap01readRegister(uint8_t reg); -void pcap01writeRegister(uint8_t reg,uint32_t value); +void pcap01writeRegister(uint8_t reg, uint32_t value); void writePCAP01_firmware(void); void pcap01_init(void); void pcap01_periodic(void); diff --git a/sw/airborne/modules/meteo/humid_sht.c b/sw/airborne/modules/meteo/humid_sht.c index 40c3e8e502..ba77043e91 100644 --- a/sw/airborne/modules/meteo/humid_sht.c +++ b/sw/airborne/modules/meteo/humid_sht.c @@ -90,26 +90,25 @@ uint8_t s_write_statusreg(uint8_t *p_value); uint8_t s_measure(uint16_t *p_value, uint8_t *p_checksum, uint8_t mode); uint8_t s_start_measure(uint8_t mode); uint8_t s_read_measure(uint16_t *p_value, uint8_t *p_checksum); -void calc_sht(uint16_t hum, uint16_t tem, float *fhum ,float *ftem); -uint8_t humid_sht_reset( void ); +void calc_sht(uint16_t hum, uint16_t tem, float *fhum , float *ftem); +uint8_t humid_sht_reset(void); uint8_t s_write_byte(uint8_t value) { - uint8_t i, error=0; + uint8_t i, error = 0; - for (i=0x80;i>0;i/=2) //shift bit for masking - { - if (i & value) DATA_SET; //masking value with i , write to SENSI-BUS - else DATA_CLR; + for (i = 0x80; i > 0; i /= 2) { //shift bit for masking + if (i & value) { DATA_SET; } //masking value with i , write to SENSI-BUS + else { DATA_CLR; } SCK_SET; //clk for SENSI-BUS - SCK_SET;SCK_SET;SCK_SET; //pulswith approx. 5 us + SCK_SET; SCK_SET; SCK_SET; //pulswith approx. 5 us // _nop_();_nop_();_nop_(); //pulswith approx. 5 us SCK_CLR; } DATA_SET; //release DATA-line SCK_SET; //clk #9 for ack - error=DATA_IN; //check ack (DATA will be pulled down by SHT11) + error = DATA_IN; //check ack (DATA will be pulled down by SHT11) SCK_CLR; return error; //error=1 in case of no acknowledge @@ -117,19 +116,18 @@ uint8_t s_write_byte(uint8_t value) uint8_t s_read_byte(uint8_t ack) { - uint8_t i,val=0; + uint8_t i, val = 0; DATA_SET; //release DATA-line - for (i=0x80;i>0;i/=2) //shift bit for masking - { + for (i = 0x80; i > 0; i /= 2) { //shift bit for masking SCK_SET; //clk for SENSI-BUS - if (DATA_IN) val=(val | i); //read bit + if (DATA_IN) { val = (val | i); } //read bit SCK_CLR; } - if (ack) DATA_CLR; //in case of "ack==1" pull down DATA-Line + if (ack) { DATA_CLR; } //in case of "ack==1" pull down DATA-Line SCK_SET; //clk #9 for ack - SCK_SET;SCK_SET;SCK_SET; //pulswith approx. 5 us + SCK_SET; SCK_SET; SCK_SET; //pulswith approx. 5 us // _nop_();_nop_();_nop_(); //pulswith approx. 5 us SCK_CLR; DATA_SET; //release DATA-line @@ -151,7 +149,7 @@ void s_transstart(void) DATA_CLR; DATA_CLR;// _nop_(); SCK_CLR; - SCK_CLR;SCK_CLR;SCK_CLR; // _nop_();_nop_();_nop_(); + SCK_CLR; SCK_CLR; SCK_CLR; // _nop_();_nop_();_nop_(); SCK_SET; SCK_SET;// _nop_(); DATA_SET; @@ -170,8 +168,7 @@ void s_connectionreset(void) uint8_t i; DATA_SET; SCK_CLR; //Initial state - for(i=0;i<9;i++) //9 SCK cycles - { + for (i = 0; i < 9; i++) { //9 SCK cycles SCK_SET; SCK_CLR; } @@ -181,40 +178,40 @@ void s_connectionreset(void) uint8_t s_read_statusreg(uint8_t *p_value, uint8_t *p_checksum) { // reads the status register with checksum (8-bit) - uint8_t error=0; + uint8_t error = 0; s_transstart(); //transmission start - error=s_write_byte(STATUS_REG_R); //send command to sensor - *p_value=s_read_byte(ACK); //read status register (8-bit) - *p_checksum=s_read_byte(noACK); //read checksum (8-bit) + error = s_write_byte(STATUS_REG_R); //send command to sensor + *p_value = s_read_byte(ACK); //read status register (8-bit) + *p_checksum = s_read_byte(noACK); //read checksum (8-bit) return error; //error=1 in case of no response form the sensor } uint8_t s_write_statusreg(uint8_t *p_value) { // writes the status register with checksum (8-bit) - uint8_t error=0; + uint8_t error = 0; s_transstart(); //transmission start - error+=s_write_byte(STATUS_REG_W);//send command to sensor - error+=s_write_byte(*p_value); //send value of status register + error += s_write_byte(STATUS_REG_W); //send command to sensor + error += s_write_byte(*p_value); //send value of status register return error; //error>=1 in case of no response form the sensor } uint8_t s_measure(uint16_t *p_value, uint8_t *p_checksum, uint8_t mode) { // makes a measurement (humidity/temperature) with checksum - uint8_t error=0; + uint8_t error = 0; uint32_t i; s_transstart(); //transmission start - switch(mode){ //send command to sensor - case TEMP : error+=s_write_byte(MEASURE_TEMP); break; - case HUMI : error+=s_write_byte(MEASURE_HUMI); break; + switch (mode) { //send command to sensor + case TEMP : error += s_write_byte(MEASURE_TEMP); break; + case HUMI : error += s_write_byte(MEASURE_HUMI); break; default : break; } - for (i=0;i<6665535;i++) if(DATA_IN==0) break; //wait until sensor has finished the measurement - if(DATA_IN) error+=1; // or timeout (~2 sec.) is reached + for (i = 0; i < 6665535; i++) if (DATA_IN == 0) { break; } //wait until sensor has finished the measurement + if (DATA_IN) { error += 1; } // or timeout (~2 sec.) is reached *(p_value) = s_read_byte(ACK) << 8; //read the first byte (MSB) *(p_value) |= s_read_byte(ACK); //read the second byte (LSB) *p_checksum = s_read_byte(noACK); //read checksum @@ -225,12 +222,12 @@ uint8_t s_measure(uint16_t *p_value, uint8_t *p_checksum, uint8_t mode) uint8_t s_start_measure(uint8_t mode) { // makes a measurement (humidity/temperature) with checksum - uint8_t error=0; + uint8_t error = 0; s_transstart(); //transmission start - switch(mode){ //send command to sensor - case TEMP : error+=s_write_byte(MEASURE_TEMP); break; - case HUMI : error+=s_write_byte(MEASURE_HUMI); break; + switch (mode) { //send command to sensor + case TEMP : error += s_write_byte(MEASURE_TEMP); break; + case HUMI : error += s_write_byte(MEASURE_HUMI); break; default : break; } @@ -240,9 +237,9 @@ uint8_t s_start_measure(uint8_t mode) uint8_t s_read_measure(uint16_t *p_value, uint8_t *p_checksum) { // reads a measurement (humidity/temperature) with checksum - uint8_t error=0; + uint8_t error = 0; - if(DATA_IN) error+=1; //still busy? + if (DATA_IN) { error += 1; } //still busy? *(p_value) = s_read_byte(ACK) << 8; //read the first byte (MSB) *(p_value) |= s_read_byte(ACK); //read the second byte (LSB) *p_checksum = s_read_byte(noACK); //read checksum @@ -250,7 +247,7 @@ uint8_t s_read_measure(uint16_t *p_value, uint8_t *p_checksum) return error; } -void calc_sht(uint16_t hum, uint16_t tem, float *fhum ,float *ftem) +void calc_sht(uint16_t hum, uint16_t tem, float *fhum , float *ftem) { // calculates temperature [ C] and humidity [%RH] // input : humi [Ticks] (12 bit) @@ -258,9 +255,9 @@ void calc_sht(uint16_t hum, uint16_t tem, float *fhum ,float *ftem) // output: humi [%RH] // temp [ C] - const float C1 =-4.0; // for 12 Bit + const float C1 = -4.0; // for 12 Bit const float C2 = 0.0405; // for 12 Bit - const float C3 =-0.0000028; // for 12 Bit + const float C3 = -0.0000028; // for 12 Bit const float T1 = 0.01; // for 14 Bit @ 5V const float T2 = 0.00008; // for 14 Bit @ 5V float rh; // rh: Humidity [Ticks] 12 Bit @@ -272,27 +269,27 @@ void calc_sht(uint16_t hum, uint16_t tem, float *fhum ,float *ftem) rh = (float)hum; //converts integer to float t = (float)tem; //converts integer to float - t_C=t*0.01 - 39.66; //calc. Temperature from ticks to [°C] @ 3.5V - rh_lin=C3*rh*rh + C2*rh + C1; //calc. Humidity from ticks to [%RH] - rh_true=(t_C-25)*(T1+T2*rh)+rh_lin; //calc. Temperature compensated humidity [%RH] - if(rh_true>100)rh_true=100; //cut if the value is outside of - if(rh_true<0.1)rh_true=0.1; //the physical possible range + t_C = t * 0.01 - 39.66; //calc. Temperature from ticks to [°C] @ 3.5V + rh_lin = C3 * rh * rh + C2 * rh + C1; //calc. Humidity from ticks to [%RH] + rh_true = (t_C - 25) * (T1 + T2 * rh) + rh_lin; //calc. Temperature compensated humidity [%RH] + if (rh_true > 100) { rh_true = 100; } //cut if the value is outside of + if (rh_true < 0.1) { rh_true = 0.1; } //the physical possible range *ftem = t_C; //return temperature [ C] *fhum = rh_true; //return humidity[%RH] } -uint8_t humid_sht_reset( void ) +uint8_t humid_sht_reset(void) { // resets the sensor by a softreset - uint8_t error=0; + uint8_t error = 0; s_connectionreset(); //reset communication - error+=s_write_byte(RESET); //send RESET-command to sensor + error += s_write_byte(RESET); //send RESET-command to sensor return error; //error=1 in case of no response form the sensor } -void humid_sht_init( void ) +void humid_sht_init(void) { /* Configure DAT/SCL pin as GPIO */ gpio_setup_input(SHT_DAT_GPIO); @@ -304,16 +301,16 @@ void humid_sht_init( void ) humid_sht_status = SHT_IDLE; } -void humid_sht_periodic(void) { - uint8_t error=0, checksum; +void humid_sht_periodic(void) +{ + uint8_t error = 0, checksum; if (humid_sht_status == SHT_IDLE) { /* init humidity read */ s_connectionreset(); s_start_measure(HUMI); humid_sht_status = SHT_MEASURING_HUMID; - } - else if (humid_sht_status == SHT_MEASURING_HUMID) { + } else if (humid_sht_status == SHT_MEASURING_HUMID) { /* get data */ error += s_read_measure(&humidsht, &checksum); @@ -321,13 +318,11 @@ void humid_sht_periodic(void) { s_connectionreset(); s_start_measure(HUMI); //restart //LED_TOGGLE(2); - } - else { + } else { error += s_start_measure(TEMP); humid_sht_status = SHT_MEASURING_TEMP; } - } - else if (humid_sht_status == SHT_MEASURING_TEMP) { + } else if (humid_sht_status == SHT_MEASURING_TEMP) { /* get data */ error += s_read_measure(&tempsht, &checksum); @@ -335,8 +330,7 @@ void humid_sht_periodic(void) { s_connectionreset(); s_start_measure(TEMP); //restart //LED_TOGGLE(2); - } - else { + } else { calc_sht(humidsht, tempsht, &fhumidsht, &ftempsht); humid_sht_available = TRUE; s_connectionreset(); diff --git a/sw/airborne/modules/meteo/humid_sht.h b/sw/airborne/modules/meteo/humid_sht.h index b8c82a8dc8..68436a6646 100644 --- a/sw/airborne/modules/meteo/humid_sht.h +++ b/sw/airborne/modules/meteo/humid_sht.h @@ -37,7 +37,7 @@ extern float fhumidsht, ftempsht; extern bool_t humid_sht_available; extern uint8_t humid_sht_status; -void humid_sht_init( void ); +void humid_sht_init(void); void humid_sht_periodic(void); #endif /* HUMID_SHT_H */ diff --git a/sw/airborne/modules/meteo/humid_sht_i2c.c b/sw/airborne/modules/meteo/humid_sht_i2c.c index 53118afa18..b56074210a 100644 --- a/sw/airborne/modules/meteo/humid_sht_i2c.c +++ b/sw/airborne/modules/meteo/humid_sht_i2c.c @@ -43,67 +43,73 @@ struct i2c_transaction sht_trans; uint8_t sht_status; uint8_t sht_serial[8] = {0}; -uint32_t sht_serial1=0, sht_serial2=0; +uint32_t sht_serial1 = 0, sht_serial2 = 0; uint16_t humidsht_i2c, tempsht_i2c; float fhumidsht_i2c, ftempsht_i2c; -int8_t humid_sht_crc(volatile uint8_t* data) { +int8_t humid_sht_crc(volatile uint8_t *data) +{ uint8_t i, bit, crc = 0; for (i = 0; i < 2; i++) { crc ^= (data[i]); for (bit = 8; bit > 0; bit--) { - if (crc & 0x80) + if (crc & 0x80) { crc = (crc << 1) ^ 0x131; - else + } else { crc = (crc << 1); + } } } - if (crc != data[2]) + if (crc != data[2]) { return -1; - else + } else { return 0; + } } -void humid_sht_init_i2c(void) { +void humid_sht_init_i2c(void) +{ sht_status = SHT2_UNINIT; } -void humid_sht_periodic_i2c( void ) { +void humid_sht_periodic_i2c(void) +{ switch (sht_status) { - case SHT2_UNINIT: - /* do soft reset, then wait at least 15ms */ - sht_status = SHT2_RESET; - sht_trans.buf[0] = SHT2_SOFT_RESET; - i2c_transmit(&SHT_I2C_DEV, &sht_trans, SHT_SLAVE_ADDR, 1); - break; + case SHT2_UNINIT: + /* do soft reset, then wait at least 15ms */ + sht_status = SHT2_RESET; + sht_trans.buf[0] = SHT2_SOFT_RESET; + i2c_transmit(&SHT_I2C_DEV, &sht_trans, SHT_SLAVE_ADDR, 1); + break; - case SHT2_SERIAL: - /* get serial number part 1 */ - sht_status = SHT2_SERIAL1; - sht_trans.buf[0] = 0xFA; - sht_trans.buf[1] = 0x0F; - i2c_transceive(&SHT_I2C_DEV, &sht_trans, SHT_SLAVE_ADDR, 2, 8); - break; + case SHT2_SERIAL: + /* get serial number part 1 */ + sht_status = SHT2_SERIAL1; + sht_trans.buf[0] = 0xFA; + sht_trans.buf[1] = 0x0F; + i2c_transceive(&SHT_I2C_DEV, &sht_trans, SHT_SLAVE_ADDR, 2, 8); + break; - case SHT2_SERIAL1: - case SHT2_SERIAL2: - break; + case SHT2_SERIAL1: + case SHT2_SERIAL2: + break; - default: - /* trigger temp measurement, no master hold */ - sht_trans.buf[0] = SHT2_TRIGGER_TEMP; - sht_status = SHT2_TRIG_TEMP; - i2c_transmit(&SHT_I2C_DEV, &sht_trans, SHT_SLAVE_ADDR, 1); - /* send serial number every 30 seconds */ - RunOnceEvery((4*30), DOWNLINK_SEND_SHT_I2C_SERIAL(DefaultChannel, DefaultDevice, &sht_serial1, &sht_serial2)); - break; + default: + /* trigger temp measurement, no master hold */ + sht_trans.buf[0] = SHT2_TRIGGER_TEMP; + sht_status = SHT2_TRIG_TEMP; + i2c_transmit(&SHT_I2C_DEV, &sht_trans, SHT_SLAVE_ADDR, 1); + /* send serial number every 30 seconds */ + RunOnceEvery((4 * 30), DOWNLINK_SEND_SHT_I2C_SERIAL(DefaultChannel, DefaultDevice, &sht_serial1, &sht_serial2)); + break; } } /* needs 85ms delay from temp trigger measurement */ -void humid_sht_p_temp( void ) { +void humid_sht_p_temp(void) +{ if (sht_status == SHT2_GET_TEMP) { /* get temp */ sht_status = SHT2_READ_TEMP; @@ -112,7 +118,8 @@ void humid_sht_p_temp( void ) { } /* needs 29ms delay from humid trigger measurement */ -void humid_sht_p_humid( void ) { +void humid_sht_p_humid(void) +{ if (sht_status == SHT2_GET_HUMID) { /* read humid */ sht_status = SHT2_READ_HUMID; @@ -120,86 +127,86 @@ void humid_sht_p_humid( void ) { } } -void humid_sht_event_i2c( void ) { +void humid_sht_event_i2c(void) +{ if (sht_trans.status == I2CTransSuccess) { switch (sht_status) { - case SHT2_TRIG_TEMP: - sht_status = SHT2_GET_TEMP; - sht_trans.status = I2CTransDone; - break; + case SHT2_TRIG_TEMP: + sht_status = SHT2_GET_TEMP; + sht_trans.status = I2CTransDone; + break; + + case SHT2_READ_TEMP: + /* read temperature */ + tempsht_i2c = (sht_trans.buf[0] << 8) | sht_trans.buf[1]; + tempsht_i2c &= 0xFFFC; + if (humid_sht_crc(sht_trans.buf) == 0) { + /* trigger humid measurement, no master hold */ + sht_trans.buf[0] = SHT2_TRIGGER_HUMID; + sht_status = SHT2_TRIG_HUMID; + i2c_transmit(&SHT_I2C_DEV, &sht_trans, SHT_SLAVE_ADDR, 1); + } else { + /* checksum error, restart */ + sht_status = SHT2_IDLE; + sht_trans.status = I2CTransDone; + } + break; + + case SHT2_TRIG_HUMID: + sht_status = SHT2_GET_HUMID; + sht_trans.status = I2CTransDone; + break; + + case SHT2_READ_HUMID: + /* read humidity */ + humidsht_i2c = (sht_trans.buf[0] << 8) | sht_trans.buf[1]; + humidsht_i2c &= 0xFFFC; + fhumidsht_i2c = -6. + 125. / 65536. * humidsht_i2c; + ftempsht_i2c = -46.85 + 175.72 / 65536. * tempsht_i2c; - case SHT2_READ_TEMP: - /* read temperature */ - tempsht_i2c = (sht_trans.buf[0] << 8) | sht_trans.buf[1]; - tempsht_i2c &= 0xFFFC; - if (humid_sht_crc(sht_trans.buf) == 0) { - /* trigger humid measurement, no master hold */ - sht_trans.buf[0] = SHT2_TRIGGER_HUMID; - sht_status = SHT2_TRIG_HUMID; - i2c_transmit(&SHT_I2C_DEV, &sht_trans, SHT_SLAVE_ADDR, 1); - } - else { - /* checksum error, restart */ sht_status = SHT2_IDLE; sht_trans.status = I2CTransDone; - } - break; - case SHT2_TRIG_HUMID: - sht_status = SHT2_GET_HUMID; - sht_trans.status = I2CTransDone; - break; + if (humid_sht_crc(sht_trans.buf) == 0) { + DOWNLINK_SEND_SHT_I2C_STATUS(DefaultChannel, DefaultDevice, &humidsht_i2c, &tempsht_i2c, &fhumidsht_i2c, &ftempsht_i2c); + } + break; - case SHT2_READ_HUMID: - /* read humidity */ - humidsht_i2c = (sht_trans.buf[0] << 8) | sht_trans.buf[1]; - humidsht_i2c &= 0xFFFC; - fhumidsht_i2c = -6. + 125. / 65536. * humidsht_i2c; - ftempsht_i2c = -46.85 + 175.72 / 65536. * tempsht_i2c; + case SHT2_RESET: + sht_status = SHT2_SERIAL; + sht_trans.status = I2CTransDone; + break; - sht_status = SHT2_IDLE; - sht_trans.status = I2CTransDone; + case SHT2_SERIAL1: + /* read serial number part 1 */ + sht_serial[5] = sht_trans.buf[0]; + sht_serial[4] = sht_trans.buf[2]; + sht_serial[3] = sht_trans.buf[4]; + sht_serial[2] = sht_trans.buf[6]; + /* get serial number part 2 */ + sht_status = SHT2_SERIAL2; + sht_trans.buf[0] = 0xFC; + sht_trans.buf[1] = 0xC9; + i2c_transceive(&SHT_I2C_DEV, &sht_trans, SHT_SLAVE_ADDR, 2, 6); + break; - if (humid_sht_crc(sht_trans.buf) == 0) { - DOWNLINK_SEND_SHT_I2C_STATUS(DefaultChannel, DefaultDevice, &humidsht_i2c, &tempsht_i2c, &fhumidsht_i2c, &ftempsht_i2c); - } - break; + case SHT2_SERIAL2: + /* read serial number part 2 */ + sht_serial[1] = sht_trans.buf[0]; + sht_serial[0] = sht_trans.buf[1]; + sht_serial[7] = sht_trans.buf[3]; + sht_serial[6] = sht_trans.buf[4]; + sht_serial1 = sht_serial[7] << 24 | sht_serial[6] << 16 | sht_serial[5] << 8 | sht_serial[4]; + sht_serial2 = sht_serial[3] << 24 | sht_serial[2] << 16 | sht_serial[1] << 8 | sht_serial[0]; + DOWNLINK_SEND_SHT_I2C_SERIAL(DefaultChannel, DefaultDevice, &sht_serial1, &sht_serial2); + sht_status = SHT2_IDLE; + sht_trans.status = I2CTransDone; + break; - case SHT2_RESET: - sht_status = SHT2_SERIAL; - sht_trans.status = I2CTransDone; - break; - - case SHT2_SERIAL1: - /* read serial number part 1 */ - sht_serial[5] = sht_trans.buf[0]; - sht_serial[4] = sht_trans.buf[2]; - sht_serial[3] = sht_trans.buf[4]; - sht_serial[2] = sht_trans.buf[6]; - /* get serial number part 2 */ - sht_status = SHT2_SERIAL2; - sht_trans.buf[0] = 0xFC; - sht_trans.buf[1] = 0xC9; - i2c_transceive(&SHT_I2C_DEV, &sht_trans, SHT_SLAVE_ADDR, 2, 6); - break; - - case SHT2_SERIAL2: - /* read serial number part 2 */ - sht_serial[1] = sht_trans.buf[0]; - sht_serial[0] = sht_trans.buf[1]; - sht_serial[7] = sht_trans.buf[3]; - sht_serial[6] = sht_trans.buf[4]; - sht_serial1=sht_serial[7]<<24|sht_serial[6]<<16|sht_serial[5]<<8|sht_serial[4]; - sht_serial2=sht_serial[3]<<24|sht_serial[2]<<16|sht_serial[1]<<8|sht_serial[0]; - DOWNLINK_SEND_SHT_I2C_SERIAL(DefaultChannel, DefaultDevice, &sht_serial1, &sht_serial2); - sht_status = SHT2_IDLE; - sht_trans.status = I2CTransDone; - break; - - default: - sht_trans.status = I2CTransDone; - break; + default: + sht_trans.status = I2CTransDone; + break; } } } diff --git a/sw/airborne/modules/meteo/humid_sht_i2c.h b/sw/airborne/modules/meteo/humid_sht_i2c.h index 1c26fe5f8d..0669bb2948 100644 --- a/sw/airborne/modules/meteo/humid_sht_i2c.h +++ b/sw/airborne/modules/meteo/humid_sht_i2c.h @@ -9,7 +9,7 @@ #define SHT2_TRIGGER_HUMID 0xF5 #define SHT2_SOFT_RESET 0xFE -enum sht_stat_i2c{ +enum sht_stat_i2c { SHT2_UNINIT, SHT2_IDLE, SHT2_RESET, @@ -26,7 +26,7 @@ enum sht_stat_i2c{ SHT2_READ_HUMID }; -int8_t humid_sht_crc(volatile uint8_t* data); +int8_t humid_sht_crc(volatile uint8_t *data); void humid_sht_init_i2c(void); void humid_sht_periodic_i2c(void); void humid_sht_p_temp(void); diff --git a/sw/airborne/modules/meteo/ir_mlx.c b/sw/airborne/modules/meteo/ir_mlx.c index 1e9a26c621..fecfad91a0 100644 --- a/sw/airborne/modules/meteo/ir_mlx.c +++ b/sw/airborne/modules/meteo/ir_mlx.c @@ -60,24 +60,28 @@ uint32_t ir_mlx_id_23; // printf("Ta = %2.2f°C (0x%04X)\n", (tp*0.02)-273.15, tp); -void ir_mlx_crc(unsigned char addr, volatile unsigned char* data) { +void ir_mlx_crc(unsigned char addr, volatile unsigned char *data) +{ unsigned char i, bit, crc = 0; for (i = 0; i < 4; i++) { - if (i != 0) crc ^= (data[i-1]); - else crc ^= addr; + if (i != 0) { crc ^= (data[i - 1]); } + else { crc ^= addr; } for (bit = 8; bit > 0; bit--) { if (crc & 0x80) /* SMBus x^8 + x^2 + x + 1 */ + { crc = (crc << 1) ^ 0x107; - else + } else { crc = (crc << 1); + } } } data[3] = crc; } -void ir_mlx_init( void ) { +void ir_mlx_init(void) +{ #ifdef IR_MLX_ONE_TIME_CONFIG #warning Starting MLX90614 in CONFIGURATION MODE, do this only once for #warning setup, then turn this MODE off again and recompile/flash @@ -87,7 +91,8 @@ void ir_mlx_init( void ) { #endif } -void ir_mlx_periodic( void ) { +void ir_mlx_periodic(void) +{ #ifdef IR_MLX_ONE_TIME_CONFIG if (sys_time.nb_sec > 4) { #else @@ -99,7 +104,7 @@ void ir_mlx_periodic( void ) { i2c_transceive(&MLX_I2C_DEV, &mlx_trans, MLX90614_ADDR, 1, 2); ir_mlx_status = IR_MLX_RD_CASE_TEMP; /* send serial number every 30 seconds */ - RunOnceEvery((8*30), DOWNLINK_SEND_MLX_SERIAL(DefaultChannel, DefaultDevice, &ir_mlx_id_01, &ir_mlx_id_23)); + RunOnceEvery((8 * 30), DOWNLINK_SEND_MLX_SERIAL(DefaultChannel, DefaultDevice, &ir_mlx_id_01, &ir_mlx_id_23)); } else if (ir_mlx_status == IR_MLX_UNINIT) { /* start two byte ID 0 */ mlx_trans.buf[0] = MLX90614_ID_0; @@ -116,8 +121,7 @@ void ir_mlx_periodic( void ) { mlx_trans.buf[2] = 0; ir_mlx_crc(MLX90614_GENERAL_ADDR, mlx_trans.buf); i2c_transmit(&MLX_I2C_DEV, &mlx_trans, MLX90614_GENERAL_ADDR, 4); - } else - if ((sys_time.nb_sec > 2) && (ir_mlx_status == IR_MLX_ADDR_ERASE)) { + } else if ((sys_time.nb_sec > 2) && (ir_mlx_status == IR_MLX_ADDR_ERASE)) { /* set address by writing it to AMBus address register */ ir_mlx_status = IR_MLX_ADDR_SET; mlx_trans.buf[0] = MLX90614_SADR; @@ -125,84 +129,84 @@ void ir_mlx_periodic( void ) { mlx_trans.buf[2] = 0; ir_mlx_crc(MLX90614_GENERAL_ADDR, mlx_trans.buf); i2c_transmit(&MLX_I2C_DEV, &mlx_trans, MLX90614_GENERAL_ADDR, 4); - } else - if ((sys_time.nb_sec > 3) && (ir_mlx_status == IR_MLX_ADDR_SET)) { + } else if ((sys_time.nb_sec > 3) && (ir_mlx_status == IR_MLX_ADDR_SET)) { ir_mlx_status = IR_MLX_UNINIT; } #endif } -void ir_mlx_event( void ) { +void ir_mlx_event(void) +{ if ((mlx_trans.status == I2CTransSuccess)) { switch (ir_mlx_status) { - case IR_MLX_RD_ID_0: - /* read two byte ID 0 */ - ir_mlx_id_01 = mlx_trans.buf[0]; - ir_mlx_id_01 |= mlx_trans.buf[1] << 8; - /* start two byte ID 1 */ - mlx_trans.buf[0] = MLX90614_ID_1; - i2c_transceive(&MLX_I2C_DEV, &mlx_trans, MLX90614_ADDR, 1, 2); - ir_mlx_status = IR_MLX_RD_ID_1; - break; + case IR_MLX_RD_ID_0: + /* read two byte ID 0 */ + ir_mlx_id_01 = mlx_trans.buf[0]; + ir_mlx_id_01 |= mlx_trans.buf[1] << 8; + /* start two byte ID 1 */ + mlx_trans.buf[0] = MLX90614_ID_1; + i2c_transceive(&MLX_I2C_DEV, &mlx_trans, MLX90614_ADDR, 1, 2); + ir_mlx_status = IR_MLX_RD_ID_1; + break; - case IR_MLX_RD_ID_1: - /* read two byte ID 1 */ - ir_mlx_id_01 |= mlx_trans.buf[0] << 16; - ir_mlx_id_01 |= mlx_trans.buf[1] << 24; - /* start two byte ID 2 */ - mlx_trans.buf[0] = MLX90614_ID_2; - i2c_transceive(&MLX_I2C_DEV, &mlx_trans, MLX90614_ADDR, 1, 2); - ir_mlx_status = IR_MLX_RD_ID_2; - break; + case IR_MLX_RD_ID_1: + /* read two byte ID 1 */ + ir_mlx_id_01 |= mlx_trans.buf[0] << 16; + ir_mlx_id_01 |= mlx_trans.buf[1] << 24; + /* start two byte ID 2 */ + mlx_trans.buf[0] = MLX90614_ID_2; + i2c_transceive(&MLX_I2C_DEV, &mlx_trans, MLX90614_ADDR, 1, 2); + ir_mlx_status = IR_MLX_RD_ID_2; + break; - case IR_MLX_RD_ID_2: - /* read two byte ID 2 */ - ir_mlx_id_23 = mlx_trans.buf[0]; - ir_mlx_id_23 |= mlx_trans.buf[1] << 8; - /* start two byte ID 3 */ - mlx_trans.buf[0] = MLX90614_ID_3; - i2c_transceive(&MLX_I2C_DEV, &mlx_trans, MLX90614_ADDR, 1, 2); - ir_mlx_status = IR_MLX_RD_ID_3; - break; + case IR_MLX_RD_ID_2: + /* read two byte ID 2 */ + ir_mlx_id_23 = mlx_trans.buf[0]; + ir_mlx_id_23 |= mlx_trans.buf[1] << 8; + /* start two byte ID 3 */ + mlx_trans.buf[0] = MLX90614_ID_3; + i2c_transceive(&MLX_I2C_DEV, &mlx_trans, MLX90614_ADDR, 1, 2); + ir_mlx_status = IR_MLX_RD_ID_3; + break; - case IR_MLX_RD_ID_3: - /* read two byte ID 3 */ - ir_mlx_id_23 |= mlx_trans.buf[0] << 16; - ir_mlx_id_23 |= mlx_trans.buf[1] << 24; - ir_mlx_status = IR_MLX_IDLE; - mlx_trans.status = I2CTransDone; - DOWNLINK_SEND_MLX_SERIAL(DefaultChannel, DefaultDevice, &ir_mlx_id_01, &ir_mlx_id_23); - break; + case IR_MLX_RD_ID_3: + /* read two byte ID 3 */ + ir_mlx_id_23 |= mlx_trans.buf[0] << 16; + ir_mlx_id_23 |= mlx_trans.buf[1] << 24; + ir_mlx_status = IR_MLX_IDLE; + mlx_trans.status = I2CTransDone; + DOWNLINK_SEND_MLX_SERIAL(DefaultChannel, DefaultDevice, &ir_mlx_id_01, &ir_mlx_id_23); + break; - case IR_MLX_RD_CASE_TEMP: - /* read two byte case temperature */ - ir_mlx_itemp_case = mlx_trans.buf[1] << 8; - ir_mlx_itemp_case |= mlx_trans.buf[0]; - ir_mlx_temp_case = ir_mlx_itemp_case*0.02 - 273.15; + case IR_MLX_RD_CASE_TEMP: + /* read two byte case temperature */ + ir_mlx_itemp_case = mlx_trans.buf[1] << 8; + ir_mlx_itemp_case |= mlx_trans.buf[0]; + ir_mlx_temp_case = ir_mlx_itemp_case * 0.02 - 273.15; - /* start two byte obj temperature */ - mlx_trans.buf[0] = MLX90614_TOBJ; - i2c_transceive(&MLX_I2C_DEV, &mlx_trans, MLX90614_ADDR, 1, 2); - ir_mlx_status = IR_MLX_RD_OBJ_TEMP; - break; + /* start two byte obj temperature */ + mlx_trans.buf[0] = MLX90614_TOBJ; + i2c_transceive(&MLX_I2C_DEV, &mlx_trans, MLX90614_ADDR, 1, 2); + ir_mlx_status = IR_MLX_RD_OBJ_TEMP; + break; - case IR_MLX_RD_OBJ_TEMP: - /* read two byte obj temperature */ - ir_mlx_itemp_obj = mlx_trans.buf[1] << 8; - ir_mlx_itemp_obj |= mlx_trans.buf[0]; - ir_mlx_temp_obj = ir_mlx_itemp_obj*0.02 - 273.15; - mlx_trans.status = I2CTransDone; + case IR_MLX_RD_OBJ_TEMP: + /* read two byte obj temperature */ + ir_mlx_itemp_obj = mlx_trans.buf[1] << 8; + ir_mlx_itemp_obj |= mlx_trans.buf[0]; + ir_mlx_temp_obj = ir_mlx_itemp_obj * 0.02 - 273.15; + mlx_trans.status = I2CTransDone; - DOWNLINK_SEND_MLX_STATUS(DefaultChannel, DefaultDevice, - &ir_mlx_itemp_case, - &ir_mlx_temp_case, - &ir_mlx_itemp_obj, - &ir_mlx_temp_obj); - break; - default: - mlx_trans.status = I2CTransDone; - break; + DOWNLINK_SEND_MLX_STATUS(DefaultChannel, DefaultDevice, + &ir_mlx_itemp_case, + &ir_mlx_temp_case, + &ir_mlx_itemp_obj, + &ir_mlx_temp_obj); + break; + default: + mlx_trans.status = I2CTransDone; + break; } } } diff --git a/sw/airborne/modules/meteo/ir_mlx.h b/sw/airborne/modules/meteo/ir_mlx.h index 74b70e5b99..77a5d6e399 100644 --- a/sw/airborne/modules/meteo/ir_mlx.h +++ b/sw/airborne/modules/meteo/ir_mlx.h @@ -25,7 +25,7 @@ enum mlx_type { IR_MLX_RD_OBJ_TEMP }; -void ir_mlx_crc(unsigned char addr, volatile unsigned char* data); +void ir_mlx_crc(unsigned char addr, volatile unsigned char *data); void ir_mlx_init(void); void ir_mlx_periodic(void); void ir_mlx_event(void); diff --git a/sw/airborne/modules/meteo/light_solar.c b/sw/airborne/modules/meteo/light_solar.c index a3f3452ed7..69a2cb5355 100644 --- a/sw/airborne/modules/meteo/light_solar.c +++ b/sw/airborne/modules/meteo/light_solar.c @@ -51,14 +51,16 @@ int32_t light_cnt; static struct adc_buf buf_light_sol_up; static struct adc_buf buf_light_sol_dn; -void light_solar_init( void ) { +void light_solar_init(void) +{ adc_buf_channel(ADC_CHANNEL_LIGHT_SOLAR_UP, &buf_light_sol_up, ADC_CHANNEL_LIGHT_NB_SAMPLES); adc_buf_channel(ADC_CHANNEL_LIGHT_SOLAR_DN, &buf_light_sol_dn, ADC_CHANNEL_LIGHT_NB_SAMPLES); light_cnt = 0; } -void light_solar_periodic( void ) { +void light_solar_periodic(void) +{ up[light_cnt] = buf_light_sol_up.sum / buf_light_sol_up.av_nb_sample; dn[light_cnt] = buf_light_sol_dn.sum / buf_light_sol_dn.av_nb_sample; @@ -66,9 +68,9 @@ void light_solar_periodic( void ) { if (++light_cnt >= LIGHT_NB) { DOWNLINK_SEND_SOLAR_RADIATION(DefaultChannel, DefaultDevice, - &up[0], &dn[0], &up[1], &dn[1], &up[2], &dn[2], &up[3], &dn[3], - &up[4], &dn[4], &up[5], &dn[5], &up[6], &dn[6], &up[7], &dn[7], - &up[8], &dn[8], &up[9], &dn[9]); + &up[0], &dn[0], &up[1], &dn[1], &up[2], &dn[2], &up[3], &dn[3], + &up[4], &dn[4], &up[5], &dn[5], &up[6], &dn[6], &up[7], &dn[7], + &up[8], &dn[8], &up[9], &dn[9]); light_cnt = 0; } } diff --git a/sw/airborne/modules/meteo/light_temt.c b/sw/airborne/modules/meteo/light_temt.c index fa486bf4f3..2b9914465d 100644 --- a/sw/airborne/modules/meteo/light_temt.c +++ b/sw/airborne/modules/meteo/light_temt.c @@ -46,11 +46,13 @@ uint16_t adc_light_temt; static struct adc_buf buf_light_temt; -void light_temt_init( void ) { +void light_temt_init(void) +{ adc_buf_channel(ADC_CHANNEL_LIGHT_TEMT, &buf_light_temt, ADC_CHANNEL_LIGHT_NB_SAMPLES); } -void light_temt_periodic( void ) { +void light_temt_periodic(void) +{ float f_light_temt; adc_light_temt = buf_light_temt.sum / buf_light_temt.av_nb_sample; diff --git a/sw/airborne/modules/meteo/light_temt.h b/sw/airborne/modules/meteo/light_temt.h index 0dc82707f8..7d2eca49c0 100644 --- a/sw/airborne/modules/meteo/light_temt.h +++ b/sw/airborne/modules/meteo/light_temt.h @@ -4,7 +4,7 @@ #include extern uint16_t adc_light_temt; -void light_temt_init( void ); -void light_temt_periodic( void ); +void light_temt_init(void); +void light_temt_periodic(void); #endif diff --git a/sw/airborne/modules/meteo/meteo_france_DAQ.c b/sw/airborne/modules/meteo/meteo_france_DAQ.c index 7c89c13c55..3ea2c403f7 100644 --- a/sw/airborne/modules/meteo/meteo_france_DAQ.c +++ b/sw/airborne/modules/meteo/meteo_france_DAQ.c @@ -54,7 +54,8 @@ bool_t log_started; INFO("MF_DAQ power pin is not defined") #endif -void init_mf_daq(void) { +void init_mf_daq(void) +{ mf_daq.nb = 0; mf_daq.power = MF_DAQ_POWER_INIT; #if (defined MF_DAQ_POWER_PORT) && (defined MF_DAQ_POWER_PIN) @@ -64,30 +65,32 @@ void init_mf_daq(void) { log_started = FALSE; } -void mf_daq_send_state(void) { +void mf_daq_send_state(void) +{ // Send aircraft state to DAQ board DOWNLINK_SEND_MF_DAQ_STATE(pprz_tp, EXTRA_DOWNLINK_DEVICE, - &autopilot_flight_time, - &stateGetBodyRates_f()->p, - &stateGetBodyRates_f()->q, - &stateGetBodyRates_f()->r, - &stateGetNedToBodyEulers_f()->phi, - &stateGetNedToBodyEulers_f()->theta, - &stateGetNedToBodyEulers_f()->psi, - &stateGetAccelNed_f()->x, - &stateGetAccelNed_f()->y, - &stateGetAccelNed_f()->z, - &stateGetSpeedEnu_f()->x, - &stateGetSpeedEnu_f()->y, - &stateGetSpeedEnu_f()->z, - &stateGetPositionLla_f()->lat, - &stateGetPositionLla_f()->lon, - &stateGetPositionLla_f()->alt, - &stateGetHorizontalWindspeed_f()->y, - &stateGetHorizontalWindspeed_f()->x); + &autopilot_flight_time, + &stateGetBodyRates_f()->p, + &stateGetBodyRates_f()->q, + &stateGetBodyRates_f()->r, + &stateGetNedToBodyEulers_f()->phi, + &stateGetNedToBodyEulers_f()->theta, + &stateGetNedToBodyEulers_f()->psi, + &stateGetAccelNed_f()->x, + &stateGetAccelNed_f()->y, + &stateGetAccelNed_f()->z, + &stateGetSpeedEnu_f()->x, + &stateGetSpeedEnu_f()->y, + &stateGetSpeedEnu_f()->z, + &stateGetPositionLla_f()->lat, + &stateGetPositionLla_f()->lon, + &stateGetPositionLla_f()->alt, + &stateGetHorizontalWindspeed_f()->y, + &stateGetHorizontalWindspeed_f()->x); } -void mf_daq_send_report(void) { +void mf_daq_send_report(void) +{ // Send report over normal telemetry if (mf_daq.nb > 0) { DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, 9, mf_daq.values); @@ -102,42 +105,43 @@ void mf_daq_send_report(void) { // Log GPS for time reference uint8_t foo = 0; int16_t climb = -gps.ned_vel.z; - int16_t course = (DegOfRad(gps.course)/((int32_t)1e6)); + int16_t course = (DegOfRad(gps.course) / ((int32_t)1e6)); DOWNLINK_SEND_GPS(pprzlog_tp, chibios_sdlog, &gps.fix, - &gps.utm_pos.east, &gps.utm_pos.north, - &course, &gps.hmsl, &gps.gspeed, &climb, - &gps.week, &gps.tow, &gps.utm_pos.zone, &foo); + &gps.utm_pos.east, &gps.utm_pos.north, + &course, &gps.hmsl, &gps.gspeed, &climb, + &gps.week, &gps.tow, &gps.utm_pos.zone, &foo); } } -void parse_mf_daq_msg(void) { +void parse_mf_daq_msg(void) +{ mf_daq.nb = DL_PAYLOAD_FLOAT_values_length(dl_buffer); if (mf_daq.nb > 0) { - if (mf_daq.nb > MF_DAQ_SIZE) mf_daq.nb = MF_DAQ_SIZE; + if (mf_daq.nb > MF_DAQ_SIZE) { mf_daq.nb = MF_DAQ_SIZE; } // Store data struct directly from dl_buffer memcpy(mf_daq.values, DL_PAYLOAD_FLOAT_values(dl_buffer), mf_daq.nb * sizeof(float)); // Log on SD card if (log_started) { DOWNLINK_SEND_PAYLOAD_FLOAT(pprzlog_tp, chibios_sdlog, mf_daq.nb, mf_daq.values); DOWNLINK_SEND_MF_DAQ_STATE(pprzlog_tp, chibios_sdlog, - &autopilot_flight_time, - &stateGetBodyRates_f()->p, - &stateGetBodyRates_f()->q, - &stateGetBodyRates_f()->r, - &stateGetNedToBodyEulers_f()->phi, - &stateGetNedToBodyEulers_f()->theta, - &stateGetNedToBodyEulers_f()->psi, - &stateGetAccelNed_f()->x, - &stateGetAccelNed_f()->y, - &stateGetAccelNed_f()->z, - &stateGetSpeedEnu_f()->x, - &stateGetSpeedEnu_f()->y, - &stateGetSpeedEnu_f()->z, - &stateGetPositionLla_f()->lat, - &stateGetPositionLla_f()->lon, - &stateGetPositionLla_f()->alt, - &stateGetHorizontalWindspeed_f()->y, - &stateGetHorizontalWindspeed_f()->x); + &autopilot_flight_time, + &stateGetBodyRates_f()->p, + &stateGetBodyRates_f()->q, + &stateGetBodyRates_f()->r, + &stateGetNedToBodyEulers_f()->phi, + &stateGetNedToBodyEulers_f()->theta, + &stateGetNedToBodyEulers_f()->psi, + &stateGetAccelNed_f()->x, + &stateGetAccelNed_f()->y, + &stateGetAccelNed_f()->z, + &stateGetSpeedEnu_f()->x, + &stateGetSpeedEnu_f()->y, + &stateGetSpeedEnu_f()->z, + &stateGetPositionLla_f()->lat, + &stateGetPositionLla_f()->lon, + &stateGetPositionLla_f()->alt, + &stateGetHorizontalWindspeed_f()->y, + &stateGetHorizontalWindspeed_f()->x); } } } diff --git a/sw/airborne/modules/meteo/meteo_france_DAQ.h b/sw/airborne/modules/meteo/meteo_france_DAQ.h index 662df56f22..efbd2dadcb 100644 --- a/sw/airborne/modules/meteo/meteo_france_DAQ.h +++ b/sw/airborne/modules/meteo/meteo_france_DAQ.h @@ -55,10 +55,10 @@ extern void parse_mf_daq_msg(void); #if (defined MF_DAQ_POWER_PORT) && (defined MF_DAQ_POWER_PIN) #define meteo_france_DAQ_SetPower(_x) { \ - mf_daq.power = _x; \ - if (mf_daq.power) { gpio_set(MF_DAQ_POWER_PORT, MF_DAQ_POWER_PIN); } \ - else { gpio_clear(MF_DAQ_POWER_PORT, MF_DAQ_POWER_PIN); } \ -} + mf_daq.power = _x; \ + if (mf_daq.power) { gpio_set(MF_DAQ_POWER_PORT, MF_DAQ_POWER_PIN); } \ + else { gpio_clear(MF_DAQ_POWER_PORT, MF_DAQ_POWER_PIN); } \ + } #else // POWER PORT and PIN undefined #define meteo_france_DAQ_SetPower(_x) {} #endif diff --git a/sw/airborne/modules/meteo/meteo_stick.c b/sw/airborne/modules/meteo/meteo_stick.c index 83855494a0..e19c51963a 100644 --- a/sw/airborne/modules/meteo/meteo_stick.c +++ b/sw/airborne/modules/meteo/meteo_stick.c @@ -152,16 +152,16 @@ void meteo_stick_periodic(void) #if LOG_MS if (pprzLogFile.fs != NULL) { if (!log_ptu_started) { - sdLogWriteLog(&pprzLogFile, "P(adc) T(adc) H(ticks) P_diff(adc) GPS_fix TOW(ms) Week Lat(1e7rad) Lon(1e7rad) HMSL(mm) gpseed(cm/s) course(1e7rad) climb(cm/s)\n"); + sdLogWriteLog(&pprzLogFile, + "P(adc) T(adc) H(ticks) P_diff(adc) GPS_fix TOW(ms) Week Lat(1e7rad) Lon(1e7rad) HMSL(mm) gpseed(cm/s) course(1e7rad) climb(cm/s)\n"); log_ptu_started = TRUE; - } - else { + } else { sdLogWriteLog(&pprzLogFile, "%d %d %d %d %d %d %d %d %d %d %d %d %d\n", - meteo_stick.pressure.data, meteo_stick.temperature.data, - meteo_stick.humidity_period, meteo_stick.diff_pressure.data, - gps.fix, gps.tow, gps.week, - gps.lla_pos.lat, gps.lla_pos.lon, gps.hmsl, - gps.gspeed, gps.course, -gps.ned_vel.z); + meteo_stick.pressure.data, meteo_stick.temperature.data, + meteo_stick.humidity_period, meteo_stick.diff_pressure.data, + gps.fix, gps.tow, gps.week, + gps.lla_pos.lat, gps.lla_pos.lon, gps.hmsl, + gps.gspeed, gps.course, -gps.ned_vel.z); } } #endif diff --git a/sw/airborne/modules/meteo/mf_ptu.c b/sw/airborne/modules/meteo/mf_ptu.c index 5a9d9d882a..035549cebd 100644 --- a/sw/airborne/modules/meteo/mf_ptu.c +++ b/sw/airborne/modules/meteo/mf_ptu.c @@ -74,7 +74,8 @@ bool_t log_ptu_started; #include "subsystems/gps.h" #endif -void mf_ptu_init(void) { +void mf_ptu_init(void) +{ adc_buf_channel(ADC_CHANNEL_PRESSURE, &pressure_buf, ADC_CHANNEL_PTU_NB_SAMPLES); adc_buf_channel(ADC_CHANNEL_TEMPERATURE, &temp_buf, ADC_CHANNEL_PTU_NB_SAMPLES); @@ -92,7 +93,8 @@ void mf_ptu_init(void) { #endif } -void mf_ptu_periodic(void) { +void mf_ptu_periodic(void) +{ // Read ADC pressure_adc = pressure_buf.sum / pressure_buf.av_nb_sample; temp_adc = temp_buf.sum / temp_buf.av_nb_sample; @@ -103,15 +105,15 @@ void mf_ptu_periodic(void) { #if LOG_PTU if (pprzLogFile.fs != NULL) { if (!log_ptu_started) { - sdLogWriteLog(&pprzLogFile, "P(adc) T(adc) H(usec) GPS_fix TOW(ms) Week Lat(1e7rad) Lon(1e7rad) HMSL(mm) gpseed(cm/s) course(1e7rad) climb(cm/s)\n"); + sdLogWriteLog(&pprzLogFile, + "P(adc) T(adc) H(usec) GPS_fix TOW(ms) Week Lat(1e7rad) Lon(1e7rad) HMSL(mm) gpseed(cm/s) course(1e7rad) climb(cm/s)\n"); log_ptu_started = TRUE; - } - else { + } else { sdLogWriteLog(&pprzLogFile, "%d %d %d %d %d %d %d %d %d %d %d %d\n", - pressure_adc, temp_adc, humid_period, - gps.fix, gps.tow, gps.week, - gps.lla_pos.lat, gps.lla_pos.lon, gps.hmsl, - gps.gspeed, gps.course, -gps.ned_vel.z); + pressure_adc, temp_adc, humid_period, + gps.fix, gps.tow, gps.week, + gps.lla_pos.lat, gps.lla_pos.lon, gps.hmsl, + gps.gspeed, gps.course, -gps.ned_vel.z); } } #endif diff --git a/sw/airborne/modules/meteo/temp_lm75.c b/sw/airborne/modules/meteo/temp_lm75.c index b6af104bc8..2318f0176c 100644 --- a/sw/airborne/modules/meteo/temp_lm75.c +++ b/sw/airborne/modules/meteo/temp_lm75.c @@ -49,16 +49,19 @@ struct i2c_transaction lm75_trans; #define LM75_SLAVE_ADDR 0x90 #endif -void lm75_init(void) { +void lm75_init(void) +{ lm75_trans.status = I2CTransDone; } -void lm75_periodic( void ) { +void lm75_periodic(void) +{ lm75_trans.buf[0] = LM75_TEMP_REG; i2c_transceive(&LM75_I2C_DEV, &lm75_trans, LM75_SLAVE_ADDR, 1, 2); } -void lm75_event( void ) { +void lm75_event(void) +{ if (lm75_trans.status == I2CTransSuccess) { uint16_t lm75_temperature; float flm75_temperature; @@ -67,8 +70,9 @@ void lm75_event( void ) { lm75_temperature = lm75_trans.buf[0] << 8; lm75_temperature |= lm75_trans.buf[1]; lm75_temperature >>= 7; - if (lm75_temperature & 0x0100) + if (lm75_temperature & 0x0100) { lm75_temperature |= 0xFE00; + } flm75_temperature = ((int16_t) lm75_temperature) / 2.; diff --git a/sw/airborne/modules/meteo/temp_tcouple_adc.c b/sw/airborne/modules/meteo/temp_tcouple_adc.c index 6455815e63..5a845fb62a 100644 --- a/sw/airborne/modules/meteo/temp_tcouple_adc.c +++ b/sw/airborne/modules/meteo/temp_tcouple_adc.c @@ -53,7 +53,8 @@ int32_t temp_cnt; static struct adc_buf buf_temp_tcouple_ref; static struct adc_buf buf_temp_tcouple_val; -void temp_tcouple_adc_init( void ) { +void temp_tcouple_adc_init(void) +{ adc_buf_channel(ADC_CHANNEL_TEMP_REF, &buf_temp_tcouple_ref, ADC_CHANNEL_TEMP_TCOUPLE_NB_SAMPLES); @@ -63,23 +64,24 @@ void temp_tcouple_adc_init( void ) { temp_cnt = 0; } -void temp_tcouple_adc_periodic( void ) { +void temp_tcouple_adc_periodic(void) +{ val[temp_cnt] = buf_temp_tcouple_val.sum / buf_temp_tcouple_val.av_nb_sample; ref[temp_cnt] = buf_temp_tcouple_ref.sum / buf_temp_tcouple_ref.av_nb_sample; /* no voltage divider, 10 bits adc, 3.3V max */ /* T = U * 52.288899706 - 7.977784737996595 */ fval[temp_cnt] = ((float)(val[temp_cnt] * 3.3) / 1023.) - * 52.288899706 - 7.977784737996595; + * 52.288899706 - 7.977784737996595; fref[temp_cnt] = ((float)(ref[temp_cnt] * 3.3) / 1023.) - * 100. - 13.; + * 100. - 13.; if (++temp_cnt >= TCOUPLE_NB) { DOWNLINK_SEND_TEMP_TCOUPLE(DefaultChannel, DefaultDevice, - &fval[0], &fval[1], &fval[2], &fval[3], - &fref[0], &fref[1], &fref[2], &fref[3], - &val[0], &val[1], &val[2], &val[3], - &ref[0], &ref[1], &ref[2], &ref[3]); + &fval[0], &fval[1], &fval[2], &fval[3], + &fref[0], &fref[1], &fref[2], &fref[3], + &val[0], &val[1], &val[2], &val[3], + &ref[0], &ref[1], &ref[2], &ref[3]); temp_cnt = 0; } } diff --git a/sw/airborne/modules/meteo/temp_temod.c b/sw/airborne/modules/meteo/temp_temod.c index 5420e164a3..e45251260f 100644 --- a/sw/airborne/modules/meteo/temp_temod.c +++ b/sw/airborne/modules/meteo/temp_temod.c @@ -47,28 +47,31 @@ struct i2c_transaction tmd_trans; #define TEMOD_SLAVE_ADDR 0xF0 -void temod_init(void) { - tmd_trans.status = I2CTransDone; +void temod_init(void) +{ + tmd_trans.status = I2CTransDone; } -void temod_periodic( void ) { - i2c_receive(&TEMOD_I2C_DEV, &tmd_trans, TEMOD_SLAVE_ADDR, 2); +void temod_periodic(void) +{ + i2c_receive(&TEMOD_I2C_DEV, &tmd_trans, TEMOD_SLAVE_ADDR, 2); } -void temod_event( void ) { +void temod_event(void) +{ if (tmd_trans.status == I2CTransSuccess) { - uint16_t tmd_temperature; + uint16_t tmd_temperature; - /* read two byte temperature */ - tmd_temperature = tmd_trans.buf[0] << 8; - tmd_temperature |= tmd_trans.buf[1]; + /* read two byte temperature */ + tmd_temperature = tmd_trans.buf[0] << 8; + tmd_temperature |= tmd_trans.buf[1]; - ftmd_temperature = (tmd_temperature / TEMOD_TYPE) - 32.; + ftmd_temperature = (tmd_temperature / TEMOD_TYPE) - 32.; - DOWNLINK_SEND_TMP_STATUS(DefaultChannel, DefaultDevice, &tmd_temperature, &ftmd_temperature); - tmd_trans.status = I2CTransDone; + DOWNLINK_SEND_TMP_STATUS(DefaultChannel, DefaultDevice, &tmd_temperature, &ftmd_temperature); + tmd_trans.status = I2CTransDone; } } diff --git a/sw/airborne/modules/meteo/temp_tmp102.c b/sw/airborne/modules/meteo/temp_tmp102.c index a4db095ad5..7c7b5d438c 100644 --- a/sw/airborne/modules/meteo/temp_tmp102.c +++ b/sw/airborne/modules/meteo/temp_tmp102.c @@ -60,7 +60,8 @@ struct i2c_transaction tmp_trans; #define TMP102_CONF2 0xF0 -void tmp102_init(void) { +void tmp102_init(void) +{ tmp_meas_started = FALSE; /* configure 8Hz and enhanced mode */ tmp_trans.buf[0] = TMP102_CONF_REG; @@ -69,28 +70,31 @@ void tmp102_init(void) { i2c_transmit(&TMP_I2C_DEV, &tmp_trans, TMP102_SLAVE_ADDR, 3); } -void tmp102_periodic( void ) { - tmp_trans.buf[0] = TMP102_TEMP_REG; - i2c_transceive(&TMP_I2C_DEV, &tmp_trans, TMP102_SLAVE_ADDR, 1, 2); - tmp_meas_started = TRUE; +void tmp102_periodic(void) +{ + tmp_trans.buf[0] = TMP102_TEMP_REG; + i2c_transceive(&TMP_I2C_DEV, &tmp_trans, TMP102_SLAVE_ADDR, 1, 2); + tmp_meas_started = TRUE; } -void tmp102_event( void ) { +void tmp102_event(void) +{ if ((tmp_trans.status == I2CTransSuccess) && (tmp_meas_started == TRUE)) { - uint16_t tmp_temperature; + uint16_t tmp_temperature; - /* read two byte temperature */ - tmp_temperature = tmp_trans.buf[0] << 8; - tmp_temperature |= tmp_trans.buf[1]; - tmp_temperature >>= 3; - if (tmp_temperature & 0x1000) - tmp_temperature |= 0xE000; + /* read two byte temperature */ + tmp_temperature = tmp_trans.buf[0] << 8; + tmp_temperature |= tmp_trans.buf[1]; + tmp_temperature >>= 3; + if (tmp_temperature & 0x1000) { + tmp_temperature |= 0xE000; + } - ftmp_temperature = ((int16_t) tmp_temperature) / 16.; + ftmp_temperature = ((int16_t) tmp_temperature) / 16.; - DOWNLINK_SEND_TMP_STATUS(DefaultChannel, DefaultDevice, &tmp_temperature, &ftmp_temperature); - tmp_trans.status = I2CTransDone; + DOWNLINK_SEND_TMP_STATUS(DefaultChannel, DefaultDevice, &tmp_temperature, &ftmp_temperature); + tmp_trans.status = I2CTransDone; } } diff --git a/sw/airborne/modules/meteo/wind_gfi.c b/sw/airborne/modules/meteo/wind_gfi.c index 3b2c07c3a0..002944ded7 100644 --- a/sw/airborne/modules/meteo/wind_gfi.c +++ b/sw/airborne/modules/meteo/wind_gfi.c @@ -54,12 +54,14 @@ struct i2c_transaction pcf_trans; uint32_t pcf_direction; uint8_t pcf_status; -void wind_gfi_init(void) { +void wind_gfi_init(void) +{ pcf_trans.status = I2CTransDone; pcf_status = PCF_IDLE; } -void wind_gfi_periodic( void ) { +void wind_gfi_periodic(void) +{ /* OE low, SEL high (for low data) */ pcf_trans.buf[0] = 0xFF; pcf_trans.buf[1] = 0xBF; @@ -67,14 +69,14 @@ void wind_gfi_periodic( void ) { i2c_transmit(&PCF_I2C_DEV, &pcf_trans, PCF_SLAVE_ADDR, 2); } -void wind_gfi_event( void ) { +void wind_gfi_event(void) +{ if (pcf_trans.status == I2CTransSuccess) { if (pcf_status == PCF_SET_OE_LSB) { pcf_status = PCF_READ_LSB; i2c_receive(&PCF_I2C_DEV, &pcf_trans, PCF_SLAVE_ADDR, 2); - } - else if (pcf_status == PCF_READ_LSB) { + } else if (pcf_status == PCF_READ_LSB) { /* read lower byte direction info */ pcf_direction = pcf_trans.buf[0]; @@ -83,12 +85,10 @@ void wind_gfi_event( void ) { pcf_trans.buf[1] = 0x3F; pcf_status = PCF_SET_OE_MSB; i2c_transmit(&PCF_I2C_DEV, &pcf_trans, PCF_SLAVE_ADDR, 2); - } - else if (pcf_status == PCF_SET_OE_MSB) { + } else if (pcf_status == PCF_SET_OE_MSB) { pcf_status = PCF_READ_MSB; i2c_receive(&PCF_I2C_DEV, &pcf_trans, PCF_SLAVE_ADDR, 2); - } - else if (pcf_status == PCF_READ_MSB) { + } else if (pcf_status == PCF_READ_MSB) { float fpcf_direction; /* read higher byte direction info */ @@ -101,11 +101,10 @@ void wind_gfi_event( void ) { i2c_transmit(&PCF_I2C_DEV, &pcf_trans, PCF_SLAVE_ADDR, 2); /* 2048 digits per 360 degrees */ - fpcf_direction = fmod((pcf_direction * (360./2048.)) + ZERO_OFFSET_DEGREES, 360.); + fpcf_direction = fmod((pcf_direction * (360. / 2048.)) + ZERO_OFFSET_DEGREES, 360.); DOWNLINK_SEND_TMP_STATUS(DefaultChannel, DefaultDevice, &pcf_direction, &fpcf_direction); - } - else if (pcf_status == PCF_IDLE) { + } else if (pcf_status == PCF_IDLE) { pcf_trans.status = I2CTransDone; } } diff --git a/sw/airborne/modules/meteo/wind_gfi.h b/sw/airborne/modules/meteo/wind_gfi.h index fe7f15f6ae..0d76e813c4 100644 --- a/sw/airborne/modules/meteo/wind_gfi.h +++ b/sw/airborne/modules/meteo/wind_gfi.h @@ -3,7 +3,7 @@ #include "std.h" -enum pcf_stat{ +enum pcf_stat { PCF_IDLE, PCF_SET_OE_LSB, PCF_READ_LSB, diff --git a/sw/airborne/modules/meteo/windturbine.c b/sw/airborne/modules/meteo/windturbine.c index e538a710f2..0c28e96fe8 100644 --- a/sw/airborne/modules/meteo/windturbine.c +++ b/sw/airborne/modules/meteo/windturbine.c @@ -38,7 +38,8 @@ #include "subsystems/datalink/downlink.h" -void windturbine_periodic( void ) { +void windturbine_periodic(void) +{ if (trigger_ext_valid == TRUE) { uint8_t ac_id = 0; uint8_t turb_id = TURBINE_ID; @@ -48,10 +49,10 @@ void windturbine_periodic( void ) { cycle_time = msec_of_sys_time_ticks(trigger_delta_t0); DOWNLINK_SEND_WINDTURBINE_STATUS_(DefaultChannel, DefaultDevice, - &ac_id, - &turb_id, - &sync_itow, - &cycle_time ); + &ac_id, + &turb_id, + &sync_itow, + &cycle_time); trigger_ext_valid = FALSE; } } diff --git a/sw/airborne/modules/meteo/windturbine.h b/sw/airborne/modules/meteo/windturbine.h index 9a42044e79..bb33b6362b 100644 --- a/sw/airborne/modules/meteo/windturbine.h +++ b/sw/airborne/modules/meteo/windturbine.h @@ -31,7 +31,7 @@ #ifndef WINDTURBINE_H #define WINDTURBINE_H -void windturbine_periodic( void ); +void windturbine_periodic(void); #endif diff --git a/sw/airborne/modules/mission/mission_common.c b/sw/airborne/modules/mission/mission_common.c index eea9810da3..75db8fc270 100644 --- a/sw/airborne/modules/mission/mission_common.c +++ b/sw/airborne/modules/mission/mission_common.c @@ -35,7 +35,8 @@ struct _mission mission; -void mission_init(void) { +void mission_init(void) +{ mission.insert_idx = 0; mission.current_idx = 0; mission.element_time = 0.; @@ -43,22 +44,23 @@ void mission_init(void) { // Insert element -bool_t mission_insert(enum MissionInsertMode insert, struct _mission_element * element) { +bool_t mission_insert(enum MissionInsertMode insert, struct _mission_element *element) +{ uint8_t tmp; // convert element if needed, return FALSE if failed - if (!mission_element_convert(element)) return FALSE; + if (!mission_element_convert(element)) { return FALSE; } switch (insert) { case Append: tmp = (mission.insert_idx + 1) % MISSION_ELEMENT_NB; - if (tmp == mission.current_idx) return FALSE; // no room to insert element + if (tmp == mission.current_idx) { return FALSE; } // no room to insert element memcpy(&mission.elements[mission.insert_idx], element, sizeof(struct _mission_element)); // add element mission.insert_idx = tmp; // move insert index break; case Prepend: - if (mission.current_idx == 0) tmp = MISSION_ELEMENT_NB-1; - else tmp = mission.current_idx - 1; - if (tmp == mission.insert_idx) return FALSE; // no room to inser element + if (mission.current_idx == 0) { tmp = MISSION_ELEMENT_NB - 1; } + else { tmp = mission.current_idx - 1; } + if (tmp == mission.insert_idx) { return FALSE; } // no room to inser element memcpy(&mission.elements[tmp], element, sizeof(struct _mission_element)); // add element mission.current_idx = tmp; // move current index break; @@ -82,11 +84,12 @@ bool_t mission_insert(enum MissionInsertMode insert, struct _mission_element * e // Weak implementation of mission_element_convert (leave element unchanged) -bool_t __attribute__((weak)) mission_element_convert(struct _mission_element * el __attribute__((unused))) { return TRUE; } +bool_t __attribute__((weak)) mission_element_convert(struct _mission_element *el __attribute__((unused))) { return TRUE; } // Get element -struct _mission_element * mission_get(void) { +struct _mission_element *mission_get(void) +{ if (mission.current_idx == mission.insert_idx) { return NULL; } @@ -95,13 +98,14 @@ struct _mission_element * mission_get(void) { // Report function -void mission_status_report(void) { +void mission_status_report(void) +{ // build task list uint8_t task_list[MISSION_ELEMENT_NB]; uint8_t i = mission.current_idx, j = 0; while (i != mission.insert_idx) { task_list[j++] = (uint8_t)mission.elements[i].type; - i = (i+1)%MISSION_ELEMENT_NB; + i = (i + 1) % MISSION_ELEMENT_NB; } if (j == 0) { task_list[j++] = 0; } // Dummy value if task list is empty //compute remaining time (or -1. if no time limit) @@ -119,8 +123,9 @@ void mission_status_report(void) { // Parsing functions // /////////////////////// -int mission_parse_GOTO_WP(void) { - if (DL_MISSION_GOTO_WP_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft +int mission_parse_GOTO_WP(void) +{ + if (DL_MISSION_GOTO_WP_ac_id(dl_buffer) != AC_ID) { return FALSE; } // not for this aircraft struct _mission_element me; me.type = MissionWP; @@ -129,13 +134,14 @@ int mission_parse_GOTO_WP(void) { me.element.mission_wp.wp.wp_f.z = DL_MISSION_GOTO_WP_wp_alt(dl_buffer); me.duration = DL_MISSION_GOTO_WP_duration(dl_buffer); - enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_GOTO_WP_insert(dl_buffer)); + enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_GOTO_WP_insert(dl_buffer)); return mission_insert(insert, &me); } -int mission_parse_GOTO_WP_LLA(void) { - if (DL_MISSION_GOTO_WP_LLA_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft +int mission_parse_GOTO_WP_LLA(void) +{ + if (DL_MISSION_GOTO_WP_LLA_ac_id(dl_buffer) != AC_ID) { return FALSE; } // not for this aircraft struct LlaCoor_i lla; lla.lat = DL_MISSION_GOTO_WP_LLA_wp_lat(dl_buffer); @@ -145,16 +151,17 @@ int mission_parse_GOTO_WP_LLA(void) { struct _mission_element me; me.type = MissionWP; // if there is no valid local coordinate, do not insert mission element - if (!mission_point_of_lla(&me.element.mission_wp.wp.wp_f, &lla)) return FALSE; + if (!mission_point_of_lla(&me.element.mission_wp.wp.wp_f, &lla)) { return FALSE; } me.duration = DL_MISSION_GOTO_WP_LLA_duration(dl_buffer); - enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_GOTO_WP_LLA_insert(dl_buffer)); + enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_GOTO_WP_LLA_insert(dl_buffer)); return mission_insert(insert, &me); } -int mission_parse_CIRCLE(void) { - if (DL_MISSION_CIRCLE_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft +int mission_parse_CIRCLE(void) +{ + if (DL_MISSION_CIRCLE_ac_id(dl_buffer) != AC_ID) { return FALSE; } // not for this aircraft struct _mission_element me; me.type = MissionCircle; @@ -164,13 +171,14 @@ int mission_parse_CIRCLE(void) { me.element.mission_circle.radius = DL_MISSION_CIRCLE_radius(dl_buffer); me.duration = DL_MISSION_CIRCLE_duration(dl_buffer); - enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_CIRCLE_insert(dl_buffer)); + enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_CIRCLE_insert(dl_buffer)); return mission_insert(insert, &me); } -int mission_parse_CIRCLE_LLA(void) { - if (DL_MISSION_CIRCLE_LLA_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft +int mission_parse_CIRCLE_LLA(void) +{ + if (DL_MISSION_CIRCLE_LLA_ac_id(dl_buffer) != AC_ID) { return FALSE; } // not for this aircraft struct LlaCoor_i lla; lla.lat = DL_MISSION_CIRCLE_LLA_center_lat(dl_buffer); @@ -180,17 +188,18 @@ int mission_parse_CIRCLE_LLA(void) { struct _mission_element me; me.type = MissionCircle; // if there is no valid local coordinate, do not insert mission element - if (!mission_point_of_lla(&me.element.mission_circle.center.center_f, &lla)) return FALSE; + if (!mission_point_of_lla(&me.element.mission_circle.center.center_f, &lla)) { return FALSE; } me.element.mission_circle.radius = DL_MISSION_CIRCLE_LLA_radius(dl_buffer); me.duration = DL_MISSION_CIRCLE_LLA_duration(dl_buffer); - enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_CIRCLE_LLA_insert(dl_buffer)); + enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_CIRCLE_LLA_insert(dl_buffer)); return mission_insert(insert, &me); } -int mission_parse_SEGMENT(void) { - if (DL_MISSION_SEGMENT_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft +int mission_parse_SEGMENT(void) +{ + if (DL_MISSION_SEGMENT_ac_id(dl_buffer) != AC_ID) { return FALSE; } // not for this aircraft struct _mission_element me; me.type = MissionSegment; @@ -202,13 +211,14 @@ int mission_parse_SEGMENT(void) { me.element.mission_segment.to.to_f.z = DL_MISSION_SEGMENT_segment_alt(dl_buffer); me.duration = DL_MISSION_SEGMENT_duration(dl_buffer); - enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_SEGMENT_insert(dl_buffer)); + enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_SEGMENT_insert(dl_buffer)); return mission_insert(insert, &me); } -int mission_parse_SEGMENT_LLA(void) { - if (DL_MISSION_SEGMENT_LLA_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft +int mission_parse_SEGMENT_LLA(void) +{ + if (DL_MISSION_SEGMENT_LLA_ac_id(dl_buffer) != AC_ID) { return FALSE; } // not for this aircraft struct LlaCoor_i from_lla, to_lla; from_lla.lat = DL_MISSION_SEGMENT_LLA_segment_lat_1(dl_buffer); @@ -221,17 +231,18 @@ int mission_parse_SEGMENT_LLA(void) { struct _mission_element me; me.type = MissionSegment; // if there is no valid local coordinate, do not insert mission element - if (!mission_point_of_lla(&me.element.mission_segment.from.from_f, &from_lla)) return FALSE; - if (!mission_point_of_lla(&me.element.mission_segment.to.to_f, &to_lla)) return FALSE; + if (!mission_point_of_lla(&me.element.mission_segment.from.from_f, &from_lla)) { return FALSE; } + if (!mission_point_of_lla(&me.element.mission_segment.to.to_f, &to_lla)) { return FALSE; } me.duration = DL_MISSION_SEGMENT_LLA_duration(dl_buffer); - enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_SEGMENT_LLA_insert(dl_buffer)); + enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_SEGMENT_LLA_insert(dl_buffer)); return mission_insert(insert, &me); } -int mission_parse_PATH(void) { - if (DL_MISSION_PATH_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft +int mission_parse_PATH(void) +{ + if (DL_MISSION_PATH_ac_id(dl_buffer) != AC_ID) { return FALSE; } // not for this aircraft struct _mission_element me; me.type = MissionPath; @@ -251,17 +262,18 @@ int mission_parse_PATH(void) { me.element.mission_path.path.path_f[4].y = DL_MISSION_PATH_point_north_5(dl_buffer); me.element.mission_path.path.path_f[4].z = DL_MISSION_PATH_path_alt(dl_buffer); me.element.mission_path.nb = DL_MISSION_PATH_nb(dl_buffer); - if (me.element.mission_path.nb > MISSION_PATH_NB) me.element.mission_path.nb = MISSION_PATH_NB; + if (me.element.mission_path.nb > MISSION_PATH_NB) { me.element.mission_path.nb = MISSION_PATH_NB; } me.element.mission_path.path_idx = 0; me.duration = DL_MISSION_PATH_duration(dl_buffer); - enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_PATH_insert(dl_buffer)); + enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_PATH_insert(dl_buffer)); return mission_insert(insert, &me); } -int mission_parse_PATH_LLA(void) { - if (DL_MISSION_PATH_LLA_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft +int mission_parse_PATH_LLA(void) +{ + if (DL_MISSION_PATH_LLA_ac_id(dl_buffer) != AC_ID) { return FALSE; } // not for this aircraft struct LlaCoor_i lla[MISSION_PATH_NB]; lla[0].lat = DL_MISSION_PATH_LLA_point_lat_1(dl_buffer); @@ -284,43 +296,45 @@ int mission_parse_PATH_LLA(void) { me.type = MissionPath; uint8_t i; me.element.mission_path.nb = DL_MISSION_PATH_LLA_nb(dl_buffer); - if (me.element.mission_path.nb > MISSION_PATH_NB) me.element.mission_path.nb = MISSION_PATH_NB; + if (me.element.mission_path.nb > MISSION_PATH_NB) { me.element.mission_path.nb = MISSION_PATH_NB; } for (i = 0; i < me.element.mission_path.nb; i++) { // if there is no valid local coordinate, do not insert mission element - if(!mission_point_of_lla(&me.element.mission_path.path.path_f[i], &lla[i])) return FALSE; + if (!mission_point_of_lla(&me.element.mission_path.path.path_f[i], &lla[i])) { return FALSE; } } me.element.mission_path.path_idx = 0; me.duration = DL_MISSION_PATH_LLA_duration(dl_buffer); - enum MissionInsertMode insert = (enum MissionInsertMode) (DL_MISSION_PATH_LLA_insert(dl_buffer)); + enum MissionInsertMode insert = (enum MissionInsertMode)(DL_MISSION_PATH_LLA_insert(dl_buffer)); return mission_insert(insert, &me); } -int mission_parse_GOTO_MISSION(void) { - if (DL_GOTO_MISSION_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft +int mission_parse_GOTO_MISSION(void) +{ + if (DL_GOTO_MISSION_ac_id(dl_buffer) != AC_ID) { return FALSE; } // not for this aircraft uint8_t mission_id = DL_GOTO_MISSION_mission_id(dl_buffer); if (mission_id < MISSION_ELEMENT_NB) { mission.current_idx = mission_id; - } - else return FALSE; + } else { return FALSE; } return TRUE; } -int mission_parse_NEXT_MISSION(void) { - if (DL_NEXT_MISSION_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft +int mission_parse_NEXT_MISSION(void) +{ + if (DL_NEXT_MISSION_ac_id(dl_buffer) != AC_ID) { return FALSE; } // not for this aircraft - if (mission.current_idx == mission.insert_idx) return FALSE; // already at the last position + if (mission.current_idx == mission.insert_idx) { return FALSE; } // already at the last position // increment current index mission.current_idx = (mission.current_idx + 1) % MISSION_ELEMENT_NB; return TRUE; } -int mission_parse_END_MISSION(void) { - if (DL_END_MISSION_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft +int mission_parse_END_MISSION(void) +{ + if (DL_END_MISSION_ac_id(dl_buffer) != AC_ID) { return FALSE; } // not for this aircraft // set current index to insert index (last position) mission.current_idx = mission.insert_idx; diff --git a/sw/airborne/modules/mission/mission_common.h b/sw/airborne/modules/mission/mission_common.h index 4b1cec706d..a9c41e3585 100644 --- a/sw/airborne/modules/mission/mission_common.h +++ b/sw/airborne/modules/mission/mission_common.h @@ -52,14 +52,14 @@ enum MissionInsertMode { }; struct _mission_wp { - union{ + union { struct EnuCoor_f wp_f; struct EnuCoor_i wp_i; } wp; }; struct _mission_circle { - union{ + union { struct EnuCoor_f center_f; struct EnuCoor_i center_i; } center; @@ -68,12 +68,12 @@ struct _mission_circle { }; struct _mission_segment { - union{ + union { struct EnuCoor_f from_f; struct EnuCoor_i from_i; } from; - union{ + union { struct EnuCoor_f to_f; struct EnuCoor_i to_i; } to; @@ -81,7 +81,7 @@ struct _mission_segment { #define MISSION_PATH_NB 5 struct _mission_path { - union{ + union { struct EnuCoor_f path_f[MISSION_PATH_NB]; struct EnuCoor_i path_i[MISSION_PATH_NB]; } path; @@ -127,18 +127,18 @@ extern void mission_init(void); * @param element mission element structure * @return return TRUE if insertion is succesful, FALSE otherwise */ -extern bool_t mission_insert(enum MissionInsertMode insert, struct _mission_element * element); +extern bool_t mission_insert(enum MissionInsertMode insert, struct _mission_element *element); /** Convert mission element's points format if needed * @param el pointer to the mission element * @return return TRUE if conversion is succesful, FALSE otherwise */ -extern bool_t mission_element_convert(struct _mission_element * el); +extern bool_t mission_element_convert(struct _mission_element *el); /** Get current mission element * @return return a pointer to the next mission element or NULL if no more elements */ -extern struct _mission_element * mission_get(void); +extern struct _mission_element *mission_get(void); /** Get the ENU component of LLA mission point * This function is firmware specific. diff --git a/sw/airborne/modules/mission/mission_fw_nav.c b/sw/airborne/modules/mission/mission_fw_nav.c index d697beb615..473aef46e2 100644 --- a/sw/airborne/modules/mission/mission_fw_nav.c +++ b/sw/airborne/modules/mission/mission_fw_nav.c @@ -35,7 +35,8 @@ #include "generated/flight_plan.h" /// Utility function: converts lla (int) to local point (float) -bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_i *lla) { +bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_i *lla) +{ /// TODO: don't convert to float, either use double or do completely in fixed point struct LlaCoor_f lla_f; LLA_FLOAT_OF_BFP(lla_f, *lla); @@ -59,7 +60,7 @@ bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_i *lla) { point->y = waypoints[WP_HOME].y + dy; point->z = lla->alt; - return TRUE; + return TRUE; } // navigation time step @@ -70,7 +71,8 @@ struct EnuCoor_f last_wp_f = { 0., 0., 0. }; /** Navigation function to a single waypoint */ -static inline bool_t mission_nav_wp(struct _mission_wp * wp) { +static inline bool_t mission_nav_wp(struct _mission_wp *wp) +{ if (nav_approaching_xy(wp->wp.wp_f.x, wp->wp.wp_f.y, last_wp_f.x, last_wp_f.y, CARROT)) { last_wp_f = wp->wp.wp_f; // store last wp return FALSE; // end of mission element @@ -84,7 +86,8 @@ static inline bool_t mission_nav_wp(struct _mission_wp * wp) { /** Navigation function on a circle */ -static inline bool_t mission_nav_circle(struct _mission_circle * circle) { +static inline bool_t mission_nav_circle(struct _mission_circle *circle) +{ nav_circle_XY(circle->center.center_f.x, circle->center.center_f.y, circle->radius); NavVerticalAutoThrottleMode(0.); NavVerticalAltitudeMode(circle->center.center_f.z, 0.); @@ -93,8 +96,10 @@ static inline bool_t mission_nav_circle(struct _mission_circle * circle) { /** Navigation function along a segment */ -static inline bool_t mission_nav_segment(struct _mission_segment * segment) { - if (nav_approaching_xy(segment->to.to_f.x, segment->to.to_f.y, segment->from.from_f.x, segment->from.from_f.y, CARROT)) { +static inline bool_t mission_nav_segment(struct _mission_segment *segment) +{ + if (nav_approaching_xy(segment->to.to_f.x, segment->to.to_f.y, segment->from.from_f.x, segment->from.from_f.y, + CARROT)) { last_wp_f = segment->to.to_f; return FALSE; // end of mission element } @@ -106,7 +111,8 @@ static inline bool_t mission_nav_segment(struct _mission_segment * segment) { /** Navigation function along a path */ -static inline bool_t mission_nav_path(struct _mission_path * path) { +static inline bool_t mission_nav_path(struct _mission_path *path) +{ if (path->nb == 0) { return FALSE; // nothing to do } @@ -116,13 +122,13 @@ static inline bool_t mission_nav_path(struct _mission_path * path) { wp.wp.wp_f = path->path.path_f[0]; return mission_nav_wp(&wp); } - if (path->path_idx == path->nb-1) { + if (path->path_idx == path->nb - 1) { last_wp_f = path->path.path_f[path->path_idx]; // store last wp return FALSE; // end of path } // normal case struct EnuCoor_f from_f = path->path.path_f[path->path_idx]; - struct EnuCoor_f to_f = path->path.path_f[path->path_idx+1]; + struct EnuCoor_f to_f = path->path.path_f[path->path_idx + 1]; nav_route_xy(from_f.x, from_f.y, to_f.x, to_f.y); NavVerticalAutoThrottleMode(0.); NavVerticalAltitudeMode(to_f.z, 0.); // both altitude should be the same anyway @@ -133,16 +139,17 @@ static inline bool_t mission_nav_path(struct _mission_path * path) { } -int mission_run() { +int mission_run() +{ // current element - struct _mission_element * el = NULL; + struct _mission_element *el = NULL; if ((el = mission_get()) == NULL) { // TODO do something special like a waiting circle before ending the mission ? return FALSE; // end of mission } bool_t el_running = FALSE; - switch (el->type){ + switch (el->type) { case MissionWP: el_running = mission_nav_wp(&(el->element.mission_wp)); break; diff --git a/sw/airborne/modules/mission/mission_rotorcraft_nav.c b/sw/airborne/modules/mission/mission_rotorcraft_nav.c index 44fadf73e4..6e624c618f 100644 --- a/sw/airborne/modules/mission/mission_rotorcraft_nav.c +++ b/sw/airborne/modules/mission/mission_rotorcraft_nav.c @@ -38,10 +38,12 @@ #define BUFFER_ZONE_DIST 10 /// Utility function: converts lla (int) to local point (float) -bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_i *lla) { +bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_i *lla) +{ // return FALSE if there is no valid local coordinate system - if (!state.ned_initialized_i) + if (!state.ned_initialized_i) { return FALSE; + } // change geoid alt to ellipsoid alt lla->alt = lla->alt - state.ned_origin_i.hmsl + state.ned_origin_i.lla.alt; @@ -72,7 +74,8 @@ bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_i *lla) { } //Function that converts target wp from float point versions to int -bool_t mission_element_convert(struct _mission_element * el) { +bool_t mission_element_convert(struct _mission_element *el) +{ struct _mission_element tmp_element = *el; uint8_t i = 0; @@ -109,17 +112,17 @@ struct EnuCoor_i last_mission_wp = { 0., 0., 0. }; /** Navigation function to a single waypoint */ -static inline bool_t mission_nav_wp(struct _mission_element * el) { - struct EnuCoor_i* target_wp = &(el->element.mission_wp.wp.wp_i); +static inline bool_t mission_nav_wp(struct _mission_element *el) +{ + struct EnuCoor_i *target_wp = &(el->element.mission_wp.wp.wp_i); //Check proximity and wait for 'duration' seconds in proximity circle if desired if (nav_approaching_from(target_wp, NULL, CARROT)) { last_mission_wp = *target_wp; if (el->duration > 0.) { - if (nav_check_wp_time(target_wp, el->duration)) return FALSE; - } - else return FALSE; + if (nav_check_wp_time(target_wp, el->duration)) { return FALSE; } + } else { return FALSE; } } //Go to Mission Waypoint @@ -133,8 +136,9 @@ static inline bool_t mission_nav_wp(struct _mission_element * el) { /** Navigation function on a circle */ -static inline bool_t mission_nav_circle(struct _mission_element * el) { - struct EnuCoor_i* center_wp = &(el->element.mission_circle.center.center_i); +static inline bool_t mission_nav_circle(struct _mission_element *el) +{ + struct EnuCoor_i *center_wp = &(el->element.mission_circle.center.center_i); int32_t radius = el->element.mission_circle.radius; //Draw the desired circle for a 'duration' time @@ -152,18 +156,18 @@ static inline bool_t mission_nav_circle(struct _mission_element * el) { /** Navigation function along a segment */ -static inline bool_t mission_nav_segment(struct _mission_element * el) { - struct EnuCoor_i* from_wp = &(el->element.mission_segment.from.from_i); - struct EnuCoor_i* to_wp = &(el->element.mission_segment.to.to_i); +static inline bool_t mission_nav_segment(struct _mission_element *el) +{ + struct EnuCoor_i *from_wp = &(el->element.mission_segment.from.from_i); + struct EnuCoor_i *to_wp = &(el->element.mission_segment.to.to_i); //Check proximity and wait for 'duration' seconds in proximity circle if desired if (nav_approaching_from(to_wp, from_wp, CARROT)) { last_mission_wp = *to_wp; if (el->duration > 0.) { - if (nav_check_wp_time(to_wp, el->duration)) return FALSE; - } - else return FALSE; + if (nav_check_wp_time(to_wp, el->duration)) { return FALSE; } + } else { return FALSE; } } //Route Between from-to @@ -178,45 +182,46 @@ static inline bool_t mission_nav_segment(struct _mission_element * el) { /** Navigation function along a path */ -static inline bool_t mission_nav_path(struct _mission_element * el) { +static inline bool_t mission_nav_path(struct _mission_element *el) +{ if (el->element.mission_path.nb == 0) { return FALSE; // nothing to do } if (el->element.mission_path.path_idx == 0) { //first wp of path el->element.mission_wp.wp.wp_i = el->element.mission_path.path.path_i[0]; - if (!mission_nav_wp(el)) el->element.mission_path.path_idx++; + if (!mission_nav_wp(el)) { el->element.mission_path.path_idx++; } } else if (el->element.mission_path.path_idx < el->element.mission_path.nb) { //standart wp of path - struct EnuCoor_i* from_wp = &(el->element.mission_path.path.path_i[(el->element.mission_path.path_idx) - 1]); - struct EnuCoor_i* to_wp = &(el->element.mission_path.path.path_i[el->element.mission_path.path_idx]); + struct EnuCoor_i *from_wp = &(el->element.mission_path.path.path_i[(el->element.mission_path.path_idx) - 1]); + struct EnuCoor_i *to_wp = &(el->element.mission_path.path.path_i[el->element.mission_path.path_idx]); //Check proximity and wait for t seconds in proximity circle if desired if (nav_approaching_from(to_wp, from_wp, CARROT)) { last_mission_wp = *to_wp; if (el->duration > 0.) { - if (nav_check_wp_time(to_wp, el->duration)) + if (nav_check_wp_time(to_wp, el->duration)) { el->element.mission_path.path_idx++; - } - else el->element.mission_path.path_idx++; + } + } else { el->element.mission_path.path_idx++; } } //Route Between from-to horizontal_mode = HORIZONTAL_MODE_ROUTE; nav_route(from_wp, to_wp); NavVerticalAutoThrottleMode(RadOfDeg(0.0)); NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(from_wp->z), 0.); - } - else return FALSE; //end of path + } else { return FALSE; } //end of path return TRUE; } -int mission_run() { +int mission_run() +{ // current element - struct _mission_element * el = NULL; + struct _mission_element *el = NULL; if ((el = mission_get()) == NULL) { mission.element_time = 0; mission.current_idx = 0; @@ -224,7 +229,7 @@ int mission_run() { } bool_t el_running = FALSE; - switch (el->type){ + switch (el->type) { case MissionWP: el_running = mission_nav_wp(el); break; diff --git a/sw/airborne/modules/multi/follow.c b/sw/airborne/modules/multi/follow.c index ec1d9e685e..0c07968e9a 100644 --- a/sw/airborne/modules/multi/follow.c +++ b/sw/airborne/modules/multi/follow.c @@ -47,11 +47,13 @@ #define FOLLOW_OFFSET_Z 0.0 #endif -void follow_init( void ) { +void follow_init(void) +{ } -void follow_change_wp( unsigned char* buffer ) { +void follow_change_wp(unsigned char *buffer) +{ struct EcefCoor_i new_pos; struct EnuCoor_i enu; new_pos.x = DL_REMOTE_GPS_ecef_x(buffer); diff --git a/sw/airborne/modules/multi/follow.h b/sw/airborne/modules/multi/follow.h index 92f3d76f26..01608d0f42 100644 --- a/sw/airborne/modules/multi/follow.h +++ b/sw/airborne/modules/multi/follow.h @@ -27,13 +27,13 @@ #ifndef FOLLOW_H #define FOLLOW_H -extern void follow_init( void ); -extern void follow_change_wp( unsigned char* buffer ); +extern void follow_init(void); +extern void follow_change_wp(unsigned char *buffer); #define ParseRemoteGps() { \ - if (DL_REMOTE_GPS_ac_id(dl_buffer) == FOLLOW_AC_ID) { \ - follow_change_wp(dl_buffer); \ - } \ -} + if (DL_REMOTE_GPS_ac_id(dl_buffer) == FOLLOW_AC_ID) { \ + follow_change_wp(dl_buffer); \ + } \ + } #endif // FOLLOW diff --git a/sw/airborne/modules/multi/formation.c b/sw/airborne/modules/multi/formation.c index c56c48b572..4a2640c4d3 100644 --- a/sw/airborne/modules/multi/formation.c +++ b/sw/airborne/modules/multi/formation.c @@ -63,9 +63,10 @@ struct slot_ formation[NB_ACS]; #define FORM_MODE 0 #endif -int formation_init(void) { +int formation_init(void) +{ int i; - for (i = 0; i < NB_ACS; ++i) formation[i].status = UNSET; + for (i = 0; i < NB_ACS; ++i) { formation[i].status = UNSET; } leader_id = 0; form_carrot = FORM_CARROT; @@ -80,9 +81,10 @@ int formation_init(void) { return FALSE; } -int add_slot(uint8_t _id, float slot_e, float slot_n, float slot_a) { - if (_id != AC_ID && the_acs_id[_id] == 0) return FALSE; // no info for this AC - DOWNLINK_SEND_FORMATION_SLOT_TM(DefaultChannel, DefaultDevice,&_id, &form_mode, &slot_e, &slot_n, &slot_a); +int add_slot(uint8_t _id, float slot_e, float slot_n, float slot_a) +{ + if (_id != AC_ID && the_acs_id[_id] == 0) { return FALSE; } // no info for this AC + DOWNLINK_SEND_FORMATION_SLOT_TM(DefaultChannel, DefaultDevice, &_id, &form_mode, &slot_e, &slot_n, &slot_a); formation[the_acs_id[_id]].status = IDLE; formation[the_acs_id[_id]].east = slot_e; formation[the_acs_id[_id]].north = slot_n; @@ -90,28 +92,30 @@ int add_slot(uint8_t _id, float slot_e, float slot_n, float slot_a) { return FALSE; } -int start_formation(void) { +int start_formation(void) +{ uint8_t i; uint8_t ac_id = AC_ID; for (i = 0; i < NB_ACS; ++i) { - if (formation[i].status == IDLE) formation[i].status = ACTIVE; + if (formation[i].status == IDLE) { formation[i].status = ACTIVE; } } enum slot_status active = ACTIVE; - DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel, DefaultDevice,&ac_id,&leader_id,&active); + DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel, DefaultDevice, &ac_id, &leader_id, &active); // store current cruise and alt old_cruise = v_ctl_auto_throttle_cruise_throttle; old_alt = nav_altitude; return FALSE; } -int stop_formation(void) { +int stop_formation(void) +{ uint8_t i; uint8_t ac_id = AC_ID; for (i = 0; i < NB_ACS; ++i) { - if (formation[i].status == ACTIVE) formation[i].status = IDLE; + if (formation[i].status == ACTIVE) { formation[i].status = IDLE; } } enum slot_status idle = IDLE; - DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel, DefaultDevice,&ac_id,&leader_id,&idle); + DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel, DefaultDevice, &ac_id, &leader_id, &idle); // restore cruise and alt v_ctl_auto_throttle_cruise_throttle = old_cruise; old_cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE; @@ -121,7 +125,8 @@ int stop_formation(void) { } -int formation_flight(void) { +int formation_flight(void) +{ static uint8_t _1Hz = 0; uint8_t nb = 0, i; @@ -139,25 +144,26 @@ int formation_flight(void) { stateGetPositionEnu_f()->y += formation[the_acs_id[AC_ID]].north; } // set info for this AC - SetAcInfo(AC_ID, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, (*stateGetHorizontalSpeedDir_f()), stateGetPositionUtm_f()->alt, (*stateGetHorizontalSpeedNorm_f()), stateGetSpeedEnu_f()->z, gps.tow); + SetAcInfo(AC_ID, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, (*stateGetHorizontalSpeedDir_f()), + stateGetPositionUtm_f()->alt, (*stateGetHorizontalSpeedNorm_f()), stateGetSpeedEnu_f()->z, gps.tow); // broadcast info uint8_t ac_id = AC_ID; enum slot_status status = formation[the_acs_id[AC_ID]].status; - DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel, DefaultDevice,&ac_id,&leader_id,&status); - if (++_1Hz>=4) { - _1Hz=0; - DOWNLINK_SEND_FORMATION_SLOT_TM(DefaultChannel, DefaultDevice,&ac_id, &form_mode, - &formation[the_acs_id[AC_ID]].east, - &formation[the_acs_id[AC_ID]].north, - &formation[the_acs_id[AC_ID]].alt); + DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel, DefaultDevice, &ac_id, &leader_id, &status); + if (++_1Hz >= 4) { + _1Hz = 0; + DOWNLINK_SEND_FORMATION_SLOT_TM(DefaultChannel, DefaultDevice, &ac_id, &form_mode, + &formation[the_acs_id[AC_ID]].east, + &formation[the_acs_id[AC_ID]].north, + &formation[the_acs_id[AC_ID]].alt); } - if (formation[the_acs_id[AC_ID]].status != ACTIVE) return FALSE; // AC not ready + if (formation[the_acs_id[AC_ID]].status != ACTIVE) { return FALSE; } // AC not ready // get leader info struct ac_info_ * leader = get_ac_info(leader_id); if (formation[the_acs_id[leader_id]].status == UNSET || - formation[the_acs_id[leader_id]].status == IDLE) return FALSE; // leader not ready or not in formation + formation[the_acs_id[leader_id]].status == IDLE) { return FALSE; } // leader not ready or not in formation // compute slots in the right reference frame struct slot_ form[NB_ACS]; @@ -167,29 +173,28 @@ int formation_flight(void) { sr = sinf(leader->course); } for (i = 0; i < NB_ACS; ++i) { - if (formation[i].status == UNSET) continue; - form[i].east = formation[i].east*sr - formation[i].north*cr; - form[i].north = formation[i].east*cr + formation[i].north*sr; + if (formation[i].status == UNSET) { continue; } + form[i].east = formation[i].east * sr - formation[i].north * cr; + form[i].north = formation[i].east * cr + formation[i].north * sr; form[i].alt = formation[i].alt; } // compute control forces for (i = 0; i < NB_ACS; ++i) { - if (the_acs[i].ac_id == AC_ID) continue; + if (the_acs[i].ac_id == AC_ID) { continue; } struct ac_info_ * ac = get_ac_info(the_acs[i].ac_id); float delta_t = Max((int)(gps.tow - ac->itow) / 1000., 0.); if (delta_t > FORM_CARROT) { // if AC not responding for too long formation[i].status = LOST; continue; - } - else formation[i].status = ACTIVE; + } else { formation[i].status = ACTIVE; } // compute control if AC is ACTIVE and around the same altitude (maybe not so usefull) if (formation[i].status == ACTIVE && fabs(stateGetPositionUtm_f()->alt - ac->alt) < form_prox && ac->alt > 0) { - form_e += (ac->east + ac->gspeed*sinf(ac->course)*delta_t - stateGetPositionEnu_f()->x) - - (form[i].east - form[the_acs_id[AC_ID]].east); - form_n += (ac->north + ac->gspeed*cosf(ac->course)*delta_t - stateGetPositionEnu_f()->y) - - (form[i].north - form[the_acs_id[AC_ID]].north); + form_e += (ac->east + ac->gspeed * sinf(ac->course) * delta_t - stateGetPositionEnu_f()->x) + - (form[i].east - form[the_acs_id[AC_ID]].east); + form_n += (ac->north + ac->gspeed * cosf(ac->course) * delta_t - stateGetPositionEnu_f()->y) + - (form[i].north - form[the_acs_id[AC_ID]].north); form_a += (ac->alt - stateGetPositionUtm_f()->alt) - (formation[i].alt - formation[the_acs_id[AC_ID]].alt); form_speed += ac->gspeed; //form_speed_e += ac->gspeed * sinf(ac->course); @@ -197,11 +202,11 @@ int formation_flight(void) { ++nb; } } - uint8_t _nb = Max(1,nb); + uint8_t _nb = Max(1, nb); form_n /= _nb; form_e /= _nb; form_a /= _nb; - form_speed = form_speed / (nb+1) - (*stateGetHorizontalSpeedNorm_f()); + form_speed = form_speed / (nb + 1) - (*stateGetHorizontalSpeedNorm_f()); //form_speed_e = form_speed_e / (nb+1) - (*stateGetHorizontalSpeedNorm_f()) * sh; //form_speed_n = form_speed_n / (nb+1) - (*stateGetHorizontalSpeedNorm_f()) * ch; @@ -210,10 +215,10 @@ int formation_flight(void) { // altitude loop float alt = 0.; - if (AC_ID == leader_id) alt = nav_altitude; - else alt = leader->alt - form[the_acs_id[leader_id]].alt; + if (AC_ID == leader_id) { alt = nav_altitude; } + else { alt = leader->alt - form[the_acs_id[leader_id]].alt; } alt += formation[the_acs_id[AC_ID]].alt + coef_form_alt * form_a; - flight_altitude = Max(alt, ground_alt+SECURITY_HEIGHT); + flight_altitude = Max(alt, ground_alt + SECURITY_HEIGHT); // carrot if (AC_ID != leader_id) { @@ -244,7 +249,8 @@ int formation_flight(void) { return TRUE; } -void formation_pre_call(void) { +void formation_pre_call(void) +{ if (leader_id == AC_ID) { stateGetPositionEnu_f()->x -= formation[the_acs_id[AC_ID]].east; stateGetPositionEnu_f()->y -= formation[the_acs_id[AC_ID]].north; diff --git a/sw/airborne/modules/multi/formation.h b/sw/airborne/modules/multi/formation.h index 9112b10cf0..c64c8aa680 100644 --- a/sw/airborne/modules/multi/formation.h +++ b/sw/airborne/modules/multi/formation.h @@ -13,7 +13,7 @@ #define FORM_MODE_GLOBAL 0 #define FORM_MODE_COURSE 1 -extern float coef_form_alt,coef_form_pos,coef_form_speed,coef_form_course; +extern float coef_form_alt, coef_form_pos, coef_form_speed, coef_form_course; extern float form_prox; extern int form_mode; extern uint8_t leader_id; @@ -21,10 +21,10 @@ extern uint8_t leader_id; enum slot_status {UNSET, ACTIVE, IDLE, LOST}; struct slot_ { - enum slot_status status; - float east; - float north; - float alt; + enum slot_status status; + float east; + float north; + float alt; }; extern struct slot_ formation[NB_ACS]; @@ -34,31 +34,31 @@ extern int formation_init(void); extern int add_slot(uint8_t _id, float slot_e, float slot_n, float slot_a); #define UpdateSlot(_id, _se, _sn, _sa) { \ - formation[the_acs_id[_id]].east = _se; \ - formation[the_acs_id[_id]].north = _sn; \ - formation[the_acs_id[_id]].alt = _sa; \ -} + formation[the_acs_id[_id]].east = _se; \ + formation[the_acs_id[_id]].north = _sn; \ + formation[the_acs_id[_id]].alt = _sa; \ + } #define UpdateFormationStatus(_id,_status) { formation[the_acs_id[_id]].status = _status; } #define ParseFormationStatus() { \ - uint8_t ac_id = DL_FORMATION_STATUS_ac_id(dl_buffer); \ - uint8_t leader = DL_FORMATION_STATUS_leader_id(dl_buffer); \ - uint8_t status = DL_FORMATION_STATUS_status(dl_buffer); \ - if (ac_id == AC_ID) leader_id = leader; \ - else if (leader == leader_id) { UpdateFormationStatus(ac_id,status); } \ - else { UpdateFormationStatus(ac_id,UNSET); } \ -} + uint8_t ac_id = DL_FORMATION_STATUS_ac_id(dl_buffer); \ + uint8_t leader = DL_FORMATION_STATUS_leader_id(dl_buffer); \ + uint8_t status = DL_FORMATION_STATUS_status(dl_buffer); \ + if (ac_id == AC_ID) leader_id = leader; \ + else if (leader == leader_id) { UpdateFormationStatus(ac_id,status); } \ + else { UpdateFormationStatus(ac_id,UNSET); } \ + } #define ParseFormationSlot() { \ - uint8_t ac_id = DL_FORMATION_SLOT_ac_id(dl_buffer); \ - uint8_t mode = DL_FORMATION_SLOT_mode(dl_buffer); \ - float slot_east = DL_FORMATION_SLOT_slot_east(dl_buffer); \ - float slot_north = DL_FORMATION_SLOT_slot_north(dl_buffer); \ - float slot_alt = DL_FORMATION_SLOT_slot_alt(dl_buffer); \ - UpdateSlot(ac_id, slot_east, slot_north, slot_alt); \ - if (ac_id == leader_id) form_mode = mode; \ -} + uint8_t ac_id = DL_FORMATION_SLOT_ac_id(dl_buffer); \ + uint8_t mode = DL_FORMATION_SLOT_mode(dl_buffer); \ + float slot_east = DL_FORMATION_SLOT_slot_east(dl_buffer); \ + float slot_north = DL_FORMATION_SLOT_slot_north(dl_buffer); \ + float slot_alt = DL_FORMATION_SLOT_slot_alt(dl_buffer); \ + UpdateSlot(ac_id, slot_east, slot_north, slot_alt); \ + if (ac_id == leader_id) form_mode = mode; \ + } extern int start_formation(void); diff --git a/sw/airborne/modules/multi/potential.c b/sw/airborne/modules/multi/potential.c index f1756b4d90..f6c7b835bf 100644 --- a/sw/airborne/modules/multi/potential.c +++ b/sw/airborne/modules/multi/potential.c @@ -40,7 +40,8 @@ float force_climb_gain; #define FORCE_MAX_DIST 100. #endif -void potential_init(void) { +void potential_init(void) +{ potential_force.east = 0.; potential_force.north = 0.; @@ -52,7 +53,8 @@ void potential_init(void) { } -int potential_task(void) { +int potential_task(void) +{ uint8_t i; @@ -65,27 +67,27 @@ int potential_task(void) { // compute control forces int8_t nb = 0; for (i = 0; i < NB_ACS; ++i) { - if (the_acs[i].ac_id == AC_ID) continue; + if (the_acs[i].ac_id == AC_ID) { continue; } struct ac_info_ * ac = get_ac_info(the_acs[i].ac_id); float delta_t = Max((int)(gps.tow - ac->itow) / 1000., 0.); // if AC not responding for too long, continue, else compute force - if (delta_t > CARROT) continue; + if (delta_t > CARROT) { continue; } else { float sha = sinf(ac->course); float cha = cosf(ac->course); - float de = ac->east + sha*delta_t - stateGetPositionEnu_f()->x; - if (de > FORCE_MAX_DIST || de < -FORCE_MAX_DIST) continue; - float dn = ac->north + cha*delta_t - stateGetPositionEnu_f()->y; - if (dn > FORCE_MAX_DIST || dn < -FORCE_MAX_DIST) continue; - float da = ac->alt + ac->climb*delta_t - stateGetPositionUtm_f()->alt; - if (da > FORCE_MAX_DIST || da < -FORCE_MAX_DIST) continue; - float dist = sqrtf(de*de + dn*dn + da*da); - if (dist == 0.) continue; + float de = ac->east + sha * delta_t - stateGetPositionEnu_f()->x; + if (de > FORCE_MAX_DIST || de < -FORCE_MAX_DIST) { continue; } + float dn = ac->north + cha * delta_t - stateGetPositionEnu_f()->y; + if (dn > FORCE_MAX_DIST || dn < -FORCE_MAX_DIST) { continue; } + float da = ac->alt + ac->climb * delta_t - stateGetPositionUtm_f()->alt; + if (da > FORCE_MAX_DIST || da < -FORCE_MAX_DIST) { continue; } + float dist = sqrtf(de * de + dn * dn + da * da); + if (dist == 0.) { continue; } float dve = (*stateGetHorizontalSpeedNorm_f()) * sh - ac->gspeed * sha; float dvn = (*stateGetHorizontalSpeedNorm_f()) * ch - ac->gspeed * cha; float dva = stateGetSpeedEnu_f()->z - the_acs[i].climb; - float scal = dve*de + dvn*dn + dva*da; - if (scal < 0.) continue; // No risk of collision + float scal = dve * de + dvn * dn + dva * da; + if (scal < 0.) { continue; } // No risk of collision float d3 = dist * dist * dist; potential_force.east += scal * de / d3; potential_force.north += scal * dn / d3; @@ -93,7 +95,7 @@ int potential_task(void) { ++nb; } } - if (nb == 0) return TRUE; + if (nb == 0) { return TRUE; } //potential_force.east /= nb; //potential_force.north /= nb; //potential_force.alt /= nb; @@ -121,7 +123,8 @@ int potential_task(void) { BoundAbs(potential_force.climb, V_CTL_ALTITUDE_MAX_CLIMB); NavVerticalClimbMode(potential_force.climb); - DOWNLINK_SEND_POTENTIAL(DefaultChannel, DefaultDevice,&potential_force.east,&potential_force.north,&potential_force.alt,&potential_force.speed,&potential_force.climb); + DOWNLINK_SEND_POTENTIAL(DefaultChannel, DefaultDevice, &potential_force.east, &potential_force.north, + &potential_force.alt, &potential_force.speed, &potential_force.climb); return TRUE; } diff --git a/sw/airborne/modules/multi/tcas.c b/sw/airborne/modules/multi/tcas.c index e8f13f5cdf..14e908837d 100644 --- a/sw/airborne/modules/multi/tcas.c +++ b/sw/airborne/modules/multi/tcas.c @@ -68,7 +68,8 @@ struct tcas_ac_status tcas_acs_status[NB_ACS]; /* AC is inside the horizontol dmod area and twice the vertical alim separation */ #define TCAS_IsInside() ( (ddh < Square(tcas_dmod) && ddv < Square(2*tcas_alim)) ? 1 : 0 ) -void tcas_init( void ) { +void tcas_init(void) +{ tcas_alt_setpoint = GROUND_ALT + SECURITY_HEIGHT; tcas_tau_ta = TCAS_TAU_TA; tcas_tau_ra = TCAS_TAU_RA; @@ -84,25 +85,26 @@ void tcas_init( void ) { } } -static inline enum tcas_resolve tcas_test_direction(uint8_t id) { +static inline enum tcas_resolve tcas_test_direction(uint8_t id) +{ struct ac_info_ * ac = get_ac_info(id); float dz = ac->alt - stateGetPositionUtm_f()->alt; - if (dz > tcas_alim/2) return RA_DESCEND; - else if (dz < -tcas_alim/2) return RA_CLIMB; - else // AC with the smallest ID descend - { - if (AC_ID < id) return RA_DESCEND; - else return RA_CLIMB; + if (dz > tcas_alim / 2) { return RA_DESCEND; } + else if (dz < -tcas_alim / 2) { return RA_CLIMB; } + else { // AC with the smallest ID descend + if (AC_ID < id) { return RA_DESCEND; } + else { return RA_CLIMB; } } } /* conflicts detection and monitoring */ -void tcas_periodic_task_1Hz( void ) { +void tcas_periodic_task_1Hz(void) +{ // no TCAS under security_height if (stateGetPositionUtm_f()->alt < GROUND_ALT + SECURITY_HEIGHT) { uint8_t i; - for (i = 0; i < NB_ACS; i++) tcas_acs_status[i].status = TCAS_NO_ALARM; + for (i = 0; i < NB_ACS; i++) { tcas_acs_status[i].status = TCAS_NO_ALARM; } return; } // test possible conflicts @@ -112,24 +114,24 @@ void tcas_periodic_task_1Hz( void ) { float vx = (*stateGetHorizontalSpeedNorm_f()) * sinf((*stateGetHorizontalSpeedDir_f())); float vy = (*stateGetHorizontalSpeedNorm_f()) * cosf((*stateGetHorizontalSpeedDir_f())); for (i = 2; i < NB_ACS; i++) { - if (the_acs[i].ac_id == 0) continue; // no AC data + if (the_acs[i].ac_id == 0) { continue; } // no AC data uint32_t dt = gps.tow - the_acs[i].itow; - if (dt > 3*TCAS_DT_MAX) { + if (dt > 3 * TCAS_DT_MAX) { tcas_acs_status[i].status = TCAS_NO_ALARM; // timeout, reset status continue; } - if (dt > TCAS_DT_MAX) continue; // lost com but keep current status + if (dt > TCAS_DT_MAX) { continue; } // lost com but keep current status float dx = the_acs[i].east - stateGetPositionEnu_f()->x; float dy = the_acs[i].north - stateGetPositionEnu_f()->y; float dz = the_acs[i].alt - stateGetPositionUtm_f()->alt; float dvx = vx - the_acs[i].gspeed * sinf(the_acs[i].course); float dvy = vy - the_acs[i].gspeed * cosf(the_acs[i].course); float dvz = stateGetSpeedEnu_f()->z - the_acs[i].climb; - float scal = dvx*dx + dvy*dy + dvz*dz; - float ddh = dx*dx + dy*dy; - float ddv = dz*dz; + float scal = dvx * dx + dvy * dy + dvz * dz; + float ddh = dx * dx + dy * dy; + float ddv = dz * dz; float tau = TCAS_HUGE_TAU; - if (scal > 0.) tau = (ddh + ddv) / scal; + if (scal > 0.) { tau = (ddh + ddv) / scal; } // monitor conflicts uint8_t inside = TCAS_IsInside(); //enum tcas_resolve test_dir = RA_NONE; @@ -138,7 +140,7 @@ void tcas_periodic_task_1Hz( void ) { if (tau >= TCAS_HUGE_TAU && !inside) { tcas_acs_status[i].status = TCAS_NO_ALARM; // conflict is now resolved tcas_acs_status[i].resolve = RA_NONE; - DOWNLINK_SEND_TCAS_RESOLVED(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id)); + DOWNLINK_SEND_TCAS_RESOLVED(DefaultChannel, DefaultDevice, &(the_acs[i].ac_id)); } break; case TCAS_TA: @@ -149,16 +151,17 @@ void tcas_periodic_task_1Hz( void ) { //DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id),&test_dir);// FIXME only one closest AC ??? break; } - if (tau > tcas_tau_ta && !inside) - tcas_acs_status[i].status = TCAS_NO_ALARM; // conflict is now resolved - tcas_acs_status[i].resolve = RA_NONE; - DOWNLINK_SEND_TCAS_RESOLVED(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id)); + if (tau > tcas_tau_ta && !inside) { + tcas_acs_status[i].status = TCAS_NO_ALARM; // conflict is now resolved + } + tcas_acs_status[i].resolve = RA_NONE; + DOWNLINK_SEND_TCAS_RESOLVED(DefaultChannel, DefaultDevice, &(the_acs[i].ac_id)); break; case TCAS_NO_ALARM: if (tau < tcas_tau_ta || inside) { tcas_acs_status[i].status = TCAS_TA; // NO_ALARM -> TA // Downlink warning - DOWNLINK_SEND_TCAS_TA(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id)); + DOWNLINK_SEND_TCAS_TA(DefaultChannel, DefaultDevice, &(the_acs[i].ac_id)); } if (tau < tcas_tau_ra || inside) { tcas_acs_status[i].status = TCAS_RA; // NO_ALARM -> RA = big problem ? @@ -187,35 +190,33 @@ void tcas_periodic_task_1Hz( void ) { uint8_t ac_resolve = tcas_acs_status[the_acs_id[tcas_ac_RA]].resolve; if (ac_resolve != RA_NONE) { // first resolution, no message received if (ac_resolve == tcas_resolve) { // same direction, lowest id go down - if (AC_ID < tcas_ac_RA) tcas_resolve = RA_DESCEND; - else tcas_resolve = RA_CLIMB; + if (AC_ID < tcas_ac_RA) { tcas_resolve = RA_DESCEND; } + else { tcas_resolve = RA_CLIMB; } } tcas_acs_status[the_acs_id[tcas_ac_RA]].resolve = RA_LEVEL; // assuming level flight for now - } - else { // second resolution or message received + } else { // second resolution or message received if (ac_resolve != RA_LEVEL) { // message received if (ac_resolve == tcas_resolve) { // same direction, lowest id go down - if (AC_ID < tcas_ac_RA) tcas_resolve = RA_DESCEND; - else tcas_resolve = RA_CLIMB; + if (AC_ID < tcas_ac_RA) { tcas_resolve = RA_DESCEND; } + else { tcas_resolve = RA_CLIMB; } } - } - else { // no message - if (tcas_resolve == RA_CLIMB && the_acs[the_acs_id[tcas_ac_RA]].climb > 1.0) tcas_resolve = RA_DESCEND; // revert resolve - else if (tcas_resolve == RA_DESCEND && the_acs[the_acs_id[tcas_ac_RA]].climb < -1.0) tcas_resolve = RA_CLIMB; // revert resolve + } else { // no message + if (tcas_resolve == RA_CLIMB && the_acs[the_acs_id[tcas_ac_RA]].climb > 1.0) { tcas_resolve = RA_DESCEND; } // revert resolve + else if (tcas_resolve == RA_DESCEND && the_acs[the_acs_id[tcas_ac_RA]].climb < -1.0) { tcas_resolve = RA_CLIMB; } // revert resolve } } // Downlink alert - DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice,&tcas_ac_RA,&tcas_resolve); - } - else tcas_ac_RA = AC_ID; // no conflict + DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice, &tcas_ac_RA, &tcas_resolve); + } else { tcas_ac_RA = AC_ID; } // no conflict #ifdef TCAS_DEBUG - if (tcas_status == TCAS_RA) DOWNLINK_SEND_TCAS_DEBUG(DefaultChannel, DefaultDevice,&ac_id_close,&tau_min); + if (tcas_status == TCAS_RA) { DOWNLINK_SEND_TCAS_DEBUG(DefaultChannel, DefaultDevice, &ac_id_close, &tau_min); } #endif } /* altitude control loop */ -void tcas_periodic_task_4Hz( void ) { +void tcas_periodic_task_4Hz(void) +{ // set alt setpoint if (stateGetPositionUtm_f()->alt > GROUND_ALT + SECURITY_HEIGHT && tcas_status == TCAS_RA) { struct ac_info_ * ac = get_ac_info(tcas_ac_RA); @@ -233,8 +234,7 @@ void tcas_periodic_task_4Hz( void ) { } // Bound alt tcas_alt_setpoint = Max(GROUND_ALT + SECURITY_HEIGHT, tcas_alt_setpoint); - } - else { + } else { tcas_alt_setpoint = nav_altitude; tcas_resolve = RA_NONE; } diff --git a/sw/airborne/modules/multi/tcas.h b/sw/airborne/modules/multi/tcas.h index 2695810ef6..411b23a920 100644 --- a/sw/airborne/modules/multi/tcas.h +++ b/sw/airborne/modules/multi/tcas.h @@ -50,17 +50,17 @@ struct tcas_ac_status { extern struct tcas_ac_status tcas_acs_status[NB_ACS]; -extern void tcas_init( void ); -extern void tcas_periodic_task_1Hz( void ); -extern void tcas_periodic_task_4Hz( void ); +extern void tcas_init(void); +extern void tcas_periodic_task_1Hz(void); +extern void tcas_periodic_task_4Hz(void); #define CallTCAS() { if (tcas_status == TCAS_RA) v_ctl_altitude_setpoint = tcas_alt_setpoint; } #define ParseTcasResolve() { \ - if (DL_TCAS_RESOLVE_ac_id(dl_buffer) == AC_ID) { \ - uint8_t ac_id_conflict = DL_TCAS_RESOLVE_ac_id_conflict(dl_buffer); \ - tcas_acs_status[the_acs_id[ac_id_conflict]].resolve = DL_TCAS_RESOLVE_resolve(dl_buffer); \ - } \ -} + if (DL_TCAS_RESOLVE_ac_id(dl_buffer) == AC_ID) { \ + uint8_t ac_id_conflict = DL_TCAS_RESOLVE_ac_id_conflict(dl_buffer); \ + tcas_acs_status[the_acs_id[ac_id_conflict]].resolve = DL_TCAS_RESOLVE_resolve(dl_buffer); \ + } \ + } #endif // TCAS diff --git a/sw/airborne/modules/nav/nav_bungee_takeoff.c b/sw/airborne/modules/nav/nav_bungee_takeoff.c index 4f39f62d28..6d0ff21fa7 100644 --- a/sw/airborne/modules/nav/nav_bungee_takeoff.c +++ b/sw/airborne/modules/nav/nav_bungee_takeoff.c @@ -89,43 +89,46 @@ bool_t nav_bungee_takeoff_setup(uint8_t BungeeWP) TDistance = fabs(Takeoff_Distance); //Translate initial position so that the position of the bungee is (0,0) - float Currentx = initialx-(WaypointX(BungeeWaypoint)); - float Currenty = initialy-(WaypointY(BungeeWaypoint)); + float Currentx = initialx - (WaypointX(BungeeWaypoint)); + float Currenty = initialy - (WaypointY(BungeeWaypoint)); //Record bungee alt (which should be the ground alt at that point) BungeeAlt = waypoints[BungeeWaypoint].a; //Find Launch line slope and Throttle line slope - float MLaunch = Currenty/Currentx; + float MLaunch = Currenty / Currentx; //Find Throttle Point (the point where the throttle line and launch line intersect) - if(Currentx < 0) - throttlePx = TDistance/sqrt(MLaunch*MLaunch+1); - else - throttlePx = -(TDistance/sqrt(MLaunch*MLaunch+1)); + if (Currentx < 0) { + throttlePx = TDistance / sqrt(MLaunch * MLaunch + 1); + } else { + throttlePx = -(TDistance / sqrt(MLaunch * MLaunch + 1)); + } - if(Currenty < 0) - throttlePy = sqrt((TDistance*TDistance)-(throttlePx*throttlePx)); - else - throttlePy = -sqrt((TDistance*TDistance)-(throttlePx*throttlePx)); + if (Currenty < 0) { + throttlePy = sqrt((TDistance * TDistance) - (throttlePx * throttlePx)); + } else { + throttlePy = -sqrt((TDistance * TDistance) - (throttlePx * throttlePx)); + } //Find ThrottleLine - ThrottleSlope = tan(atan2(Currenty,Currentx)+(3.14/2)); - ThrottleB = (throttlePy - (ThrottleSlope*throttlePx)); + ThrottleSlope = tan(atan2(Currenty, Currentx) + (3.14 / 2)); + ThrottleB = (throttlePy - (ThrottleSlope * throttlePx)); //Determine whether the UAV is below or above the throttle line - if(Currenty > ((ThrottleSlope*Currentx)+ThrottleB)) + if (Currenty > ((ThrottleSlope * Currentx) + ThrottleB)) { AboveLine = TRUE; - else + } else { AboveLine = FALSE; + } //Enable Launch Status and turn kill throttle on CTakeoffStatus = Launch; kill_throttle = 1; //Translate the throttle point back - throttlePx = throttlePx+(WaypointX(BungeeWP)); - throttlePy = throttlePy+(WaypointY(BungeeWP)); + throttlePx = throttlePx + (WaypointX(BungeeWP)); + throttlePy = throttlePy + (WaypointY(BungeeWP)); return FALSE; } @@ -133,93 +136,92 @@ bool_t nav_bungee_takeoff_setup(uint8_t BungeeWP) bool_t nav_bungee_takeoff_run(void) { //Translate current position so Throttle point is (0,0) - float Currentx = stateGetPositionEnu_f()->x-throttlePx; - float Currenty = stateGetPositionEnu_f()->y-throttlePy; + float Currentx = stateGetPositionEnu_f()->x - throttlePx; + float Currenty = stateGetPositionEnu_f()->y - throttlePy; bool_t CurrentAboveLine; float ThrottleB; - switch(CTakeoffStatus) - { - case Launch: - //Follow Launch Line - NavVerticalAutoThrottleMode(0); - NavVerticalAltitudeMode(BungeeAlt+Takeoff_Height, 0.); - nav_route_xy(initialx,initialy,throttlePx,throttlePy); + switch (CTakeoffStatus) { + case Launch: + //Follow Launch Line + NavVerticalAutoThrottleMode(0); + NavVerticalAltitudeMode(BungeeAlt + Takeoff_Height, 0.); + nav_route_xy(initialx, initialy, throttlePx, throttlePy); - kill_throttle = 1; + kill_throttle = 1; - //recalculate lines if below min speed - if((*stateGetHorizontalSpeedNorm_f()) < Takeoff_MinSpeed) - { - initialx = stateGetPositionEnu_f()->x; - initialy = stateGetPositionEnu_f()->y; + //recalculate lines if below min speed + if ((*stateGetHorizontalSpeedNorm_f()) < Takeoff_MinSpeed) { + initialx = stateGetPositionEnu_f()->x; + initialy = stateGetPositionEnu_f()->y; - //Translate initial position so that the position of the bungee is (0,0) - Currentx = initialx-(WaypointX(BungeeWaypoint)); - Currenty = initialy-(WaypointY(BungeeWaypoint)); + //Translate initial position so that the position of the bungee is (0,0) + Currentx = initialx - (WaypointX(BungeeWaypoint)); + Currenty = initialy - (WaypointY(BungeeWaypoint)); - //Find Launch line slope - float MLaunch = Currenty/Currentx; + //Find Launch line slope + float MLaunch = Currenty / Currentx; - //Find Throttle Point (the point where the throttle line and launch line intersect) - if(Currentx < 0) - throttlePx = TDistance/sqrt(MLaunch*MLaunch+1); - else - throttlePx = -(TDistance/sqrt(MLaunch*MLaunch+1)); + //Find Throttle Point (the point where the throttle line and launch line intersect) + if (Currentx < 0) { + throttlePx = TDistance / sqrt(MLaunch * MLaunch + 1); + } else { + throttlePx = -(TDistance / sqrt(MLaunch * MLaunch + 1)); + } - if(Currenty < 0) - throttlePy = sqrt((TDistance*TDistance)-(throttlePx*throttlePx)); - else - throttlePy = -sqrt((TDistance*TDistance)-(throttlePx*throttlePx)); + if (Currenty < 0) { + throttlePy = sqrt((TDistance * TDistance) - (throttlePx * throttlePx)); + } else { + throttlePy = -sqrt((TDistance * TDistance) - (throttlePx * throttlePx)); + } - //Find ThrottleLine - ThrottleSlope = tan(atan2(Currenty,Currentx)+(3.14/2)); - ThrottleB = (throttlePy - (ThrottleSlope*throttlePx)); + //Find ThrottleLine + ThrottleSlope = tan(atan2(Currenty, Currentx) + (3.14 / 2)); + ThrottleB = (throttlePy - (ThrottleSlope * throttlePx)); - //Determine whether the UAV is below or above the throttle line - if(Currenty > ((ThrottleSlope*Currentx)+ThrottleB)) - AboveLine = TRUE; - else - AboveLine = FALSE; + //Determine whether the UAV is below or above the throttle line + if (Currenty > ((ThrottleSlope * Currentx) + ThrottleB)) { + AboveLine = TRUE; + } else { + AboveLine = FALSE; + } - //Translate the throttle point back - throttlePx = throttlePx+(WaypointX(BungeeWaypoint)); - throttlePy = throttlePy+(WaypointY(BungeeWaypoint)); - } + //Translate the throttle point back + throttlePx = throttlePx + (WaypointX(BungeeWaypoint)); + throttlePy = throttlePy + (WaypointY(BungeeWaypoint)); + } - //Find out if the UAV is currently above the line - if(Currenty > (ThrottleSlope*Currentx)) - CurrentAboveLine = TRUE; - else - CurrentAboveLine = FALSE; + //Find out if the UAV is currently above the line + if (Currenty > (ThrottleSlope * Currentx)) { + CurrentAboveLine = TRUE; + } else { + CurrentAboveLine = FALSE; + } - //Find out if UAV has crossed the line - if(AboveLine != CurrentAboveLine && (*stateGetHorizontalSpeedNorm_f()) > Takeoff_MinSpeed) - { - CTakeoffStatus = Throttle; + //Find out if UAV has crossed the line + if (AboveLine != CurrentAboveLine && (*stateGetHorizontalSpeedNorm_f()) > Takeoff_MinSpeed) { + CTakeoffStatus = Throttle; + kill_throttle = 0; + nav_init_stage(); + } + break; + case Throttle: + //Follow Launch Line + NavVerticalAutoThrottleMode(AGR_CLIMB_PITCH); + NavVerticalThrottleMode(9600 * (1)); + nav_route_xy(initialx, initialy, throttlePx, throttlePy); kill_throttle = 0; - nav_init_stage(); - } - break; - case Throttle: - //Follow Launch Line - NavVerticalAutoThrottleMode(AGR_CLIMB_PITCH); - NavVerticalThrottleMode(9600*(1)); - nav_route_xy(initialx,initialy,throttlePx,throttlePy); - kill_throttle = 0; - if((stateGetPositionUtm_f()->alt > BungeeAlt+Takeoff_Height-10) && ((*stateGetHorizontalSpeedNorm_f()) > Takeoff_Speed)) - { - CTakeoffStatus = Finished; - return FALSE; - } - else - { - return TRUE; - } - break; - default: - break; + if ((stateGetPositionUtm_f()->alt > BungeeAlt + Takeoff_Height - 10) + && ((*stateGetHorizontalSpeedNorm_f()) > Takeoff_Speed)) { + CTakeoffStatus = Finished; + return FALSE; + } else { + return TRUE; + } + break; + default: + break; } return TRUE; } diff --git a/sw/airborne/modules/nav/nav_catapult.c b/sw/airborne/modules/nav/nav_catapult.c index 679284baa0..2fea81ec17 100644 --- a/sw/airborne/modules/nav/nav_catapult.c +++ b/sw/airborne/modules/nav/nav_catapult.c @@ -63,7 +63,7 @@ static uint16_t nav_catapult_launch = 0; float nav_catapult_acceleration_threshold = NAV_CATAPULT_ACCELERATION_THRESHOLD; #ifndef NAV_CATAPULT_MOTOR_DELAY -#define NAV_CATAPULT_MOTOR_DELAY 0.75 // seconds +#define NAV_CATAPULT_MOTOR_DELAY 0.75 // seconds #endif float nav_catapult_motor_delay = NAV_CATAPULT_MOTOR_DELAY; @@ -97,15 +97,13 @@ static float nav_catapult_y = 0; void nav_catapult_highrate_module(void) { // Only run when - if (nav_catapult_armed) - { + if (nav_catapult_armed) { if (nav_catapult_launch < nav_catapult_heading_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ) { nav_catapult_launch++; } // Launch detection Filter - if (nav_catapult_launch < 5) - { + if (nav_catapult_launch < 5) { // Five consecutive measurements > 1.5 #ifndef SITL struct Int32Vect3 accel_meas_body; @@ -120,15 +118,12 @@ void nav_catapult_highrate_module(void) } } // Launch was detected: Motor Delay Counter - else if (nav_catapult_launch >= nav_catapult_motor_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ) - { + else if (nav_catapult_launch >= nav_catapult_motor_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ) { // Turn on Motor - NavVerticalThrottleMode(9600*(nav_catapult_initial_throttle)); + NavVerticalThrottleMode(9600 * (nav_catapult_initial_throttle)); launch = 1; } - } - else - { + } else { nav_catapult_launch = 0; } } @@ -154,11 +149,10 @@ bool_t nav_catapult_run(uint8_t _to, uint8_t _climb) nav_catapult_armed = 1; // No Roll, Climb Pitch, No motor Phase - if (nav_catapult_launch <= nav_catapult_motor_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ) - { + if (nav_catapult_launch <= nav_catapult_motor_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ) { NavAttitude(RadOfDeg(0)); NavVerticalAutoThrottleMode(nav_catapult_initial_pitch); - NavVerticalThrottleMode(9600*(0)); + NavVerticalThrottleMode(9600 * (0)); nav_catapult_x = stateGetPositionEnu_f()->x; @@ -171,21 +165,17 @@ bool_t nav_catapult_run(uint8_t _to, uint8_t _climb) } // No Roll, Climb Pitch, Full Power - else if (nav_catapult_launch < nav_catapult_heading_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ) - { + else if (nav_catapult_launch < nav_catapult_heading_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ) { NavAttitude(RadOfDeg(0)); NavVerticalAutoThrottleMode(nav_catapult_initial_pitch); - NavVerticalThrottleMode(9600*(nav_catapult_initial_throttle)); + NavVerticalThrottleMode(9600 * (nav_catapult_initial_throttle)); } // Normal Climb: Heading Locked by Waypoint Target - else if (nav_catapult_launch == 0xffff) - { - NavVerticalAltitudeMode(alt, 0); // vertical mode (folow glideslope) - NavVerticalAutoThrottleMode(0); // throttle mode - NavGotoWaypoint(_climb); // horizontal mode (stay on localiser) - } - else - { + else if (nav_catapult_launch == 0xffff) { + NavVerticalAltitudeMode(alt, 0); // vertical mode (folow glideslope) + NavVerticalAutoThrottleMode(0); // throttle mode + NavGotoWaypoint(_climb); // horizontal mode (stay on localiser) + } else { // Store Heading, move Climb nav_catapult_launch = 0xffff; @@ -201,7 +191,7 @@ bool_t nav_catapult_run(uint8_t _to, uint8_t _climb) } -return TRUE; + return TRUE; } diff --git a/sw/airborne/modules/nav/nav_cube.c b/sw/airborne/modules/nav/nav_cube.c index 05a84dfeef..cd1cb1e05c 100644 --- a/sw/airborne/modules/nav/nav_cube.c +++ b/sw/airborne/modules/nav/nav_cube.c @@ -36,7 +36,8 @@ struct NavCube nav_cube; -bool_t nav_cube_setup(uint8_t center, uint8_t tb, uint8_t te) { +bool_t nav_cube_setup(uint8_t center, uint8_t tb, uint8_t te) +{ int32_t j, start_bx, start_by, start_bz, start_ex, start_ey, start_ez; int32_t bx, by, ex, ey; @@ -47,38 +48,44 @@ bool_t nav_cube_setup(uint8_t center, uint8_t tb, uint8_t te) { int32_t cube_line_z_start, cube_line_z_end; /* sanity checks */ - if (nav_cube.nsect_x <= 0) + if (nav_cube.nsect_x <= 0) { nav_cube.nsect_x = 1; - if (nav_cube.nsect_z <= 0) + } + if (nav_cube.nsect_z <= 0) { nav_cube.nsect_z = 1; - if ((nav_cube.sect <= 0) || (nav_cube.sect > (nav_cube.nsect_x*nav_cube.nsect_z))) + } + if ((nav_cube.sect <= 0) || (nav_cube.sect > (nav_cube.nsect_x * nav_cube.nsect_z))) { nav_cube.sect = 1; + } /* total number of lines/layers to fly */ - if (nav_cube.grid_x == 0) + if (nav_cube.grid_x == 0) { cube_nline_x_t = 1; - else + } else { cube_nline_x_t = nav_cube.size.x / nav_cube.grid_x + 1; - if (nav_cube.grid_z == 0) + } + if (nav_cube.grid_z == 0) { cube_nline_z_t = 1; - else + } else { cube_nline_z_t = nav_cube.size.z / nav_cube.grid_z + 1; + } /* position and number of lines in this sector */ - cube_pos_x = (nav_cube.sect-1) % nav_cube.nsect_x; - cube_line_x_start = (cube_pos_x * cube_nline_x_t)/nav_cube.nsect_x; - cube_line_x_end = ((cube_pos_x+1) * cube_nline_x_t)/nav_cube.nsect_x; - if (cube_line_x_end > cube_nline_x_t) + cube_pos_x = (nav_cube.sect - 1) % nav_cube.nsect_x; + cube_line_x_start = (cube_pos_x * cube_nline_x_t) / nav_cube.nsect_x; + cube_line_x_end = ((cube_pos_x + 1) * cube_nline_x_t) / nav_cube.nsect_x; + if (cube_line_x_end > cube_nline_x_t) { cube_line_x_end = cube_nline_x_t; + } nav_cube.nline_x = cube_line_x_end - cube_line_x_start; /* do not do more than pre-set number of lines */ - if (nav_cube.nline_x >= MAX_LINES_X) nav_cube.nline_x = MAX_LINES_X-1; + if (nav_cube.nline_x >= MAX_LINES_X) { nav_cube.nline_x = MAX_LINES_X - 1; } /* position and number of layers in this sector */ - cube_pos_z = (nav_cube.sect-1) / nav_cube.nsect_x; - cube_line_z_start = (cube_pos_z * cube_nline_z_t)/nav_cube.nsect_z; - cube_line_z_end = ((cube_pos_z+1) * cube_nline_z_t)/nav_cube.nsect_z; + cube_pos_z = (nav_cube.sect - 1) / nav_cube.nsect_x; + cube_line_z_start = (cube_pos_z * cube_nline_z_t) / nav_cube.nsect_z; + cube_line_z_end = ((cube_pos_z + 1) * cube_nline_z_t) / nav_cube.nsect_z; nav_cube.nline_z = cube_line_z_end - cube_line_z_start; /* do the costly stuff only once */ @@ -87,8 +94,8 @@ bool_t nav_cube_setup(uint8_t center, uint8_t tb, uint8_t te) { sin_alpha = sin(alpha); /* calculate lower left start begin/end x coord */ - start_bx = WaypointX(center) - (((cube_nline_x_t-1) * nav_cube.grid_x)/2) - + nav_cube.offset.x; + start_bx = WaypointX(center) - (((cube_nline_x_t - 1) * nav_cube.grid_x) / 2) + + nav_cube.offset.x; start_ex = start_bx; /* calculate lower left start end point y coord */ @@ -98,33 +105,33 @@ bool_t nav_cube_setup(uint8_t center, uint8_t tb, uint8_t te) { start_by = start_ey - nav_cube.size.y; /* calculate lower left start begin/end z coord */ - start_bz = waypoints[center].a - (((cube_nline_z_t-1) * nav_cube.grid_z)/2) - + (cube_line_z_start*nav_cube.grid_z) + nav_cube.offset.z; + start_bz = waypoints[center].a - (((cube_nline_z_t - 1) * nav_cube.grid_z) / 2) + + (cube_line_z_start * nav_cube.grid_z) + nav_cube.offset.z; start_ez = start_bz; /* reset all waypoints to the standby position */ - for (j=0; j < MAX_LINES_X; j++) { - waypoints[tb+j].x = WaypointX(center) + STBY_OFFSET; - waypoints[tb+j].y = WaypointY(center); - waypoints[te+j].x = WaypointX(center) + STBY_OFFSET; - waypoints[te+j].y = WaypointY(center); + for (j = 0; j < MAX_LINES_X; j++) { + waypoints[tb + j].x = WaypointX(center) + STBY_OFFSET; + waypoints[tb + j].y = WaypointY(center); + waypoints[te + j].x = WaypointX(center) + STBY_OFFSET; + waypoints[te + j].y = WaypointY(center); } /* set used waypoints */ - for (j=0; j < nav_cube.nline_x; j++) { - int i = cube_line_x_start+j; + for (j = 0; j < nav_cube.nline_x; j++) { + int i = cube_line_x_start + j; /* set waypoints and vectorize in regard to center */ - bx = (start_bx + i*nav_cube.grid_x) - WaypointX(center); + bx = (start_bx + i * nav_cube.grid_x) - WaypointX(center); by = start_by - WaypointY(center); - ex = (start_ex + i*nav_cube.grid_x) - WaypointX(center); + ex = (start_ex + i * nav_cube.grid_x) - WaypointX(center); ey = start_ey - WaypointY(center); /* rotate clockwise with alpha and un-vectorize*/ - waypoints[tb+j].x = bx * cos_alpha - by * sin_alpha + WaypointX(center); - waypoints[tb+j].y = bx * sin_alpha + by * cos_alpha + WaypointY(center); - waypoints[tb+j].a = start_bz; - waypoints[te+j].x = ex * cos_alpha - ey * sin_alpha + WaypointX(center); - waypoints[te+j].y = ex * sin_alpha + ey * cos_alpha + WaypointY(center); - waypoints[te+j].a = start_ez; + waypoints[tb + j].x = bx * cos_alpha - by * sin_alpha + WaypointX(center); + waypoints[tb + j].y = bx * sin_alpha + by * cos_alpha + WaypointY(center); + waypoints[tb + j].a = start_bz; + waypoints[te + j].x = ex * cos_alpha - ey * sin_alpha + WaypointX(center); + waypoints[te + j].y = ex * sin_alpha + ey * cos_alpha + WaypointY(center); + waypoints[te + j].a = start_ez; } /* bug in ? */ @@ -135,27 +142,32 @@ bool_t nav_cube_setup(uint8_t center, uint8_t tb, uint8_t te) { } bool_t nav_cube_run(int8_t j, int8_t i, - uint8_t dest_b, uint8_t dest_e, - uint8_t src_b, uint8_t src_e) { + uint8_t dest_b, uint8_t dest_e, + uint8_t src_b, uint8_t src_e) +{ - if (i > nav_cube.nline_x) + if (i > nav_cube.nline_x) { return FALSE; - if (j > nav_cube.nline_z) + } + if (j > nav_cube.nline_z) { return FALSE; + } - waypoints[dest_b].x = waypoints[src_b+i].x; - waypoints[dest_b].y = waypoints[src_b+i].y; - waypoints[dest_b].a = waypoints[src_b+i].a + j*nav_cube.grid_z; + waypoints[dest_b].x = waypoints[src_b + i].x; + waypoints[dest_b].y = waypoints[src_b + i].y; + waypoints[dest_b].a = waypoints[src_b + i].a + j * nav_cube.grid_z; /* always keep at least security altitude */ - if (waypoints[dest_b].a < (ground_alt+SECURITY_HEIGHT)) - waypoints[dest_b].a = ground_alt+SECURITY_HEIGHT; + if (waypoints[dest_b].a < (ground_alt + SECURITY_HEIGHT)) { + waypoints[dest_b].a = ground_alt + SECURITY_HEIGHT; + } - waypoints[dest_e].x = waypoints[src_e+i].x; - waypoints[dest_e].y = waypoints[src_e+i].y; - waypoints[dest_e].a = waypoints[src_e+i].a + j*nav_cube.grid_z; + waypoints[dest_e].x = waypoints[src_e + i].x; + waypoints[dest_e].y = waypoints[src_e + i].y; + waypoints[dest_e].a = waypoints[src_e + i].a + j * nav_cube.grid_z; /* always keep at least security altitude */ - if (waypoints[dest_e].a < (ground_alt+SECURITY_HEIGHT)) - waypoints[dest_e].a = ground_alt+SECURITY_HEIGHT; + if (waypoints[dest_e].a < (ground_alt + SECURITY_HEIGHT)) { + waypoints[dest_e].a = ground_alt + SECURITY_HEIGHT; + } return FALSE; } diff --git a/sw/airborne/modules/nav/nav_cube.h b/sw/airborne/modules/nav/nav_cube.h index 5e8fc735ad..590cfb7fed 100644 --- a/sw/airborne/modules/nav/nav_cube.h +++ b/sw/airborne/modules/nav/nav_cube.h @@ -124,8 +124,8 @@ extern struct NavCube nav_cube; extern bool_t nav_cube_setup(uint8_t turb, uint8_t tb, uint8_t te); bool_t nav_cube_run(int8_t j, int8_t i, - uint8_t dest_b, uint8_t dest_e, - uint8_t src_b, uint8_t src_e); + uint8_t dest_b, uint8_t dest_e, + uint8_t src_b, uint8_t src_e); #define nav_cube_SetAlpha(i) { nav_cube.alpha=i; } #define nav_cube_SetSect(i) { nav_cube.sect=i; } diff --git a/sw/airborne/modules/nav/nav_drop.c b/sw/airborne/modules/nav/nav_drop.c index e35d18a933..9655e104a8 100644 --- a/sw/airborne/modules/nav/nav_drop.c +++ b/sw/airborne/modules/nav/nav_drop.c @@ -75,7 +75,8 @@ static float nav_drop_x, nav_drop_y, nav_drop_z; static float nav_drop_vx, nav_drop_vy, nav_drop_vz; -static void integrate( uint8_t wp_target ) { +static void integrate(uint8_t wp_target) +{ /* Inspired from Arnold Schroeter's code */ int i = 0; while (nav_drop_z > 0. && i < MAX_STEPS) { @@ -85,7 +86,7 @@ static void integrate( uint8_t wp_target ) { float airz = -nav_drop_vz; /* alpha / m * air */ - float beta = ALPHA_M * sqrt(airx*airx+airy*airy+airz*airz); + float beta = ALPHA_M * sqrt(airx * airx + airy * airy + airz * airz); /* Euler integration */ nav_drop_vx += airx * beta * DT; @@ -113,7 +114,8 @@ static void integrate( uint8_t wp_target ) { /** Update the RELEASE location with the actual ground speed and altitude */ -unit_t nav_drop_update_release( uint8_t wp_target ) { +unit_t nav_drop_update_release(uint8_t wp_target) +{ nav_drop_z = stateGetPositionUtm_f()->alt - waypoints[wp_target].a; nav_drop_x = 0.; @@ -131,7 +133,9 @@ unit_t nav_drop_update_release( uint8_t wp_target ) { /** Compute a first approximation for the RELEASE waypoint from wind and expected airspeed and altitude */ -unit_t nav_drop_compute_approach( uint8_t wp_target, uint8_t wp_start, uint8_t wp_baseturn, uint8_t wp_climbout, float nav_drop_radius ) { +unit_t nav_drop_compute_approach(uint8_t wp_target, uint8_t wp_start, uint8_t wp_baseturn, uint8_t wp_climbout, + float nav_drop_radius) +{ waypoints[WP_RELEASE].a = waypoints[wp_start].a; nav_drop_z = waypoints[WP_RELEASE].a - waypoints[wp_target].a; nav_drop_x = 0.; @@ -143,7 +147,7 @@ unit_t nav_drop_compute_approach( uint8_t wp_target, uint8_t wp_start, uint8_t w float y_0 = waypoints[wp_target].y - waypoints[wp_start].y; /* Unit vector from START to TARGET */ - float d = sqrt(x_0*x_0+y_0*y_0); + float d = sqrt(x_0 * x_0 + y_0 * y_0); float x1 = x_0 / d; float y_1 = y_0 / d; @@ -151,15 +155,15 @@ unit_t nav_drop_compute_approach( uint8_t wp_target, uint8_t wp_start, uint8_t w waypoints[wp_baseturn].y = waypoints[wp_start].y - x1 * nav_drop_radius; waypoints[wp_baseturn].a = waypoints[wp_start].a; nav_drop_start_qdr = M_PI - atan2(-y_1, -x1); - if (nav_drop_radius < 0) + if (nav_drop_radius < 0) { nav_drop_start_qdr += M_PI; + } // wind in NED frame if (stateIsAirspeedValid()) { - nav_drop_vx = x1 * *stateGetAirspeed_f() + stateGetHorizontalWindspeed_f()->y; - nav_drop_vy = y_1 * *stateGetAirspeed_f() + stateGetHorizontalWindspeed_f()->x; - } - else { + nav_drop_vx = x1 **stateGetAirspeed_f() + stateGetHorizontalWindspeed_f()->y; + nav_drop_vy = y_1 **stateGetAirspeed_f() + stateGetHorizontalWindspeed_f()->x; + } else { // use approximate airspeed, initially set to AIRSPEED_AT_RELEASE nav_drop_vx = x1 * airspeed + stateGetHorizontalWindspeed_f()->y; nav_drop_vy = y_1 * airspeed + stateGetHorizontalWindspeed_f()->x; @@ -180,18 +184,20 @@ unit_t nav_drop_compute_approach( uint8_t wp_target, uint8_t wp_start, uint8_t w -unit_t nav_drop_shoot( void ) { +unit_t nav_drop_shoot(void) +{ ap_state->commands[COMMAND_HATCH] = MAX_PPRZ; return 0; } /* Compute start and end waypoints to be aligned on w1-w2 */ -bool_t compute_alignment(uint8_t w1, uint8_t w2, uint8_t wp_before, uint8_t wp_after, float d_before, float d_after) { +bool_t compute_alignment(uint8_t w1, uint8_t w2, uint8_t wp_before, uint8_t wp_after, float d_before, float d_after) +{ float x_0 = waypoints[w2].x - waypoints[w1].x; float y_0 = waypoints[w2].y - waypoints[w1].y; /* Unit vector from W1 to W2 */ - float d = sqrt(x_0*x_0+y_0*y_0); + float d = sqrt(x_0 * x_0 + y_0 * y_0); x_0 /= d; y_0 /= d; diff --git a/sw/airborne/modules/nav/nav_drop.h b/sw/airborne/modules/nav/nav_drop.h index 86abe5c7da..0f873501a1 100644 --- a/sw/airborne/modules/nav/nav_drop.h +++ b/sw/airborne/modules/nav/nav_drop.h @@ -32,9 +32,10 @@ #include "std.h" #include "firmwares/fixedwing/nav.h" -extern unit_t nav_drop_compute_approach( uint8_t wp_target, uint8_t wp_start, uint8_t wp_baseturn, uint8_t wp_climbout, float radius ); -extern unit_t nav_drop_update_release( uint8_t wp_target ); -extern unit_t nav_drop_shoot( void ); +extern unit_t nav_drop_compute_approach(uint8_t wp_target, uint8_t wp_start, uint8_t wp_baseturn, uint8_t wp_climbout, + float radius); +extern unit_t nav_drop_update_release(uint8_t wp_target); +extern unit_t nav_drop_shoot(void); extern float nav_drop_trigger_delay, nav_drop_start_qdr; extern bool_t compute_alignment(uint8_t w1, uint8_t w2, uint8_t start, uint8_t end, float d_before, float d_after); diff --git a/sw/airborne/modules/nav/nav_flower.c b/sw/airborne/modules/nav/nav_flower.c index 39df617a48..4653d3dffd 100644 --- a/sw/airborne/modules/nav/nav_flower.c +++ b/sw/airborne/modules/nav/nav_flower.c @@ -35,8 +35,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 }; @@ -65,22 +65,23 @@ bool_t nav_flower_setup(uint8_t CenterWP, uint8_t EdgeWP) EdgeCurrentX = WaypointX(Edge) - WaypointX(Center); EdgeCurrentY = WaypointY(Edge) - WaypointY(Center); - Flowerradius = sqrtf(EdgeCurrentX*EdgeCurrentX+EdgeCurrentY*EdgeCurrentY); + Flowerradius = sqrtf(EdgeCurrentX * EdgeCurrentX + EdgeCurrentY * EdgeCurrentY); TransCurrentX = stateGetPositionEnu_f()->x - WaypointX(Center); TransCurrentY = stateGetPositionEnu_f()->y - WaypointY(Center); - DistanceFromCenter = sqrtf(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY); + DistanceFromCenter = sqrtf(TransCurrentX * TransCurrentX + TransCurrentY * TransCurrentY); - FlowerTheta = atan2f(TransCurrentY,TransCurrentX); - Fly2X = Flowerradius*cosf(FlowerTheta+3.14)+WaypointX(Center); - Fly2Y = Flowerradius*sinf(FlowerTheta+3.14)+WaypointY(Center); + FlowerTheta = atan2f(TransCurrentY, TransCurrentX); + Fly2X = Flowerradius * cosf(FlowerTheta + 3.14) + WaypointX(Center); + Fly2Y = Flowerradius * sinf(FlowerTheta + 3.14) + WaypointY(Center); FlyFromX = stateGetPositionEnu_f()->x; FlyFromY = stateGetPositionEnu_f()->y; - if(DistanceFromCenter > Flowerradius) + if (DistanceFromCenter > Flowerradius) { CFlowerStatus = Outside; - else + } else { CFlowerStatus = FlowerLine; + } CircleX = 0; CircleY = 0; @@ -91,65 +92,63 @@ bool_t nav_flower_run(void) { TransCurrentX = stateGetPositionEnu_f()->x - WaypointX(Center); TransCurrentY = stateGetPositionEnu_f()->y - WaypointY(Center); - DistanceFromCenter = sqrtf(TransCurrentX*TransCurrentX+TransCurrentY*TransCurrentY); + DistanceFromCenter = sqrtf(TransCurrentX * TransCurrentX + TransCurrentY * TransCurrentY); bool_t InCircle = TRUE; float CircleTheta; - if(DistanceFromCenter > Flowerradius) + if (DistanceFromCenter > Flowerradius) { InCircle = FALSE; + } NavVerticalAutoThrottleMode(0); /* No pitch */ NavVerticalAltitudeMode(waypoints[Center].a, 0.); - switch(CFlowerStatus) - { - case Outside: - nav_route_xy(FlyFromX,FlyFromY,Fly2X,Fly2Y); - if(InCircle) - { - CFlowerStatus = FlowerLine; - FlowerTheta = atan2f(TransCurrentY,TransCurrentX); - Fly2X = Flowerradius*cosf(FlowerTheta+3.14)+WaypointX(Center); - Fly2Y = Flowerradius*sinf(FlowerTheta+3.14)+WaypointY(Center); - FlyFromX = stateGetPositionEnu_f()->x; - FlyFromY = stateGetPositionEnu_f()->y; - nav_init_stage(); - } - break; - case FlowerLine: - nav_route_xy(FlyFromX,FlyFromY,Fly2X,Fly2Y); - if(!InCircle) - { - CFlowerStatus = Circle; - CircleTheta = nav_radius/Flowerradius; - CircleX = Flowerradius*cosf(FlowerTheta+3.14-CircleTheta)+WaypointX(Center); - CircleY = Flowerradius*sinf(FlowerTheta+3.14-CircleTheta)+WaypointY(Center); - nav_init_stage(); - } - break; - case Circle: - nav_circle_XY(CircleX, CircleY, nav_radius); - if(InCircle) - { - EdgeCurrentX = WaypointX(Edge) - WaypointX(Center); - EdgeCurrentY = WaypointY(Edge) - WaypointY(Center); - Flowerradius = sqrtf(EdgeCurrentX*EdgeCurrentX+EdgeCurrentY*EdgeCurrentY); - if(DistanceFromCenter > Flowerradius) - CFlowerStatus = Outside; - else + switch (CFlowerStatus) { + case Outside: + nav_route_xy(FlyFromX, FlyFromY, Fly2X, Fly2Y); + if (InCircle) { CFlowerStatus = FlowerLine; - FlowerTheta = atan2f(TransCurrentY,TransCurrentX); - Fly2X = Flowerradius*cosf(FlowerTheta+3.14)+WaypointX(Center); - Fly2Y = Flowerradius*sinf(FlowerTheta+3.14)+WaypointY(Center); - FlyFromX = stateGetPositionEnu_f()->x; - FlyFromY = stateGetPositionEnu_f()->y; - nav_init_stage(); - } - break; + FlowerTheta = atan2f(TransCurrentY, TransCurrentX); + Fly2X = Flowerradius * cosf(FlowerTheta + 3.14) + WaypointX(Center); + Fly2Y = Flowerradius * sinf(FlowerTheta + 3.14) + WaypointY(Center); + FlyFromX = stateGetPositionEnu_f()->x; + FlyFromY = stateGetPositionEnu_f()->y; + nav_init_stage(); + } + break; + case FlowerLine: + nav_route_xy(FlyFromX, FlyFromY, Fly2X, Fly2Y); + if (!InCircle) { + CFlowerStatus = Circle; + CircleTheta = nav_radius / Flowerradius; + CircleX = Flowerradius * cosf(FlowerTheta + 3.14 - CircleTheta) + WaypointX(Center); + CircleY = Flowerradius * sinf(FlowerTheta + 3.14 - CircleTheta) + WaypointY(Center); + nav_init_stage(); + } + break; + case Circle: + nav_circle_XY(CircleX, CircleY, nav_radius); + if (InCircle) { + EdgeCurrentX = WaypointX(Edge) - WaypointX(Center); + EdgeCurrentY = WaypointY(Edge) - WaypointY(Center); + Flowerradius = sqrtf(EdgeCurrentX * EdgeCurrentX + EdgeCurrentY * EdgeCurrentY); + if (DistanceFromCenter > Flowerradius) { + CFlowerStatus = Outside; + } else { + CFlowerStatus = FlowerLine; + } + FlowerTheta = atan2f(TransCurrentY, TransCurrentX); + Fly2X = Flowerradius * cosf(FlowerTheta + 3.14) + WaypointX(Center); + Fly2Y = Flowerradius * sinf(FlowerTheta + 3.14) + WaypointY(Center); + FlyFromX = stateGetPositionEnu_f()->x; + FlyFromY = stateGetPositionEnu_f()->y; + nav_init_stage(); + } + break; - default: - break; + default: + break; } return TRUE; } diff --git a/sw/airborne/modules/nav/nav_gls.c b/sw/airborne/modules/nav/nav_gls.c index dc2c42cd38..3a4ca19c12 100644 --- a/sw/airborne/modules/nav/nav_gls.c +++ b/sw/airborne/modules/nav/nav_gls.c @@ -79,15 +79,16 @@ float sd_intercept; float sd_tod_x; float sd_tod_y; -static inline bool_t gls_compute_TOD(uint8_t _af, uint8_t _sd, uint8_t _tod, uint8_t _td) { +static inline bool_t gls_compute_TOD(uint8_t _af, uint8_t _sd, uint8_t _tod, uint8_t _td) +{ - if ((WaypointX(_af)==WaypointX(_td))&&(WaypointY(_af)==WaypointY(_td))){ - WaypointX(_af)=WaypointX(_td)-1; + if ((WaypointX(_af) == WaypointX(_td)) && (WaypointY(_af) == WaypointY(_td))) { + WaypointX(_af) = WaypointX(_td) - 1; } float td_af_x = WaypointX(_af) - WaypointX(_td); float td_af_y = WaypointY(_af) - WaypointY(_td); - float td_af = sqrt( td_af_x*td_af_x + td_af_y*td_af_y); + float td_af = sqrt(td_af_x * td_af_x + td_af_y * td_af_y); float td_tod = (WaypointAlt(_af) - WaypointAlt(_td)) / (tanf(app_angle)); // set wapoint TOD (top of decent) @@ -96,11 +97,11 @@ static inline bool_t gls_compute_TOD(uint8_t _af, uint8_t _sd, uint8_t _tod, uin WaypointAlt(_tod) = WaypointAlt(_af); // calculate ground speed on final (target_speed - head wind) - struct FloatVect2* wind = stateGetHorizontalWindspeed_f(); - float wind_norm = sqrt(wind->x*wind->x + wind->y*wind->y); - float wind_on_final = wind_norm * (((td_af_x*wind->y)/(td_af*wind_norm)) + - ((td_af_y*wind->x)/(td_af*wind_norm))); - Bound(wind_on_final,-MAX_WIND_ON_FINAL,MAX_WIND_ON_FINAL); + struct FloatVect2 *wind = stateGetHorizontalWindspeed_f(); + float wind_norm = sqrt(wind->x * wind->x + wind->y * wind->y); + float wind_on_final = wind_norm * (((td_af_x * wind->y) / (td_af * wind_norm)) + + ((td_af_y * wind->x) / (td_af * wind_norm))); + Bound(wind_on_final, -MAX_WIND_ON_FINAL, MAX_WIND_ON_FINAL); gs_on_final = target_speed - wind_on_final; // calculate position of SD (start decent) @@ -116,7 +117,7 @@ static inline bool_t gls_compute_TOD(uint8_t _af, uint8_t _sd, uint8_t _tod, uin // calculate td_sd float td_sd_x = WaypointX(_sd) - WaypointX(_td); float td_sd_y = WaypointY(_sd) - WaypointY(_td); - float td_sd = sqrt( td_sd_x*td_sd_x + td_sd_y*td_sd_y); + float td_sd = sqrt(td_sd_x * td_sd_x + td_sd_y * td_sd_y); // calculate sd_tod in x,y sd_tod_x = WaypointX(_tod) - WaypointX(_sd); @@ -128,10 +129,11 @@ static inline bool_t gls_compute_TOD(uint8_t _af, uint8_t _sd, uint8_t _tod, uin WaypointY(_af) = WaypointY(_sd) + td_af_y / td_af * app_distance_af_sd; } return FALSE; -} /* end of gls_copute_TOD */ +} /* end of gls_copute_TOD */ -bool_t gls_start(uint8_t _af,uint8_t _sd, uint8_t _tod, uint8_t _td) { +bool_t gls_start(uint8_t _af, uint8_t _sd, uint8_t _tod, uint8_t _td) +{ init = TRUE; @@ -145,7 +147,7 @@ bool_t gls_start(uint8_t _af,uint8_t _sd, uint8_t _tod, uint8_t _td) { app_intercept_rate = APP_INTERCEPT_RATE; app_intercept_rate = ABS(app_intercept_rate); app_distance_af_sd = APP_DISTANCE_AF_SD; - Bound(app_distance_af_sd,0,200); + Bound(app_distance_af_sd, 0, 200); // calculate Top Of Decent gls_compute_TOD(_af, _sd, _tod, _td); @@ -154,11 +156,12 @@ bool_t gls_start(uint8_t _af,uint8_t _sd, uint8_t _tod, uint8_t _td) { } /* end of gls_init() */ -bool_t gls_run(uint8_t _af,uint8_t _sd, uint8_t _tod, uint8_t _td) { +bool_t gls_run(uint8_t _af, uint8_t _sd, uint8_t _tod, uint8_t _td) +{ // set target speed for approach on final - if (init){ + if (init) { #if USE_AIRSPEED v_ctl_auto_airspeed_setpoint = target_speed; #endif @@ -170,12 +173,12 @@ bool_t gls_run(uint8_t _af,uint8_t _sd, uint8_t _tod, uint8_t _td) { float final_y = WaypointY(_td) - WaypointY(_tod); float final2 = Max(final_x * final_x + final_y * final_y, 1.); - struct EnuCoor_f* pos_enu = stateGetPositionEnu_f(); + struct EnuCoor_f *pos_enu = stateGetPositionEnu_f(); float hspeed = *stateGetHorizontalSpeedNorm_f(); float nav_final_progress = ((pos_enu->x - WaypointX(_tod)) * final_x + (pos_enu->y - WaypointY(_tod)) * final_y) / final2; - Bound(nav_final_progress,-1,1); + Bound(nav_final_progress, -1, 1); // float nav_final_length = sqrt(final2); // calculate requiered decent rate on glideslope @@ -187,10 +190,10 @@ bool_t gls_run(uint8_t _af,uint8_t _sd, uint8_t _tod, uint8_t _td) { float alt_glideslope = start_alt + nav_final_progress * diff_alt; // calculate intercept - float nav_intercept_progress = ((pos_enu->x - WaypointX(_sd)) * 2*sd_tod_x + - (pos_enu->y - WaypointY(_sd)) * 2*sd_tod_y) / - Max((sd_intercept*sd_intercept),1.); - Bound(nav_intercept_progress,-1,1); + float nav_intercept_progress = ((pos_enu->x - WaypointX(_sd)) * 2 * sd_tod_x + + (pos_enu->y - WaypointY(_sd)) * 2 * sd_tod_y) / + Max((sd_intercept * sd_intercept), 1.); + Bound(nav_intercept_progress, -1, 1); float tmp = nav_intercept_progress * sd_intercept / gs_on_final; float alt_intercept = WaypointAlt(_tod) - (0.5 * app_intercept_rate * tmp * tmp); float pre_climb_intercept = -nav_intercept_progress * hspeed * (tanf(app_angle)); @@ -203,19 +206,17 @@ bool_t gls_run(uint8_t _af,uint8_t _sd, uint8_t _tod, uint8_t _td) { float alt = 0.; // distance - float f_af = sqrt((pos_enu->x - WaypointX(_af))*(pos_enu->x - WaypointX(_af))+ - (pos_enu->y - WaypointY(_af))*(pos_enu->y - WaypointY(_af))); + float f_af = sqrt((pos_enu->x - WaypointX(_af)) * (pos_enu->x - WaypointX(_af)) + + (pos_enu->y - WaypointY(_af)) * (pos_enu->y - WaypointY(_af))); - if(f_afapp_distance_af_sd) && (f_af<(app_distance_af_sd+sd_intercept))){ + } else if ((f_af > app_distance_af_sd) && (f_af < (app_distance_af_sd + sd_intercept))) { // start descent (SD) to intercept alt = alt_intercept; pre_climb = pre_climb_intercept; - } - else{ //glideslope (intercept to touch down) + } else { //glideslope (intercept to touch down) alt = alt_glideslope; pre_climb = pre_climb_glideslope; } @@ -224,9 +225,9 @@ bool_t gls_run(uint8_t _af,uint8_t _sd, uint8_t _tod, uint8_t _td) { //######################### autopilot modes - NavVerticalAltitudeMode(alt, pre_climb); // vertical mode (folow glideslope) - NavVerticalAutoThrottleMode(0); // throttle mode - NavSegment(_af, _td); // horizontal mode (stay on localiser) + NavVerticalAltitudeMode(alt, pre_climb); // vertical mode (folow glideslope) + NavVerticalAutoThrottleMode(0); // throttle mode + NavSegment(_af, _td); // horizontal mode (stay on localiser) return TRUE; -} // end of gls() +} // end of gls() diff --git a/sw/airborne/modules/nav/nav_line.c b/sw/airborne/modules/nav/nav_line.c index 3fe2ac6765..3569638aff 100644 --- a/sw/airborne/modules/nav/nav_line.c +++ b/sw/airborne/modules/nav/nav_line.c @@ -33,19 +33,21 @@ enum line_status { LR12, LQC21, LTC2, LQC22, LR21, LQC12, LTC1, LQC11 }; static enum line_status line_status; -bool_t nav_line_setup( void ) { +bool_t nav_line_setup(void) +{ line_status = LR12; return FALSE; } -bool_t nav_line_run(uint8_t l1, uint8_t l2, float radius) { +bool_t nav_line_run(uint8_t l1, uint8_t l2, float radius) +{ radius = fabs(radius); float alt = waypoints[l1].a; waypoints[l2].a = alt; float l2_l1_x = WaypointX(l1) - WaypointX(l2); float l2_l1_y = WaypointY(l1) - WaypointY(l2); - float d = sqrt(l2_l1_x*l2_l1_x+l2_l1_y*l2_l1_y); + float d = sqrt(l2_l1_x * l2_l1_x + l2_l1_y * l2_l1_y); /* Unit vector from l1 to l2 */ float u_x = l2_l1_x / d; @@ -53,27 +55,33 @@ bool_t nav_line_run(uint8_t l1, uint8_t l2, float radius) { /* The half circle centers and the other leg */ struct point l2_c1 = { WaypointX(l1) + radius * u_y, - WaypointY(l1) + radius * -u_x, - alt }; - struct point l2_c2 = { WaypointX(l1) + 1.732*radius * u_x, - WaypointY(l1) + 1.732*radius * u_y, - alt }; + WaypointY(l1) + radius * -u_x, + alt + }; + struct point l2_c2 = { WaypointX(l1) + 1.732 * radius * u_x, + WaypointY(l1) + 1.732 * radius * u_y, + alt + }; struct point l2_c3 = { WaypointX(l1) + radius * -u_y, - WaypointY(l1) + radius * u_x, - alt }; + WaypointY(l1) + radius * u_x, + alt + }; struct point l1_c1 = { WaypointX(l2) + radius * -u_y, - WaypointY(l2) + radius * u_x, - alt }; - struct point l1_c2 = { WaypointX(l2) +1.732*radius * -u_x, - WaypointY(l2) + 1.732*radius * -u_y, - alt }; + WaypointY(l2) + radius * u_x, + alt + }; + struct point l1_c2 = { WaypointX(l2) + 1.732 * radius * -u_x, + WaypointY(l2) + 1.732 * radius * -u_y, + alt + }; struct point l1_c3 = { WaypointX(l2) + radius * u_y, - WaypointY(l2) + radius * -u_x, - alt }; - float qdr_out_2_1 = M_PI/3. - atan2(u_y, u_x); + WaypointY(l2) + radius * -u_x, + alt + }; + float qdr_out_2_1 = M_PI / 3. - atan2(u_y, u_x); - float qdr_out_2_2 = -M_PI/3. - atan2(u_y, u_x); + float qdr_out_2_2 = -M_PI / 3. - atan2(u_y, u_x); float qdr_out_2_3 = M_PI - atan2(u_y, u_x); /* Vertical target */ @@ -81,64 +89,64 @@ bool_t nav_line_run(uint8_t l1, uint8_t l2, float radius) { NavVerticalAltitudeMode(WaypointAlt(l1), 0.); switch (line_status) { - case LR12: /* From wp l2 to wp l1 */ - NavSegment(l2, l1); - if (NavApproachingFrom(l1, l2, CARROT)) { - line_status = LQC21; - nav_init_stage(); - } - break; - case LQC21: - nav_circle_XY(l2_c1.x, l2_c1.y, radius); - if (NavQdrCloseTo(DegOfRad(qdr_out_2_1)-10)) { - line_status = LTC2; - nav_init_stage(); - } - break; - case LTC2: - nav_circle_XY(l2_c2.x, l2_c2.y, -radius); - if (NavQdrCloseTo(DegOfRad(qdr_out_2_2)+10)) { - line_status = LQC22; - nav_init_stage(); - } - break; - case LQC22: - nav_circle_XY(l2_c3.x, l2_c3.y, radius); - if (NavQdrCloseTo(DegOfRad(qdr_out_2_3)-10)) { - line_status = LR21; - nav_init_stage(); - } - break; - case LR21: /* From wp l1 to wp l2 */ - NavSegment(l1, l2); - if (NavApproachingFrom(l2, l1, CARROT)) { - line_status = LQC12; - nav_init_stage(); - } - break; - case LQC12: - nav_circle_XY(l1_c1.x, l1_c1.y, radius); - if (NavQdrCloseTo(DegOfRad(qdr_out_2_1 + M_PI)-10)) { - line_status = LTC1; - nav_init_stage(); - } - break; - case LTC1: - nav_circle_XY(l1_c2.x, l1_c2.y, -radius); - if (NavQdrCloseTo(DegOfRad(qdr_out_2_2 + M_PI)+10)) { - line_status = LQC11; - nav_init_stage(); - } - break; - case LQC11: - nav_circle_XY(l1_c3.x, l1_c3.y, radius); - if (NavQdrCloseTo(DegOfRad(qdr_out_2_3 + M_PI)-10)) { - line_status = LR12; - nav_init_stage(); - } - break; - default: /* Should not occur !!! End the pattern */ - return FALSE; + case LR12: /* From wp l2 to wp l1 */ + NavSegment(l2, l1); + if (NavApproachingFrom(l1, l2, CARROT)) { + line_status = LQC21; + nav_init_stage(); + } + break; + case LQC21: + nav_circle_XY(l2_c1.x, l2_c1.y, radius); + if (NavQdrCloseTo(DegOfRad(qdr_out_2_1) - 10)) { + line_status = LTC2; + nav_init_stage(); + } + break; + case LTC2: + nav_circle_XY(l2_c2.x, l2_c2.y, -radius); + if (NavQdrCloseTo(DegOfRad(qdr_out_2_2) + 10)) { + line_status = LQC22; + nav_init_stage(); + } + break; + case LQC22: + nav_circle_XY(l2_c3.x, l2_c3.y, radius); + if (NavQdrCloseTo(DegOfRad(qdr_out_2_3) - 10)) { + line_status = LR21; + nav_init_stage(); + } + break; + case LR21: /* From wp l1 to wp l2 */ + NavSegment(l1, l2); + if (NavApproachingFrom(l2, l1, CARROT)) { + line_status = LQC12; + nav_init_stage(); + } + break; + case LQC12: + nav_circle_XY(l1_c1.x, l1_c1.y, radius); + if (NavQdrCloseTo(DegOfRad(qdr_out_2_1 + M_PI) - 10)) { + line_status = LTC1; + nav_init_stage(); + } + break; + case LTC1: + nav_circle_XY(l1_c2.x, l1_c2.y, -radius); + if (NavQdrCloseTo(DegOfRad(qdr_out_2_2 + M_PI) + 10)) { + line_status = LQC11; + nav_init_stage(); + } + break; + case LQC11: + nav_circle_XY(l1_c3.x, l1_c3.y, radius); + if (NavQdrCloseTo(DegOfRad(qdr_out_2_3 + M_PI) - 10)) { + line_status = LR12; + nav_init_stage(); + } + break; + default: /* Should not occur !!! End the pattern */ + return FALSE; } return TRUE; /* This pattern never ends */ } diff --git a/sw/airborne/modules/nav/nav_line.h b/sw/airborne/modules/nav/nav_line.h index 0af33d5fbf..a255fa58b6 100644 --- a/sw/airborne/modules/nav/nav_line.h +++ b/sw/airborne/modules/nav/nav_line.h @@ -30,7 +30,7 @@ #include "std.h" -extern bool_t nav_line_setup( void ); +extern bool_t nav_line_setup(void); extern bool_t nav_line_run(uint8_t wp1, uint8_t wp2, float radius); #endif /* NAV_LINE_H */ diff --git a/sw/airborne/modules/nav/nav_line_border.c b/sw/airborne/modules/nav/nav_line_border.c index c9dc276f63..52c846e802 100644 --- a/sw/airborne/modules/nav/nav_line_border.c +++ b/sw/airborne/modules/nav/nav_line_border.c @@ -37,43 +37,49 @@ enum line_border_status { LR12, LQC21, LTC2, LQC22, LR21, LQC12, LTC1, LQC11 }; static enum line_border_status line_border_status; -bool_t nav_line_border_setup( void ) { +bool_t nav_line_border_setup(void) +{ line_border_status = LR12; return FALSE; } -bool_t nav_line_border_run(uint8_t l1, uint8_t l2, float radius) { +bool_t nav_line_border_run(uint8_t l1, uint8_t l2, float radius) +{ radius = fabs(radius); float alt = waypoints[l1].a; waypoints[l2].a = alt; float l2_l1_x = WaypointX(l1) - WaypointX(l2); float l2_l1_y = WaypointY(l1) - WaypointY(l2); - float d = sqrt(l2_l1_x*l2_l1_x+l2_l1_y*l2_l1_y); + float d = sqrt(l2_l1_x * l2_l1_x + l2_l1_y * l2_l1_y); float u_x = l2_l1_x / d; float u_y = l2_l1_y / d; - float angle = atan2((WaypointY(l1)-WaypointY(l2)),(WaypointX(l2)-WaypointX(l1))); + float angle = atan2((WaypointY(l1) - WaypointY(l2)), (WaypointX(l2) - WaypointX(l1))); - struct point l2_c1 = { WaypointX(l1) - sin(angle)*radius, - WaypointY(l1) - cos(angle)*radius, - alt }; - struct point l2_c2 = { l2_c1.x + 2*radius*cos(angle) , - l2_c1.y - 2*radius*sin(angle), - alt }; + struct point l2_c1 = { WaypointX(l1) - sin(angle) *radius, + WaypointY(l1) - cos(angle) *radius, + alt + }; + struct point l2_c2 = { l2_c1.x + 2 * radius * cos(angle) , + l2_c1.y - 2 * radius * sin(angle), + alt + }; - struct point l1_c2 = { WaypointX(l2) - sin(angle)*radius, - WaypointY(l2) - cos(angle)*radius, - alt }; - struct point l1_c3 = { l1_c2.x - 2*radius*cos(angle) , - l1_c2.y + 2*radius*sin(angle), - alt }; + struct point l1_c2 = { WaypointX(l2) - sin(angle) *radius, + WaypointY(l2) - cos(angle) *radius, + alt + }; + struct point l1_c3 = { l1_c2.x - 2 * radius * cos(angle) , + l1_c2.y + 2 * radius * sin(angle), + alt + }; - float qdr_out_2_1 = M_PI/3. - atan2(u_y, u_x); - float qdr_out_2_2 = -M_PI/3. - atan2(u_y, u_x); + float qdr_out_2_1 = M_PI / 3. - atan2(u_y, u_x); + float qdr_out_2_2 = -M_PI / 3. - atan2(u_y, u_x); float qdr_out_2_3 = M_PI - atan2(u_y, u_x); @@ -90,14 +96,14 @@ bool_t nav_line_border_run(uint8_t l1, uint8_t l2, float radius) { break; case LQC21: nav_circle_XY(l2_c1.x, l2_c1.y, -radius); - if (NavQdrCloseTo(DegOfRad(qdr_out_2_2)-10)) { + if (NavQdrCloseTo(DegOfRad(qdr_out_2_2) - 10)) { line_border_status = LTC2; nav_init_stage(); } break; case LTC2: nav_circle_XY(l2_c2.x, l2_c2.y, radius); - if (NavQdrCloseTo(DegOfRad(qdr_out_2_3)-10)) { + if (NavQdrCloseTo(DegOfRad(qdr_out_2_3) - 10)) { line_border_status = LR21; nav_init_stage(); } diff --git a/sw/airborne/modules/nav/nav_line_border.h b/sw/airborne/modules/nav/nav_line_border.h index a3ced33aba..fce937c0e4 100644 --- a/sw/airborne/modules/nav/nav_line_border.h +++ b/sw/airborne/modules/nav/nav_line_border.h @@ -29,7 +29,7 @@ #include "std.h" -extern bool_t nav_line_border_setup( void ); +extern bool_t nav_line_border_setup(void); extern bool_t nav_line_border_run(uint8_t wp1, uint8_t wp2, float radius); #endif /* NAV_LINE_BORDER_H */ diff --git a/sw/airborne/modules/nav/nav_line_osam.c b/sw/airborne/modules/nav/nav_line_osam.c index 76cbc6d82a..2dbc6be484 100644 --- a/sw/airborne/modules/nav/nav_line_osam.c +++ b/sw/airborne/modules/nav/nav_line_osam.c @@ -63,8 +63,8 @@ static void TranslateAndRotateFromWorld(struct Point2D *p, float Zrot, float tra p->y = p->y - transY; temp = p->x; - p->x = p->x*cosf(Zrot)+p->y*sinf(Zrot); - p->y = -temp*sinf(Zrot)+p->y*cosf(Zrot); + p->x = p->x * cosf(Zrot) + p->y * sinf(Zrot); + p->y = -temp * sinf(Zrot) + p->y * cosf(Zrot); } @@ -74,100 +74,98 @@ bool_t nav_line_osam_run(uint8_t From_WP, uint8_t To_WP, float radius, float Spa struct Point2D P; float dv; - switch(CFLStatus) - { - case FLInitialize: + switch (CFLStatus) { + case FLInitialize: - //Translate WPs so From_WP is origin - V.x = WaypointX(To_WP) - WaypointX(From_WP); - V.y = WaypointY(To_WP) - WaypointY(From_WP); + //Translate WPs so From_WP is origin + V.x = WaypointX(To_WP) - WaypointX(From_WP); + V.y = WaypointY(To_WP) - WaypointY(From_WP); - //Record Aircraft Position - P.x = stateGetPositionEnu_f()->x; - P.y = stateGetPositionEnu_f()->y; + //Record Aircraft Position + P.x = stateGetPositionEnu_f()->x; + P.y = stateGetPositionEnu_f()->y; - //Rotate Aircraft Position so V is aligned with x axis - TranslateAndRotateFromWorld(&P, atan2f(V.y,V.x), WaypointX(From_WP), WaypointY(From_WP)); + //Rotate Aircraft Position so V is aligned with x axis + TranslateAndRotateFromWorld(&P, atan2f(V.y, V.x), WaypointX(From_WP), WaypointY(From_WP)); - //Find which side of the flight line the aircraft is on - if(P.y > 0) - FLRadius = -radius; - else - FLRadius = radius; + //Find which side of the flight line the aircraft is on + if (P.y > 0) { + FLRadius = -radius; + } else { + FLRadius = radius; + } - //Find unit vector of V - dv = sqrtf(V.x*V.x+V.y*V.y); - V.x = V.x / dv; - V.y = V.y / dv; + //Find unit vector of V + dv = sqrtf(V.x * V.x + V.y * V.y); + V.x = V.x / dv; + V.y = V.y / dv; - //Find begin and end points of flight line - FLFROMWP.x = -V.x*Space_Before; - FLFROMWP.y = -V.y*Space_Before; + //Find begin and end points of flight line + FLFROMWP.x = -V.x * Space_Before; + FLFROMWP.y = -V.y * Space_Before; - FLTOWP.x = V.x*(dv+Space_After); - FLTOWP.y = V.y*(dv+Space_After); + FLTOWP.x = V.x * (dv + Space_After); + FLTOWP.y = V.y * (dv + Space_After); - //Find center of circle - FLCircle.x = FLFROMWP.x + V.y * FLRadius; - FLCircle.y = FLFROMWP.y - V.x * FLRadius; + //Find center of circle + FLCircle.x = FLFROMWP.x + V.y * FLRadius; + FLCircle.y = FLFROMWP.y - V.x * FLRadius; - //Find the angle to exit the circle - FLQDR = atan2f(FLFROMWP.x-FLCircle.x, FLFROMWP.y-FLCircle.y); + //Find the angle to exit the circle + FLQDR = atan2f(FLFROMWP.x - FLCircle.x, FLFROMWP.y - FLCircle.y); - //Translate back - FLFROMWP.x = FLFROMWP.x + WaypointX(From_WP); - FLFROMWP.y = FLFROMWP.y + WaypointY(From_WP); + //Translate back + FLFROMWP.x = FLFROMWP.x + WaypointX(From_WP); + FLFROMWP.y = FLFROMWP.y + WaypointY(From_WP); - FLTOWP.x = FLTOWP.x + WaypointX(From_WP); - FLTOWP.y = FLTOWP.y + WaypointY(From_WP); + FLTOWP.x = FLTOWP.x + WaypointX(From_WP); + FLTOWP.y = FLTOWP.y + WaypointY(From_WP); - FLCircle.x = FLCircle.x + WaypointX(From_WP); - FLCircle.y = FLCircle.y + WaypointY(From_WP); + FLCircle.x = FLCircle.x + WaypointX(From_WP); + FLCircle.y = FLCircle.y + WaypointY(From_WP); - CFLStatus = FLCircleS; - nav_init_stage(); - - break; - - case FLCircleS: - - NavVerticalAutoThrottleMode(0); /* No pitch */ - NavVerticalAltitudeMode(waypoints[From_WP].a, 0); - - nav_circle_XY(FLCircle.x, FLCircle.y, FLRadius); - - if(NavCircleCount() > 0.2 && NavQdrCloseTo(DegOfRad(FLQDR))) - { - CFLStatus = FLLine; - LINE_START_FUNCTION; + CFLStatus = FLCircleS; nav_init_stage(); - } - break; - case FLLine: + break; - NavVerticalAutoThrottleMode(0); /* No pitch */ - NavVerticalAltitudeMode(waypoints[From_WP].a, 0); + case FLCircleS: - nav_route_xy(FLFROMWP.x,FLFROMWP.y,FLTOWP.x,FLTOWP.y); + NavVerticalAutoThrottleMode(0); /* No pitch */ + NavVerticalAltitudeMode(waypoints[From_WP].a, 0); + + nav_circle_XY(FLCircle.x, FLCircle.y, FLRadius); + + if (NavCircleCount() > 0.2 && NavQdrCloseTo(DegOfRad(FLQDR))) { + CFLStatus = FLLine; + LINE_START_FUNCTION; + nav_init_stage(); + } + break; + + case FLLine: + + NavVerticalAutoThrottleMode(0); /* No pitch */ + NavVerticalAltitudeMode(waypoints[From_WP].a, 0); + + nav_route_xy(FLFROMWP.x, FLFROMWP.y, FLTOWP.x, FLTOWP.y); - if(nav_approaching_xy(FLTOWP.x,FLTOWP.y,FLFROMWP.x,FLFROMWP.y, 0)) - { - CFLStatus = FLFinished; - LINE_STOP_FUNCTION; + if (nav_approaching_xy(FLTOWP.x, FLTOWP.y, FLFROMWP.x, FLFROMWP.y, 0)) { + CFLStatus = FLFinished; + LINE_STOP_FUNCTION; + nav_init_stage(); + } + break; + + case FLFinished: + CFLStatus = FLInitialize; nav_init_stage(); - } - break; + return FALSE; + break; - case FLFinished: - CFLStatus = FLInitialize; - nav_init_stage(); - return FALSE; - break; - - default: - break; + default: + break; } return TRUE; @@ -177,29 +175,22 @@ static uint8_t FLBlockCount = 0; bool_t nav_line_osam_block_run(uint8_t First_WP, uint8_t Last_WP, float radius, float Space_Before, float Space_After) { - if(First_WP < Last_WP) - { - nav_line_osam_run(First_WP+FLBlockCount, First_WP+FLBlockCount+1, radius, Space_Before, Space_After); + if (First_WP < Last_WP) { + nav_line_osam_run(First_WP + FLBlockCount, First_WP + FLBlockCount + 1, radius, Space_Before, Space_After); - if(CFLStatus == FLInitialize) - { + if (CFLStatus == FLInitialize) { FLBlockCount++; - if(First_WP+FLBlockCount >= Last_WP) - { + if (First_WP + FLBlockCount >= Last_WP) { FLBlockCount = 0; return FALSE; } } - } - else - { - nav_line_osam_run(First_WP-FLBlockCount, First_WP-FLBlockCount-1, radius, Space_Before, Space_After); + } else { + nav_line_osam_run(First_WP - FLBlockCount, First_WP - FLBlockCount - 1, radius, Space_Before, Space_After); - if(CFLStatus == FLInitialize) - { + if (CFLStatus == FLInitialize) { FLBlockCount++; - if(First_WP-FLBlockCount <= Last_WP) - { + if (First_WP - FLBlockCount <= Last_WP) { FLBlockCount = 0; return FALSE; } diff --git a/sw/airborne/modules/nav/nav_line_osam.h b/sw/airborne/modules/nav/nav_line_osam.h index 3a6fa04826..70dda5297f 100644 --- a/sw/airborne/modules/nav/nav_line_osam.h +++ b/sw/airborne/modules/nav/nav_line_osam.h @@ -30,6 +30,7 @@ #include "std.h" extern bool_t nav_line_osam_run(uint8_t From_WP, uint8_t To_WP, float radius, float Space_Before, float Space_After); -extern bool_t nav_line_osam_block_run(uint8_t First_WP, uint8_t Last_WP, float radius, float Space_Before, float Space_After); +extern bool_t nav_line_osam_block_run(uint8_t First_WP, uint8_t Last_WP, float radius, float Space_Before, + float Space_After); #endif diff --git a/sw/airborne/modules/nav/nav_smooth.c b/sw/airborne/modules/nav/nav_smooth.c index 49de503d79..2dfe414996 100644 --- a/sw/airborne/modules/nav/nav_smooth.c +++ b/sw/airborne/modules/nav/nav_smooth.c @@ -46,7 +46,8 @@ static float u_a_ca_x, u_a_ca_y; static uint8_t ground_speed_timer; /* D is the current position */ -bool_t snav_init(uint8_t a, float desired_course_rad, float radius) { +bool_t snav_init(uint8_t a, float desired_course_rad, float radius) +{ wp_a = a; radius = fabs(radius); @@ -56,7 +57,7 @@ bool_t snav_init(uint8_t a, float desired_course_rad, float radius) { /* D_CD orthogonal to current course, CD on the side of A */ float u_x = cos(M_PI_2 - (*stateGetHorizontalSpeedDir_f())); float u_y = sin(M_PI_2 - (*stateGetHorizontalSpeedDir_f())); - d_radius = - Sign(u_x*da_y - u_y*da_x) * radius; + d_radius = - Sign(u_x * da_y - u_y * da_x) * radius; wp_cd.x = stateGetPositionEnu_f()->x + d_radius * u_y; wp_cd.y = stateGetPositionEnu_f()->y - d_radius * u_x; wp_cd.a = WaypointAlt(wp_a); @@ -64,7 +65,7 @@ bool_t snav_init(uint8_t a, float desired_course_rad, float radius) { /* A_CA orthogonal to desired course, CA on the side of D */ float desired_u_x = cos(M_PI_2 - desired_course_rad); float desired_u_y = sin(M_PI_2 - desired_course_rad); - a_radius = Sign(desired_u_x*da_y - desired_u_y*da_x) * radius; + a_radius = Sign(desired_u_x * da_y - desired_u_y * da_x) * radius; u_a_ca_x = desired_u_y; u_a_ca_y = - desired_u_x; wp_ca.x = WaypointX(wp_a) + a_radius * u_a_ca_x; @@ -74,7 +75,7 @@ bool_t snav_init(uint8_t a, float desired_course_rad, float radius) { /* Unit vector along CD-CA */ u_x = wp_ca.x - wp_cd.x; u_y = wp_ca.y - wp_cd.y; - float cd_ca = sqrt(u_x*u_x+u_y*u_y); + float cd_ca = sqrt(u_x * u_x + u_y * u_y); /* If it is too close in reverse direction, set CA on the other side */ if (a_radius * d_radius < 0 && cd_ca < 2 * radius) { @@ -83,7 +84,7 @@ bool_t snav_init(uint8_t a, float desired_course_rad, float radius) { wp_ca.y = WaypointY(wp_a) + a_radius * u_a_ca_y; u_x = wp_ca.x - wp_cd.x; u_y = wp_ca.y - wp_cd.y; - cd_ca = sqrt(u_x*u_x+u_y*u_y); + cd_ca = sqrt(u_x * u_x + u_y * u_y); } u_x /= cd_ca; @@ -100,15 +101,15 @@ bool_t snav_init(uint8_t a, float desired_course_rad, float radius) { wp_ta.y = wp_ca.y + a_radius * u_x; } else { /* Arcs are in reverse directions: trigonemetric puzzle :-) */ - float alpha = atan2(u_y, u_x) + acos(d_radius/(cd_ca/2)); + float alpha = atan2(u_y, u_x) + acos(d_radius / (cd_ca / 2)); wp_td.x = wp_cd.x + d_radius * cos(alpha); wp_td.y = wp_cd.y + d_radius * sin(alpha); wp_ta.x = wp_ca.x + a_radius * cos(alpha); wp_ta.y = wp_ca.y + a_radius * sin(alpha); } - qdr_td = M_PI_2 - atan2(wp_td.y-wp_cd.y, wp_td.x-wp_cd.x); - qdr_a = M_PI_2 - atan2(WaypointY(wp_a)-wp_ca.y, WaypointX(wp_a)-wp_ca.x); + qdr_td = M_PI_2 - atan2(wp_td.y - wp_cd.y, wp_td.x - wp_cd.x); + qdr_a = M_PI_2 - atan2(WaypointY(wp_a) - wp_ca.y, WaypointX(wp_a) - wp_ca.x); wp_td.a = wp_cd.a; wp_ta.a = wp_ca.a; ground_speed_timer = 0; @@ -117,15 +118,17 @@ bool_t snav_init(uint8_t a, float desired_course_rad, float radius) { } -bool_t snav_circle1(void) { +bool_t snav_circle1(void) +{ /* circle around CD until QDR_TD */ NavVerticalAutoThrottleMode(0); /* No pitch */ NavVerticalAltitudeMode(wp_cd.a, 0.); nav_circle_XY(wp_cd.x, wp_cd.y, d_radius); - return(! NavQdrCloseTo(DegOfRad(qdr_td))); + return (! NavQdrCloseTo(DegOfRad(qdr_td))); } -bool_t snav_route(void) { +bool_t snav_route(void) +{ /* Straight route from TD to TA */ NavVerticalAutoThrottleMode(0); /* No pitch */ NavVerticalAltitudeMode(wp_cd.a, 0.); @@ -134,59 +137,65 @@ bool_t snav_route(void) { return (! nav_approaching_xy(wp_ta.x, wp_ta.y, wp_td.x, wp_td.y, CARROT)); } -bool_t snav_circle2(void) { +bool_t snav_circle2(void) +{ /* circle around CA until QDR_A */ NavVerticalAutoThrottleMode(0); /* No pitch */ NavVerticalAltitudeMode(wp_cd.a, 0.); nav_circle_XY(wp_ca.x, wp_ca.y, a_radius); - return(! NavQdrCloseTo(DegOfRad(qdr_a))); + return (! NavQdrCloseTo(DegOfRad(qdr_a))); } #define NB_ANGLES 24 #define ANGLE_STEP (2.*M_PI/NB_ANGLES) static float ground_speeds[NB_ANGLES]; /* Indexed by trigo angles */ -static inline float ground_speed_of_course(float x) { - uint8_t i = Norm2Pi(M_PI_2-x)/ANGLE_STEP; +static inline float ground_speed_of_course(float x) +{ + uint8_t i = Norm2Pi(M_PI_2 - x) / ANGLE_STEP; return ground_speeds[i]; } /* 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; - for( i=0; i < NB_ANGLES; i++, alpha+=ANGLE_STEP) { + float c = wind_x * wind_x + wind_y * wind_y - airspeed * airspeed; + for (i = 0; i < NB_ANGLES; i++, alpha += ANGLE_STEP) { /* g^2 -2 scal g + c = 0 */ - float scal = wind_x*cos(alpha) + wind_y*sin(alpha); - float delta = 4 * (scal*scal - c); - ground_speeds[i] = scal + sqrt(delta)/2.; - Bound(ground_speeds[i], NOMINAL_AIRSPEED/4, 2*NOMINAL_AIRSPEED); + float scal = wind_x * cos(alpha) + wind_y * sin(alpha); + float delta = 4 * (scal * scal - c); + ground_speeds[i] = scal + sqrt(delta) / 2.; + Bound(ground_speeds[i], NOMINAL_AIRSPEED / 4, 2 * NOMINAL_AIRSPEED); } } /* Adjusting a circle around CA, tangent in A, to end at snav_desired_tow */ -bool_t snav_on_time(float nominal_radius) { +bool_t snav_on_time(float nominal_radius) +{ nominal_radius = fabs(nominal_radius); - float current_qdr = M_PI_2 - atan2(stateGetPositionEnu_f()->y-wp_ca.y, stateGetPositionEnu_f()->x-wp_ca.x); - float remaining_angle = Norm2Pi(Sign(a_radius)*(qdr_a - current_qdr)); + float current_qdr = M_PI_2 - atan2(stateGetPositionEnu_f()->y - wp_ca.y, stateGetPositionEnu_f()->x - wp_ca.x); + float remaining_angle = Norm2Pi(Sign(a_radius) * (qdr_a - current_qdr)); float remaining_time = snav_desired_tow - gps.tow / 1000.; /* Use the nominal airspeed if the estimated one is not realistic */ float airspeed = *stateGetAirspeed_f(); if (airspeed < NOMINAL_AIRSPEED / 2. || - airspeed > 2.* NOMINAL_AIRSPEED) + airspeed > 2.* NOMINAL_AIRSPEED) { airspeed = NOMINAL_AIRSPEED; + } /* Recompute ground speeds every 10 s */ if (ground_speed_timer == 0) { ground_speed_timer = 40; /* every 10s, called at 40Hz */ - compute_ground_speed(airspeed, stateGetHorizontalWindspeed_f()->y, stateGetHorizontalWindspeed_f()->x); // Wind in NED frame + compute_ground_speed(airspeed, stateGetHorizontalWindspeed_f()->y, + stateGetHorizontalWindspeed_f()->x); // Wind in NED frame } ground_speed_timer--; @@ -196,18 +205,19 @@ bool_t snav_on_time(float nominal_radius) { float a; float ground_speed = NOMINAL_AIRSPEED; /* Init to avoid a warning */ /* Going one step too far */ - for(a = 0; a < remaining_angle + ANGLE_STEP; a += ANGLE_STEP) { - float qdr = current_qdr + Sign(a_radius)*a; - ground_speed = ground_speed_of_course(qdr+Sign(a_radius)*M_PI_2); - nominal_time += ANGLE_STEP*nominal_radius/ground_speed; + for (a = 0; a < remaining_angle + ANGLE_STEP; a += ANGLE_STEP) { + float qdr = current_qdr + Sign(a_radius) * a; + ground_speed = ground_speed_of_course(qdr + Sign(a_radius) * M_PI_2); + nominal_time += ANGLE_STEP * nominal_radius / ground_speed; } /* Removing what exceeds remaining_angle */ - nominal_time -= (a-remaining_angle)*nominal_radius/ground_speed; + nominal_time -= (a - remaining_angle) * nominal_radius / ground_speed; /* Radius size to finish in one single circle */ float radius = remaining_time / nominal_time * nominal_radius; - if (radius > 2. * nominal_radius) + if (radius > 2. * nominal_radius) { radius = nominal_radius; + } NavVerticalAutoThrottleMode(0); /* No pitch */ NavVerticalAltitudeMode(wp_cd.a, 0.); @@ -218,5 +228,5 @@ bool_t snav_on_time(float nominal_radius) { nav_circle_XY(wp_ca.x, wp_ca.y, radius); /* Stay in this mode until the end of time */ - return(remaining_time > 0); + return (remaining_time > 0); } diff --git a/sw/airborne/modules/nav/nav_spiral.c b/sw/airborne/modules/nav/nav_spiral.c index 65c2607919..1abbf489c0 100644 --- a/sw/airborne/modules/nav/nav_spiral.c +++ b/sw/airborne/modules/nav/nav_spiral.c @@ -54,8 +54,9 @@ bool_t nav_spiral_setup(uint8_t center_wp, uint8_t edge_wp, float radius_start, nav_spiral.radius_start = radius_start; // start radius of the helix nav_spiral.segments = segments; nav_spiral.radius_min = NAV_SPIRAL_MIN_CIRCLE_RADIUS; - if (nav_spiral.radius_start < nav_spiral.radius_min) + if (nav_spiral.radius_start < nav_spiral.radius_min) { nav_spiral.radius_start = nav_spiral.radius_min; + } nav_spiral.radius_increment = radius_inc; // multiplier for increasing the spiral struct FloatVect2 edge; @@ -73,13 +74,14 @@ bool_t nav_spiral_setup(uint8_t center_wp, uint8_t edge_wp, float radius_start, nav_spiral.dist_from_center = FLOAT_VECT3_NORM(nav_spiral.trans_current); // nav_spiral.alpha_limit denotes angle, where the radius will be increased - nav_spiral.alpha_limit = 2*M_PI / nav_spiral.segments; + nav_spiral.alpha_limit = 2 * M_PI / nav_spiral.segments; //current position nav_spiral.fly_from.x = stateGetPositionEnu_f()->x; nav_spiral.fly_from.y = stateGetPositionEnu_f()->y; - if(nav_spiral.dist_from_center > nav_spiral.radius) + if (nav_spiral.dist_from_center > nav_spiral.radius) { nav_spiral.status = SpiralOutside; + } return FALSE; } @@ -94,8 +96,7 @@ bool_t nav_spiral_run(void) float DistanceStartEstim; float CircleAlpha; - switch(nav_spiral.status) - { + switch (nav_spiral.status) { case SpiralOutside: //flys until center of the helix is reached an start helix nav_route_xy(nav_spiral.fly_from.x, nav_spiral.fly_from.y, nav_spiral.center.x, nav_spiral.center.y); @@ -113,12 +114,12 @@ bool_t nav_spiral_run(void) // storage of current coordinates // calculation needed, State switch to SpiralCircle nav_circle_XY(nav_spiral.center.y, nav_spiral.center.y, nav_spiral.radius_start); - if(nav_spiral.dist_from_center >= nav_spiral.radius_start){ + if (nav_spiral.dist_from_center >= nav_spiral.radius_start) { VECT2_COPY(nav_spiral.last_circle, pos_enu); nav_spiral.status = SpiralCircle; // Start helix #ifdef DIGITAL_CAM - dc_Circle(360/nav_spiral.segments); + dc_Circle(360 / nav_spiral.segments); #endif } break; @@ -133,7 +134,7 @@ bool_t nav_spiral_run(void) struct FloatVect2 pos_diff; VECT2_DIFF(pos_diff, nav_spiral.last_circle, pos_enu); DistanceStartEstim = float_vect2_norm(&pos_diff); - CircleAlpha = (2.0 * asin (DistanceStartEstim / (2 * nav_spiral.radius_start))); + CircleAlpha = (2.0 * asin(DistanceStartEstim / (2 * nav_spiral.radius_start))); if (CircleAlpha >= nav_spiral.alpha_limit) { VECT2_COPY(nav_spiral.last_circle, pos_enu); nav_spiral.status = SpiralInc; @@ -142,18 +143,16 @@ bool_t nav_spiral_run(void) } case SpiralInc: // increasing circle radius as long as it is smaller than max helix radius - if(nav_spiral.radius_start + nav_spiral.radius_increment < nav_spiral.radius) - { + if (nav_spiral.radius_start + nav_spiral.radius_increment < nav_spiral.radius) { nav_spiral.radius_start = nav_spiral.radius_start + nav_spiral.radius_increment; #ifdef DIGITAL_CAM if (dc_cam_tracing) { // calculating Cam angle for camera alignment nav_spiral.trans_current.z = stateGetPositionUtm_f()->alt - nav_spiral.center.z; - dc_cam_angle = atan(nav_spiral.radius_start/nav_spiral.trans_current.z) * 180 / M_PI; + dc_cam_angle = atan(nav_spiral.radius_start / nav_spiral.trans_current.z) * 180 / M_PI; } #endif - } - else { + } else { nav_spiral.radius_start = nav_spiral.radius; #ifdef DIGITAL_CAM // Stopps DC diff --git a/sw/airborne/modules/nav/nav_spiral.h b/sw/airborne/modules/nav/nav_spiral.h index cd8b82b90e..51ddf0ec11 100644 --- a/sw/airborne/modules/nav/nav_spiral.h +++ b/sw/airborne/modules/nav/nav_spiral.h @@ -53,6 +53,6 @@ extern struct NavSpiral nav_spiral; extern bool_t nav_spiral_run(void); extern bool_t nav_spiral_setup(uint8_t center_wp, uint8_t edge_wp, float radius_start, - float radius_inc, float segments); + float radius_inc, float segments); #endif // NAV_SPIRAL_H diff --git a/sw/airborne/modules/nav/nav_survey_disc.c b/sw/airborne/modules/nav/nav_survey_disc.c index 505b241c68..9ec01faa27 100644 --- a/sw/airborne/modules/nav/nav_survey_disc.c +++ b/sw/airborne/modules/nav/nav_survey_disc.c @@ -46,7 +46,8 @@ struct DiscSurvey { static struct DiscSurvey disc_survey; -bool_t nav_survey_disc_setup( float grid ) { +bool_t nav_survey_disc_setup(float grid) +{ nav_survey_shift = grid; disc_survey.status = DOWNWIND; disc_survey.sign = 1; @@ -55,8 +56,9 @@ bool_t nav_survey_disc_setup( float grid ) { return FALSE; } -bool_t nav_survey_disc_run( uint8_t center_wp, float radius) { - struct FloatVect2* wind = stateGetHorizontalWindspeed_f(); +bool_t nav_survey_disc_run(uint8_t center_wp, float radius) +{ + struct FloatVect2 *wind = stateGetHorizontalWindspeed_f(); float wind_dir = atan2(wind->x, wind->y) + M_PI; /** Not null even if wind_east=wind_north=0 */ @@ -67,52 +69,52 @@ bool_t nav_survey_disc_run( uint8_t center_wp, float radius) { float grid = nav_survey_shift / 2; switch (disc_survey.status) { - case UTURN: - nav_circle_XY(disc_survey.c.x, disc_survey.c.y, grid*disc_survey.sign); - if (NavQdrCloseTo(DegOfRad(M_PI_2-wind_dir))) { - disc_survey.c1.x = stateGetPositionEnu_f()->x; - disc_survey.c1.y = stateGetPositionEnu_f()->y; + case UTURN: + nav_circle_XY(disc_survey.c.x, disc_survey.c.y, grid * disc_survey.sign); + if (NavQdrCloseTo(DegOfRad(M_PI_2 - wind_dir))) { + disc_survey.c1.x = stateGetPositionEnu_f()->x; + disc_survey.c1.y = stateGetPositionEnu_f()->y; - struct FloatVect2 dist; - VECT2_DIFF(dist, disc_survey.c1, waypoints[center_wp]); - float d = VECT2_DOT_PRODUCT(upwind, dist); - if (d > radius) { - disc_survey.status = DOWNWIND; - } else { - float w = sqrtf(radius*radius - d*d) - 1.5*grid; + struct FloatVect2 dist; + VECT2_DIFF(dist, disc_survey.c1, waypoints[center_wp]); + float d = VECT2_DOT_PRODUCT(upwind, dist); + if (d > radius) { + disc_survey.status = DOWNWIND; + } else { + float w = sqrtf(radius * radius - d * d) - 1.5 * grid; - struct FloatVect2 crosswind; - crosswind.x = -upwind.y; - crosswind.y = upwind.x; + struct FloatVect2 crosswind; + crosswind.x = -upwind.y; + crosswind.y = upwind.x; - disc_survey.c2.x = waypoints[center_wp].x + d*upwind.x - w*disc_survey.sign*crosswind.x; - disc_survey.c2.y = waypoints[center_wp].y + d*upwind.y - w*disc_survey.sign*crosswind.y; + disc_survey.c2.x = waypoints[center_wp].x + d * upwind.x - w * disc_survey.sign * crosswind.x; + disc_survey.c2.y = waypoints[center_wp].y + d * upwind.y - w * disc_survey.sign * crosswind.y; - disc_survey.status = SEGMENT; + disc_survey.status = SEGMENT; + } + nav_init_stage(); } - nav_init_stage(); - } - break; + break; - case DOWNWIND: - disc_survey.c2.x = waypoints[center_wp].x - upwind.x * radius; - disc_survey.c2.y = waypoints[center_wp].y - upwind.y * radius; - disc_survey.status = SEGMENT; - /* No break; */ + case DOWNWIND: + disc_survey.c2.x = waypoints[center_wp].x - upwind.x * radius; + disc_survey.c2.y = waypoints[center_wp].y - upwind.y * radius; + disc_survey.status = SEGMENT; + /* No break; */ - case SEGMENT: - nav_route_xy(disc_survey.c1.x, disc_survey.c1.y, disc_survey.c2.x, disc_survey.c2.y); - if (nav_approaching_xy(disc_survey.c2.x, disc_survey.c2.y, disc_survey.c1.x, disc_survey.c1.y, CARROT)) { - disc_survey.c.x = disc_survey.c2.x + grid*upwind.x; - disc_survey.c.y = disc_survey.c2.y + grid*upwind.y; + case SEGMENT: + nav_route_xy(disc_survey.c1.x, disc_survey.c1.y, disc_survey.c2.x, disc_survey.c2.y); + if (nav_approaching_xy(disc_survey.c2.x, disc_survey.c2.y, disc_survey.c1.x, disc_survey.c1.y, CARROT)) { + disc_survey.c.x = disc_survey.c2.x + grid * upwind.x; + disc_survey.c.y = disc_survey.c2.y + grid * upwind.y; - disc_survey.sign = -disc_survey.sign; - disc_survey.status = UTURN; - nav_init_stage(); - } - break; - default: - break; + disc_survey.sign = -disc_survey.sign; + disc_survey.status = UTURN; + nav_init_stage(); + } + break; + default: + break; } NavVerticalAutoThrottleMode(0.); /* No pitch */ diff --git a/sw/airborne/modules/nav/nav_survey_disc.h b/sw/airborne/modules/nav/nav_survey_disc.h index 1752d3b1a4..385268ce81 100644 --- a/sw/airborne/modules/nav/nav_survey_disc.h +++ b/sw/airborne/modules/nav/nav_survey_disc.h @@ -29,7 +29,7 @@ #include "std.h" -extern bool_t nav_survey_disc_setup( float grid ); +extern bool_t nav_survey_disc_setup(float grid); extern bool_t nav_survey_disc_run(uint8_t c, float radius); #endif /* NAV_SURVEY_DISC_H */ diff --git a/sw/airborne/modules/nav/nav_survey_poly_osam.c b/sw/airborne/modules/nav/nav_survey_poly_osam.c index 17053d7c81..4edd3acfcd 100644 --- a/sw/airborne/modules/nav/nav_survey_poly_osam.c +++ b/sw/airborne/modules/nav/nav_survey_poly_osam.c @@ -75,16 +75,16 @@ bool_t nav_survey_poly_osam_setup_towards(uint8_t FirstWP, uint8_t Size, float S { float dx = waypoints[SecondWP].x - waypoints[FirstWP].x; float dy = waypoints[SecondWP].y - waypoints[FirstWP].y; - if (dx == 0.0f) dx = 0.000000001; - float ang = atan(dy/dx); + if (dx == 0.0f) { dx = 0.000000001; } + float ang = atan(dy / dx); //if values passed, use it. - if ( Size == 0 ) {Size = Poly_Size;} - if ( Sweep == 0) {Sweep = Poly_Sweep;} + if (Size == 0) {Size = Poly_Size;} + if (Sweep == 0) {Sweep = Poly_Sweep;} return nav_survey_poly_osam_setup(FirstWP, Size, Sweep, DegOfRad(ang)); } struct Point2D {float x; float y;}; -struct Line {float m;float b;float x;}; +struct Line {float m; float b; float x;}; static void TranslateAndRotateFromWorld(struct Point2D *p, float Zrot, float transX, float transY); static void RotateAndTranslateToWorld(struct Point2D *p, float Zrot, float transX, float transY); @@ -144,15 +144,17 @@ bool_t nav_survey_poly_osam_setup(uint8_t EntryWP, uint8_t Size, float sw, float float PolySurveyEntryDistance = POLY_OSAM_FIRST_SWEEP_DISTANCE; float PolySurveyEntryRadius = POLY_OSAM_ENTRY_RADIUS; - if (PolySurveyEntryDistance == 0) - entry_distance = sw/2; - else + if (PolySurveyEntryDistance == 0) { + entry_distance = sw / 2; + } else { entry_distance = PolySurveyEntryDistance; + } - if (PolySurveyEntryRadius == 0) - EntryRadius = sw/2; - else + if (PolySurveyEntryRadius == 0) { + EntryRadius = sw / 2; + } else { EntryRadius = PolySurveyEntryRadius; + } SurveyTheta = RadOfDeg(Orientation); PolySurveySweepNum = 0; @@ -165,38 +167,40 @@ bool_t nav_survey_poly_osam_setup(uint8_t EntryWP, uint8_t Size, float sw, float CSurveyStatus = Init; - if (Size == 0) + if (Size == 0) { return TRUE; + } //Don't initialize if Polygon is too big or if the orientation is not between 0 and 90 - if(Size <= PolygonSize && Orientation >= -90 && Orientation <= 90) - { + if (Size <= PolygonSize && Orientation >= -90 && Orientation <= 90) { //Initialize Corners - for(i = 0; i < Size; i++) - { - Corners[i].x = waypoints[i+EntryWP].x; - Corners[i].y = waypoints[i+EntryWP].y; + for (i = 0; i < Size; i++) { + Corners[i].x = waypoints[i + EntryWP].x; + Corners[i].y = waypoints[i + EntryWP].y; } //Rotate Corners so sweeps are parellel with x axis - for(i = 0; i < Size; i++) + for (i = 0; i < Size; i++) { TranslateAndRotateFromWorld(&Corners[i], SurveyTheta, 0, 0); + } //Find min x and min y SmallestCorner.y = Corners[0].y; SmallestCorner.x = Corners[0].x; - for(i = 1; i < Size; i++) - { - if(Corners[i].y < SmallestCorner.y) + for (i = 1; i < Size; i++) { + if (Corners[i].y < SmallestCorner.y) { SmallestCorner.y = Corners[i].y; + } - if(Corners[i].x < SmallestCorner.x) + if (Corners[i].x < SmallestCorner.x) { SmallestCorner.x = Corners[i].x; + } } //Translate Corners all exist in quad #1 - for(i = 0; i < Size; i++) + for (i = 0; i < Size; i++) { TranslateAndRotateFromWorld(&Corners[i], 0, SmallestCorner.x, SmallestCorner.y); + } //Rotate and Translate Entry Point EntryPoint.x = Corners[0].x; @@ -204,119 +208,102 @@ bool_t nav_survey_poly_osam_setup(uint8_t EntryWP, uint8_t Size, float sw, float //Find max y MaxY = Corners[0].y; - for(i = 1; i < Size; i++) - { - if(Corners[i].y > MaxY) + for (i = 1; i < Size; i++) { + if (Corners[i].y > MaxY) { MaxY = Corners[i].y; + } } //Find polygon edges - for(i = 0; i < Size; i++) - { - if(i == 0) - if(Corners[Size-1].x == Corners[i].x) //Don't divide by zero! + for (i = 0; i < Size; i++) { + if (i == 0) + if (Corners[Size - 1].x == Corners[i].x) { //Don't divide by zero! Edges[i].m = MaxFloat; - else - Edges[i].m = ((Corners[Size-1].y-Corners[i].y)/(Corners[Size-1].x-Corners[i].x)); - else - if(Corners[i].x == Corners[i-1].x) - Edges[i].m = MaxFloat; - else - Edges[i].m = ((Corners[i].y-Corners[i-1].y)/(Corners[i].x-Corners[i-1].x)); + } else { + Edges[i].m = ((Corners[Size - 1].y - Corners[i].y) / (Corners[Size - 1].x - Corners[i].x)); + } + else if (Corners[i].x == Corners[i - 1].x) { + Edges[i].m = MaxFloat; + } else { + Edges[i].m = ((Corners[i].y - Corners[i - 1].y) / (Corners[i].x - Corners[i - 1].x)); + } //Edges[i].m = MaxFloat; - Edges[i].b = (Corners[i].y - (Corners[i].x*Edges[i].m)); + Edges[i].b = (Corners[i].y - (Corners[i].x * Edges[i].m)); } //Find Min and Max y for each line FindInterceptOfTwoLines(&temp, &LeftYInt, Edges[0], Edges[1]); - FindInterceptOfTwoLines(&temp, &RightYInt, Edges[0], Edges[Size-1]); + FindInterceptOfTwoLines(&temp, &RightYInt, Edges[0], Edges[Size - 1]); - if(LeftYInt > RightYInt) - { + if (LeftYInt > RightYInt) { EdgeMaxY[0] = LeftYInt; EdgeMinY[0] = RightYInt; - } - else - { + } else { EdgeMaxY[0] = RightYInt; EdgeMinY[0] = LeftYInt; } - for(i = 1; i < Size-1; i++) - { - FindInterceptOfTwoLines(&temp, &LeftYInt, Edges[i], Edges[i+1]); - FindInterceptOfTwoLines(&temp, &RightYInt, Edges[i], Edges[i-1]); + for (i = 1; i < Size - 1; i++) { + FindInterceptOfTwoLines(&temp, &LeftYInt, Edges[i], Edges[i + 1]); + FindInterceptOfTwoLines(&temp, &RightYInt, Edges[i], Edges[i - 1]); - if(LeftYInt > RightYInt) - { + if (LeftYInt > RightYInt) { EdgeMaxY[i] = LeftYInt; EdgeMinY[i] = RightYInt; - } - else - { + } else { EdgeMaxY[i] = RightYInt; EdgeMinY[i] = LeftYInt; } } - FindInterceptOfTwoLines(&temp, &LeftYInt, Edges[Size-1], Edges[0]); - FindInterceptOfTwoLines(&temp, &RightYInt, Edges[Size-1], Edges[Size-2]); + FindInterceptOfTwoLines(&temp, &LeftYInt, Edges[Size - 1], Edges[0]); + FindInterceptOfTwoLines(&temp, &RightYInt, Edges[Size - 1], Edges[Size - 2]); - if(LeftYInt > RightYInt) - { - EdgeMaxY[Size-1] = LeftYInt; - EdgeMinY[Size-1] = RightYInt; - } - else - { - EdgeMaxY[Size-1] = RightYInt; - EdgeMinY[Size-1] = LeftYInt; + if (LeftYInt > RightYInt) { + EdgeMaxY[Size - 1] = LeftYInt; + EdgeMinY[Size - 1] = RightYInt; + } else { + EdgeMaxY[Size - 1] = RightYInt; + EdgeMinY[Size - 1] = LeftYInt; } //Find amount to increment by every sweep - if(EntryPoint.y >= MaxY/2) - { + if (EntryPoint.y >= MaxY / 2) { entry_distance = -entry_distance; EntryRadius = -EntryRadius; dSweep = -sw; - } - else - { + } else { EntryRadius = EntryRadius; dSweep = sw; } //CircleQdr tells the plane when to exit the circle - if(dSweep >= 0) + if (dSweep >= 0) { SurveyCircleQdr = -DegOfRad(SurveyTheta); - else - SurveyCircleQdr = 180-DegOfRad(SurveyTheta); + } else { + SurveyCircleQdr = 180 - DegOfRad(SurveyTheta); + } //Find y value of the first sweep - ys = EntryPoint.y+entry_distance; + ys = EntryPoint.y + entry_distance; //Find the edges which intercet the sweep line first - for(i = 0; i < SurveySize; i++) - { - if(EdgeMinY[i] <= ys && EdgeMaxY[i] > ys) - { + for (i = 0; i < SurveySize; i++) { + if (EdgeMinY[i] <= ys && EdgeMaxY[i] > ys) { XIntercept2 = XIntercept1; XIntercept1 = EvaluateLineForX(ys, Edges[i]); } } //Find point to come from and point to go to - if(fabs(EntryPoint.x - XIntercept2) <= fabs(EntryPoint.x - XIntercept1)) - { + if (fabs(EntryPoint.x - XIntercept2) <= fabs(EntryPoint.x - XIntercept1)) { SurveyToWP.x = XIntercept1; SurveyToWP.y = ys; SurveyFromWP.x = XIntercept2; SurveyFromWP.y = ys; - } - else - { + } else { SurveyToWP.x = XIntercept2; SurveyToWP.y = ys; @@ -325,12 +312,13 @@ bool_t nav_survey_poly_osam_setup(uint8_t EntryWP, uint8_t Size, float sw, float } //Find the direction to circle - if(ys > 0 && SurveyToWP.x > SurveyFromWP.x) + if (ys > 0 && SurveyToWP.x > SurveyFromWP.x) { SurveyRadius = EntryRadius; - else if(ys < 0 && SurveyToWP.x < SurveyFromWP.x) + } else if (ys < 0 && SurveyToWP.x < SurveyFromWP.x) { SurveyRadius = EntryRadius; - else + } else { SurveyRadius = -EntryRadius; + } //Find the entry circle SurveyCircle.x = SurveyFromWP.x; @@ -364,226 +352,204 @@ bool_t nav_survey_poly_osam_run(void) NavVerticalAutoThrottleMode(0); /* No pitch */ NavVerticalAltitudeMode(waypoints[SurveyEntryWP].a, 0.); - switch(CSurveyStatus) - { - case Entry: - //Rotate and translate circle point into real world - C.x = SurveyCircle.x; - C.y = SurveyCircle.y; - RotateAndTranslateToWorld(&C, 0, SmallestCorner.x, SmallestCorner.y); - RotateAndTranslateToWorld(&C, SurveyTheta, 0, 0); + switch (CSurveyStatus) { + case Entry: + //Rotate and translate circle point into real world + C.x = SurveyCircle.x; + C.y = SurveyCircle.y; + RotateAndTranslateToWorld(&C, 0, SmallestCorner.x, SmallestCorner.y); + RotateAndTranslateToWorld(&C, SurveyTheta, 0, 0); - //follow the circle - nav_circle_XY(C.x, C.y, SurveyRadius); + //follow the circle + nav_circle_XY(C.x, C.y, SurveyRadius); - if(NavQdrCloseTo(SurveyCircleQdr) && NavCircleCountNoRewind() > .1 && stateGetPositionUtm_f()->alt > waypoints[SurveyEntryWP].a-10) - { - CSurveyStatus = Sweep; - nav_init_stage(); - //LINE_START_FUNCTION; - } - break; - case Sweep: - LastHalfSweep = HalfSweep; - ToP.x = SurveyToWP.x; - ToP.y = SurveyToWP.y; - FromP.x = SurveyFromWP.x; - FromP.y = SurveyFromWP.y; + if (NavQdrCloseTo(SurveyCircleQdr) && NavCircleCountNoRewind() > .1 + && stateGetPositionUtm_f()->alt > waypoints[SurveyEntryWP].a - 10) { + CSurveyStatus = Sweep; + nav_init_stage(); + //LINE_START_FUNCTION; + } + break; + case Sweep: + LastHalfSweep = HalfSweep; + ToP.x = SurveyToWP.x; + ToP.y = SurveyToWP.y; + FromP.x = SurveyFromWP.x; + FromP.y = SurveyFromWP.y; - //Rotate and Translate de plane position to local world - C.x = stateGetPositionEnu_f()->x; - C.y = stateGetPositionEnu_f()->y; - TranslateAndRotateFromWorld(&C, SurveyTheta, 0, 0); - TranslateAndRotateFromWorld(&C, 0, SmallestCorner.x, SmallestCorner.y); + //Rotate and Translate de plane position to local world + C.x = stateGetPositionEnu_f()->x; + C.y = stateGetPositionEnu_f()->y; + TranslateAndRotateFromWorld(&C, SurveyTheta, 0, 0); + TranslateAndRotateFromWorld(&C, 0, SmallestCorner.x, SmallestCorner.y); #ifdef DIGITAL_CAM - { - //calc distance from line start and plane position (use only X position because y can be far due to wind or other factor) - float dist = FromP.x - C.x; - - // verify if plane are less than 10 meter from line start - if ((dc_autoshoot == DC_AUTOSHOOT_STOP) && (fabs(dist) < 10)) { - LINE_START_FUNCTION; + //calc distance from line start and plane position (use only X position because y can be far due to wind or other factor) + float dist = FromP.x - C.x; + + // verify if plane are less than 10 meter from line start + if ((dc_autoshoot == DC_AUTOSHOOT_STOP) && (fabs(dist) < 10)) { + LINE_START_FUNCTION; + } } - } #endif - //Rotate and Translate Line points into real world - RotateAndTranslateToWorld(&ToP, 0, SmallestCorner.x, SmallestCorner.y); - RotateAndTranslateToWorld(&ToP, SurveyTheta, 0, 0); + //Rotate and Translate Line points into real world + RotateAndTranslateToWorld(&ToP, 0, SmallestCorner.x, SmallestCorner.y); + RotateAndTranslateToWorld(&ToP, SurveyTheta, 0, 0); - RotateAndTranslateToWorld(&FromP, 0, SmallestCorner.x, SmallestCorner.y); - RotateAndTranslateToWorld(&FromP, SurveyTheta, 0, 0); + RotateAndTranslateToWorld(&FromP, 0, SmallestCorner.x, SmallestCorner.y); + RotateAndTranslateToWorld(&FromP, SurveyTheta, 0, 0); - //follow the line - nav_route_xy(FromP.x,FromP.y,ToP.x,ToP.y); - if(nav_approaching_xy(ToP.x,ToP.y,FromP.x,FromP.y, 0)) - { - LastPoint.x = SurveyToWP.x; - LastPoint.y = SurveyToWP.y; + //follow the line + nav_route_xy(FromP.x, FromP.y, ToP.x, ToP.y); + if (nav_approaching_xy(ToP.x, ToP.y, FromP.x, FromP.y, 0)) { + LastPoint.x = SurveyToWP.x; + LastPoint.y = SurveyToWP.y; - if(LastPoint.y+dSweep >= MaxY || LastPoint.y+dSweep <= 0) //Your out of the Polygon so Sweep Back or Half Sweep - { - if(LastPoint.y+(dSweep/2) >= MaxY || LastPoint.y+(dSweep/2) <= 0) //Sweep back - { - dSweep = -dSweep; - if (LastHalfSweep) - { - HalfSweep = FALSE; - ys = LastPoint.y+(dSweep); - } - else - { + if (LastPoint.y + dSweep >= MaxY || LastPoint.y + dSweep <= 0) { //Your out of the Polygon so Sweep Back or Half Sweep + if (LastPoint.y + (dSweep / 2) >= MaxY || LastPoint.y + (dSweep / 2) <= 0) { //Sweep back + dSweep = -dSweep; + if (LastHalfSweep) { + HalfSweep = FALSE; + ys = LastPoint.y + (dSweep); + } else { + HalfSweep = TRUE; + ys = LastPoint.y + (dSweep / 2); + } + + if (dSweep >= 0) { + SurveyCircleQdr = -DegOfRad(SurveyTheta); + } else { + SurveyCircleQdr = 180 - DegOfRad(SurveyTheta); + } + PolySurveySweepBackNum++; + } else { // Half Sweep forward + ys = LastPoint.y + (dSweep / 2); + + if (dSweep >= 0) { + SurveyCircleQdr = -DegOfRad(SurveyTheta); + } else { + SurveyCircleQdr = 180 - DegOfRad(SurveyTheta); + } HalfSweep = TRUE; - ys = LastPoint.y+(dSweep/2); } - if(dSweep >= 0) - SurveyCircleQdr = -DegOfRad(SurveyTheta); - else - SurveyCircleQdr = 180-DegOfRad(SurveyTheta); - PolySurveySweepBackNum++; - } - else // Half Sweep forward - { - ys = LastPoint.y+(dSweep/2); - if(dSweep >= 0) - SurveyCircleQdr = -DegOfRad(SurveyTheta); - else - SurveyCircleQdr = 180-DegOfRad(SurveyTheta); - HalfSweep = TRUE; - } + } else { // Normal sweep + //Find y value of the first sweep + HalfSweep = FALSE; + ys = LastPoint.y + dSweep; + } + //Find the edges which intercet the sweep line first + for (i = 0; i < SurveySize; i++) { + if (EdgeMinY[i] < ys && EdgeMaxY[i] >= ys) { + XIntercept2 = XIntercept1; + XIntercept1 = EvaluateLineForX(ys, Edges[i]); + } + } - } - else // Normal sweep - { - //Find y value of the first sweep - HalfSweep = FALSE; - ys = LastPoint.y+dSweep; + //Find point to come from and point to go to + DInt1 = XIntercept1 - LastPoint.x; + DInt2 = XIntercept2 - LastPoint.x; + + if (DInt1 * DInt2 >= 0) { + if (fabs(DInt2) <= fabs(DInt1)) { + SurveyToWP.x = XIntercept1; + SurveyToWP.y = ys; + + SurveyFromWP.x = XIntercept2; + SurveyFromWP.y = ys; + } else { + SurveyToWP.x = XIntercept2; + SurveyToWP.y = ys; + + SurveyFromWP.x = XIntercept1; + SurveyFromWP.y = ys; + } + } else { + if ((SurveyToWP.x - SurveyFromWP.x) > 0 && DInt2 > 0) { + SurveyToWP.x = XIntercept1; + SurveyToWP.y = ys; + + SurveyFromWP.x = XIntercept2; + SurveyFromWP.y = ys; + } else if ((SurveyToWP.x - SurveyFromWP.x) < 0 && DInt2 < 0) { + SurveyToWP.x = XIntercept1; + SurveyToWP.y = ys; + + SurveyFromWP.x = XIntercept2; + SurveyFromWP.y = ys; + } else { + SurveyToWP.x = XIntercept2; + SurveyToWP.y = ys; + + SurveyFromWP.x = XIntercept1; + SurveyFromWP.y = ys; + } + } + + //Find the radius to circle + if (!HalfSweep || use_full_circle) { + temp = dSweep / 2; + } else { + temp = dSweep / 4; + } + + //if less than min radius + if (fabs(temp) < min_radius) { + if (temp < 0) { temp = -min_radius; } else { temp = min_radius; } + } + + //Find the direction to circle + if (ys > 0 && SurveyToWP.x > SurveyFromWP.x) { + SurveyRadius = temp; + } else if (ys < 0 && SurveyToWP.x < SurveyFromWP.x) { + SurveyRadius = temp; + } else { + SurveyRadius = -temp; + } + + //find x position to circle + if (fabs(LastPoint.x - SurveyToWP.x) > fabs(SurveyFromWP.x - SurveyToWP.x)) { + SurveyCircle.x = LastPoint.x; + } else { + SurveyCircle.x = SurveyFromWP.x; + } + + //y position to circle + SurveyCircle.y = ys - temp; + + //Go into circle state + CSurveyStatus = SweepCircle; + nav_init_stage(); + LINE_STOP_FUNCTION; + PolySurveySweepNum++; } - //Find the edges which intercet the sweep line first - for(i = 0; i < SurveySize; i++) - { - if(EdgeMinY[i] < ys && EdgeMaxY[i] >= ys) - { - XIntercept2 = XIntercept1; - XIntercept1 = EvaluateLineForX(ys, Edges[i]); - } + break; + case SweepCircle: + //Rotate and translate circle point into real world + C.x = SurveyCircle.x; + C.y = SurveyCircle.y; + RotateAndTranslateToWorld(&C, 0, SmallestCorner.x, SmallestCorner.y); + RotateAndTranslateToWorld(&C, SurveyTheta, 0, 0); + + //follow the circle + nav_circle_XY(C.x, C.y, SurveyRadius); + + if (NavQdrCloseTo(SurveyCircleQdr) && NavCircleCount() > 0) { + CSurveyStatus = Sweep; + nav_init_stage(); + //LINE_START_FUNCTION; } - - //Find point to come from and point to go to - DInt1 = XIntercept1 - LastPoint.x; - DInt2 = XIntercept2 - LastPoint.x; - - if(DInt1 * DInt2 >= 0) - { - if(fabs(DInt2) <= fabs(DInt1)) - { - SurveyToWP.x = XIntercept1; - SurveyToWP.y = ys; - - SurveyFromWP.x = XIntercept2; - SurveyFromWP.y = ys; - } - else - { - SurveyToWP.x = XIntercept2; - SurveyToWP.y = ys; - - SurveyFromWP.x = XIntercept1; - SurveyFromWP.y = ys; - } - } - else - { - if((SurveyToWP.x - SurveyFromWP.x) > 0 && DInt2 > 0) - { - SurveyToWP.x = XIntercept1; - SurveyToWP.y = ys; - - SurveyFromWP.x = XIntercept2; - SurveyFromWP.y = ys; - } - else if((SurveyToWP.x - SurveyFromWP.x) < 0 && DInt2 < 0) - { - SurveyToWP.x = XIntercept1; - SurveyToWP.y = ys; - - SurveyFromWP.x = XIntercept2; - SurveyFromWP.y = ys; - } - else - { - SurveyToWP.x = XIntercept2; - SurveyToWP.y = ys; - - SurveyFromWP.x = XIntercept1; - SurveyFromWP.y = ys; - } - } - - //Find the radius to circle - if (!HalfSweep || use_full_circle) - temp = dSweep/2; - else - temp = dSweep/4; - - //if less than min radius - if (fabs(temp) < min_radius) - { - if (temp < 0) temp = -min_radius; else temp = min_radius; - } - - //Find the direction to circle - if(ys > 0 && SurveyToWP.x > SurveyFromWP.x) - SurveyRadius = temp; - else if(ys < 0 && SurveyToWP.x < SurveyFromWP.x) - SurveyRadius = temp; - else - SurveyRadius = -temp; - - //find x position to circle - if(fabs(LastPoint.x-SurveyToWP.x) > fabs(SurveyFromWP.x-SurveyToWP.x)) - SurveyCircle.x = LastPoint.x; - else - SurveyCircle.x = SurveyFromWP.x; - - //y position to circle - SurveyCircle.y = ys - temp; - - //Go into circle state - CSurveyStatus = SweepCircle; - nav_init_stage(); - LINE_STOP_FUNCTION; - PolySurveySweepNum++; - } - - break; - case SweepCircle: - //Rotate and translate circle point into real world - C.x = SurveyCircle.x; - C.y = SurveyCircle.y; - RotateAndTranslateToWorld(&C, 0, SmallestCorner.x, SmallestCorner.y); - RotateAndTranslateToWorld(&C, SurveyTheta, 0, 0); - - //follow the circle - nav_circle_XY(C.x, C.y, SurveyRadius); - - if(NavQdrCloseTo(SurveyCircleQdr) && NavCircleCount() > 0) - { - CSurveyStatus = Sweep; - nav_init_stage(); - //LINE_START_FUNCTION; - } - break; - case Init: - return FALSE; - default: - return FALSE; + break; + case Init: + return FALSE; + default: + return FALSE; } return TRUE; } @@ -600,8 +566,8 @@ void TranslateAndRotateFromWorld(struct Point2D *p, float Zrot, float transX, fl p->y = p->y - transY; temp = p->x; - p->x = p->x*cosf(Zrot)+p->y*sinf(Zrot); - p->y = -temp*sinf(Zrot)+p->y*cosf(Zrot); + p->x = p->x * cosf(Zrot) + p->y * sinf(Zrot); + p->y = -temp * sinf(Zrot) + p->y * cosf(Zrot); } /// Rotates point round z by -Zrot then translates so (0,0) becomes (transX,transY) @@ -609,8 +575,8 @@ void RotateAndTranslateToWorld(struct Point2D *p, float Zrot, float transX, floa { float temp = p->x; - p->x = p->x*cosf(Zrot)-p->y*sinf(Zrot); - p->y = temp*sinf(Zrot)+p->y*cosf(Zrot); + p->x = p->x * cosf(Zrot) - p->y * sinf(Zrot); + p->y = temp * sinf(Zrot) + p->y * cosf(Zrot); p->x = p->x + transX; p->y = p->y + transY; @@ -618,12 +584,12 @@ void RotateAndTranslateToWorld(struct Point2D *p, float Zrot, float transX, floa void FindInterceptOfTwoLines(float *x, float *y, struct Line L1, struct Line L2) { - *x = ((L2.b-L1.b)/(L1.m-L2.m)); - *y = L1.m*(*x)+L1.b; + *x = ((L2.b - L1.b) / (L1.m - L2.m)); + *y = L1.m * (*x) + L1.b; } float EvaluateLineForX(float y, struct Line L) { - return ((y-L.b)/L.m); + return ((y - L.b) / L.m); } diff --git a/sw/airborne/modules/nav/nav_survey_polygon.c b/sw/airborne/modules/nav/nav_survey_polygon.c index 49715aa6af..609f1d07c6 100644 --- a/sw/airborne/modules/nav/nav_survey_polygon.c +++ b/sw/airborne/modules/nav/nav_survey_polygon.c @@ -52,18 +52,19 @@ static void nav_points(struct FloatVect2 start, struct FloatVect2 end) * @param x, y first line is defined by point x and y (goes through this points) * @param a1, a2, b1, b2 second line by coordinates a1/a2, b1/b2 */ -static bool_t intercept_two_lines(struct FloatVect2 *p, struct FloatVect2 x, struct FloatVect2 y, float a1, float a2, float b1, float b2) +static bool_t intercept_two_lines(struct FloatVect2 *p, struct FloatVect2 x, struct FloatVect2 y, float a1, float a2, + float b1, float b2) { float divider, fac; - divider = (((b2 - a2)*(y.x - x.x)) + ((x.y - y.y)*(b1 - a1))); - if (divider == 0) return FALSE; - fac = ((y.x*(x.y - a2)) + (x.x*(a2 - y.y)) + (a1*(y.y - x.y))) / divider; - if (fac > 1.0) return FALSE; - if (fac < 0.0) return FALSE; + divider = (((b2 - a2) * (y.x - x.x)) + ((x.y - y.y) * (b1 - a1))); + if (divider == 0) { return FALSE; } + fac = ((y.x * (x.y - a2)) + (x.x * (a2 - y.y)) + (a1 * (y.y - x.y))) / divider; + if (fac > 1.0) { return FALSE; } + if (fac < 0.0) { return FALSE; } - p->x = a1 + fac*(b1 - a1); - p->y = a2 + fac*(b2 - a2); + p->x = a1 + fac * (b1 - a1); + p->y = a2 + fac * (b2 - a2); return TRUE; } @@ -79,13 +80,13 @@ static bool_t get_two_intersects(struct FloatVect2 *x, struct FloatVect2 *y, str int i, count = 0; struct FloatVect2 tmp; - for (i=0;i fabs(survey.dir_vec.y)) { - if ((y->x - x->x) / survey.dir_vec.x < 0.0){ + if ((y->x - x->x) / survey.dir_vec.x < 0.0) { tmp = *x; *x = *y; *y = tmp; } + } else if ((y->y - x->y) / survey.dir_vec.y < 0.0) { + tmp = *x; + *x = *y; + *y = tmp; } - else - if ((y->y - x->y) / survey.dir_vec.y < 0.0) { - tmp = *x; - *x = *y; - *y = tmp; - } return TRUE; } @@ -129,14 +132,15 @@ static bool_t get_two_intersects(struct FloatVect2 *x, struct FloatVect2 *y, str * @param min_rad minimal radius when navigating * @param altitude the altitude that must be reached before the flyover starts **/ -bool_t nav_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist, float min_rad, float altitude) +bool_t nav_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist, + float min_rad, float altitude) { int i; struct FloatVect2 small, sweep; - float divider, angle_rad = angle/180.0*M_PI; + float divider, angle_rad = angle / 180.0 * M_PI; - if (angle < 0.0) angle += 360.0; - if (angle >= 360.0) angle -= 360.0; + if (angle < 0.0) { angle += 360.0; } + if (angle >= 360.0) { angle -= 360.0; } survey.poly_first = first_wp; survey.poly_count = size; @@ -147,34 +151,31 @@ bool_t nav_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, flo survey.psa_altitude = altitude; survey.segment_angle = angle; - survey.return_angle = angle+180; - if (survey.return_angle > 359) survey.return_angle -= 360; + survey.return_angle = angle + 180; + if (survey.return_angle > 359) { survey.return_angle -= 360; } if (angle <= 45.0 || angle >= 315.0) { //north survey.dir_vec.y = 1.0; - survey.dir_vec.x = 1.0*tanf(angle_rad); + survey.dir_vec.x = 1.0 * tanf(angle_rad); sweep.x = 1.0; sweep.y = - survey.dir_vec.x / survey.dir_vec.y; - } - else if (angle <= 135.0) { + } else if (angle <= 135.0) { //east survey.dir_vec.x = 1.0; - survey.dir_vec.y = 1.0/tanf(angle_rad); + survey.dir_vec.y = 1.0 / tanf(angle_rad); sweep.y = - 1.0; sweep.x = survey.dir_vec.y / survey.dir_vec.x; - } - else if (angle <= 225.0) { + } else if (angle <= 225.0) { //south survey.dir_vec.y = -1.0; - survey.dir_vec.x = -1.0*tanf(angle_rad); + survey.dir_vec.x = -1.0 * tanf(angle_rad); sweep.x = -1.0; sweep.y = survey.dir_vec.x / survey.dir_vec.y; - } - else { + } else { //west survey.dir_vec.x = -1.0; - survey.dir_vec.y = -1.0/tanf(angle_rad); + survey.dir_vec.y = -1.0 / tanf(angle_rad); sweep.y = 1.0; sweep.x = - survey.dir_vec.y / survey.dir_vec.x; } @@ -186,26 +187,27 @@ bool_t nav_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, flo VECT2_SMUL(survey.sweep_vec, sweep, survey.psa_sweep_width); //begin at leftmost position (relative to survey.dir_vec) - VECT2_COPY(small,waypoints[survey.poly_first]); + VECT2_COPY(small, waypoints[survey.poly_first]); - divider = (survey.sweep_vec.y*survey.dir_vec.x) - (survey.sweep_vec.x*survey.dir_vec.y); + divider = (survey.sweep_vec.y * survey.dir_vec.x) - (survey.sweep_vec.x * survey.dir_vec.y); //calculate the leftmost point if one sees the dir vec as going "up" and the sweep vec as going right if (divider < 0.0) { - for(i=1;i 0.0) { - VECT2_COPY(small, waypoints[survey.poly_first+i]); + for (i = 1; i < survey.poly_count; i++) + if ((survey.dir_vec.x * (waypoints[survey.poly_first + i].y - small.y)) + (survey.dir_vec.y * + (small.x - waypoints[survey.poly_first + i].x)) > 0.0) { + VECT2_COPY(small, waypoints[survey.poly_first + i]); } - } - else - for(i=1;i 0.0) { - VECT2_COPY(small, waypoints[survey.poly_first+i]); + } else + for (i = 1; i < survey.poly_count; i++) + if ((survey.dir_vec.x * (waypoints[survey.poly_first + i].y - small.y)) + (survey.dir_vec.y * + (small.x - waypoints[survey.poly_first + i].x)) > 0.0) { + VECT2_COPY(small, waypoints[survey.poly_first + i]); } //calculate the line the defines the first flyover - survey.seg_start.x = small.x + 0.5*survey.sweep_vec.x; - survey.seg_start.y = small.y + 0.5*survey.sweep_vec.y; + survey.seg_start.x = small.x + 0.5 * survey.sweep_vec.x; + survey.seg_start.y = small.y + 0.5 * survey.sweep_vec.y; VECT2_SUM(survey.seg_end, survey.seg_start, survey.dir_vec); if (!get_two_intersects(&survey.seg_start, &survey.seg_end, survey.seg_start, survey.seg_end)) { @@ -244,7 +246,8 @@ bool_t nav_survey_polygon_run(void) survey.stage = SEG; nav_init_stage(); #ifdef DIGITAL_CAM - dc_survey(survey.psa_shot_dist, survey.seg_start.x - survey.dir_vec.x*survey.psa_shot_dist*0.5, survey.seg_start.y - survey.dir_vec.y*survey.psa_shot_dist*0.5); + dc_survey(survey.psa_shot_dist, survey.seg_start.x - survey.dir_vec.x * survey.psa_shot_dist * 0.5, + survey.seg_start.y - survey.dir_vec.y * survey.psa_shot_dist * 0.5); #endif } } @@ -257,22 +260,23 @@ bool_t nav_survey_polygon_run(void) dc_stop(); #endif VECT2_DIFF(survey.seg_center1, survey.seg_end, survey.rad_vec); - survey.ret_start.x = survey.seg_end.x - 2*survey.rad_vec.x; - survey.ret_start.y = survey.seg_end.y - 2*survey.rad_vec.y; + survey.ret_start.x = survey.seg_end.x - 2 * survey.rad_vec.x; + survey.ret_start.y = survey.seg_end.y - 2 * survey.rad_vec.y; //if we get no intersection the survey is finished static struct FloatVect2 sum_start_sweep; static struct FloatVect2 sum_end_sweep; VECT2_SUM(sum_start_sweep, survey.seg_start, survey.sweep_vec); VECT2_SUM(sum_end_sweep, survey.seg_end, survey.sweep_vec); - if (!get_two_intersects(&survey.seg_start, &survey.seg_end, sum_start_sweep, sum_end_sweep)) + if (!get_two_intersects(&survey.seg_start, &survey.seg_end, sum_start_sweep, sum_end_sweep)) { return FALSE; + } - survey.ret_end.x = survey.seg_start.x - survey.sweep_vec.x - 2*survey.rad_vec.x; - survey.ret_end.y = survey.seg_start.y - survey.sweep_vec.y - 2*survey.rad_vec.y; + survey.ret_end.x = survey.seg_start.x - survey.sweep_vec.x - 2 * survey.rad_vec.x; + survey.ret_end.y = survey.seg_start.y - survey.sweep_vec.y - 2 * survey.rad_vec.y; - survey.seg_center2.x = survey.seg_start.x - 0.5*(2.0*survey.rad_vec.x+survey.sweep_vec.x); - survey.seg_center2.y = survey.seg_start.y - 0.5*(2.0*survey.rad_vec.y+survey.sweep_vec.y); + survey.seg_center2.x = survey.seg_start.x - 0.5 * (2.0 * survey.rad_vec.x + survey.sweep_vec.x); + survey.seg_center2.y = survey.seg_start.y - 0.5 * (2.0 * survey.rad_vec.y + survey.sweep_vec.y); survey.stage = TURN1; nav_init_stage(); @@ -294,12 +298,13 @@ bool_t nav_survey_polygon_run(void) } //turn from return to stage } else if (survey.stage == TURN2) { - nav_circle_XY(survey.seg_center2.x, survey.seg_center2.y, -(2*survey.psa_min_rad+survey.psa_sweep_width)*0.5); + nav_circle_XY(survey.seg_center2.x, survey.seg_center2.y, -(2 * survey.psa_min_rad + survey.psa_sweep_width) * 0.5); if (NavCourseCloseTo(survey.segment_angle)) { survey.stage = SEG; nav_init_stage(); #ifdef DIGITAL_CAM - dc_survey(survey.psa_shot_dist, survey.seg_start.x - survey.dir_vec.x*survey.psa_shot_dist*0.5, survey.seg_start.y - survey.dir_vec.y*survey.psa_shot_dist*0.5); + dc_survey(survey.psa_shot_dist, survey.seg_start.x - survey.dir_vec.x * survey.psa_shot_dist * 0.5, + survey.seg_start.y - survey.dir_vec.y * survey.psa_shot_dist * 0.5); #endif } } diff --git a/sw/airborne/modules/nav/nav_survey_polygon.h b/sw/airborne/modules/nav/nav_survey_polygon.h index 6a22019728..3e67c54d52 100644 --- a/sw/airborne/modules/nav/nav_survey_polygon.h +++ b/sw/airborne/modules/nav/nav_survey_polygon.h @@ -82,7 +82,8 @@ struct SurveyPolyAdv { struct FloatVect2 ret_end; }; -extern bool_t nav_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist, float min_rad, float altitude); +extern bool_t nav_survey_polygon_setup(uint8_t first_wp, uint8_t size, float angle, float sweep_width, float shot_dist, + float min_rad, float altitude); extern bool_t nav_survey_polygon_run(void); #endif diff --git a/sw/airborne/modules/nav/nav_survey_zamboni.c b/sw/airborne/modules/nav/nav_survey_zamboni.c index 614fb01e0b..c35d6013fb 100644 --- a/sw/airborne/modules/nav/nav_survey_zamboni.c +++ b/sw/airborne/modules/nav/nav_survey_zamboni.c @@ -54,7 +54,8 @@ struct ZamboniSurvey zs; * @param sweep_lines number of sweep_lines to fly * @param altitude the altitude that must be reached before the flyover starts */ -bool_t nav_survey_zamboni_setup(uint8_t center_wp, uint8_t dir_wp, float sweep_length, float sweep_spacing, int sweep_lines, float altitude) +bool_t nav_survey_zamboni_setup(uint8_t center_wp, uint8_t dir_wp, float sweep_length, float sweep_spacing, + int sweep_lines, float altitude) { zs.current_laps = 0; zs.pre_leave_angle = 2; @@ -87,8 +88,8 @@ bool_t nav_survey_zamboni_setup(uint8_t center_wp, uint8_t dir_wp, float sweep_l zs.sweep_width.y = +flight_vec.x * sweep_spacing; // calculate number of laps to fly and turning radius for each end - zs.total_laps = (sweep_lines+1)/2; - zs.turnradius1 = (zs.total_laps-1) * sweep_spacing * 0.5; + zs.total_laps = (sweep_lines + 1) / 2; + zs.turnradius1 = (zs.total_laps - 1) * sweep_spacing * 0.5; zs.turnradius2 = zs.total_laps * sweep_spacing * 0.5; struct FloatVect2 flight_line; @@ -100,7 +101,7 @@ bool_t nav_survey_zamboni_setup(uint8_t center_wp, uint8_t dir_wp, float sweep_l VECT2_SUM(zs.seg_end, zs.wp_center, flight_line); struct FloatVect2 sweep_span; - VECT2_SMUL(sweep_span, zs.sweep_width, zs.total_laps-1); + VECT2_SMUL(sweep_span, zs.sweep_width, zs.total_laps - 1); //start and end of flight-line in return-direction VECT2_DIFF(zs.ret_start, zs.seg_end, sweep_span); VECT2_DIFF(zs.ret_end, zs.seg_start, sweep_span); @@ -146,7 +147,7 @@ bool_t nav_survey_zamboni_run(void) //Turn from stage to return else if (zs.stage == Z_TURN1) { nav_circle_XY(zs.turn_center1.x, zs.turn_center1.y, zs.turnradius1); - if (NavCourseCloseTo(zs.return_angle + zs.pre_leave_angle)){ + if (NavCourseCloseTo(zs.return_angle + zs.pre_leave_angle)) { // && nav_approaching_xy(zs.seg_end.x, zs.seg_end.y, zs.seg_start.x, zs.seg_start.y, CARROT //calculate SEG-points for the next flyover VECT2_ADD(zs.seg_start, zs.sweep_width); @@ -193,8 +194,8 @@ bool_t nav_survey_zamboni_run(void) // calculate the rest of the points for the next fly-over VECT2_ADD(zs.ret_start, zs.sweep_width); VECT2_ADD(zs.ret_end, zs.sweep_width); - zs.turn_center1.x = (zs.seg_end.x + zs.ret_start.x)/2; - zs.turn_center1.y = (zs.seg_end.y + zs.ret_start.y)/2; + zs.turn_center1.x = (zs.seg_end.x + zs.ret_start.x) / 2; + zs.turn_center1.y = (zs.seg_end.y + zs.ret_start.y) / 2; zs.turn_center2.x = (zs.seg_start.x + zs.ret_end.x + zs.sweep_width.x) / 2; zs.turn_center2.y = (zs.seg_start.y + zs.ret_end.y + zs.sweep_width.y) / 2; @@ -211,8 +212,7 @@ bool_t nav_survey_zamboni_run(void) LINE_STOP_FUNCTION; #endif return FALSE; - } - else { + } else { return TRUE; } } diff --git a/sw/airborne/modules/nav/nav_survey_zamboni.h b/sw/airborne/modules/nav/nav_survey_zamboni.h index 9f60be2812..4b5153253f 100644 --- a/sw/airborne/modules/nav/nav_survey_zamboni.h +++ b/sw/airborne/modules/nav/nav_survey_zamboni.h @@ -70,7 +70,8 @@ struct ZamboniSurvey { }; -extern bool_t nav_survey_zamboni_setup(uint8_t center_wp, uint8_t dir_wp, float sweep_length, float sweep_spacing, int sweep_lines, float altitude); +extern bool_t nav_survey_zamboni_setup(uint8_t center_wp, uint8_t dir_wp, float sweep_length, float sweep_spacing, + int sweep_lines, float altitude); extern bool_t nav_survey_zamboni_run(void); #endif //ZAMBONI_SURVEY_H diff --git a/sw/airborne/modules/nav/nav_vertical_raster.c b/sw/airborne/modules/nav/nav_vertical_raster.c index 6f376c22dc..be29c3afd5 100644 --- a/sw/airborne/modules/nav/nav_vertical_raster.c +++ b/sw/airborne/modules/nav/nav_vertical_raster.c @@ -40,19 +40,21 @@ enum line_status { LR12, LQC21, LTC2, LQC22, LR21, LQC12, LTC1, LQC11 }; static enum line_status line_status; -bool_t nav_vertical_raster_setup( void ) { +bool_t nav_vertical_raster_setup(void) +{ line_status = LR12; return FALSE; } -bool_t nav_vertical_raster_run(uint8_t l1, uint8_t l2, float radius, float AltSweep) { +bool_t nav_vertical_raster_run(uint8_t l1, uint8_t l2, float radius, float AltSweep) +{ radius = fabs(radius); float alt = waypoints[l1].a; waypoints[l2].a = alt; float l2_l1_x = WaypointX(l1) - WaypointX(l2); float l2_l1_y = WaypointY(l1) - WaypointY(l2); - float d = sqrt(l2_l1_x*l2_l1_x+l2_l1_y*l2_l1_y); + float d = sqrt(l2_l1_x * l2_l1_x + l2_l1_y * l2_l1_y); /* Unit vector from l1 to l2 */ float u_x = l2_l1_x / d; @@ -60,27 +62,33 @@ bool_t nav_vertical_raster_run(uint8_t l1, uint8_t l2, float radius, float AltSw /* The half circle centers and the other leg */ struct point l2_c1 = { WaypointX(l1) + radius * u_y, - WaypointY(l1) + radius * -u_x, - alt }; - struct point l2_c2 = { WaypointX(l1) + 1.732*radius * u_x, - WaypointY(l1) + 1.732*radius * u_y, - alt }; + WaypointY(l1) + radius * -u_x, + alt + }; + struct point l2_c2 = { WaypointX(l1) + 1.732 * radius * u_x, + WaypointY(l1) + 1.732 * radius * u_y, + alt + }; struct point l2_c3 = { WaypointX(l1) + radius * -u_y, - WaypointY(l1) + radius * u_x, - alt }; + WaypointY(l1) + radius * u_x, + alt + }; struct point l1_c1 = { WaypointX(l2) + radius * -u_y, - WaypointY(l2) + radius * u_x, - alt }; - struct point l1_c2 = { WaypointX(l2) +1.732*radius * -u_x, - WaypointY(l2) + 1.732*radius * -u_y, - alt }; + WaypointY(l2) + radius * u_x, + alt + }; + struct point l1_c2 = { WaypointX(l2) + 1.732 * radius * -u_x, + WaypointY(l2) + 1.732 * radius * -u_y, + alt + }; struct point l1_c3 = { WaypointX(l2) + radius * u_y, - WaypointY(l2) + radius * -u_x, - alt }; - float qdr_out_2_1 = M_PI/3. - atan2(u_y, u_x); + WaypointY(l2) + radius * -u_x, + alt + }; + float qdr_out_2_1 = M_PI / 3. - atan2(u_y, u_x); - float qdr_out_2_2 = -M_PI/3. - atan2(u_y, u_x); + float qdr_out_2_2 = -M_PI / 3. - atan2(u_y, u_x); float qdr_out_2_3 = M_PI - atan2(u_y, u_x); /* Vertical target */ @@ -88,65 +96,65 @@ bool_t nav_vertical_raster_run(uint8_t l1, uint8_t l2, float radius, float AltSw NavVerticalAltitudeMode(WaypointAlt(l1), 0.); switch (line_status) { - case LR12: /* From wp l2 to wp l1 */ - NavSegment(l2, l1); - if (NavApproachingFrom(l1, l2, CARROT)) { - line_status = LQC21; - waypoints[l1].a = waypoints[l1].a+AltSweep; - nav_init_stage(); - } - break; - case LQC21: - nav_circle_XY(l2_c1.x, l2_c1.y, radius); - if (NavQdrCloseTo(DegOfRad(qdr_out_2_1)-10)) { - line_status = LTC2; - nav_init_stage(); - } - break; - case LTC2: - nav_circle_XY(l2_c2.x, l2_c2.y, -radius); - if (NavQdrCloseTo(DegOfRad(qdr_out_2_2)+10) && stateGetPositionUtm_f()->alt >= (waypoints[l1].a-10)) { - line_status = LQC22; - nav_init_stage(); - } - break; - case LQC22: - nav_circle_XY(l2_c3.x, l2_c3.y, radius); - if (NavQdrCloseTo(DegOfRad(qdr_out_2_3)-10)) { - line_status = LR21; - nav_init_stage(); - } - break; - case LR21: /* From wp l1 to wp l2 */ - NavSegment(l1, l2); - if (NavApproachingFrom(l2, l1, CARROT)) { - line_status = LQC12; - waypoints[l1].a = waypoints[l1].a+AltSweep; - nav_init_stage(); - } - break; - case LQC12: - nav_circle_XY(l1_c1.x, l1_c1.y, radius); - if (NavQdrCloseTo(DegOfRad(qdr_out_2_1 + M_PI)-10)) { - line_status = LTC1; - nav_init_stage(); - } - break; - case LTC1: - nav_circle_XY(l1_c2.x, l1_c2.y, -radius); - if (NavQdrCloseTo(DegOfRad(qdr_out_2_2 + M_PI)+10) && stateGetPositionUtm_f()->alt >= (waypoints[l1].a-5)) { - line_status = LQC11; - nav_init_stage(); - } - break; - case LQC11: - nav_circle_XY(l1_c3.x, l1_c3.y, radius); - if (NavQdrCloseTo(DegOfRad(qdr_out_2_3 + M_PI)-10)) { - line_status = LR12; - nav_init_stage(); - } - default: - break; + case LR12: /* From wp l2 to wp l1 */ + NavSegment(l2, l1); + if (NavApproachingFrom(l1, l2, CARROT)) { + line_status = LQC21; + waypoints[l1].a = waypoints[l1].a + AltSweep; + nav_init_stage(); + } + break; + case LQC21: + nav_circle_XY(l2_c1.x, l2_c1.y, radius); + if (NavQdrCloseTo(DegOfRad(qdr_out_2_1) - 10)) { + line_status = LTC2; + nav_init_stage(); + } + break; + case LTC2: + nav_circle_XY(l2_c2.x, l2_c2.y, -radius); + if (NavQdrCloseTo(DegOfRad(qdr_out_2_2) + 10) && stateGetPositionUtm_f()->alt >= (waypoints[l1].a - 10)) { + line_status = LQC22; + nav_init_stage(); + } + break; + case LQC22: + nav_circle_XY(l2_c3.x, l2_c3.y, radius); + if (NavQdrCloseTo(DegOfRad(qdr_out_2_3) - 10)) { + line_status = LR21; + nav_init_stage(); + } + break; + case LR21: /* From wp l1 to wp l2 */ + NavSegment(l1, l2); + if (NavApproachingFrom(l2, l1, CARROT)) { + line_status = LQC12; + waypoints[l1].a = waypoints[l1].a + AltSweep; + nav_init_stage(); + } + break; + case LQC12: + nav_circle_XY(l1_c1.x, l1_c1.y, radius); + if (NavQdrCloseTo(DegOfRad(qdr_out_2_1 + M_PI) - 10)) { + line_status = LTC1; + nav_init_stage(); + } + break; + case LTC1: + nav_circle_XY(l1_c2.x, l1_c2.y, -radius); + if (NavQdrCloseTo(DegOfRad(qdr_out_2_2 + M_PI) + 10) && stateGetPositionUtm_f()->alt >= (waypoints[l1].a - 5)) { + line_status = LQC11; + nav_init_stage(); + } + break; + case LQC11: + nav_circle_XY(l1_c3.x, l1_c3.y, radius); + if (NavQdrCloseTo(DegOfRad(qdr_out_2_3 + M_PI) - 10)) { + line_status = LR12; + nav_init_stage(); + } + default: + break; } return TRUE; /* This pattern never ends */ } diff --git a/sw/airborne/modules/nav/nav_vertical_raster.h b/sw/airborne/modules/nav/nav_vertical_raster.h index 801cca5485..150c514030 100644 --- a/sw/airborne/modules/nav/nav_vertical_raster.h +++ b/sw/airborne/modules/nav/nav_vertical_raster.h @@ -29,7 +29,7 @@ #include "std.h" -extern bool_t nav_vertical_raster_setup( void ); +extern bool_t nav_vertical_raster_setup(void); extern bool_t nav_vertical_raster_run(uint8_t wp1, uint8_t wp2, float radius, float AltSweep); #endif diff --git a/sw/airborne/modules/nav/takeoff_detect.c b/sw/airborne/modules/nav/takeoff_detect.c index 865561e28c..71734bb9b3 100644 --- a/sw/airborne/modules/nav/takeoff_detect.c +++ b/sw/airborne/modules/nav/takeoff_detect.c @@ -91,8 +91,7 @@ void takeoff_detect_periodic(void) if (stateGetNedToBodyEulers_f()->theta > TAKEOFF_DETECT_LAUNCH_PITCH && pprz_mode == PPRZ_MODE_AUTO2) { takeoff_detect.timer++; - } - else { + } else { // else reset timer takeoff_detect.timer = 0; } diff --git a/sw/airborne/modules/openlog/openlog.c b/sw/airborne/modules/openlog/openlog.c index b6a407498a..7947cbd72b 100644 --- a/sw/airborne/modules/openlog/openlog.c +++ b/sw/airborne/modules/openlog/openlog.c @@ -35,10 +35,12 @@ uint32_t timestamp = 0; ///< Timestamp to be incremented during operation -void init_openlog(void) { +void init_openlog(void) +{ } -void periodic_2Hz_openlog(void) { - timestamp=timestamp+500; +void periodic_2Hz_openlog(void) +{ + timestamp = timestamp + 500; DOWNLINK_SEND_TIMESTAMP(DefaultChannel, DefaultDevice, ×tamp); } diff --git a/sw/airborne/modules/optical_flow/px4flow.c b/sw/airborne/modules/optical_flow/px4flow.c index 345ed01b0b..e842bf33a0 100644 --- a/sw/airborne/modules/optical_flow/px4flow.c +++ b/sw/airborne/modules/optical_flow/px4flow.c @@ -42,19 +42,21 @@ bool_t optical_flow_available; struct mavlink_msg_req req; // callback function on message reception -static void decode_optical_flow_msg(struct mavlink_message * msg __attribute__ ((unused))) { +static void decode_optical_flow_msg(struct mavlink_message *msg __attribute__((unused))) +{ optical_flow_available = TRUE; } /** Initialization function */ -void px4flow_init(void) { +void px4flow_init(void) +{ optical_flow_available = FALSE; // register a mavlink message req.msg_id = MAVLINK_OPTICAL_FLOW_MSG_ID; req.callback = decode_optical_flow_msg; - req.msg.payload = (uint8_t*)(&optical_flow); + req.msg.payload = (uint8_t *)(&optical_flow); mavlink_register_msg(&mavlink_tp, &req); } @@ -66,14 +68,15 @@ void px4flow_init(void) { /** Downlink message for debug */ -void px4flow_downlink(void) { +void px4flow_downlink(void) +{ DOWNLINK_SEND_PX4FLOW(DefaultChannel, DefaultDevice, - &optical_flow.sensor_id, - &optical_flow.flow_x, - &optical_flow.flow_y, - &optical_flow.flow_comp_m_x, - &optical_flow.flow_comp_m_y, - &optical_flow.quality, - &optical_flow.ground_distance); + &optical_flow.sensor_id, + &optical_flow.flow_x, + &optical_flow.flow_y, + &optical_flow.flow_comp_m_x, + &optical_flow.flow_comp_m_y, + &optical_flow.quality, + &optical_flow.ground_distance); } diff --git a/sw/airborne/modules/optical_flow/px4flow.h b/sw/airborne/modules/optical_flow/px4flow.h index 0804f45bb1..57c0fcff23 100644 --- a/sw/airborne/modules/optical_flow/px4flow.h +++ b/sw/airborne/modules/optical_flow/px4flow.h @@ -36,8 +36,7 @@ * Message ID 100 * Fields are ordered to guarentee alignment */ -struct mavlink_optical_flow -{ +struct mavlink_optical_flow { uint64_t time_usec; ///< Timestamp (UNIX) float flow_comp_m_x; ///< Flow in meters in x-sensor direction, angular-speed compensated float flow_comp_m_y; ///< Flow in meters in y-sensor direction, angular-speed compensated diff --git a/sw/airborne/modules/poles/nav_poles.c b/sw/airborne/modules/poles/nav_poles.c index cd75107e77..66d05ac059 100644 --- a/sw/airborne/modules/poles/nav_poles.c +++ b/sw/airborne/modules/poles/nav_poles.c @@ -11,10 +11,11 @@ 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); + float d = sqrt(x * x + y * y); /* Unit vector from wp1 to wp2 */ x /= d; diff --git a/sw/airborne/modules/poles/nav_poles.h b/sw/airborne/modules/poles/nav_poles.h index c9f6ad3aa2..14ae57a888 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_ads1114.c b/sw/airborne/modules/sensors/airspeed_ads1114.c index b472beb4ad..7ceea669fc 100644 --- a/sw/airborne/modules/sensors/airspeed_ads1114.c +++ b/sw/airborne/modules/sensors/airspeed_ads1114.c @@ -29,7 +29,8 @@ #include "subsystems/sensors/baro.h" #include "baro_board.h" -void airspeed_periodic(void) { +void airspeed_periodic(void) +{ ads1114_read(&BARO_DIFF_ADS); } diff --git a/sw/airborne/modules/sensors/airspeed_amsys.c b/sw/airborne/modules/sensors/airspeed_amsys.c index 88f979be5e..ee1b7bf5a8 100644 --- a/sw/airborne/modules/sensors/airspeed_amsys.c +++ b/sw/airborne/modules/sensors/airspeed_amsys.c @@ -88,7 +88,8 @@ uint16_t airspeed_amsys_cnt; void airspeed_amsys_downlink(void); -void airspeed_amsys_init( void ) { +void airspeed_amsys_init(void) +{ airspeed_amsys_raw = 0; airspeed_amsys = 0.0; airspeed_amsys_p = 0.0; @@ -101,10 +102,11 @@ void airspeed_amsys_init( void ) { airspeed_filter = AIRSPEED_AMSYS_FILTER; airspeed_amsys_i2c_trans.status = I2CTransDone; airspeed_amsys_cnt = AIRSPEED_AMSYS_OFFSET_NBSAMPLES_INIT + - AIRSPEED_AMSYS_OFFSET_NBSAMPLES_AVRG; + AIRSPEED_AMSYS_OFFSET_NBSAMPLES_AVRG; } -void airspeed_amsys_read_periodic( void ) { +void airspeed_amsys_read_periodic(void) +{ #ifndef SITL if (airspeed_amsys_i2c_trans.status == I2CTransDone) { #ifndef MEASURE_AMSYS_TEMPERATURE @@ -115,12 +117,12 @@ void airspeed_amsys_read_periodic( void ) { } #if USE_AIRSPEED_AMSYS - 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 @@ -129,46 +131,51 @@ void airspeed_amsys_read_periodic( void ) { #endif } -void airspeed_amsys_downlink(void) { +void airspeed_amsys_downlink(void) +{ DOWNLINK_SEND_AMSYS_AIRSPEED(DefaultChannel, DefaultDevice, &airspeed_amsys_raw, &airspeed_amsys_p, &airspeed_amsys_tmp, &airspeed_amsys, &airspeed_temperature); } -void airspeed_amsys_read_event( void ) { +void airspeed_amsys_read_event(void) +{ // Get raw airspeed from buffer airspeed_amsys_raw = 0; - airspeed_amsys_raw = (airspeed_amsys_i2c_trans.buf[0]<<8) | airspeed_amsys_i2c_trans.buf[1]; + airspeed_amsys_raw = (airspeed_amsys_i2c_trans.buf[0] << 8) | airspeed_amsys_i2c_trans.buf[1]; #ifdef MEASURE_AMSYS_TEMPERATURE - tempAS_amsys_raw = (airspeed_amsys_i2c_trans.buf[2]<<8) | airspeed_amsys_i2c_trans.buf[3]; + tempAS_amsys_raw = (airspeed_amsys_i2c_trans.buf[2] << 8) | airspeed_amsys_i2c_trans.buf[3]; const float temp_off_scale = (float)(TEMPERATURE_AMSYS_MAX) / - (TEMPERATURE_AMSYS_OFFSET_MAX - TEMPERATURE_AMSYS_OFFSET_MIN); + (TEMPERATURE_AMSYS_OFFSET_MAX - TEMPERATURE_AMSYS_OFFSET_MIN); // Tmin=-25, Tmax=85 airspeed_temperature = temp_off_scale * (tempAS_amsys_raw - TEMPERATURE_AMSYS_OFFSET_MIN) + - TEMPERATURE_AMSYS_MIN; + TEMPERATURE_AMSYS_MIN; #endif // Check if this is valid airspeed - if (airspeed_amsys_raw == 0) + if (airspeed_amsys_raw == 0) { airspeed_amsys_valid = FALSE; - else + } else { airspeed_amsys_valid = TRUE; + } // Continue only if a new airspeed value was received if (airspeed_amsys_valid) { // raw not under offest min - if (airspeed_amsys_raw < AIRSPEED_AMSYS_OFFSET_MIN) + if (airspeed_amsys_raw < AIRSPEED_AMSYS_OFFSET_MIN) { airspeed_amsys_raw = AIRSPEED_AMSYS_OFFSET_MIN; + } // raw not over offest max - if (airspeed_amsys_raw > AIRSPEED_AMSYS_OFFSET_MAX) + if (airspeed_amsys_raw > AIRSPEED_AMSYS_OFFSET_MAX) { airspeed_amsys_raw = AIRSPEED_AMSYS_OFFSET_MAX; + } // calculate raw to pressure const float p_off_scale = (float)(AIRSPEED_AMSYS_MAXPRESURE) / - (AIRSPEED_AMSYS_OFFSET_MAX - AIRSPEED_AMSYS_OFFSET_MIN); + (AIRSPEED_AMSYS_OFFSET_MAX - AIRSPEED_AMSYS_OFFSET_MIN); airspeed_amsys_p = p_off_scale * (airspeed_amsys_raw - AIRSPEED_AMSYS_OFFSET_MIN); if (!airspeed_amsys_offset_init) { @@ -186,16 +193,16 @@ void airspeed_amsys_read_event( void ) { airspeed_amsys = 0.; - } - else { + } else { airspeed_amsys_p = airspeed_amsys_p - airspeed_amsys_offset; - if (airspeed_amsys_p <= 0) + if (airspeed_amsys_p <= 0) { airspeed_amsys_p = 0.000000001; + } // convert pressure to airspeed airspeed_amsys_tmp = sqrtf(2 * airspeed_amsys_p * airspeed_scale / 1.2041); // Lowpassfiltering airspeed_amsys = airspeed_filter * airspeed_old + - (1.0 - airspeed_filter) * airspeed_amsys_tmp; + (1.0 - airspeed_filter) * airspeed_amsys_tmp; airspeed_old = airspeed_amsys; //New value available diff --git a/sw/airborne/modules/sensors/airspeed_amsys.h b/sw/airborne/modules/sensors/airspeed_amsys.h index ac518b750a..83546659cb 100644 --- a/sw/airborne/modules/sensors/airspeed_amsys.h +++ b/sw/airborne/modules/sensors/airspeed_amsys.h @@ -33,9 +33,9 @@ extern float airspeed_filter; extern struct i2c_transaction airspeed_amsys_i2c_trans; -extern void airspeed_amsys_init( void ); -extern void airspeed_amsys_read_periodic( void ); -extern void airspeed_amsys_read_event( void ); +extern void airspeed_amsys_init(void); +extern void airspeed_amsys_read_periodic(void); +extern void airspeed_amsys_read_event(void); #define AirspeedAmsysEvent() { if (airspeed_amsys_i2c_trans.status == I2CTransSuccess) airspeed_amsys_read_event(); } diff --git a/sw/airborne/modules/sensors/airspeed_ets.c b/sw/airborne/modules/sensors/airspeed_ets.c index e43ebdf489..a673c36c8b 100644 --- a/sw/airborne/modules/sensors/airspeed_ets.c +++ b/sw/airborne/modules/sensors/airspeed_ets.c @@ -101,7 +101,8 @@ uint16_t airspeed_ets_cnt; uint32_t airspeed_ets_delay_time; bool_t airspeed_ets_delay_done; -void airspeed_ets_init( void ) { +void airspeed_ets_init(void) +{ int n; airspeed_ets_raw = 0; airspeed_ets = 0.0; @@ -113,8 +114,9 @@ void airspeed_ets_init( void ) { airspeed_ets_cnt = AIRSPEED_ETS_OFFSET_NBSAMPLES_INIT + AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG; airspeed_ets_buffer_idx = 0; - for (n=0; n < AIRSPEED_ETS_NBSAMPLES_AVRG; ++n) + for (n = 0; n < AIRSPEED_ETS_NBSAMPLES_AVRG; ++n) { airspeed_ets_buffer[n] = 0.0; + } airspeed_ets_i2c_trans.status = I2CTransDone; @@ -122,31 +124,35 @@ void airspeed_ets_init( void ) { SysTimeTimerStart(airspeed_ets_delay_time); } -void airspeed_ets_read_periodic( void ) { +void airspeed_ets_read_periodic(void) +{ #ifndef SITL if (!airspeed_ets_delay_done) { - if (SysTimeTimer(airspeed_ets_delay_time) < USEC_OF_SEC(AIRSPEED_ETS_START_DELAY)) return; - else airspeed_ets_delay_done = TRUE; + if (SysTimeTimer(airspeed_ets_delay_time) < USEC_OF_SEC(AIRSPEED_ETS_START_DELAY)) { return; } + else { airspeed_ets_delay_done = TRUE; } } - if (airspeed_ets_i2c_trans.status == I2CTransDone) + if (airspeed_ets_i2c_trans.status == I2CTransDone) { i2c_receive(&AIRSPEED_ETS_I2C_DEV, &airspeed_ets_i2c_trans, AIRSPEED_ETS_ADDR, 2); + } #elif !defined USE_NPS extern float sim_air_speed; stateSetAirspeed_f(&sim_air_speed); #endif //SITL } -void airspeed_ets_read_event( void ) { +void airspeed_ets_read_event(void) +{ int n; float airspeed_tmp = 0.0; // Get raw airspeed from buffer airspeed_ets_raw = ((uint16_t)(airspeed_ets_i2c_trans.buf[1]) << 8) | (uint16_t)(airspeed_ets_i2c_trans.buf[0]); // Check if this is valid airspeed - if (airspeed_ets_raw == 0) + if (airspeed_ets_raw == 0) { airspeed_ets_valid = FALSE; - else + } else { airspeed_ets_valid = TRUE; + } // Continue only if a new airspeed value was received if (airspeed_ets_valid) { @@ -158,36 +164,45 @@ void airspeed_ets_read_event( void ) { // Calculate average airspeed_ets_offset = (uint16_t)(airspeed_ets_offset_tmp / AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG); // Limit offset - if (airspeed_ets_offset < AIRSPEED_ETS_OFFSET_MIN) + if (airspeed_ets_offset < AIRSPEED_ETS_OFFSET_MIN) { airspeed_ets_offset = AIRSPEED_ETS_OFFSET_MIN; - if (airspeed_ets_offset > AIRSPEED_ETS_OFFSET_MAX) + } + if (airspeed_ets_offset > AIRSPEED_ETS_OFFSET_MAX) { airspeed_ets_offset = AIRSPEED_ETS_OFFSET_MAX; + } airspeed_ets_offset_init = TRUE; } // Check if averaging needs to continue - else if (airspeed_ets_cnt <= AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG) + else if (airspeed_ets_cnt <= AIRSPEED_ETS_OFFSET_NBSAMPLES_AVRG) { airspeed_ets_offset_tmp += airspeed_ets_raw; + } } // Convert raw to m/s #ifdef AIRSPEED_ETS_REVERSE - if (airspeed_ets_offset_init && airspeed_ets_raw < airspeed_ets_offset) - airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf( (float)(airspeed_ets_offset-airspeed_ets_raw) ) - AIRSPEED_ETS_OFFSET; + if (airspeed_ets_offset_init && airspeed_ets_raw < airspeed_ets_offset) { + airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf((float)(airspeed_ets_offset - airspeed_ets_raw)) - AIRSPEED_ETS_OFFSET; + } #else - if (airspeed_ets_offset_init && airspeed_ets_raw > airspeed_ets_offset) - airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf( (float)(airspeed_ets_raw-airspeed_ets_offset) ) - AIRSPEED_ETS_OFFSET; + if (airspeed_ets_offset_init && airspeed_ets_raw > airspeed_ets_offset) { + airspeed_tmp = AIRSPEED_ETS_SCALE * sqrtf((float)(airspeed_ets_raw - airspeed_ets_offset)) - AIRSPEED_ETS_OFFSET; + } #endif - else + else { airspeed_tmp = 0.0; + } // Airspeed should always be positive - if (airspeed_tmp < 0.0) + if (airspeed_tmp < 0.0) { airspeed_tmp = 0.0; + } // Moving average airspeed_ets_buffer[airspeed_ets_buffer_idx++] = airspeed_tmp; - if (airspeed_ets_buffer_idx >= AIRSPEED_ETS_NBSAMPLES_AVRG) + if (airspeed_ets_buffer_idx >= AIRSPEED_ETS_NBSAMPLES_AVRG) { airspeed_ets_buffer_idx = 0; + } airspeed_ets = 0.0; - for (n = 0; n < AIRSPEED_ETS_NBSAMPLES_AVRG; ++n) + for (n = 0; n < AIRSPEED_ETS_NBSAMPLES_AVRG; ++n) { airspeed_ets += airspeed_ets_buffer[n]; + } airspeed_ets = airspeed_ets / (float)AIRSPEED_ETS_NBSAMPLES_AVRG; #if USE_AIRSPEED_ETS stateSetAirspeed_f(&airspeed_ets); diff --git a/sw/airborne/modules/sensors/airspeed_ets.h b/sw/airborne/modules/sensors/airspeed_ets.h index d0eb9972d8..de9565d5c3 100644 --- a/sw/airborne/modules/sensors/airspeed_ets.h +++ b/sw/airborne/modules/sensors/airspeed_ets.h @@ -51,16 +51,15 @@ extern float airspeed_ets; extern struct i2c_transaction airspeed_ets_i2c_trans; -extern void airspeed_ets_init( void ); -extern void airspeed_ets_read_periodic( void ); -extern void airspeed_ets_read_event( void ); +extern void airspeed_ets_init(void); +extern void airspeed_ets_read_periodic(void); +extern void airspeed_ets_read_event(void); static inline void AirspeedEtsEvent(void) { if (airspeed_ets_i2c_trans.status == I2CTransSuccess) { airspeed_ets_read_event(); - } - else if (airspeed_ets_i2c_trans.status == I2CTransFailed) { + } else if (airspeed_ets_i2c_trans.status == I2CTransFailed) { // if transaction failed, mark as done so can be retried airspeed_ets_i2c_trans.status = I2CTransDone; } diff --git a/sw/airborne/modules/sensors/airspeed_otf.c b/sw/airborne/modules/sensors/airspeed_otf.c index 9455d68949..94f7ff147c 100644 --- a/sw/airborne/modules/sensors/airspeed_otf.c +++ b/sw/airborne/modules/sensors/airspeed_otf.c @@ -51,8 +51,8 @@ #define OTF_END 0x0D /* workaround for newlib */ -void* _sbrk(int); -void* _sbrk(int a) {return 0;} +void *_sbrk(int); +void *_sbrk(int a) {return 0;} /* airspeed_otf_parse */ void airspeed_otf_parse(char c) @@ -66,95 +66,99 @@ void airspeed_otf_parse(char c) switch (otf_status) { - case OTF_WAIT_START: - if (c == OTF_START) { - otf_status++; - otf_idx = 0; - } else { - otf_status = OTF_UNINIT; - } - break; + case OTF_WAIT_START: + if (c == OTF_START) { + otf_status++; + otf_idx = 0; + } else { + otf_status = OTF_UNINIT; + } + break; - case OTF_WAIT_COUNTER: - if (isdigit((int)c)) { - if (otf_idx == 0) { + case OTF_WAIT_COUNTER: + if (isdigit((int)c)) { + if (otf_idx == 0) { //FIXME otf_timestamp = getclock(); - } - otf_inp[otf_idx++] = c; - } else { - if ((otf_idx == 5) && (c == OTF_LIMITER)) { - otf_inp[otf_idx] = 0; - counter = atoi(otf_inp); - otf_idx = 0; - otf_crs_idx = 0; - otf_status++; - } else { - otf_status = OTF_UNINIT; - } - } - break; - - case OTF_WAIT_ANGLES: - if (isdigit((int)c) || (c == '-') || (c == '.')) { - otf_inp[otf_idx++] = c; - } else { - if ((otf_idx > 1) && (otf_idx < 9) && (c == OTF_LIMITER)) { - otf_inp[otf_idx] = 0; - course[otf_crs_idx] = (int16_t) (100. * atof(otf_inp)); - otf_idx = 0; - if (otf_crs_idx++ == 2) { - otf_status++; } + otf_inp[otf_idx++] = c; } else { + if ((otf_idx == 5) && (c == OTF_LIMITER)) { + otf_inp[otf_idx] = 0; + counter = atoi(otf_inp); + otf_idx = 0; + otf_crs_idx = 0; + otf_status++; + } else { + otf_status = OTF_UNINIT; + } + } + break; + + case OTF_WAIT_ANGLES: + if (isdigit((int)c) || (c == '-') || (c == '.')) { + otf_inp[otf_idx++] = c; + } else { + if ((otf_idx > 1) && (otf_idx < 9) && (c == OTF_LIMITER)) { + otf_inp[otf_idx] = 0; + course[otf_crs_idx] = (int16_t)(100. * atof(otf_inp)); + otf_idx = 0; + if (otf_crs_idx++ == 2) { + otf_status++; + } + } else { + otf_status = OTF_UNINIT; + } + } + break; + + case OTF_WAIT_ALTITUDE: + if (isdigit((int)c) || (c == '-') || (c == '.')) { + otf_inp[otf_idx++] = c; + } else { + if ((otf_idx > 1) && (otf_idx < 9) && (c == OTF_LIMITER)) { + otf_inp[otf_idx] = 0; + altitude = (int32_t)(100. * atof(otf_inp)); + otf_idx = 0; + otf_status++; + } else { + otf_status = OTF_UNINIT; + } + } + break; + + case OTF_WAIT_CHECKSUM: + if (isxdigit((int)c)) { + otf_inp[otf_idx++] = c; + } else { + if ((otf_idx == 2) && (c == OTF_END)) { + otf_inp[otf_idx] = 0; + checksum = strtol(otf_inp, NULL, 16); + otf_idx = 0; + DOWNLINK_SEND_FLOW_AP_OTF(DefaultChannel, DefaultDevice, &counter, &course[0], &course[1], &course[2], &altitude, + &checksum); + } otf_status = OTF_UNINIT; } - } - break; + break; - case OTF_WAIT_ALTITUDE: - if (isdigit((int)c) || (c == '-') || (c == '.')) { - otf_inp[otf_idx++] = c; - } else { - if ((otf_idx > 1) && (otf_idx < 9) && (c == OTF_LIMITER)) { - otf_inp[otf_idx] = 0; - altitude = (int32_t) (100. * atof(otf_inp)); - otf_idx = 0; - otf_status++; - } else { - otf_status = OTF_UNINIT; - } - } - break; - - case OTF_WAIT_CHECKSUM: - if (isxdigit((int)c)) { - otf_inp[otf_idx++] = c; - } else { - if ((otf_idx == 2) && (c == OTF_END)) { - otf_inp[otf_idx] = 0; - checksum = strtol(otf_inp, NULL, 16); - otf_idx = 0; - DOWNLINK_SEND_FLOW_AP_OTF(DefaultChannel, DefaultDevice, &counter, &course[0], &course[1], &course[2], &altitude, &checksum); - } + default: otf_status = OTF_UNINIT; - } - break; - - default: - otf_status = OTF_UNINIT; - break; + break; } } -void airspeed_otf_init(void) { +void airspeed_otf_init(void) +{ } -void airspeed_otf_event(void) { +void airspeed_otf_event(void) +{ while (MetLink(ChAvailable())) { uint8_t ch = MetLink(Getch()); airspeed_otf_parse(ch); } } -void airspeed_otf_periodic(void) { +void airspeed_otf_periodic(void) +{ } diff --git a/sw/airborne/modules/sensors/alt_srf08.c b/sw/airborne/modules/sensors/alt_srf08.c index 0bc5370d5d..048043ced1 100644 --- a/sw/airborne/modules/sensors/alt_srf08.c +++ b/sw/airborne/modules/sensors/alt_srf08.c @@ -65,29 +65,33 @@ void srf08_init(void) } /*###########################################################################*/ -void srf08_initiate_ranging(void) { -LED_ON(2); +void srf08_initiate_ranging(void) +{ + LED_ON(2); srf_trans.buf[0] = SRF08_COMMAND; srf_trans.buf[1] = SRF08_CENTIMETERS; i2c_transmit(&SRF08_I2C_DEV, &srf_trans, SRF08_UNIT_0, 2); } /** Ask the value to the device */ -void srf08_receive(void) { -LED_OFF(2); +void srf08_receive(void) +{ + LED_OFF(2); srf_trans.buf[0] = SRF08_ECHO_1; srf08_received = TRUE; i2c_transmit(&SRF08_I2C_DEV, &srf_trans, SRF08_UNIT_0, 1); } /** Read values on the bus */ -void srf08_read(void) { +void srf08_read(void) +{ srf08_got = TRUE; i2c_receive(&SRF08_I2C_DEV, &srf_trans, SRF08_UNIT_0, 2); } /** Copy the I2C buffer */ -void srf08_copy(void) { +void srf08_copy(void) +{ srf08_range = srf_trans.buf[0] << 8 | srf_trans.buf[1]; } @@ -113,36 +117,36 @@ uint32_t srf08_read_register(uint8_t srf08_register) srf_trans.buf[0] = srf08_register; /* get high byte msb first */ - if (srf08_register>=2) + if (srf08_register >= 2) { cnt = 2; - else + } else { cnt = 1; + } i2c_transceive(&SRF08_I2C_DEV, &srf_trans, SRF08_UNIT_0, 1, cnt); /* get high byte msb first */ - if(srf08_register>=2) { - i2c.rx_byte[1]=srf_trans.buf[1]; + if (srf08_register >= 2) { + i2c.rx_byte[1] = srf_trans.buf[1]; } /* get low byte msb first */ - i2c.rx_byte[0]=srf_trans.buf[0]; + i2c.rx_byte[0] = srf_trans.buf[0]; - return(i2c.rx_word); + return (i2c.rx_word); } void srf08_event(void) { - float f=0; - uint8_t i=0; + float f = 0; + uint8_t i = 0; /** Handling of data sent by the device (initiated by srf08_receive() */ if (srf_trans.status == I2CTransSuccess) { if (srf08_received) { srf08_received = FALSE; srf08_read(); - } - else if (srf08_got) { + } else if (srf08_got) { srf08_got = FALSE; srf08_copy(); DOWNLINK_SEND_RANGEFINDER(DefaultChannel, DefaultDevice, &srf08_range, &f, &f, &f, &f, &f, &i); diff --git a/sw/airborne/modules/sensors/aoa_adc.c b/sw/airborne/modules/sensors/aoa_adc.c index 14af6512b4..ae0b4da3ea 100644 --- a/sw/airborne/modules/sensors/aoa_adc.c +++ b/sw/airborne/modules/sensors/aoa_adc.c @@ -63,7 +63,8 @@ struct Aoa_Adc aoa_adc; -void aoa_adc_init(void) { +void aoa_adc_init(void) +{ aoa_adc.offset = AOA_OFFSET; aoa_adc.filter = AOA_FILTER; aoa_adc.sens = AOA_SENS; @@ -71,14 +72,15 @@ void aoa_adc_init(void) { adc_buf_channel(ADC_CHANNEL_AOA, &aoa_adc.buf, ADC_CHANNEL_AOA_NB_SAMPLES); } -void aoa_adc_update(void) { +void aoa_adc_update(void) +{ static float prev_aoa = 0.0; aoa_adc.raw = aoa_adc.buf.sum / aoa_adc.buf.av_nb_sample; // PT1 filter and convert to rad aoa_adc.angle = aoa_adc.filter * prev_aoa + - (1.0 - aoa_adc.filter) * (aoa_adc.raw * aoa_adc.sens - aoa_adc.offset); + (1.0 - aoa_adc.filter) * (aoa_adc.raw * aoa_adc.sens - aoa_adc.offset); prev_aoa = aoa_adc.angle; #ifdef USE_AOA diff --git a/sw/airborne/modules/sensors/baro_MS5534A.c b/sw/airborne/modules/sensors/baro_MS5534A.c index e966a70b8a..b6827a98ea 100644 --- a/sw/airborne/modules/sensors/baro_MS5534A.c +++ b/sw/airborne/modules/sensors/baro_MS5534A.c @@ -64,11 +64,11 @@ static uint16_t words[4]; #define InitStatus() (status <= STATUS_INIT4) #define NextStatus() { \ - if (status_read_data) { \ - status++; if (status > STATUS_MEASURE_TEMPERATURE) status = STATUS_MEASURE_PRESSURE; \ - }; \ - status_read_data = !status_read_data; \ -} + if (status_read_data) { \ + status++; if (status > STATUS_MEASURE_TEMPERATURE) status = STATUS_MEASURE_PRESSURE; \ + }; \ + status_read_data = !status_read_data; \ + } #define CMD_INIT 0x1D #define CMD_MEASUREMENT 0x0F @@ -113,16 +113,17 @@ static uint8_t buf_output[3]; #define PWM_DUTY (PWM_PERIOD / 2) -static void calibration( void ); +static void calibration(void); -void baro_MS5534A_init( void ) { +void baro_MS5534A_init(void) +{ /* 32768Hz on PWM2 */ /* Configure P0.7 pin as PWM */ PINSEL0 &= ~(_BV(14)); PINSEL0 |= _BV(15); /* No prescaler */ - PWMPR = PWM_PRESCALER-1; + PWMPR = PWM_PRESCALER - 1; /* To get 32768Hz from a base frequency of 15MHz */ PWMMR0 = PWM_PERIOD; @@ -158,13 +159,15 @@ void baro_MS5534A_init( void ) { baro_MS5534A_do_reset = FALSE; } -void baro_MS5534A_reset( void ) { +void baro_MS5534A_reset(void) +{ status = STATUS_INIT1; status_read_data = FALSE; } /* To be called not faster than 30Hz */ -void baro_MS5534A_send(void) { +void baro_MS5534A_send(void) +{ if (!SpiCheckAvailable()) { SpiOverRun(); return; @@ -172,8 +175,9 @@ void baro_MS5534A_send(void) { if (status == STATUS_RESET) { uint8_t i; - for(i = 0; i < 3; i++) + for (i = 0; i < 3; i++) { buf_output[i] = reset[i]; + } spi_buffer_length = 3; } else { if (status_read_data) { @@ -185,12 +189,13 @@ void baro_MS5534A_send(void) { spi_buffer_length = 2; } - spi_buffer_input = (uint8_t*)&buf_input; - spi_buffer_output = (uint8_t*)&buf_output; - if (status_read_data) + spi_buffer_input = (uint8_t *)&buf_input; + spi_buffer_output = (uint8_t *)&buf_output; + if (status_read_data) { SpiSetCPHA(); - else + } else { SpiClrCPHA(); + } SpiStart(); } @@ -198,7 +203,8 @@ static uint16_t d1, d2; static uint16_t c1, c2, c3, c4, ut1, c6; -static void calibration( void ) { +static void calibration(void) +{ /* End of init, configuration (page 11) */ c1 = words[0] >> 1; c2 = ((words[2] & 0x3f) << 6) | (words[3] & 0x3f); @@ -217,32 +223,33 @@ static void calibration( void ) { /* Handle the SPI message, i.e. store the received values in variables */ -void baro_MS5534A_event_task( void ) { +void baro_MS5534A_event_task(void) +{ if (status_read_data) { switch (status) { - case STATUS_MEASURE_PRESSURE: - d1 = Uint16(buf_input); - break; - case STATUS_MEASURE_TEMPERATURE: - d2 = Uint16(buf_input); - /* Compute pressure and temp (page 10) */ - int16_t dT = d2 - ut1; - baro_MS5534A_temp = 200 + (dT*(c6+50)) / (1 << 10); - int16_t off = c2*4 + ((c4-512)*dT)/(1 << 12); - uint32_t sens = c1 + (c3*dT)/(1<<10) + 24576; - uint16_t x = (sens*(d1-7168))/(1<<14) - off; - // baro_MS5534A = ((x*10)>>5) + 250*10; - baro_MS5534A_pressure = ((x*100)>>5) + 250*100; - baro_MS5534A_available = TRUE; + case STATUS_MEASURE_PRESSURE: + d1 = Uint16(buf_input); + break; + case STATUS_MEASURE_TEMPERATURE: + d2 = Uint16(buf_input); + /* Compute pressure and temp (page 10) */ + int16_t dT = d2 - ut1; + baro_MS5534A_temp = 200 + (dT * (c6 + 50)) / (1 << 10); + int16_t off = c2 * 4 + ((c4 - 512) * dT) / (1 << 12); + uint32_t sens = c1 + (c3 * dT) / (1 << 10) + 24576; + uint16_t x = (sens * (d1 - 7168)) / (1 << 14) - off; + // baro_MS5534A = ((x*10)>>5) + 250*10; + baro_MS5534A_pressure = ((x * 100) >> 5) + 250 * 100; + baro_MS5534A_available = TRUE; - break; - case STATUS_RESET: - break; - default: /* Init status */ - words[status] = Uint16(buf_input); - if (status == STATUS_INIT4) { - calibration(); - } + break; + case STATUS_RESET: + break; + default: /* Init status */ + words[status] = Uint16(buf_input); + if (status == STATUS_INIT4) { + calibration(); + } } } /* else nothing to read */ @@ -253,14 +260,15 @@ void baro_MS5534A_event_task( void ) { } } -void baro_MS5534A_event( void ) { +void baro_MS5534A_event(void) +{ if (spi_message_received) { /* Got a message on SPI. */ spi_message_received = FALSE; baro_MS5534A_event_task(); if (baro_MS5534A_available) { baro_MS5534A_available = FALSE; - baro_MS5534A_z = ground_alt +((float)baro_MS5534A_ground_pressure - baro_MS5534A_pressure)*0.084; + baro_MS5534A_z = ground_alt + ((float)baro_MS5534A_ground_pressure - baro_MS5534A_pressure) * 0.084; #if SENSO_SYNC_SEND DOWNLINK_SEND_BARO_MS5534A(DefaultChannel, DefaultDevice, &baro_MS5534A_pressure, &baro_MS5534A_temp, &baro_MS5534A_z); #endif diff --git a/sw/airborne/modules/sensors/baro_MS5534A.h b/sw/airborne/modules/sensors/baro_MS5534A.h index 0b7b0f6373..c141b12760 100644 --- a/sw/airborne/modules/sensors/baro_MS5534A.h +++ b/sw/airborne/modules/sensors/baro_MS5534A.h @@ -50,9 +50,9 @@ void baro_MS5534A_reset(void); void baro_MS5534A_send(void); /* Set baro_MS5534A_available when pressure and temp are readable */ -void baro_MS5534A_event_task( void ); +void baro_MS5534A_event_task(void); -void baro_MS5534A_event( void ); +void baro_MS5534A_event(void); #endif // USE_BARO_MS5534A diff --git a/sw/airborne/modules/sensors/baro_amsys.c b/sw/airborne/modules/sensors/baro_amsys.c index e45e89b246..af28af453e 100644 --- a/sw/airborne/modules/sensors/baro_amsys.c +++ b/sw/airborne/modules/sensors/baro_amsys.c @@ -94,12 +94,13 @@ bool_t baro_amsys_offset_init; double baro_amsys_offset_tmp; uint16_t baro_amsys_cnt; -void baro_amsys_init( void ) { - baro_filter=BARO_AMSYS_FILTER; +void baro_amsys_init(void) +{ + baro_filter = BARO_AMSYS_FILTER; pBaroRaw = 0; tBaroRaw = 0; baro_amsys_altitude = 0.0; - baro_amsys_p=0.0; + baro_amsys_p = 0.0; baro_amsys_offset = 0; baro_amsys_offset_tmp = 0; baro_amsys_valid = TRUE; @@ -114,9 +115,10 @@ void baro_amsys_init( void ) { baro_amsys_i2c_trans.status = I2CTransDone; } -void baro_amsys_read_periodic( void ) { +void baro_amsys_read_periodic(void) +{ // Initiate next read - if (baro_amsys_i2c_trans.status == I2CTransDone){ + if (baro_amsys_i2c_trans.status == I2CTransDone) { #ifndef MEASURE_AMSYS_TEMPERATURE i2c_receive(&BARO_AMSYS_I2C_DEV, &baro_amsys_i2c_trans, BARO_AMSYS_ADDR, 2); #else @@ -125,25 +127,30 @@ void baro_amsys_read_periodic( void ) { } #ifdef BARO_AMSYS_SYNC_SEND - DOWNLINK_SEND_AMSYS_BARO(DefaultChannel, DefaultDevice, &pBaroRaw, &baro_amsys_p, &baro_amsys_offset, &ref_alt_init, &baro_amsys_abs_altitude, &baro_amsys_altitude, &baro_amsys_temp); + DOWNLINK_SEND_AMSYS_BARO(DefaultChannel, DefaultDevice, &pBaroRaw, &baro_amsys_p, &baro_amsys_offset, &ref_alt_init, + &baro_amsys_abs_altitude, &baro_amsys_altitude, &baro_amsys_temp); #else - RunOnceEvery(10, DOWNLINK_SEND_AMSYS_BARO(DefaultChannel, DefaultDevice, &pBaroRaw, &baro_amsys_p, &baro_amsys_offset, &ref_alt_init, &baro_amsys_abs_altitude, &baro_amsys_altitude, &baro_amsys_temp)); + RunOnceEvery(10, DOWNLINK_SEND_AMSYS_BARO(DefaultChannel, DefaultDevice, &pBaroRaw, &baro_amsys_p, &baro_amsys_offset, + &ref_alt_init, &baro_amsys_abs_altitude, &baro_amsys_altitude, &baro_amsys_temp)); #endif } -void baro_amsys_read_event( void ) { +void baro_amsys_read_event(void) +{ pBaroRaw = 0; // Get raw altimeter from buffer pBaroRaw = (baro_amsys_i2c_trans.buf[0] << 8) | baro_amsys_i2c_trans.buf[1]; #ifdef MEASURE_AMSYS_TEMPERATURE tBaroRaw = (baro_amsys_i2c_trans.buf[2] << 8) | baro_amsys_i2c_trans.buf[3]; - baro_amsys_temp = (float)(tBaroRaw-TEMPERATURE_AMSYS_OFFSET_MIN)*TEMPERATURE_AMSYS_MAX/(float)(TEMPERATURE_AMSYS_OFFSET_MAX-TEMPERATURE_AMSYS_OFFSET_MIN)+(float)TEMPERATURE_AMSYS_MIN; + baro_amsys_temp = (float)(tBaroRaw - TEMPERATURE_AMSYS_OFFSET_MIN) * TEMPERATURE_AMSYS_MAX / (float)( + TEMPERATURE_AMSYS_OFFSET_MAX - TEMPERATURE_AMSYS_OFFSET_MIN) + (float)TEMPERATURE_AMSYS_MIN; #endif // Check if this is valid altimeter - if (pBaroRaw == 0) + if (pBaroRaw == 0) { baro_amsys_valid = FALSE; - else + } else { baro_amsys_valid = TRUE; + } baro_amsys_adc = pBaroRaw; @@ -151,13 +158,16 @@ void baro_amsys_read_event( void ) { //if (baro_amsys_valid && GpsFixValid()) { if (baro_amsys_valid) { //Cut RAW Min and Max - if (pBaroRaw < BARO_AMSYS_OFFSET_MIN) + if (pBaroRaw < BARO_AMSYS_OFFSET_MIN) { pBaroRaw = BARO_AMSYS_OFFSET_MIN; - if (pBaroRaw > BARO_AMSYS_OFFSET_MAX) + } + if (pBaroRaw > BARO_AMSYS_OFFSET_MAX) { pBaroRaw = BARO_AMSYS_OFFSET_MAX; + } //Convert to pressure - baro_amsys_p = (float)(pBaroRaw-BARO_AMSYS_OFFSET_MIN)*BARO_AMSYS_MAX_PRESSURE/(float)(BARO_AMSYS_OFFSET_MAX-BARO_AMSYS_OFFSET_MIN); + baro_amsys_p = (float)(pBaroRaw - BARO_AMSYS_OFFSET_MIN) * BARO_AMSYS_MAX_PRESSURE / (float)( + BARO_AMSYS_OFFSET_MAX - BARO_AMSYS_OFFSET_MIN); // Send pressure over ABI AbiSendMsgBARO_ABS(BARO_AMSYS_SENDER_ID, &baro_amsys_p); // compute altitude localy @@ -174,24 +184,25 @@ void baro_amsys_read_event( void ) { //baro_amsys_offset_altitude = 288.15 / 0.0065 * (1 - pow((baro_amsys_p)/1013.25 , 1/5.255)); } // Check if averaging needs to continue - else if (baro_amsys_cnt <= BARO_AMSYS_OFFSET_NBSAMPLES_AVRG) + else if (baro_amsys_cnt <= BARO_AMSYS_OFFSET_NBSAMPLES_AVRG) { baro_amsys_offset_tmp += baro_amsys_p; + } baro_amsys_altitude = 0.0; - } - else { + } else { // Lowpassfiltering and convert pressure to altitude - baro_amsys_altitude = baro_filter * baro_old + (1 - baro_filter) * (baro_amsys_offset-baro_amsys_p)*baro_scale/(1.2041*9.81); + baro_amsys_altitude = baro_filter * baro_old + (1 - baro_filter) * (baro_amsys_offset - baro_amsys_p) * baro_scale / + (1.2041 * 9.81); baro_old = baro_amsys_altitude; //New value available } - baro_amsys_abs_altitude=baro_amsys_altitude+ref_alt_init; + baro_amsys_abs_altitude = baro_amsys_altitude + ref_alt_init; } /*else { baro_amsys_abs_altitude = 0.0; }*/ - // Transaction has been read + // Transaction has been read baro_amsys_i2c_trans.status = I2CTransDone; } diff --git a/sw/airborne/modules/sensors/baro_amsys.h b/sw/airborne/modules/sensors/baro_amsys.h index 5fda78c154..f6a3817d34 100644 --- a/sw/airborne/modules/sensors/baro_amsys.h +++ b/sw/airborne/modules/sensors/baro_amsys.h @@ -42,9 +42,9 @@ extern float baro_filter; extern struct i2c_transaction baro_amsys_i2c_trans; -extern void baro_amsys_init( void ); -extern void baro_amsys_read_periodic( void ); -extern void baro_amsys_read_event( void ); +extern void baro_amsys_init(void); +extern void baro_amsys_read_periodic(void); +extern void baro_amsys_read_event(void); #define BaroAmsysEvent() { if (baro_amsys_i2c_trans.status == I2CTransSuccess) baro_amsys_read_event(); } diff --git a/sw/airborne/modules/sensors/baro_bmp.c b/sw/airborne/modules/sensors/baro_bmp.c index 5a0dba3a2d..22dee4d3e2 100644 --- a/sw/airborne/modules/sensors/baro_bmp.c +++ b/sw/airborne/modules/sensors/baro_bmp.c @@ -56,7 +56,8 @@ float baro_bmp_r; float baro_bmp_sigma2; int32_t baro_bmp_alt; -void baro_bmp_init(void) { +void baro_bmp_init(void) +{ bmp085_init(&baro_bmp, &BMP_I2C_DEV, BMP085_SLAVE_ADDR); @@ -66,16 +67,19 @@ void baro_bmp_init(void) { } -void baro_bmp_periodic(void) { +void baro_bmp_periodic(void) +{ - if (baro_bmp.initialized) + if (baro_bmp.initialized) { bmp085_periodic(&baro_bmp); - else + } else { bmp085_read_eeprom_calib(&baro_bmp); + } } -void baro_bmp_event(void) { +void baro_bmp_event(void) +{ bmp085_event(&baro_bmp); @@ -97,9 +101,9 @@ void baro_bmp_event(void) { &baro_bmp.temperature); #else RunOnceEvery(10, DOWNLINK_SEND_BMP_STATUS(DefaultChannel, DefaultDevice, - &baro_bmp.up, &baro_bmp.ut, - &baro_bmp.pressure, - &baro_bmp.temperature)); + &baro_bmp.up, &baro_bmp.ut, + &baro_bmp.pressure, + &baro_bmp.temperature)); #endif } } diff --git a/sw/airborne/modules/sensors/baro_ets.c b/sw/airborne/modules/sensors/baro_ets.c index 7651da3d79..24d70d5970 100644 --- a/sw/airborne/modules/sensors/baro_ets.c +++ b/sw/airborne/modules/sensors/baro_ets.c @@ -117,7 +117,8 @@ uint16_t baro_ets_cnt; uint32_t baro_ets_delay_time; bool_t baro_ets_delay_done; -void baro_ets_init( void ) { +void baro_ets_init(void) +{ baro_ets_adc = 0; baro_ets_altitude = 0.0; baro_ets_offset = 0; @@ -135,25 +136,28 @@ void baro_ets_init( void ) { SysTimeTimerStart(baro_ets_delay_time); } -void baro_ets_read_periodic( void ) { +void baro_ets_read_periodic(void) +{ // Initiate next read if (!baro_ets_delay_done) { - if (SysTimeTimer(baro_ets_delay_time) < USEC_OF_SEC(BARO_ETS_START_DELAY)) return; - else baro_ets_delay_done = TRUE; + if (SysTimeTimer(baro_ets_delay_time) < USEC_OF_SEC(BARO_ETS_START_DELAY)) { return; } + else { baro_ets_delay_done = TRUE; } } if (baro_ets_i2c_trans.status == I2CTransDone) { i2c_receive(&BARO_ETS_I2C_DEV, &baro_ets_i2c_trans, BARO_ETS_ADDR, 2); } } -void baro_ets_read_event( void ) { +void baro_ets_read_event(void) +{ // Get raw altimeter from buffer baro_ets_adc = ((uint16_t)(baro_ets_i2c_trans.buf[1]) << 8) | (uint16_t)(baro_ets_i2c_trans.buf[0]); // Check if this is valid altimeter - if (baro_ets_adc == 0) + if (baro_ets_adc == 0) { baro_ets_valid = FALSE; - else + } else { baro_ets_valid = TRUE; + } // Continue only if a new altimeter value was received if (baro_ets_valid) { @@ -165,19 +169,22 @@ void baro_ets_read_event( void ) { // Calculate average baro_ets_offset = (uint16_t)(baro_ets_offset_tmp / BARO_ETS_OFFSET_NBSAMPLES_AVRG); // Limit offset - if (baro_ets_offset < BARO_ETS_OFFSET_MIN) + if (baro_ets_offset < BARO_ETS_OFFSET_MIN) { baro_ets_offset = BARO_ETS_OFFSET_MIN; - if (baro_ets_offset > BARO_ETS_OFFSET_MAX) + } + if (baro_ets_offset > BARO_ETS_OFFSET_MAX) { baro_ets_offset = BARO_ETS_OFFSET_MAX; + } baro_ets_offset_init = TRUE; } // Check if averaging needs to continue - else if (baro_ets_cnt <= BARO_ETS_OFFSET_NBSAMPLES_AVRG) + else if (baro_ets_cnt <= BARO_ETS_OFFSET_NBSAMPLES_AVRG) { baro_ets_offset_tmp += baro_ets_adc; + } } // Convert raw to m/s if (baro_ets_offset_init) { - baro_ets_altitude = ground_alt + BARO_ETS_ALT_SCALE * (float)(baro_ets_offset-baro_ets_adc); + baro_ets_altitude = ground_alt + BARO_ETS_ALT_SCALE * (float)(baro_ets_offset - baro_ets_adc); // New value available float pressure = BARO_ETS_SCALE * (float) baro_ets_adc + BARO_ETS_PRESSURE_OFFSET; AbiSendMsgBARO_ABS(BARO_ETS_SENDER_ID, &pressure); diff --git a/sw/airborne/modules/sensors/baro_ets.h b/sw/airborne/modules/sensors/baro_ets.h index 90d02ab4c6..3af6a94fb3 100644 --- a/sw/airborne/modules/sensors/baro_ets.h +++ b/sw/airborne/modules/sensors/baro_ets.h @@ -58,9 +58,9 @@ extern float baro_ets_sigma2; extern struct i2c_transaction baro_ets_i2c_trans; -extern void baro_ets_init( void ); -extern void baro_ets_read_periodic( void ); -extern void baro_ets_read_event( void ); +extern void baro_ets_init(void); +extern void baro_ets_read_periodic(void); +extern void baro_ets_read_event(void); #define BaroEtsEvent() { if (baro_ets_i2c_trans.status == I2CTransSuccess) baro_ets_read_event(); } diff --git a/sw/airborne/modules/sensors/baro_hca.c b/sw/airborne/modules/sensors/baro_hca.c index 899b29dbc8..fa514da67c 100644 --- a/sw/airborne/modules/sensors/baro_hca.c +++ b/sw/airborne/modules/sensors/baro_hca.c @@ -59,39 +59,45 @@ float baro_hca_p; struct i2c_transaction baro_hca_i2c_trans; -void baro_hca_init( void ) { +void baro_hca_init(void) +{ pBaroRaw = 0; baro_hca_valid = TRUE; baro_hca_i2c_trans.status = I2CTransDone; } -void baro_hca_read_periodic( void ) { - if (baro_hca_i2c_trans.status == I2CTransDone){ +void baro_hca_read_periodic(void) +{ + if (baro_hca_i2c_trans.status == I2CTransDone) { i2c_receive(&BARO_HCA_I2C_DEV, &baro_hca_i2c_trans, BARO_HCA_ADDR, 2); } } -void baro_hca_read_event( void ) { +void baro_hca_read_event(void) +{ pBaroRaw = 0; // Get raw altimeter from buffer pBaroRaw = ((uint16_t)baro_hca_i2c_trans.buf[0] << 8) | baro_hca_i2c_trans.buf[1]; - if (pBaroRaw == 0) + if (pBaroRaw == 0) { baro_hca_valid = FALSE; - else + } else { baro_hca_valid = TRUE; + } if (baro_hca_valid) { //Cut RAW Min and Max - if (pBaroRaw < BARO_HCA_MIN_OUT) + if (pBaroRaw < BARO_HCA_MIN_OUT) { pBaroRaw = BARO_HCA_MIN_OUT; - if (pBaroRaw > BARO_HCA_MAX_OUT) + } + if (pBaroRaw > BARO_HCA_MAX_OUT) { pBaroRaw = BARO_HCA_MAX_OUT; + } - float pressure = BARO_HCA_SCALE*(float)pBaroRaw + BARO_HCA_PRESSURE_OFFSET; + float pressure = BARO_HCA_SCALE * (float)pBaroRaw + BARO_HCA_PRESSURE_OFFSET; AbiSendMsgBARO_ABS(BARO_HCA_SENDER_ID, &pressure); } baro_hca_i2c_trans.status = I2CTransDone; @@ -101,7 +107,7 @@ void baro_hca_read_event( void ) { #ifdef SENSOR_SYNC_SEND DOWNLINK_SEND_BARO_ETS(DefaultChannel, DefaultDevice, &pBaroRaw, &foo, &bar) #else - RunOnceEvery(10, DOWNLINK_SEND_BARO_ETS(DefaultChannel, DefaultDevice, &pBaroRaw, &foo, &bar)); + RunOnceEvery(10, DOWNLINK_SEND_BARO_ETS(DefaultChannel, DefaultDevice, &pBaroRaw, &foo, &bar)); #endif } diff --git a/sw/airborne/modules/sensors/baro_hca.h b/sw/airborne/modules/sensors/baro_hca.h index ddae84031c..9bae48e0dc 100644 --- a/sw/airborne/modules/sensors/baro_hca.h +++ b/sw/airborne/modules/sensors/baro_hca.h @@ -29,13 +29,13 @@ extern struct i2c_transaction baro_hca_i2c_trans; -extern void baro_hca_init( void ); -extern void baro_hca_read_periodic( void ); -extern void baro_hca_read_event( void ); +extern void baro_hca_init(void); +extern void baro_hca_read_periodic(void); +extern void baro_hca_read_event(void); #define BaroHcaEvent() { \ - if (baro_hca_i2c_trans.status == I2CTransSuccess) baro_hca_read_event(); \ - else if (baro_hca_i2c_trans.status == I2CTransFailed) baro_hca_i2c_trans.status = I2CTransDone; \ -} + if (baro_hca_i2c_trans.status == I2CTransSuccess) baro_hca_read_event(); \ + else if (baro_hca_i2c_trans.status == I2CTransFailed) baro_hca_i2c_trans.status = I2CTransDone; \ + } #endif diff --git a/sw/airborne/modules/sensors/baro_mpl3115.c b/sw/airborne/modules/sensors/baro_mpl3115.c index 19da3c7710..d34a505747 100644 --- a/sw/airborne/modules/sensors/baro_mpl3115.c +++ b/sw/airborne/modules/sensors/baro_mpl3115.c @@ -47,20 +47,23 @@ struct Mpl3115 baro_mpl; -void baro_mpl3115_init( void ) { +void baro_mpl3115_init(void) +{ mpl3115_init(&baro_mpl, &BARO_MPL3115_I2C_DEV, BARO_MPL3115_I2C_SLAVE_ADDR); } -void baro_mpl3115_read_periodic( void ) { +void baro_mpl3115_read_periodic(void) +{ mpl3115_periodic(&baro_mpl); } -void baro_mpl3115_read_event( void ) { +void baro_mpl3115_read_event(void) +{ mpl3115_event(&baro_mpl); if (baro_mpl.data_available) { - float pressure = (float)baro_mpl.pressure/(1<<2); + float pressure = (float)baro_mpl.pressure / (1 << 2); AbiSendMsgBARO_ABS(BARO_MPL3115_SENDER_ID, &pressure); float temp = (float)baro_mpl.pressure / 16.0f; AbiSendMsgTEMPERATURE(BARO_MPL3115_SENDER_ID, &temp); diff --git a/sw/airborne/modules/sensors/baro_mpl3115.h b/sw/airborne/modules/sensors/baro_mpl3115.h index 4267386071..cb28ab4629 100644 --- a/sw/airborne/modules/sensors/baro_mpl3115.h +++ b/sw/airborne/modules/sensors/baro_mpl3115.h @@ -29,9 +29,9 @@ #ifndef BARO_MPL3115_H #define BARO_MPL3115_H -extern void baro_mpl3115_init( void ); -extern void baro_mpl3115_read_periodic( void ); -extern void baro_mpl3115_read_event( void ); +extern void baro_mpl3115_init(void); +extern void baro_mpl3115_read_periodic(void); +extern void baro_mpl3115_read_event(void); #define BaroMpl3115Update(_b, _h) { if (mpl3115_data_available) { _b = mpl3115_pressure; _h(); mpl3115_data_available = FALSE; } } diff --git a/sw/airborne/modules/sensors/baro_ms5611_i2c.c b/sw/airborne/modules/sensors/baro_ms5611_i2c.c index 73af502d76..a9dfefe97a 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_i2c.c +++ b/sw/airborne/modules/sensors/baro_ms5611_i2c.c @@ -57,7 +57,8 @@ float baro_ms5611_r; float baro_ms5611_sigma2; -void baro_ms5611_init(void) { +void baro_ms5611_init(void) +{ ms5611_i2c_init(&baro_ms5611, &MS5611_I2C_DEV, MS5611_SLAVE_ADDR); baro_ms5611_enabled = TRUE; @@ -67,24 +68,27 @@ void baro_ms5611_init(void) { baro_ms5611_sigma2 = BARO_MS5611_SIGMA2; } -void baro_ms5611_periodic_check( void ) { +void baro_ms5611_periodic_check(void) +{ ms5611_i2c_periodic_check(&baro_ms5611); #if SENSOR_SYNC_SEND // send coeff every 30s - RunOnceEvery((30*BARO_MS5611_PERIODIC_CHECK_FREQ), baro_ms5611_send_coeff()); + RunOnceEvery((30 * BARO_MS5611_PERIODIC_CHECK_FREQ), baro_ms5611_send_coeff()); #endif } /// trigger new measurement or initialize if needed -void baro_ms5611_read(void) { +void baro_ms5611_read(void) +{ if (sys_time.nb_sec > 1) { ms5611_i2c_read(&baro_ms5611); } } -void baro_ms5611_event( void ) { +void baro_ms5611_event(void) +{ ms5611_i2c_event(&baro_ms5611); @@ -107,7 +111,8 @@ void baro_ms5611_event( void ) { } } -void baro_ms5611_send_coeff(void) { +void baro_ms5611_send_coeff(void) +{ if (baro_ms5611.initialized) { DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice, &baro_ms5611.data.c[0], diff --git a/sw/airborne/modules/sensors/baro_ms5611_spi.c b/sw/airborne/modules/sensors/baro_ms5611_spi.c index 21e549afc8..1d2bf01c6c 100644 --- a/sw/airborne/modules/sensors/baro_ms5611_spi.c +++ b/sw/airborne/modules/sensors/baro_ms5611_spi.c @@ -56,7 +56,8 @@ float baro_ms5611_r; float baro_ms5611_sigma2; -void baro_ms5611_init(void) { +void baro_ms5611_init(void) +{ ms5611_spi_init(&baro_ms5611, &MS5611_SPI_DEV, MS5611_SLAVE_IDX); baro_ms5611_enabled = TRUE; @@ -66,25 +67,28 @@ void baro_ms5611_init(void) { baro_ms5611_sigma2 = BARO_MS5611_SIGMA2; } -void baro_ms5611_periodic_check( void ) { +void baro_ms5611_periodic_check(void) +{ ms5611_spi_periodic_check(&baro_ms5611); #if SENSOR_SYNC_SEND // send coeff every 30s - RunOnceEvery((5*BARO_MS5611_PERIODIC_CHECK_FREQ), baro_ms5611_send_coeff()); + RunOnceEvery((5 * BARO_MS5611_PERIODIC_CHECK_FREQ), baro_ms5611_send_coeff()); #endif } /// trigger new measurement or initialize if needed -void baro_ms5611_read(void) { +void baro_ms5611_read(void) +{ if (sys_time.nb_sec > 1) { ms5611_spi_read(&baro_ms5611); } } -void baro_ms5611_event( void ) { +void baro_ms5611_event(void) +{ ms5611_spi_event(&baro_ms5611); @@ -107,7 +111,8 @@ void baro_ms5611_event( void ) { } } -void baro_ms5611_send_coeff(void) { +void baro_ms5611_send_coeff(void) +{ if (baro_ms5611.initialized) { DOWNLINK_SEND_MS5611_COEFF(DefaultChannel, DefaultDevice, &baro_ms5611.data.c[0], diff --git a/sw/airborne/modules/sensors/baro_scp.c b/sw/airborne/modules/sensors/baro_scp.c index aa6d9a785e..97eccdcd4e 100644 --- a/sw/airborne/modules/sensors/baro_scp.c +++ b/sw/airborne/modules/sensors/baro_scp.c @@ -31,7 +31,8 @@ static void baro_scp_read(void); static void EXTINT_ISR(void) __attribute__((naked)); static void SPI1_ISR(void) __attribute__((naked)); -void baro_scp_periodic(void) { +void baro_scp_periodic(void) +{ if (baro_scp_status == STA_UNINIT && sys_time.nb_sec > 1) { baro_scp_start_high_res_measurement(); baro_scp_status = STA_INITIALISING; @@ -68,7 +69,8 @@ void baro_scp_periodic(void) { #define SPI1_VIC_SLOT 7 #endif -void baro_scp_init( void ) { +void baro_scp_init(void) +{ /* setup pins for SSP (SCK, MISO, MOSI) */ PINSEL1 |= 2 << 2 | 2 << 4 | 2 << 6; @@ -76,7 +78,7 @@ void baro_scp_init( void ) { SSPCR0 = SSP_DDS | SSP_FRF | SSP_CPOL | SSP_CPHA | SSP_SCR; SSPCR1 = SSP_LBM | SSP_MS | SSP_SOD; /* set prescaler for SSP clock */ - SSPCPSR = PCLK/SSP_CLOCK; + SSPCPSR = PCLK / SSP_CLOCK; /* initialize interrupt vector */ VICIntSelect &= ~VIC_BIT(VIC_SPI1); // SPI1 selected as IRQ @@ -92,31 +94,31 @@ void baro_scp_init( void ) { /* connected pin to EXINT */ SPI1_DRDY_PINSEL |= SPI1_DRDY_PINSEL_VAL << SPI1_DRDY_PINSEL_BIT; SetBit(EXTMODE, SPI1_DRDY_EINT); /* EINT is edge trigered */ - SetBit(EXTPOLAR,SPI1_DRDY_EINT); /* EINT is trigered on rising edge */ - SetBit(EXTINT,SPI1_DRDY_EINT); /* clear pending EINT */ + SetBit(EXTPOLAR, SPI1_DRDY_EINT); /* EINT is trigered on rising edge */ + SetBit(EXTINT, SPI1_DRDY_EINT); /* clear pending EINT */ /* initialize interrupt vector */ - VICIntSelect &= ~VIC_BIT( SPI1_DRDY_VIC_IT ); /* select EINT as IRQ source */ - VICIntEnable = VIC_BIT( SPI1_DRDY_VIC_IT ); /* enable it */ + VICIntSelect &= ~VIC_BIT(SPI1_DRDY_VIC_IT); /* select EINT as IRQ source */ + VICIntEnable = VIC_BIT(SPI1_DRDY_VIC_IT); /* enable it */ VICVectCntl11 = VIC_ENABLE | SPI1_DRDY_VIC_IT; VICVectAddr11 = (uint32_t)EXTINT_ISR; // address of the ISR baro_scp_status = STA_UNINIT; } -void SPI1_ISR(void) { +void SPI1_ISR(void) +{ ISR_ENTRY(); if (baro_scp_status == STA_INITIALISING) { uint8_t foo1 = SSPDR; uint8_t foo2 = SSPDR; baro_scp_status = STA_IDLE; - foo1=foo2; - } - else if (baro_scp_status == STA_IDLE) { + foo1 = foo2; + } else if (baro_scp_status == STA_IDLE) { uint8_t foo0 = SSPDR; - baro_scp_temperature = SSPDR<<8; + baro_scp_temperature = SSPDR << 8; baro_scp_temperature += SSPDR; if (baro_scp_temperature & 0x2000) { baro_scp_temperature |= 0xC000; @@ -124,15 +126,15 @@ void SPI1_ISR(void) { baro_scp_temperature *= 5; uint8_t foo1 = SSPDR; - uint32_t datard8 = SSPDR<<16; + uint32_t datard8 = SSPDR << 16; uint8_t foo2 = SSPDR; - baro_scp_pressure = SSPDR<<8; + baro_scp_pressure = SSPDR << 8; baro_scp_pressure += SSPDR; baro_scp_pressure += datard8; baro_scp_pressure *= 25; baro_scp_available = TRUE; - foo1=foo2; - foo0=foo2; + foo1 = foo2; + foo0 = foo2; } ScpUnselect(); @@ -143,18 +145,20 @@ void SPI1_ISR(void) { ISR_EXIT(); } -void EXTINT_ISR(void) { +void EXTINT_ISR(void) +{ ISR_ENTRY(); baro_scp_read(); - SetBit(EXTINT,SPI1_DRDY_EINT); /* clear EINT2 */ + SetBit(EXTINT, SPI1_DRDY_EINT); /* clear EINT2 */ VICVectAddr = 0x00000000; /* clear this interrupt from the VIC */ ISR_EXIT(); } /* write 0x0A to 0x03 */ -static void baro_scp_start_high_res_measurement(void) { - uint8_t cmd = 0x03<<2|0x02; +static void baro_scp_start_high_res_measurement(void) +{ + uint8_t cmd = 0x03 << 2 | 0x02; uint8_t data = 0x0A; ScpSelect(); SSPDR = cmd; @@ -164,7 +168,8 @@ static void baro_scp_start_high_res_measurement(void) { } /* read 0x21 (TEMP), 0x1F (MSB) and 0x20 (LSB) */ -static void baro_scp_read(void) { +static void baro_scp_read(void) +{ uint8_t cmd0 = 0x21 << 2; uint8_t cmd1 = 0x1F << 2; uint8_t cmd2 = 0x20 << 2; @@ -180,7 +185,8 @@ static void baro_scp_read(void) { SpiEnable(); } -void baro_scp_event( void ) { +void baro_scp_event(void) +{ if (baro_scp_available == TRUE) { float pressure = (float)baro_scp_pressure; AbiSendMsgBARO_ABS(BARO_SCP_SENDER_ID, &pressure); diff --git a/sw/airborne/modules/sensors/baro_scp_i2c.c b/sw/airborne/modules/sensors/baro_scp_i2c.c index 0b78ce7516..58fdc78df7 100644 --- a/sw/airborne/modules/sensors/baro_scp_i2c.c +++ b/sw/airborne/modules/sensors/baro_scp_i2c.c @@ -33,18 +33,21 @@ struct i2c_transaction scp_trans; #define SCP1000_SLAVE_ADDR 0x22 -static void baro_scp_start_high_res_measurement(void) { +static void baro_scp_start_high_res_measurement(void) +{ /* switch to high resolution */ scp_trans.buf[0] = SCP1000_OPERATION; scp_trans.buf[1] = SCP1000_HIGH_RES; i2c_transmit(&SCP_I2C_DEV, &scp_trans, SCP1000_SLAVE_ADDR, 2); } -void baro_scp_init( void ) { +void baro_scp_init(void) +{ baro_scp_status = BARO_SCP_UNINIT; } -void baro_scp_periodic( void ) { +void baro_scp_periodic(void) +{ if (baro_scp_status == BARO_SCP_UNINIT && sys_time.nb_sec > 1) { @@ -59,7 +62,8 @@ void baro_scp_periodic( void ) { } } -void baro_scp_event( void ) { +void baro_scp_event(void) +{ if (scp_trans.status == I2CTransSuccess) { @@ -106,6 +110,6 @@ void baro_scp_event( void ) { baro_scp_status = BARO_SCP_IDLE; } - else baro_scp_status = BARO_SCP_IDLE; + else { baro_scp_status = BARO_SCP_IDLE; } } } diff --git a/sw/airborne/modules/sensors/baro_sim.c b/sw/airborne/modules/sensors/baro_sim.c index b78e6995c4..67ff539ab8 100644 --- a/sw/airborne/modules/sensors/baro_sim.c +++ b/sw/airborne/modules/sensors/baro_sim.c @@ -31,11 +31,13 @@ PRINT_CONFIG_VAR(BARO_SIM_SENDER_ID) -void baro_sim_init(void) { +void baro_sim_init(void) +{ } -void baro_sim_periodic(void) { +void baro_sim_periodic(void) +{ float pressure = pprz_isa_pressure_of_altitude(gps.hmsl / 1000.0); AbiSendMsgBARO_ABS(BARO_SIM_SENDER_ID, &pressure); } diff --git a/sw/airborne/modules/sensors/ezcurrent.c b/sw/airborne/modules/sensors/ezcurrent.c index 306ff43a51..9f44650343 100644 --- a/sw/airborne/modules/sensors/ezcurrent.c +++ b/sw/airborne/modules/sensors/ezcurrent.c @@ -47,7 +47,8 @@ struct i2c_transaction ezcurrent_i2c_trans; -void ezcurrent_init( void ) { +void ezcurrent_init(void) +{ electrical.vsupply = 0; electrical.current = 0; @@ -55,7 +56,8 @@ void ezcurrent_init( void ) { ezcurrent_i2c_trans.slave_addr = EZCURRENT_ADDR; } -void ezcurrent_read_periodic( void ) { +void ezcurrent_read_periodic(void) +{ if (ezcurrent_i2c_trans.status == I2CTransDone) { i2c_receive(&EZCURRENT_I2C_DEV, &ezcurrent_i2c_trans, ezcurrent_i2c_trans.slave_addr, 10); } @@ -64,7 +66,8 @@ void ezcurrent_read_periodic( void ) { #define Uint16FromBuf(_buf,_idx) ((uint16_t)((_buf[_idx+1]<<8) | _buf[_idx])) #define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx+1]<<8) | _buf[_idx])) -void ezcurrent_read_event( void ) { +void ezcurrent_read_event(void) +{ if (ezcurrent_i2c_trans.status == I2CTransSuccess) { /* voltage of EzOSD sensor is provided in mV, convert to deciVolt */ electrical.vsupply = Uint16FromBuf(ezcurrent_i2c_trans.buf, 2) / 100; @@ -75,7 +78,7 @@ void ezcurrent_read_event( void ) { // Transaction has been read ezcurrent_i2c_trans.status = I2CTransDone; - } else if ( ezcurrent_i2c_trans.status == I2CTransFailed ) { + } else if (ezcurrent_i2c_trans.status == I2CTransFailed) { ezcurrent_i2c_trans.status = I2CTransDone; // ezcurrent_i2c_trans.slave_addr++; } diff --git a/sw/airborne/modules/sensors/ezcurrent.h b/sw/airborne/modules/sensors/ezcurrent.h index 89effa7938..7b9ff32ce7 100644 --- a/sw/airborne/modules/sensors/ezcurrent.h +++ b/sw/airborne/modules/sensors/ezcurrent.h @@ -40,8 +40,8 @@ #include "std.h" -extern void ezcurrent_init( void ); -extern void ezcurrent_read_periodic( void ); -extern void ezcurrent_read_event( void ); +extern void ezcurrent_init(void); +extern void ezcurrent_read_periodic(void); +extern void ezcurrent_read_event(void); #endif // EZCURRENT_H diff --git a/sw/airborne/modules/sensors/imu_aspirin2.c b/sw/airborne/modules/sensors/imu_aspirin2.c index d2eb364485..624587464b 100644 --- a/sw/airborne/modules/sensors/imu_aspirin2.c +++ b/sw/airborne/modules/sensors/imu_aspirin2.c @@ -68,7 +68,7 @@ void imu_impl_init(void) aspirin2_mpu60x0.buf[0] = MPU60X0_REG_PWR_MGMT_1; aspirin2_mpu60x0.buf[1] = 0x01; i2c_transmit(&PPZUAVIMU_I2C_DEV, &aspirin2_mpu60x0, MPU60X0_ADDR, 2); - while(aspirin2_mpu60x0.status == I2CTransPending); + while (aspirin2_mpu60x0.status == I2CTransPending); // MPU60X0_REG_PWR_MGMT_2: Nothing should be in standby: default OK @@ -88,169 +88,168 @@ void imu_impl_init(void) aspirin2_mpu60x0.buf[0] = MPU60X0_REG_CONFIG; aspirin2_mpu60x0.buf[1] = (2 << 3) | (3 << 0); i2c_transmit(&PPZUAVIMU_I2C_DEV, &aspirin2_mpu60x0, MPU60X0_ADDR, 2); - while(aspirin2_mpu60x0.status == I2CTransPending); + while (aspirin2_mpu60x0.status == I2CTransPending); // MPU60X0_REG_SMPLRT_DIV // -100Hz output = 1kHz / (9 + 1) aspirin2_mpu60x0.buf[0] = MPU60X0_REG_SMPLRT_DIV; aspirin2_mpu60x0.buf[1] = 9; i2c_transmit(&PPZUAVIMU_I2C_DEV, &aspirin2_mpu60x0, MPU60X0_ADDR, 2); - while(aspirin2_mpu60x0.status == I2CTransPending); + while (aspirin2_mpu60x0.status == I2CTransPending); // MPU60X0_REG_GYRO_CONFIG // -2000deg/sec aspirin2_mpu60x0.buf[0] = MPU60X0_REG_GYRO_CONFIG; - aspirin2_mpu60x0.buf[1] = (3<<3); + aspirin2_mpu60x0.buf[1] = (3 << 3); i2c_transmit(&PPZUAVIMU_I2C_DEV, &aspirin2_mpu60x0, MPU60X0_ADDR, 2); - while(aspirin2_mpu60x0.status == I2CTransPending); + while (aspirin2_mpu60x0.status == I2CTransPending); // MPU60X0_REG_ACCEL_CONFIG // 16g, no HPFL aspirin2_mpu60x0.buf[0] = MPU60X0_REG_ACCEL_CONFIG; - aspirin2_mpu60x0.buf[1] = (3<<3); + aspirin2_mpu60x0.buf[1] = (3 << 3); i2c_transmit(&PPZUAVIMU_I2C_DEV, &aspirin2_mpu60x0, MPU60X0_ADDR, 2); - while(aspirin2_mpu60x0.status == I2CTransPending); + while (aspirin2_mpu60x0.status == I2CTransPending); -/* - // no interrupts for now, but set data ready interrupt to enable reading status bits - aspirin2_mpu60x0.buf[0] = ITG3200_REG_INT_CFG; - aspirin2_mpu60x0.buf[1] = 0x01; - i2c_submit(&PPZUAVIMU_I2C_DEV,&aspirin2_mpu60x0); - while(aspirin2_mpu60x0.status == I2CTransPending); -*/ + /* + // no interrupts for now, but set data ready interrupt to enable reading status bits + aspirin2_mpu60x0.buf[0] = ITG3200_REG_INT_CFG; + aspirin2_mpu60x0.buf[1] = 0x01; + i2c_submit(&PPZUAVIMU_I2C_DEV,&aspirin2_mpu60x0); + while(aspirin2_mpu60x0.status == I2CTransPending); + */ -/* - ///////////////////////////////////////////////////////////////////// - // HMC5843 - ppzuavimu_hmc5843.slave_addr = HMC5843_ADDR; - ppzuavimu_hmc5843.type = I2CTransTx; - ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGA; // set to rate to max speed: 50Hz no bias - ppzuavimu_hmc5843.buf[1] = 0x00 | (0x06 << 2); - ppzuavimu_hmc5843.len_w = 2; - i2c_submit(&PPZUAVIMU_I2C_DEV,&ppzuavimu_hmc5843); - while(ppzuavimu_hmc5843.status == I2CTransPending); + /* + ///////////////////////////////////////////////////////////////////// + // HMC5843 + ppzuavimu_hmc5843.slave_addr = HMC5843_ADDR; + ppzuavimu_hmc5843.type = I2CTransTx; + ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGA; // set to rate to max speed: 50Hz no bias + ppzuavimu_hmc5843.buf[1] = 0x00 | (0x06 << 2); + ppzuavimu_hmc5843.len_w = 2; + i2c_submit(&PPZUAVIMU_I2C_DEV,&ppzuavimu_hmc5843); + while(ppzuavimu_hmc5843.status == I2CTransPending); - ppzuavimu_hmc5843.type = I2CTransTx; - ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGB; // set to gain to 1 Gauss - ppzuavimu_hmc5843.buf[1] = 0x01<<5; - ppzuavimu_hmc5843.len_w = 2; - i2c_submit(&PPZUAVIMU_I2C_DEV,&ppzuavimu_hmc5843); - while(ppzuavimu_hmc5843.status == I2CTransPending); + ppzuavimu_hmc5843.type = I2CTransTx; + ppzuavimu_hmc5843.buf[0] = HMC5843_REG_CFGB; // set to gain to 1 Gauss + ppzuavimu_hmc5843.buf[1] = 0x01<<5; + ppzuavimu_hmc5843.len_w = 2; + i2c_submit(&PPZUAVIMU_I2C_DEV,&ppzuavimu_hmc5843); + while(ppzuavimu_hmc5843.status == I2CTransPending); - ppzuavimu_hmc5843.type = I2CTransTx; - ppzuavimu_hmc5843.buf[0] = HMC5843_REG_MODE; // set to continuous mode - ppzuavimu_hmc5843.buf[1] = 0x00; - ppzuavimu_hmc5843.len_w = 2; - i2c_submit(&PPZUAVIMU_I2C_DEV,&ppzuavimu_hmc5843); - while(ppzuavimu_hmc5843.status == I2CTransPending); -*/ + ppzuavimu_hmc5843.type = I2CTransTx; + ppzuavimu_hmc5843.buf[0] = HMC5843_REG_MODE; // set to continuous mode + ppzuavimu_hmc5843.buf[1] = 0x00; + ppzuavimu_hmc5843.len_w = 2; + i2c_submit(&PPZUAVIMU_I2C_DEV,&ppzuavimu_hmc5843); + while(ppzuavimu_hmc5843.status == I2CTransPending); + */ } -void imu_periodic( void ) +void imu_periodic(void) { // Start reading the latest gyroscope data aspirin2_mpu60x0.buf[0] = MPU60X0_REG_INT_STATUS; i2c_transceive(&PPZUAVIMU_I2C_DEV, &aspirin2_mpu60x0, MPU60X0_ADDR, 1, 21); -/* - // Start reading the latest accelerometer data - ppzuavimu_adxl345.buf[0] = ADXL345_REG_DATA_X0; - i2c_transceive(&PPZUAVIMU_I2C_DEV, &ppzuavimu_adxl345, ADXL345_ADDR, 1, 6); -*/ + /* + // Start reading the latest accelerometer data + ppzuavimu_adxl345.buf[0] = ADXL345_REG_DATA_X0; + i2c_transceive(&PPZUAVIMU_I2C_DEV, &ppzuavimu_adxl345, ADXL345_ADDR, 1, 6); + */ // Start reading the latest magnetometer data #if PERIODIC_FREQUENCY > 60 - RunOnceEvery(2,{ + RunOnceEvery(2, { #endif -/* ppzuavimu_hmc5843.type = I2CTransTxRx; - ppzuavimu_hmc5843.len_r = 6; - ppzuavimu_hmc5843.len_w = 1; - ppzuavimu_hmc5843.buf[0] = HMC5843_REG_DATXM; - i2c_submit(&PPZUAVIMU_I2C_DEV, &ppzuavimu_hmc5843); -*/ + /* ppzuavimu_hmc5843.type = I2CTransTxRx; + ppzuavimu_hmc5843.len_r = 6; + ppzuavimu_hmc5843.len_w = 1; + ppzuavimu_hmc5843.buf[0] = HMC5843_REG_DATXM; + i2c_submit(&PPZUAVIMU_I2C_DEV, &ppzuavimu_hmc5843); + */ #if PERIODIC_FREQUENCY > 60 }); #endif //RunOnceEvery(10,aspirin2_subsystem_downlink_raw()); } -void aspirin2_subsystem_downlink_raw( void ) +void aspirin2_subsystem_downlink_raw(void) { - DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,DefaultDevice,&imu.gyro_unscaled.p,&imu.gyro_unscaled.q,&imu.gyro_unscaled.r); - DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,DefaultDevice,&imu.accel_unscaled.x,&imu.accel_unscaled.y,&imu.accel_unscaled.z); - DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,DefaultDevice,&imu.mag_unscaled.x,&imu.mag_unscaled.y,&imu.mag_unscaled.z); + DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, + &imu.gyro_unscaled.r); + DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, &imu.accel_unscaled.x, &imu.accel_unscaled.y, + &imu.accel_unscaled.z); + DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z); } -void aspirin2_subsystem_event( void ) +void aspirin2_subsystem_event(void) { int32_t x, y, z; // If the itg3200 I2C transaction has succeeded: convert the data - if (aspirin2_mpu60x0.status == I2CTransSuccess) - { + if (aspirin2_mpu60x0.status == I2CTransSuccess) { #define MPU_OFFSET_GYRO 9 - x = (int16_t) ((aspirin2_mpu60x0.buf[0+MPU_OFFSET_GYRO] << 8) | aspirin2_mpu60x0.buf[1+MPU_OFFSET_GYRO]); - y = (int16_t) ((aspirin2_mpu60x0.buf[2+MPU_OFFSET_GYRO] << 8) | aspirin2_mpu60x0.buf[3+MPU_OFFSET_GYRO]); - z = (int16_t) ((aspirin2_mpu60x0.buf[4+MPU_OFFSET_GYRO] << 8) | aspirin2_mpu60x0.buf[5+MPU_OFFSET_GYRO]); + x = (int16_t)((aspirin2_mpu60x0.buf[0 + MPU_OFFSET_GYRO] << 8) | aspirin2_mpu60x0.buf[1 + MPU_OFFSET_GYRO]); + y = (int16_t)((aspirin2_mpu60x0.buf[2 + MPU_OFFSET_GYRO] << 8) | aspirin2_mpu60x0.buf[3 + MPU_OFFSET_GYRO]); + z = (int16_t)((aspirin2_mpu60x0.buf[4 + MPU_OFFSET_GYRO] << 8) | aspirin2_mpu60x0.buf[5 + MPU_OFFSET_GYRO]); RATES_ASSIGN(imu.gyro_unscaled, x, y, z); #define MPU_OFFSET_ACC 1 - x = (int16_t) ((aspirin2_mpu60x0.buf[0+MPU_OFFSET_ACC] << 8) | aspirin2_mpu60x0.buf[1+MPU_OFFSET_ACC]); - y = (int16_t) ((aspirin2_mpu60x0.buf[2+MPU_OFFSET_ACC] << 8) | aspirin2_mpu60x0.buf[3+MPU_OFFSET_ACC]); - z = (int16_t) ((aspirin2_mpu60x0.buf[4+MPU_OFFSET_ACC] << 8) | aspirin2_mpu60x0.buf[5+MPU_OFFSET_ACC]); + x = (int16_t)((aspirin2_mpu60x0.buf[0 + MPU_OFFSET_ACC] << 8) | aspirin2_mpu60x0.buf[1 + MPU_OFFSET_ACC]); + y = (int16_t)((aspirin2_mpu60x0.buf[2 + MPU_OFFSET_ACC] << 8) | aspirin2_mpu60x0.buf[3 + MPU_OFFSET_ACC]); + z = (int16_t)((aspirin2_mpu60x0.buf[4 + MPU_OFFSET_ACC] << 8) | aspirin2_mpu60x0.buf[5 + MPU_OFFSET_ACC]); VECT3_ASSIGN(imu.accel_unscaled, x, y, z); // Is this is new data - if (aspirin2_mpu60x0.buf[0] & 0x01) - { + if (aspirin2_mpu60x0.buf[0] & 0x01) { gyr_valid = TRUE; acc_valid = TRUE; + } else { } - else + + aspirin2_mpu60x0.status = + I2CTransDone; // remove the I2CTransSuccess status, otherwise data ready will be triggered again without new data + } + /* + // If the adxl345 I2C transaction has succeeded: convert the data + if (ppzuavimu_adxl345.status == I2CTransSuccess) { + x = (int16_t) ((ppzuavimu_adxl345.buf[1] << 8) | ppzuavimu_adxl345.buf[0]); + y = (int16_t) ((ppzuavimu_adxl345.buf[3] << 8) | ppzuavimu_adxl345.buf[2]); + z = (int16_t) ((ppzuavimu_adxl345.buf[5] << 8) | ppzuavimu_adxl345.buf[4]); + + #ifdef ASPIRIN_IMU + VECT3_ASSIGN(imu.accel_unscaled, x, -y, -z); + #else // PPZIMU + VECT3_ASSIGN(imu.accel_unscaled, -x, y, -z); + #endif + + acc_valid = TRUE; + ppzuavimu_adxl345.status = I2CTransDone; } - aspirin2_mpu60x0.status = I2CTransDone; // remove the I2CTransSuccess status, otherwise data ready will be triggered again without new data - } -/* - // If the adxl345 I2C transaction has succeeded: convert the data - if (ppzuavimu_adxl345.status == I2CTransSuccess) - { - x = (int16_t) ((ppzuavimu_adxl345.buf[1] << 8) | ppzuavimu_adxl345.buf[0]); - y = (int16_t) ((ppzuavimu_adxl345.buf[3] << 8) | ppzuavimu_adxl345.buf[2]); - z = (int16_t) ((ppzuavimu_adxl345.buf[5] << 8) | ppzuavimu_adxl345.buf[4]); + // If the hmc5843 I2C transaction has succeeded: convert the data + if (ppzuavimu_hmc5843.status == I2CTransSuccess) + { + x = (int16_t) ((ppzuavimu_hmc5843.buf[0] << 8) | ppzuavimu_hmc5843.buf[1]); + y = (int16_t) ((ppzuavimu_hmc5843.buf[2] << 8) | ppzuavimu_hmc5843.buf[3]); + z = (int16_t) ((ppzuavimu_hmc5843.buf[4] << 8) | ppzuavimu_hmc5843.buf[5]); -#ifdef ASPIRIN_IMU - VECT3_ASSIGN(imu.accel_unscaled, x, -y, -z); -#else // PPZIMU - VECT3_ASSIGN(imu.accel_unscaled, -x, y, -z); -#endif + #ifdef ASPIRIN_IMU + VECT3_ASSIGN(imu.mag_unscaled, x, -y, -z); + #else // PPZIMU + VECT3_ASSIGN(imu.mag_unscaled, -y, -x, -z); + #endif - acc_valid = TRUE; - ppzuavimu_adxl345.status = I2CTransDone; - } - - // If the hmc5843 I2C transaction has succeeded: convert the data - if (ppzuavimu_hmc5843.status == I2CTransSuccess) - { - x = (int16_t) ((ppzuavimu_hmc5843.buf[0] << 8) | ppzuavimu_hmc5843.buf[1]); - y = (int16_t) ((ppzuavimu_hmc5843.buf[2] << 8) | ppzuavimu_hmc5843.buf[3]); - z = (int16_t) ((ppzuavimu_hmc5843.buf[4] << 8) | ppzuavimu_hmc5843.buf[5]); - -#ifdef ASPIRIN_IMU - VECT3_ASSIGN(imu.mag_unscaled, x, -y, -z); -#else // PPZIMU - VECT3_ASSIGN(imu.mag_unscaled, -y, -x, -z); -#endif - - mag_valid = TRUE; - ppzuavimu_hmc5843.status = I2CTransDone; - } -*/ + mag_valid = TRUE; + ppzuavimu_hmc5843.status = I2CTransDone; + } + */ } diff --git a/sw/airborne/modules/sensors/imu_aspirin2.h b/sw/airborne/modules/sensors/imu_aspirin2.h index f60c7082b2..4aaa08a5a9 100644 --- a/sw/airborne/modules/sensors/imu_aspirin2.h +++ b/sw/airborne/modules/sensors/imu_aspirin2.h @@ -47,11 +47,11 @@ extern void imu_periodic(void); mag_valid = FALSE; \ _mag_handler(); \ } \ -} + } /* Own Extra Functions */ -extern void aspirin2_subsystem_event( void ); -extern void aspirin2_subsystem_downlink_raw( void ); +extern void aspirin2_subsystem_event(void); +extern void aspirin2_subsystem_downlink_raw(void); #endif // PPZUAVIMU_H diff --git a/sw/airborne/modules/sensors/mag_hmc5843.c b/sw/airborne/modules/sensors/mag_hmc5843.c index 75f3266a35..2ed2f7c403 100644 --- a/sw/airborne/modules/sensors/mag_hmc5843.c +++ b/sw/airborne/modules/sensors/mag_hmc5843.c @@ -32,20 +32,21 @@ bool_t mag_valid; -void hmc5843_module_init( void ) { +void hmc5843_module_init(void) +{ hmc5843_init(); } -void hmc5843_module_periodic ( void ) +void hmc5843_module_periodic(void) { hmc5843_periodic(); mag_x = hmc5843.data.value[0]; mag_y = hmc5843.data.value[1]; mag_z = hmc5843.data.value[2]; - RunOnceEvery(30,DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice,&mag_x,&mag_y,&mag_z)); + RunOnceEvery(30, DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, &mag_x, &mag_y, &mag_z)); } -void hmc5843_module_event( void ) +void hmc5843_module_event(void) { hmc5843_idle_task(); } diff --git a/sw/airborne/modules/sensors/mag_hmc5843.h b/sw/airborne/modules/sensors/mag_hmc5843.h index d21dbb5037..dec8be9a5d 100644 --- a/sw/airborne/modules/sensors/mag_hmc5843.h +++ b/sw/airborne/modules/sensors/mag_hmc5843.h @@ -26,8 +26,8 @@ extern int32_t mag_x, mag_y, mag_z; -extern void hmc5843_module_init( void ); -extern void hmc5843_module_periodic( void ); -extern void hmc5843_module_event( void ); +extern void hmc5843_module_init(void); +extern void hmc5843_module_periodic(void); +extern void hmc5843_module_event(void); #endif // HMC5843__H diff --git a/sw/airborne/modules/sensors/mag_hmc58xx.c b/sw/airborne/modules/sensors/mag_hmc58xx.c index ebe2c149b3..87bc8eff41 100644 --- a/sw/airborne/modules/sensors/mag_hmc58xx.c +++ b/sw/airborne/modules/sensors/mag_hmc58xx.c @@ -48,22 +48,25 @@ struct Hmc58xx mag_hmc58xx; -void mag_hmc58xx_module_init(void) { +void mag_hmc58xx_module_init(void) +{ hmc58xx_init(&mag_hmc58xx, &(MAG_HMC58XX_I2C_DEV), HMC58XX_ADDR); } -void mag_hmc58xx_module_periodic(void) { +void mag_hmc58xx_module_periodic(void) +{ hmc58xx_periodic(&mag_hmc58xx); } -void mag_hmc58xx_module_event(void) { +void mag_hmc58xx_module_event(void) +{ #if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY) -PRINT_CONFIG_MSG("Calculating dt for AHRS mag update.") + PRINT_CONFIG_MSG("Calculating dt for AHRS mag update.") // timestamp in usec when last callback was received static uint32_t last_ts = 0; #else -PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS mag update.") -PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) + PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS mag update.") + PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY); #endif @@ -103,7 +106,8 @@ PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY) #endif } -void mag_hmc58xx_report(void) { +void mag_hmc58xx_report(void) +{ struct Int32Vect3 mag = { (int32_t)(mag_hmc58xx.data.vect.x), (int32_t)(mag_hmc58xx.data.vect.y), diff --git a/sw/airborne/modules/sensors/mag_micromag_fw.c b/sw/airborne/modules/sensors/mag_micromag_fw.c index f6b721ae10..7492334ec4 100644 --- a/sw/airborne/modules/sensors/mag_micromag_fw.c +++ b/sw/airborne/modules/sensors/mag_micromag_fw.c @@ -10,7 +10,8 @@ volatile uint8_t micromag_status; volatile int16_t micromag_values[MM_NB_AXIS]; -void micromag_periodic( void ) { +void micromag_periodic(void) +{ static uint8_t cnt = 0; @@ -19,53 +20,54 @@ void micromag_periodic( void ) { // DOWNLINK_SEND_DEBUG(1,tab); cnt = 0; MmSendReq(); - } - else if (micromag_status == MM_GOT_EOC) { + } else if (micromag_status == MM_GOT_EOC) { MmReadRes(); - } - else if (micromag_status == MM_WAITING_EOC) { + } else if (micromag_status == MM_WAITING_EOC) { cnt++; if (cnt > 50) {cnt = 0; micromag_status = MM_IDLE;} } } -void micromag_event( void ) { +void micromag_event(void) +{ - int32_t mx=micromag_values[0]; - int32_t my=micromag_values[1]; - int32_t mz=micromag_values[2]; + int32_t mx = micromag_values[0]; + int32_t my = micromag_values[1]; + int32_t mz = micromag_values[2]; if (micromag_status == MM_DATA_AVAILABLE) { DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, - &mx, - &my, - &mz ); + &mx, + &my, + &mz); micromag_status = MM_IDLE; } } -void micromag_init( void ) { +void micromag_init(void) +{ micromag_hw_init(); uint8_t i; - for (i=0; i AGL_DIST_SONAR_MIN_RANGE) { agl_dist_value = *distance; agl_dist_valid = TRUE; - agl_dist_value_filtered = (AGL_DIST_SONAR_FILTER * agl_dist_value_filtered + agl_dist_value) / (AGL_DIST_SONAR_FILTER + 1); - } - else { + agl_dist_value_filtered = (AGL_DIST_SONAR_FILTER * agl_dist_value_filtered + agl_dist_value) / + (AGL_DIST_SONAR_FILTER + 1); + } else { agl_dist_valid = FALSE; } } diff --git a/sw/airborne/modules/sonar/sonar_adc.c b/sw/airborne/modules/sonar/sonar_adc.c index 3828af1738..195fef52a6 100644 --- a/sw/airborne/modules/sonar/sonar_adc.c +++ b/sw/airborne/modules/sonar/sonar_adc.c @@ -53,7 +53,8 @@ struct SonarAdc sonar_adc; static struct adc_buf sonar_adc_buf; #endif -void sonar_adc_init(void) { +void sonar_adc_init(void) +{ sonar_adc.meas = 0; sonar_adc.offset = SONAR_OFFSET; @@ -64,7 +65,8 @@ void sonar_adc_init(void) { /** Read ADC value to update sonar measurement */ -void sonar_adc_read(void) { +void sonar_adc_read(void) +{ #ifndef SITL sonar_adc.meas = sonar_adc_buf.sum / sonar_adc_buf.av_nb_sample; sonar_adc.distance = (float)(sonar_adc.meas - sonar_adc.offset) * SONAR_SCALE; diff --git a/sw/airborne/modules/vehicle_interface/vi.c b/sw/airborne/modules/vehicle_interface/vi.c index f674c0ad53..3665570a85 100644 --- a/sw/airborne/modules/vehicle_interface/vi.c +++ b/sw/airborne/modules/vehicle_interface/vi.c @@ -33,7 +33,8 @@ struct VehicleInterface vi; #define VI_TIMEOUT 100 #endif -void vi_init(void) { +void vi_init(void) +{ vi.enabled = FALSE; vi.timeouted = TRUE; @@ -47,11 +48,12 @@ void vi_init(void) { } -void vi_periodic(void) { +void vi_periodic(void) +{ #if (VI_TIMEOUT != 0) - if (vi.last_msg < VI_TIMEOUT) + if (vi.last_msg < VI_TIMEOUT) { vi.last_msg++; - else { + } else { vi.timeouted = TRUE; vi.input.h_mode = GUIDANCE_H_MODE_ATTITUDE; INT_EULERS_ZERO(vi.input.h_sp.attitude); @@ -62,11 +64,13 @@ void vi_periodic(void) { vi_impl_periodic(); } -void vi_set_enabled(bool_t enabled) { +void vi_set_enabled(bool_t enabled) +{ vi_impl_set_enabled(enabled); } -void vi_update_info(void) { +void vi_update_info(void) +{ } diff --git a/sw/airborne/modules/vehicle_interface/vi.h b/sw/airborne/modules/vehicle_interface/vi.h index 7dd6eba739..576ce96d9a 100644 --- a/sw/airborne/modules/vehicle_interface/vi.h +++ b/sw/airborne/modules/vehicle_interface/vi.h @@ -101,9 +101,9 @@ extern void vi_impl_periodic(void); extern void vi_impl_set_enabled(bool_t enabled); -#define vi_SetEnabled(_val) { \ - vi.enabled = _val; \ - vi_set_enabled(_val); \ +#define vi_SetEnabled(_val) { \ + vi.enabled = _val; \ + vi_set_enabled(_val); \ } #endif /* VI_H */ diff --git a/sw/airborne/modules/vehicle_interface/vi_datalink.c b/sw/airborne/modules/vehicle_interface/vi_datalink.c index d4f05608c3..183740f517 100644 --- a/sw/airborne/modules/vehicle_interface/vi_datalink.c +++ b/sw/airborne/modules/vehicle_interface/vi_datalink.c @@ -21,13 +21,16 @@ #include "modules/vehicle_interface/vi_datalink.h" -void vi_impl_init(void) { +void vi_impl_init(void) +{ } -void vi_impl_periodic(void) { +void vi_impl_periodic(void) +{ } -void vi_impl_set_enabled(bool_t enabled __attribute__ ((unused))) { +void vi_impl_set_enabled(bool_t enabled __attribute__((unused))) +{ } #define ViMaxHSpeed ((int16_t)SPEED_BFP_OF_REAL(VI_MAX_H_SPEED)) @@ -36,7 +39,8 @@ void vi_impl_set_enabled(bool_t enabled __attribute__ ((unused))) { struct Int16Vect3 wp_speed_max = { ViMaxHSpeed, ViMaxHSpeed, ViMaxVSpeed }; -void vi_update_wp(uint8_t wp_id) { +void vi_update_wp(uint8_t wp_id) +{ struct Int16Vect3 wp_speed; wp_speed.x = ViMaxHSpeed * vi.input.h_sp.speed.x / 128; wp_speed.y = ViMaxHSpeed * vi.input.h_sp.speed.y / 128; diff --git a/sw/airborne/modules/vehicle_interface/vi_datalink.h b/sw/airborne/modules/vehicle_interface/vi_datalink.h index 9363549ca6..220155e6c6 100644 --- a/sw/airborne/modules/vehicle_interface/vi_datalink.h +++ b/sw/airborne/modules/vehicle_interface/vi_datalink.h @@ -41,75 +41,75 @@ extern void vi_update_wp(uint8_t wp_id); #ifdef VI_PHI_THETA_MAX -#define VI_LIMIT_ATTITUDE(_att) { \ - BoundAbs(_att.phi, VI_PHI_THETA_MAX); \ - BoundAbs(_att.theta, VI_PHI_THETA_MAX); \ +#define VI_LIMIT_ATTITUDE(_att) { \ + BoundAbs(_att.phi, VI_PHI_THETA_MAX); \ + BoundAbs(_att.theta, VI_PHI_THETA_MAX); \ } #else #define VI_LIMIT_ATTITUDE(_x) {} #endif -#define VI_PARSE_DATALINK(_dl_buffer) { \ - vi.last_msg = 0; \ - vi.input.h_mode = DL_BOOZ2_FMS_COMMAND_h_mode(_dl_buffer); \ - vi.input.v_mode = DL_BOOZ2_FMS_COMMAND_v_mode(_dl_buffer); \ - switch (vi.input.h_mode) { \ - case GUIDANCE_H_MODE_KILL: \ - case GUIDANCE_H_MODE_RATE : \ - break; \ - case GUIDANCE_H_MODE_ATTITUDE : \ - { \ - vi.input.h_sp.attitude.phi = DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer); \ +#define VI_PARSE_DATALINK(_dl_buffer) { \ + vi.last_msg = 0; \ + vi.input.h_mode = DL_BOOZ2_FMS_COMMAND_h_mode(_dl_buffer); \ + vi.input.v_mode = DL_BOOZ2_FMS_COMMAND_v_mode(_dl_buffer); \ + switch (vi.input.h_mode) { \ + case GUIDANCE_H_MODE_KILL: \ + case GUIDANCE_H_MODE_RATE : \ + 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_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); \ + 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); \ - } \ - break; \ - case GUIDANCE_H_MODE_NAV : \ - { \ + } \ + break; \ + case GUIDANCE_H_MODE_NAV : \ + { \ vi.input.h_sp.speed.x = DL_BOOZ2_FMS_COMMAND_h_sp_1(_dl_buffer); \ vi.input.h_sp.speed.y = DL_BOOZ2_FMS_COMMAND_h_sp_2(_dl_buffer); \ vi.input.h_sp.speed.z = DL_BOOZ2_FMS_COMMAND_h_sp_3(_dl_buffer); \ - } \ - break; \ - default: \ - break; \ - } \ - switch (vi.input.v_mode) { \ - case GUIDANCE_V_MODE_KILL: \ - case GUIDANCE_V_MODE_RC_DIRECT: \ - case GUIDANCE_V_MODE_RC_CLIMB: \ - break; \ - case GUIDANCE_V_MODE_CLIMB : \ - vi.input.v_sp.climb = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \ - break; \ - case GUIDANCE_V_MODE_HOVER : \ - vi.input.v_sp.height = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \ - break; \ - case GUIDANCE_V_MODE_NAV : \ - vi.input.v_sp.climb = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \ - break; \ - default: \ - break; \ - } \ + } \ + break; \ + default: \ + break; \ + } \ + switch (vi.input.v_mode) { \ + case GUIDANCE_V_MODE_KILL: \ + case GUIDANCE_V_MODE_RC_DIRECT: \ + case GUIDANCE_V_MODE_RC_CLIMB: \ + break; \ + case GUIDANCE_V_MODE_CLIMB : \ + vi.input.v_sp.climb = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \ + break; \ + case GUIDANCE_V_MODE_HOVER : \ + vi.input.v_sp.height = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \ + break; \ + case GUIDANCE_V_MODE_NAV : \ + vi.input.v_sp.climb = DL_BOOZ2_FMS_COMMAND_v_sp(_dl_buffer); \ + break; \ + default: \ + break; \ + } \ } #define VI_NAV_STICK_PARSE_DL(_dl_buffer) { \ - vi.last_msg = 0; \ - vi.input.h_mode = GUIDANCE_H_MODE_NAV; \ - vi.input.v_mode = GUIDANCE_V_MODE_NAV; \ - vi.input.h_sp.speed.x = DL_BOOZ_NAV_STICK_vx_sp(_dl_buffer); \ - vi.input.h_sp.speed.y = DL_BOOZ_NAV_STICK_vy_sp(_dl_buffer); \ - vi.input.h_sp.speed.z = DL_BOOZ_NAV_STICK_r_sp(_dl_buffer); \ - vi.input.v_sp.climb = DL_BOOZ_NAV_STICK_vz_sp(_dl_buffer); \ -} + vi.last_msg = 0; \ + vi.input.h_mode = GUIDANCE_H_MODE_NAV; \ + vi.input.v_mode = GUIDANCE_V_MODE_NAV; \ + vi.input.h_sp.speed.x = DL_BOOZ_NAV_STICK_vx_sp(_dl_buffer); \ + vi.input.h_sp.speed.y = DL_BOOZ_NAV_STICK_vy_sp(_dl_buffer); \ + vi.input.h_sp.speed.z = DL_BOOZ_NAV_STICK_r_sp(_dl_buffer); \ + vi.input.v_sp.climb = DL_BOOZ_NAV_STICK_vz_sp(_dl_buffer); \ + } #define NavUpdateWPFromVI(_wp) { if (vi.enabled) { vi_update_wp(uint8_t _wp); } } diff --git a/sw/airborne/modules/vehicle_interface/vi_test_signal.c b/sw/airborne/modules/vehicle_interface/vi_test_signal.c index 2bf57f4bca..ee2615a0c0 100644 --- a/sw/airborne/modules/vehicle_interface/vi_test_signal.c +++ b/sw/airborne/modules/vehicle_interface/vi_test_signal.c @@ -30,7 +30,8 @@ struct BoozFmsTestSignal fms_test_signal; -void booz_fms_impl_init(void) { +void booz_fms_impl_init(void) +{ fms_test_signal.mode = FMS_TEST_SIGNAL_DEFAULT_MODE; fms_test_signal.period = FMS_TEST_SIGNAL_DEFAULT_PERIOD; fms_test_signal.amplitude = FMS_TEST_SIGNAL_DEFAULT_AMPLITUDE; @@ -39,51 +40,53 @@ void booz_fms_impl_init(void) { fms.input.v_mode = GUIDANCE_V_MODE_HOVER; } -void booz_fms_impl_periodic(void) { +void booz_fms_impl_periodic(void) +{ switch (fms_test_signal.mode) { - case STEP_ROLL: { - if (fms_test_signal.counter < fms_test_signal.period) { - EULERS_ASSIGN(fms.input.h_sp.attitude, fms_test_signal.amplitude, 0, 0); + case STEP_ROLL: { + if (fms_test_signal.counter < fms_test_signal.period) { + EULERS_ASSIGN(fms.input.h_sp.attitude, fms_test_signal.amplitude, 0, 0); + } else { + EULERS_ASSIGN(fms.input.h_sp.attitude, -fms_test_signal.amplitude, 0, 0); + } } - else { - EULERS_ASSIGN(fms.input.h_sp.attitude, -fms_test_signal.amplitude, 0, 0); - } - } break; - case STEP_YAW: { - if (fms_test_signal.counter < fms_test_signal.period) { - EULERS_ASSIGN(fms.input.h_sp.attitude, 0, 0, fms_test_signal.amplitude); + case STEP_YAW: { + if (fms_test_signal.counter < fms_test_signal.period) { + EULERS_ASSIGN(fms.input.h_sp.attitude, 0, 0, fms_test_signal.amplitude); + } else { + EULERS_ASSIGN(fms.input.h_sp.attitude, 0, 0, -fms_test_signal.amplitude); + } } - else { - EULERS_ASSIGN(fms.input.h_sp.attitude, 0, 0, -fms_test_signal.amplitude); - } - } - break; - case STEP_PITCH: - case STEP_VERT: break; + case STEP_PITCH: + case STEP_VERT: + break; #if 0 - case BOOZ_FMS_TEST_SIGNAL_MODE_VERTICAL: { - if (guidance_v_mode < GUIDANCE_V_MODE_HOVER) - 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_INT_OF_FLOAT(-0.5, IPOS_FRAC) + case BOOZ_FMS_TEST_SIGNAL_MODE_VERTICAL: { + if (guidance_v_mode < GUIDANCE_V_MODE_HOVER) { + 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_INT_OF_FLOAT(-0.5, IPOS_FRAC) + } } - } - break; + break; #endif } fms_test_signal.counter++; - if (fms_test_signal.counter >= (2 * fms_test_signal.period)) + if (fms_test_signal.counter >= (2 * fms_test_signal.period)) { fms_test_signal.counter = 0; + } } -void booz_fms_impl_set_enabled(bool_t enabled) { - if (enabled) +void booz_fms_impl_set_enabled(bool_t enabled) +{ + if (enabled) { fms_test_signal.counter = 0; + } } diff --git a/sw/airborne/modules/vehicle_interface/vi_test_signal.h b/sw/airborne/modules/vehicle_interface/vi_test_signal.h index 3e62684389..4c02e5c621 100644 --- a/sw/airborne/modules/vehicle_interface/vi_test_signal.h +++ b/sw/airborne/modules/vehicle_interface/vi_test_signal.h @@ -41,14 +41,14 @@ struct BoozFmsTestSignal { extern struct BoozFmsTestSignal fms_test_signal; -#define booz_fms_test_signal_SetPeriod(_val) { \ - fms_test_signal.period = _val; \ - fms_test_signal.counter = 0; \ +#define booz_fms_test_signal_SetPeriod(_val) { \ + fms_test_signal.period = _val; \ + fms_test_signal.counter = 0; \ } -#define booz_fms_test_signal_SetMode(_val) { \ - fms_test_signal.mode = (enum fms_ts_mode)(_val); \ - fms_test_signal.counter = 0; \ +#define booz_fms_test_signal_SetMode(_val) { \ + fms_test_signal.mode = (enum fms_ts_mode)(_val); \ + fms_test_signal.counter = 0; \ } #endif /* BOOZ_FMS_TEST_SIGNAL_H */ diff --git a/sw/airborne/paparazzi.h b/sw/airborne/paparazzi.h index b0f78765d5..e68875183f 100644 --- a/sw/airborne/paparazzi.h +++ b/sw/airborne/paparazzi.h @@ -10,10 +10,10 @@ typedef int16_t pprz_t; /* type of commands */ #define TRIM_PPRZ(pprz) (pprz < MIN_PPRZ ? MIN_PPRZ : \ (pprz > MAX_PPRZ ? MAX_PPRZ : \ - pprz)) + pprz)) #define TRIM_UPPRZ(pprz) (pprz < 0 ? 0 : \ (pprz > MAX_PPRZ ? MAX_PPRZ : \ - pprz)) + pprz)) #if defined FBW && defined AP #define SINGLE_MCU 1 diff --git a/sw/airborne/peripherals/ads1114.c b/sw/airborne/peripherals/ads1114.c index d5c3c60e08..55867bfeaa 100644 --- a/sw/airborne/peripherals/ads1114.c +++ b/sw/airborne/peripherals/ads1114.c @@ -1,24 +1,24 @@ - /* - * Copyright (C) 2010 ENAC - * - * This file is part of paparazzi. - * - * paparazzi is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2, or (at your option) - * any later version. - * - * paparazzi is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with paparazzi; see the file COPYING. If not, write to - * the Free Software Foundation, 59 Temple Place - Suite 330, - * Boston, MA 02111-1307, USA. - * - */ +/* +* Copyright (C) 2010 ENAC +* +* This file is part of paparazzi. +* +* paparazzi is free software; you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation; either version 2, or (at your option) +* any later version. +* +* paparazzi is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with paparazzi; see the file COPYING. If not, write to +* the Free Software Foundation, 59 Temple Place - Suite 330, +* Boston, MA 02111-1307, USA. +* +*/ /* driver for the ADC ads1114 (16 bits I2C 860SpS max) from Texas instruments @@ -34,7 +34,8 @@ struct ads1114_periph ads1114_1; struct ads1114_periph ads1114_2; #endif -void ads1114_init( void ) { +void ads1114_init(void) +{ /* configure the ads1114_1 */ #if USE_ADS1114_1 ads1114_1.i2c_addr = ADS1114_1_I2C_ADDR; @@ -59,7 +60,8 @@ void ads1114_init( void ) { } -void ads1114_read( struct ads1114_periph * p ) { +void ads1114_read(struct ads1114_periph *p) +{ // Config done with success // start new reading when previous is done (and read if success) if (p->config_done && p->trans.status == I2CTransDone) { diff --git a/sw/airborne/peripherals/ads1114.h b/sw/airborne/peripherals/ads1114.h index faa15ffc1e..42107279cb 100644 --- a/sw/airborne/peripherals/ads1114.h +++ b/sw/airborne/peripherals/ads1114.h @@ -129,18 +129,18 @@ extern struct ads1114_periph ads1114_2; #endif extern void ads1114_init(void); -extern void ads1114_read(struct ads1114_periph * p); +extern void ads1114_read(struct ads1114_periph *p); // Generic Event Macro #define _Ads1114Event(_p) {\ - if (!_p.config_done) { \ - if (_p.trans.status == I2CTransSuccess) { _p.config_done = TRUE; _p.trans.status = I2CTransDone; } \ - if (_p.trans.status == I2CTransFailed) { _p.trans.status = I2CTransDone; } \ - } else { \ - if (_p.trans.status == I2CTransSuccess) { _p.data_available = TRUE; _p.trans.status = I2CTransDone; } \ - if (_p.trans.status == I2CTransFailed) { _p.trans.status = I2CTransDone; } \ - } \ -} + if (!_p.config_done) { \ + if (_p.trans.status == I2CTransSuccess) { _p.config_done = TRUE; _p.trans.status = I2CTransDone; } \ + if (_p.trans.status == I2CTransFailed) { _p.trans.status = I2CTransDone; } \ + } else { \ + if (_p.trans.status == I2CTransSuccess) { _p.data_available = TRUE; _p.trans.status = I2CTransDone; } \ + if (_p.trans.status == I2CTransFailed) { _p.trans.status = I2CTransDone; } \ + } \ + } #if USE_ADS1114_1 #define Ads1114_1Event() _Ads1114Event(ads1114_1) @@ -156,9 +156,9 @@ extern void ads1114_read(struct ads1114_periph * p); // Final event macro #define Ads1114Event() { \ - Ads1114_1Event(); \ - Ads1114_2Event(); \ -} + Ads1114_1Event(); \ + Ads1114_2Event(); \ + } // Get value macro // @param ads1114 periph diff --git a/sw/airborne/peripherals/ads1220.c b/sw/airborne/peripherals/ads1220.c index fbcb07a04f..25dc1336d7 100644 --- a/sw/airborne/peripherals/ads1220.c +++ b/sw/airborne/peripherals/ads1220.c @@ -83,18 +83,18 @@ static void ads1220_send_config(struct Ads1220 *ads) ads->spi_trans.input_length = 0; ads->tx_buf[0] = ADS1220_WREG(ADS1220_CONF0, 4); ads->tx_buf[1] = ( - (ads->config.pga_bypass << 0) | - (ads->config.gain << 1) | - (ads->config.mux << 4)); + (ads->config.pga_bypass << 0) | + (ads->config.gain << 1) | + (ads->config.mux << 4)); ads->tx_buf[2] = ( - (ads->config.conv << 2) | - (ads->config.rate << 5)); + (ads->config.conv << 2) | + (ads->config.rate << 5)); ads->tx_buf[3] = ( - (ads->config.idac << 0) | - (ads->config.vref << 6)); + (ads->config.idac << 0) | + (ads->config.vref << 6)); ads->tx_buf[4] = ( - (ads->config.i2mux << 2) | - (ads->config.i1mux << 5)); + (ads->config.i2mux << 2) | + (ads->config.i1mux << 5)); spi_submit(ads->spi_p, &(ads->spi_trans)); } @@ -109,8 +109,7 @@ void ads1220_configure(struct Ads1220 *ads) spi_submit(ads->spi_p, &(ads->spi_trans)); ads->config.status = ADS1220_SEND_RESET; } - } - else if (ads->config.status == ADS1220_INITIALIZING) { // Configuring but not yet initialized + } else if (ads->config.status == ADS1220_INITIALIZING) { // Configuring but not yet initialized if (ads->spi_trans.status == SPITransSuccess || ads->spi_trans.status == SPITransDone) { ads1220_send_config(ads); // do config } @@ -133,31 +132,26 @@ void ads1220_event(struct Ads1220 *ads) if (ads->config.status == ADS1220_INITIALIZED) { if (ads->spi_trans.status == SPITransFailed) { ads->spi_trans.status = SPITransDone; - } - else if (ads->spi_trans.status == SPITransSuccess) { + } else if (ads->spi_trans.status == SPITransSuccess) { // Successfull reading of 24bits adc - ads->data = (uint32_t)(((uint32_t)(ads->rx_buf[0])<<16)|((uint32_t)(ads->rx_buf[1])<<8)|(ads->rx_buf[2])); + ads->data = (uint32_t)(((uint32_t)(ads->rx_buf[0]) << 16) | ((uint32_t)(ads->rx_buf[1]) << 8) | (ads->rx_buf[2])); ads->data_available = TRUE; ads->spi_trans.status = SPITransDone; } - } - else if (ads->config.status == ADS1220_SEND_RESET) { // Reset ads1220 before configuring + } else if (ads->config.status == ADS1220_SEND_RESET) { // Reset ads1220 before configuring if (ads->spi_trans.status == SPITransFailed) { ads->spi_trans.status = SPITransDone; ads->config.status = ADS1220_UNINIT; // config failed - } - else if (ads->spi_trans.status == SPITransSuccess) { + } else if (ads->spi_trans.status == SPITransSuccess) { ads->spi_trans.status = SPITransDone; ads->config.status = ADS1220_INITIALIZING; // do config at next call of ads1220_configure() (or ads1220_periodic()) } - } - else if (ads->config.status == ADS1220_INITIALIZING) { // Configuring but not yet initialized + } else if (ads->config.status == ADS1220_INITIALIZING) { // Configuring but not yet initialized if (ads->spi_trans.status == SPITransFailed) { ads->spi_trans.status = SPITransDone; ads->config.status = ADS1220_UNINIT; // config failed - } - else if (ads->spi_trans.status == SPITransSuccess) { + } else if (ads->spi_trans.status == SPITransSuccess) { ads->spi_trans.status = SPITransDone; ads->config.status = ADS1220_INITIALIZED; // config done } diff --git a/sw/airborne/peripherals/ads1220.h b/sw/airborne/peripherals/ads1220.h index efe722e467..40e5279273 100644 --- a/sw/airborne/peripherals/ads1220.h +++ b/sw/airborne/peripherals/ads1220.h @@ -168,11 +168,13 @@ extern void ads1220_read(struct Ads1220 *ads); extern void ads1220_event(struct Ads1220 *ads); /// convenience function: read or start configuration if not already initialized -static inline void ads1220_periodic(struct Ads1220 *ads) { - if (ads->config.status == ADS1220_INITIALIZED) +static inline void ads1220_periodic(struct Ads1220 *ads) +{ + if (ads->config.status == ADS1220_INITIALIZED) { ads1220_read(ads); - else + } else { ads1220_configure(ads); + } } #endif // ADS1220_H diff --git a/sw/airborne/peripherals/adxl345_i2c.c b/sw/airborne/peripherals/adxl345_i2c.c index be3dcca5a7..5732a0ae16 100644 --- a/sw/airborne/peripherals/adxl345_i2c.c +++ b/sw/airborne/peripherals/adxl345_i2c.c @@ -74,7 +74,7 @@ static void adxl345_i2c_send_config(struct Adxl345_I2c *adxl) break; case ADXL_CONF_ENABLE: /* enable measurement, is in standby after power up */ - adxl345_i2c_tx_reg(adxl, ADXL345_REG_POWER_CTL, (0x1<<3)); + adxl345_i2c_tx_reg(adxl, ADXL345_REG_POWER_CTL, (0x1 << 3)); adxl->init_status++; break; case ADXL_CONF_DONE: @@ -116,17 +116,15 @@ void adxl345_i2c_event(struct Adxl345_I2c *adxl) if (adxl->initialized) { if (adxl->i2c_trans.status == I2CTransFailed) { adxl->i2c_trans.status = I2CTransDone; - } - else if (adxl->i2c_trans.status == I2CTransSuccess) { - // Successfull reading - adxl->data.vect.x = Int16FromBuf(adxl->i2c_trans.buf,0); - adxl->data.vect.y = Int16FromBuf(adxl->i2c_trans.buf,2); - adxl->data.vect.z = Int16FromBuf(adxl->i2c_trans.buf,4); + } else if (adxl->i2c_trans.status == I2CTransSuccess) { + // Successfull reading + adxl->data.vect.x = Int16FromBuf(adxl->i2c_trans.buf, 0); + adxl->data.vect.y = Int16FromBuf(adxl->i2c_trans.buf, 2); + adxl->data.vect.z = Int16FromBuf(adxl->i2c_trans.buf, 4); adxl->data_available = TRUE; adxl->i2c_trans.status = I2CTransDone; } - } - else if (adxl->init_status != ADXL_CONF_UNINIT) { // Configuring but not yet initialized + } else if (adxl->init_status != ADXL_CONF_UNINIT) { // Configuring but not yet initialized if (adxl->i2c_trans.status == I2CTransSuccess || adxl->i2c_trans.status == I2CTransDone) { adxl->i2c_trans.status = I2CTransDone; adxl345_i2c_send_config(adxl); diff --git a/sw/airborne/peripherals/adxl345_i2c.h b/sw/airborne/peripherals/adxl345_i2c.h index 1db8d4968e..2bbd51f709 100644 --- a/sw/airborne/peripherals/adxl345_i2c.h +++ b/sw/airborne/peripherals/adxl345_i2c.h @@ -56,11 +56,13 @@ extern void adxl345_i2c_read(struct Adxl345_I2c *adxl); extern void adxl345_i2c_event(struct Adxl345_I2c *adxl); /// convenience function: read or start configuration if not already initialized -static inline void adxl345_i2c_periodic(struct Adxl345_I2c *adxl) { - if (adxl->initialized) +static inline void adxl345_i2c_periodic(struct Adxl345_I2c *adxl) +{ + if (adxl->initialized) { adxl345_i2c_read(adxl); - else + } else { adxl345_i2c_start_configure(adxl); + } } #endif // ADXL345_I2C_H diff --git a/sw/airborne/peripherals/adxl345_spi.c b/sw/airborne/peripherals/adxl345_spi.c index 9f488eb67f..6447b29adb 100644 --- a/sw/airborne/peripherals/adxl345_spi.c +++ b/sw/airborne/peripherals/adxl345_spi.c @@ -66,7 +66,8 @@ void adxl345_spi_init(struct Adxl345_Spi *adxl, struct spi_periph *spi_p, uint8_ } -static void adxl345_spi_write_to_reg(struct Adxl345_Spi *adxl, uint8_t _reg, uint8_t _val) { +static void adxl345_spi_write_to_reg(struct Adxl345_Spi *adxl, uint8_t _reg, uint8_t _val) +{ adxl->spi_trans.output_length = 2; adxl->spi_trans.input_length = 0; adxl->tx_buf[0] = _reg; @@ -92,7 +93,7 @@ static void adxl345_spi_send_config(struct Adxl345_Spi *adxl) break; case ADXL_CONF_ENABLE: /* enable measurement, is in standby after power up */ - adxl345_spi_write_to_reg(adxl, ADXL345_REG_POWER_CTL, (0x1<<3)); + adxl345_spi_write_to_reg(adxl, ADXL345_REG_POWER_CTL, (0x1 << 3)); adxl->init_status++; break; case ADXL_CONF_DONE: @@ -120,7 +121,7 @@ void adxl345_spi_read(struct Adxl345_Spi *adxl) adxl->spi_trans.output_length = 1; adxl->spi_trans.input_length = 7; /* set read bit and multiple byte bit, then address */ - adxl->tx_buf[0] = (1<<7|1<<6|ADXL345_REG_DATA_X0); + adxl->tx_buf[0] = (1 << 7 | 1 << 6 | ADXL345_REG_DATA_X0); spi_submit(adxl->spi_p, &(adxl->spi_trans)); } } @@ -132,17 +133,15 @@ void adxl345_spi_event(struct Adxl345_Spi *adxl) if (adxl->initialized) { if (adxl->spi_trans.status == SPITransFailed) { adxl->spi_trans.status = SPITransDone; - } - else if (adxl->spi_trans.status == SPITransSuccess) { + } else if (adxl->spi_trans.status == SPITransSuccess) { // Successfull reading - adxl->data.vect.x = Int16FromBuf(adxl->rx_buf,1); - adxl->data.vect.y = Int16FromBuf(adxl->rx_buf,3); - adxl->data.vect.z = Int16FromBuf(adxl->rx_buf,5); + adxl->data.vect.x = Int16FromBuf(adxl->rx_buf, 1); + adxl->data.vect.y = Int16FromBuf(adxl->rx_buf, 3); + adxl->data.vect.z = Int16FromBuf(adxl->rx_buf, 5); adxl->data_available = TRUE; adxl->spi_trans.status = SPITransDone; } - } - else if (adxl->init_status != ADXL_CONF_UNINIT) { // Configuring but not yet initialized + } else if (adxl->init_status != ADXL_CONF_UNINIT) { // Configuring but not yet initialized switch (adxl->spi_trans.status) { case SPITransFailed: adxl->init_status--; // Retry config (TODO max retry) diff --git a/sw/airborne/peripherals/adxl345_spi.h b/sw/airborne/peripherals/adxl345_spi.h index 0c4de5f64a..fc4139a754 100644 --- a/sw/airborne/peripherals/adxl345_spi.h +++ b/sw/airborne/peripherals/adxl345_spi.h @@ -58,11 +58,13 @@ extern void adxl345_spi_read(struct Adxl345_Spi *adxl); extern void adxl345_spi_event(struct Adxl345_Spi *adxl); /// convenience function: read or start configuration if not already initialized -static inline void adxl345_spi_periodic(struct Adxl345_Spi *adxl) { - if (adxl->initialized) +static inline void adxl345_spi_periodic(struct Adxl345_Spi *adxl) +{ + if (adxl->initialized) { adxl345_spi_read(adxl); - else + } else { adxl345_spi_start_configure(adxl); + } } #endif // ADXL345_SPI_H diff --git a/sw/airborne/peripherals/ak8963.c b/sw/airborne/peripherals/ak8963.c index f029394ec8..3033048e7a 100644 --- a/sw/airborne/peripherals/ak8963.c +++ b/sw/airborne/peripherals/ak8963.c @@ -46,7 +46,8 @@ void ak8963_init(struct Ak8963 *ak, struct i2c_periph *i2c_p, uint8_t addr) void ak8963_configure(struct Ak8963 *ak) { // Only configure when not busy - if (ak->i2c_trans.status != I2CTransSuccess && ak->i2c_trans.status != I2CTransFailed && ak->i2c_trans.status != I2CTransDone) { + if (ak->i2c_trans.status != I2CTransSuccess && ak->i2c_trans.status != I2CTransFailed + && ak->i2c_trans.status != I2CTransDone) { return; } diff --git a/sw/airborne/peripherals/ami601.c b/sw/airborne/peripherals/ami601.c index 1f7f3d4c76..cbf5e0b116 100644 --- a/sw/airborne/peripherals/ami601.c +++ b/sw/airborne/peripherals/ami601.c @@ -9,10 +9,11 @@ volatile uint8_t ami601_status; struct i2c_transaction ami601_i2c_trans; volatile uint32_t ami601_nb_err; -void ami601_init( void ) { +void ami601_init(void) +{ uint8_t i; - for (i=0; i< AMI601_NB_CHAN; i++) { + for (i = 0; i < AMI601_NB_CHAN; i++) { ami601_values[i] = 0; } ami601_i2c_trans.status = I2CTransSuccess; @@ -22,12 +23,12 @@ void ami601_init( void ) { } -void ami601_read( void ) { +void ami601_read(void) +{ if (ami601_status != AMI601_IDLE) { ami601_nb_err++; ami601_status = AMI601_IDLE; - } - else { + } else { ami601_status = AMI601_SENDING_REQ; ami601_i2c_trans.type = I2CTransTx; ami601_i2c_trans.len_w = 3; diff --git a/sw/airborne/peripherals/ami601.h b/sw/airborne/peripherals/ami601.h index 4bfc851d93..5c69bf95b1 100644 --- a/sw/airborne/peripherals/ami601.h +++ b/sw/airborne/peripherals/ami601.h @@ -4,10 +4,10 @@ #include "std.h" #include "mcu_periph/i2c.h" -extern void ami601_init( void ); +extern void ami601_init(void); -extern void ami601_read( void ); -extern void ami601_periodic( void ); +extern void ami601_read(void); +extern void ami601_periodic(void); extern void ami601_scale_measures(void); #define AMI601_NB_CHAN 6 @@ -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/peripherals/bmp085.c b/sw/airborne/peripherals/bmp085.c index 061d2701d2..0d3d55f1dc 100644 --- a/sw/airborne/peripherals/bmp085.c +++ b/sw/airborne/peripherals/bmp085.c @@ -28,9 +28,9 @@ #include "peripherals/bmp085_regs.h" -static int32_t bmp085_compensated_temperature(struct Bmp085Calib* calib, int32_t raw) +static int32_t bmp085_compensated_temperature(struct Bmp085Calib *calib, int32_t raw) { - int32_t x1 = (raw - calib->ac6) * calib->ac5 / (1<<15); + int32_t x1 = (raw - calib->ac6) * calib->ac5 / (1 << 15); int32_t x2 = (calib->mc << 11) / (x1 + calib->md); calib->b5 = x1 + x2; return (calib->b5 + 8) >> 4; @@ -41,17 +41,17 @@ static int32_t bmp085_compensated_temperature(struct Bmp085Calib* calib, int32_t /** Apply temp calibration and sensor calibration to raw measurement to get Pa * (from BMP085 datasheet) */ -static int32_t bmp085_compensated_pressure(struct Bmp085Calib* calib, int32_t raw) +static int32_t bmp085_compensated_pressure(struct Bmp085Calib *calib, int32_t raw) { int32_t b6 = calib->b5 - 4000; int32_t x1 = (calib->b2 * (b6 * b6 >> 12)) >> 11; int32_t x2 = calib->ac2 * b6 >> 11; int32_t x3 = x1 + x2; - int32_t b3 = (((calib->ac1 * 4 + x3) << BMP085_OSS) + 2)/4; + int32_t b3 = (((calib->ac1 * 4 + x3) << BMP085_OSS) + 2) / 4; x1 = calib->ac3 * b6 >> 13; x2 = (calib->b1 * (b6 * b6 >> 12)) >> 16; x3 = ((x1 + x2) + 2) >> 2; - uint32_t b4 = (calib->ac4 * (uint32_t) (x3 + 32768)) >> 15; + uint32_t b4 = (calib->ac4 * (uint32_t)(x3 + 32768)) >> 15; uint32_t b7 = (raw - b3) * (50000 >> BMP085_OSS); int32_t p = b7 < 0x80000000 ? (b7 * 2) / b4 : (b7 / b4) * 2; x1 = (p >> 8) * (p >> 8); @@ -70,7 +70,7 @@ static bool_t bmp085_eoc_true(void) } -void bmp085_read_eeprom_calib(struct Bmp085* bmp) +void bmp085_read_eeprom_calib(struct Bmp085 *bmp) { if (bmp->status == BMP085_STATUS_UNINIT && bmp->i2c_trans.status == I2CTransDone) { bmp->initialized = FALSE; @@ -80,7 +80,7 @@ void bmp085_read_eeprom_calib(struct Bmp085* bmp) } -void bmp085_init(struct Bmp085* bmp, struct i2c_periph *i2c_p, uint8_t addr) +void bmp085_init(struct Bmp085 *bmp, struct i2c_periph *i2c_p, uint8_t addr) { /* set i2c_peripheral */ bmp->i2c_p = i2c_p; @@ -105,7 +105,7 @@ void bmp085_init(struct Bmp085* bmp, struct i2c_periph *i2c_p, uint8_t addr) * Should run at < 40Hz unless eoc check function is provided. * At ultra high resolution (oss = 3) conversion time is max 25.5ms. */ -void bmp085_periodic(struct Bmp085* bmp) +void bmp085_periodic(struct Bmp085 *bmp) { switch (bmp->status) { case BMP085_STATUS_IDLE: @@ -139,7 +139,7 @@ void bmp085_periodic(struct Bmp085* bmp) } } -void bmp085_event(struct Bmp085* bmp) +void bmp085_event(struct Bmp085 *bmp) { if (bmp->i2c_trans.status == I2CTransSuccess) { switch (bmp->status) { @@ -172,9 +172,9 @@ void bmp085_event(struct Bmp085* bmp) case BMP085_STATUS_READ_PRESS: /* get uncompensated pressure */ - bmp->up = (bmp->i2c_trans.buf[0]<<16) | + bmp->up = (bmp->i2c_trans.buf[0] << 16) | (bmp->i2c_trans.buf[1] << 8) | - bmp->i2c_trans.buf[2]; + bmp->i2c_trans.buf[2]; bmp->up = bmp->up >> (8 - BMP085_OSS); bmp->pressure = bmp085_compensated_pressure(&(bmp->calib), bmp->up); bmp->data_available = TRUE; @@ -184,13 +184,13 @@ void bmp085_event(struct Bmp085* bmp) default: break; } - } - else if (bmp->i2c_trans.status == I2CTransFailed) { + } else if (bmp->i2c_trans.status == I2CTransFailed) { /* try again */ - if (bmp->initialized) + if (bmp->initialized) { bmp->status = BMP085_STATUS_IDLE; - else + } else { bmp->status = BMP085_STATUS_UNINIT; + } bmp->i2c_trans.status = I2CTransDone; } } diff --git a/sw/airborne/peripherals/bmp085.h b/sw/airborne/peripherals/bmp085.h index a335f1e3b5..eab6924d03 100644 --- a/sw/airborne/peripherals/bmp085.h +++ b/sw/airborne/peripherals/bmp085.h @@ -73,9 +73,9 @@ struct Bmp085 { int32_t pressure; ///< pressure in Pascal }; -extern void bmp085_read_eeprom_calib(struct Bmp085* bmp); -extern void bmp085_init(struct Bmp085* bmp, struct i2c_periph *i2c_p, uint8_t addr); -extern void bmp085_periodic(struct Bmp085* bmp); -extern void bmp085_event(struct Bmp085* bmp); +extern void bmp085_read_eeprom_calib(struct Bmp085 *bmp); +extern void bmp085_init(struct Bmp085 *bmp, struct i2c_periph *i2c_p, uint8_t addr); +extern void bmp085_periodic(struct Bmp085 *bmp); +extern void bmp085_event(struct Bmp085 *bmp); #endif diff --git a/sw/airborne/peripherals/cyrf6936.c b/sw/airborne/peripherals/cyrf6936.c index 1b1e678a48..49d3217cb4 100644 --- a/sw/airborne/peripherals/cyrf6936.c +++ b/sw/airborne/peripherals/cyrf6936.c @@ -36,14 +36,17 @@ /* Static functions used in the different statuses */ static bool_t cyrf6936_write_register(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t data); -static bool_t cyrf6936_write_block(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t data[], const uint8_t length); +static bool_t cyrf6936_write_block(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t data[], + const uint8_t length); static bool_t cyrf6936_read_register(struct Cyrf6936 *cyrf, const uint8_t addr); static bool_t cyrf6936_read_block(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t length); /** * Initializing the cyrf chip */ -void cyrf6936_init(struct Cyrf6936 *cyrf, struct spi_periph *spi_p, const uint8_t slave_idx, const uint32_t rst_port, const uint16_t rst_pin) { +void cyrf6936_init(struct Cyrf6936 *cyrf, struct spi_periph *spi_p, const uint8_t slave_idx, const uint32_t rst_port, + const uint16_t rst_pin) +{ /* Set spi_peripheral and the status */ cyrf->spi_p = spi_p; cyrf->status = CYRF6936_UNINIT; @@ -80,201 +83,216 @@ void cyrf6936_init(struct Cyrf6936 *cyrf, struct spi_periph *spi_p, const uint8_ /** * The on event call for the CYRF6936 chip */ -void cyrf6936_event(struct Cyrf6936 *cyrf) { +void cyrf6936_event(struct Cyrf6936 *cyrf) +{ int i; // Check if cyrf is initialized - if(cyrf->status == CYRF6936_UNINIT) + if (cyrf->status == CYRF6936_UNINIT) { return; + } // Check if there is still a transaction in progress - if(cyrf->spi_t.status == SPITransPending || cyrf->spi_t.status == SPITransRunning) + if (cyrf->spi_t.status == SPITransPending || cyrf->spi_t.status == SPITransRunning) { return; + } /* Check the status of the cyrf */ switch (cyrf->status) { - /* Getting the MFG id */ - case CYRF6936_GET_MFG_ID: - // When the last transaction isn't failed send the next - if(cyrf->spi_t.status != SPITransFailed) - cyrf->buffer_idx++; + /* Getting the MFG id */ + case CYRF6936_GET_MFG_ID: + // When the last transaction isn't failed send the next + if (cyrf->spi_t.status != SPITransFailed) { + cyrf->buffer_idx++; + } - cyrf->spi_t.status = SPITransDone; + cyrf->spi_t.status = SPITransDone; - // Switch for the different states - switch (cyrf->buffer_idx) { - case 0: - cyrf6936_write_register(cyrf, CYRF_MFG_ID, 0xFF); - break; - case 1: - cyrf6936_read_block(cyrf, CYRF_MFG_ID, 6); - break; - case 2: - // Copy the MFG id - for(i = 0; i < 6; i++) - cyrf->mfg_id[i] = cyrf->input_buf[i+1]; + // Switch for the different states + switch (cyrf->buffer_idx) { + case 0: + cyrf6936_write_register(cyrf, CYRF_MFG_ID, 0xFF); + break; + case 1: + cyrf6936_read_block(cyrf, CYRF_MFG_ID, 6); + break; + case 2: + // Copy the MFG id + for (i = 0; i < 6; i++) { + cyrf->mfg_id[i] = cyrf->input_buf[i + 1]; + } - cyrf6936_write_register(cyrf, CYRF_MFG_ID, 0x00); + cyrf6936_write_register(cyrf, CYRF_MFG_ID, 0x00); + break; + default: + cyrf->status = CYRF6936_IDLE; + break; + } break; + + /* Do a multi write */ + case CYRF6936_MULTIWRITE: + // When the last transaction isn't failed send the next + if (cyrf->spi_t.status != SPITransFailed) { + cyrf->buffer_idx++; + } + + cyrf->spi_t.status = SPITransDone; + + // When we are done writing + if (cyrf->buffer_idx == cyrf->buffer_length) { + cyrf->status = CYRF6936_IDLE; + break; + } + + // Write the next register from the buffer + cyrf6936_write_register(cyrf, + ((uint8_t ( *)[2])cyrf->buffer)[cyrf->buffer_idx][0], + ((uint8_t ( *)[2])cyrf->buffer)[cyrf->buffer_idx][1]); + break; + + /* Do a write of the data code */ + case CYRF6936_DATA_CODE: + break; + + /* Do a write of channel, sop, data and crc */ + case CYRF6936_CHAN_SOP_DATA_CRC: + // When the last transaction isn't failed send the next + if (cyrf->spi_t.status != SPITransFailed) { + cyrf->buffer_idx++; + } + + cyrf->spi_t.status = SPITransDone; + + // Switch for the different states + switch (cyrf->buffer_idx) { + case 0: // Write the CRC LSB + cyrf6936_write_register(cyrf, CYRF_CRC_SEED_LSB, cyrf->buffer[0]); + break; + case 1: // Write the CRC MSB + cyrf6936_write_register(cyrf, CYRF_CRC_SEED_MSB, cyrf->buffer[1]); + break; + case 2: // Write the SOP code + cyrf6936_write_block(cyrf, CYRF_SOP_CODE, &(cyrf->buffer[2]), 8); + break; + case 3: // Write the DATA code + cyrf6936_write_block(cyrf, CYRF_DATA_CODE, &(cyrf->buffer[10]), 16); + break; + case 4: // Write the Channel + cyrf6936_write_register(cyrf, CYRF_CHANNEL, cyrf->buffer[26]); + break; + default: + cyrf->status = CYRF6936_IDLE; + break; + } + break; + + /* Do a read of the receive irq status, receive status and the receive packet */ + case CYRF6936_RX_IRQ_STATUS_PACKET: + // When the last transaction isn't failed send the next + if (cyrf->spi_t.status != SPITransFailed) { + cyrf->buffer_idx++; + } + + cyrf->spi_t.status = SPITransDone; + + // Switch for the different states + switch (cyrf->buffer_idx) { + case 0: // Read the receive IRQ status + cyrf6936_read_register(cyrf, CYRF_RX_IRQ_STATUS); + break; + case 1: // Read the send IRQ status + cyrf->rx_irq_status = cyrf->input_buf[1]; + cyrf6936_read_register(cyrf, CYRF_TX_IRQ_STATUS); + break; + case 2: // Read the receive status + cyrf->tx_irq_status = cyrf->input_buf[1]; + cyrf6936_read_register(cyrf, CYRF_RX_STATUS); + break; + case 3: // Set the packet length + cyrf->rx_status = cyrf->input_buf[1]; + cyrf6936_read_register(cyrf, CYRF_RX_COUNT); + break; + case 4: // Read the receive packet + cyrf->rx_count = cyrf->input_buf[1]; + cyrf6936_read_block(cyrf, CYRF_RX_BUFFER, 16); + break; + default: + // Copy the receive packet + for (i = 0; i < 16; i++) { + cyrf->rx_packet[i] = cyrf->input_buf[i + 1]; + } + + cyrf->has_irq = TRUE; + cyrf->status = CYRF6936_IDLE; + break; + } + break; + + /* The CYRF6936 is busy sending a packet */ + case CYRF6936_SEND: + // When the last transaction isn't failed send the next + if (cyrf->spi_t.status != SPITransFailed) { + cyrf->buffer_idx++; + } + + cyrf->spi_t.status = SPITransDone; + + // Switch for the different states + switch (cyrf->buffer_idx) { + case 0: // Set the packet length + cyrf6936_write_register(cyrf, CYRF_TX_LENGTH, cyrf->buffer[0]); + break; + case 1: // Clear the TX buffer + cyrf6936_write_register(cyrf, CYRF_TX_CTRL, CYRF_TX_CLR); + break; + case 2: // Write the send packet + cyrf6936_write_block(cyrf, CYRF_TX_BUFFER, &cyrf->buffer[1], 16); + break; + case 3: // Send the packet + cyrf6936_write_register(cyrf, CYRF_TX_CTRL, CYRF_TX_GO | CYRF_TXC_IRQEN | CYRF_TXE_IRQEN); + break; + default: + cyrf->status = CYRF6936_IDLE; + break; + } + break; + + /* This should not happen */ default: - cyrf->status = CYRF6936_IDLE; break; - } - break; - - /* Do a multi write */ - case CYRF6936_MULTIWRITE: - // When the last transaction isn't failed send the next - if(cyrf->spi_t.status != SPITransFailed) - cyrf->buffer_idx++; - - cyrf->spi_t.status = SPITransDone; - - // When we are done writing - if(cyrf->buffer_idx == cyrf->buffer_length) { - cyrf->status = CYRF6936_IDLE; - break; - } - - // Write the next register from the buffer - cyrf6936_write_register(cyrf, - ((uint8_t (*)[2])cyrf->buffer)[cyrf->buffer_idx][0], - ((uint8_t (*)[2])cyrf->buffer)[cyrf->buffer_idx][1]); - break; - - /* Do a write of the data code */ - case CYRF6936_DATA_CODE: - break; - - /* Do a write of channel, sop, data and crc */ - case CYRF6936_CHAN_SOP_DATA_CRC: - // When the last transaction isn't failed send the next - if(cyrf->spi_t.status != SPITransFailed) - cyrf->buffer_idx++; - - cyrf->spi_t.status = SPITransDone; - - // Switch for the different states - switch (cyrf->buffer_idx) { - case 0: // Write the CRC LSB - cyrf6936_write_register(cyrf, CYRF_CRC_SEED_LSB, cyrf->buffer[0]); - break; - case 1: // Write the CRC MSB - cyrf6936_write_register(cyrf, CYRF_CRC_SEED_MSB, cyrf->buffer[1]); - break; - case 2: // Write the SOP code - cyrf6936_write_block(cyrf, CYRF_SOP_CODE, &(cyrf->buffer[2]), 8); - break; - case 3: // Write the DATA code - cyrf6936_write_block(cyrf, CYRF_DATA_CODE, &(cyrf->buffer[10]), 16); - break; - case 4: // Write the Channel - cyrf6936_write_register(cyrf, CYRF_CHANNEL, cyrf->buffer[26]); - break; - default: - cyrf->status = CYRF6936_IDLE; - break; - } - break; - - /* Do a read of the receive irq status, receive status and the receive packet */ - case CYRF6936_RX_IRQ_STATUS_PACKET: - // When the last transaction isn't failed send the next - if(cyrf->spi_t.status != SPITransFailed) - cyrf->buffer_idx++; - - cyrf->spi_t.status = SPITransDone; - - // Switch for the different states - switch (cyrf->buffer_idx) { - case 0: // Read the receive IRQ status - cyrf6936_read_register(cyrf, CYRF_RX_IRQ_STATUS); - break; - case 1: // Read the send IRQ status - cyrf->rx_irq_status = cyrf->input_buf[1]; - cyrf6936_read_register(cyrf, CYRF_TX_IRQ_STATUS); - break; - case 2: // Read the receive status - cyrf->tx_irq_status = cyrf->input_buf[1]; - cyrf6936_read_register(cyrf, CYRF_RX_STATUS); - break; - case 3: // Set the packet length - cyrf->rx_status = cyrf->input_buf[1]; - cyrf6936_read_register(cyrf, CYRF_RX_COUNT); - break; - case 4: // Read the receive packet - cyrf->rx_count = cyrf->input_buf[1]; - cyrf6936_read_block(cyrf, CYRF_RX_BUFFER, 16); - break; - default: - // Copy the receive packet - for(i = 0; i < 16; i++) - cyrf->rx_packet[i] = cyrf->input_buf[i+1]; - - cyrf->has_irq = TRUE; - cyrf->status = CYRF6936_IDLE; - break; - } - break; - - /* The CYRF6936 is busy sending a packet */ - case CYRF6936_SEND: - // When the last transaction isn't failed send the next - if(cyrf->spi_t.status != SPITransFailed) - cyrf->buffer_idx++; - - cyrf->spi_t.status = SPITransDone; - - // Switch for the different states - switch (cyrf->buffer_idx) { - case 0: // Set the packet length - cyrf6936_write_register(cyrf, CYRF_TX_LENGTH, cyrf->buffer[0]); - break; - case 1: // Clear the TX buffer - cyrf6936_write_register(cyrf, CYRF_TX_CTRL, CYRF_TX_CLR); - break; - case 2: // Write the send packet - cyrf6936_write_block(cyrf, CYRF_TX_BUFFER, &cyrf->buffer[1], 16); - break; - case 3: // Send the packet - cyrf6936_write_register(cyrf, CYRF_TX_CTRL, CYRF_TX_GO | CYRF_TXC_IRQEN | CYRF_TXE_IRQEN); - break; - default: - cyrf->status = CYRF6936_IDLE; - break; - } - break; - - /* This should not happen */ - default: - break; } } /** * Write a byte to a register */ -static bool_t cyrf6936_write_register(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t data) { +static bool_t cyrf6936_write_register(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t data) +{ return cyrf6936_write_block(cyrf, addr, &data, 1); } /** * Write multiple bytes to a register */ -static bool_t cyrf6936_write_block(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t data[], const uint8_t length) { +static bool_t cyrf6936_write_block(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t data[], + const uint8_t length) +{ uint8_t i; /* Check if there is already a SPI transaction busy */ - if(cyrf->spi_t.status != SPITransDone) + if (cyrf->spi_t.status != SPITransDone) { return FALSE; + } /* Set the buffer and commit the transaction */ - cyrf->spi_t.output_length = length+1; + cyrf->spi_t.output_length = length + 1; cyrf->spi_t.input_length = 0; cyrf->output_buf[0] = addr | CYRF_DIR; // Copy the data - for(i = 0; i < length; i++) - cyrf->output_buf[i+1] = data[i]; + for (i = 0; i < length; i++) { + cyrf->output_buf[i + 1] = data[i]; + } // Submit the transaction return spi_submit(cyrf->spi_p, &(cyrf->spi_t)); @@ -283,16 +301,19 @@ static bool_t cyrf6936_write_block(struct Cyrf6936 *cyrf, const uint8_t addr, co /** * Read a byte from a register */ -static bool_t cyrf6936_read_register(struct Cyrf6936 *cyrf, const uint8_t addr) { +static bool_t cyrf6936_read_register(struct Cyrf6936 *cyrf, const uint8_t addr) +{ return cyrf6936_read_block(cyrf, addr, 1); } /** * Read multiple bytes from a register */ -static bool_t cyrf6936_read_block(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t length) { - if(cyrf->spi_t.status != SPITransDone) +static bool_t cyrf6936_read_block(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t length) +{ + if (cyrf->spi_t.status != SPITransDone) { return FALSE; + } /* Set the buffer and commit the transaction */ cyrf->spi_t.output_length = 1; @@ -306,9 +327,10 @@ static bool_t cyrf6936_read_block(struct Cyrf6936 *cyrf, const uint8_t addr, con /** * Write to one register */ -bool_t cyrf6936_write(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t data) { +bool_t cyrf6936_write(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t data) +{ const uint8_t data_multi[][2] = { - {addr, data} + {addr, data} }; return cyrf6936_multi_write(cyrf, data_multi, 1); } @@ -316,11 +338,13 @@ bool_t cyrf6936_write(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t d /** * Write to multiple registers one byte */ -bool_t cyrf6936_multi_write(struct Cyrf6936 *cyrf, const uint8_t data[][2], const uint8_t length) { +bool_t cyrf6936_multi_write(struct Cyrf6936 *cyrf, const uint8_t data[][2], const uint8_t length) +{ uint8_t i; /* Check if the cyrf6936 isn't busy */ - if(cyrf->status != CYRF6936_IDLE) + if (cyrf->status != CYRF6936_IDLE) { return FALSE; + } // Set the status cyrf->status = CYRF6936_MULTIWRITE; @@ -330,14 +354,15 @@ bool_t cyrf6936_multi_write(struct Cyrf6936 *cyrf, const uint8_t data[][2], cons cyrf->buffer_idx = 0; // Copy the buffer - for(i = 0; i< length; i++) { - cyrf->buffer[i*2] = data[i][0]; - cyrf->buffer[i*2+1] = data[i][1]; + for (i = 0; i < length; i++) { + cyrf->buffer[i * 2] = data[i][0]; + cyrf->buffer[i * 2 + 1] = data[i][1]; } /* Write the first regiter */ - if(length > 0) + if (length > 0) { cyrf6936_write_register(cyrf, data[0][0], data[0][1]); + } return TRUE; } @@ -345,11 +370,14 @@ bool_t cyrf6936_multi_write(struct Cyrf6936 *cyrf, const uint8_t data[][2], cons /** * Set the channel, SOP code, DATA code and the CRC seed */ -bool_t cyrf6936_write_chan_sop_data_crc(struct Cyrf6936 *cyrf, const uint8_t chan, const uint8_t sop_code[], const uint8_t data_code[], const uint16_t crc_seed) { +bool_t cyrf6936_write_chan_sop_data_crc(struct Cyrf6936 *cyrf, const uint8_t chan, const uint8_t sop_code[], + const uint8_t data_code[], const uint16_t crc_seed) +{ uint8_t i; /* Check if the cyrf6936 isn't busy */ - if(cyrf->status != CYRF6936_IDLE) + if (cyrf->status != CYRF6936_IDLE) { return FALSE; + } // Set the status cyrf->status = CYRF6936_CHAN_SOP_DATA_CRC; @@ -359,12 +387,14 @@ bool_t cyrf6936_write_chan_sop_data_crc(struct Cyrf6936 *cyrf, const uint8_t cha cyrf->buffer[1] = (crc_seed >> 8) & 0xFF; // Copy the SOP code - for(i = 0; i < 8; i++) - cyrf->buffer[i+2] = sop_code[i]; + for (i = 0; i < 8; i++) { + cyrf->buffer[i + 2] = sop_code[i]; + } // Copy the DATA code - for(i = 0; i < 16; i++) - cyrf->buffer[i+10] = data_code[i]; + for (i = 0; i < 16; i++) { + cyrf->buffer[i + 10] = data_code[i]; + } // Copy the channel cyrf->buffer[26] = chan; @@ -379,10 +409,12 @@ bool_t cyrf6936_write_chan_sop_data_crc(struct Cyrf6936 *cyrf, const uint8_t cha /** * Read the RX IRQ status register, the rx status register and the rx packet */ -bool_t cyrf6936_read_rx_irq_status_packet(struct Cyrf6936 *cyrf) { +bool_t cyrf6936_read_rx_irq_status_packet(struct Cyrf6936 *cyrf) +{ /* Check if the cyrf6936 isn't busy */ - if(cyrf->status != CYRF6936_IDLE) + if (cyrf->status != CYRF6936_IDLE) { return FALSE; + } // Set the status cyrf->status = CYRF6936_RX_IRQ_STATUS_PACKET; @@ -397,20 +429,23 @@ bool_t cyrf6936_read_rx_irq_status_packet(struct Cyrf6936 *cyrf) { /** * Send a packet with a certain length */ -bool_t cyrf6936_send(struct Cyrf6936 *cyrf, const uint8_t data[], const uint8_t length) { +bool_t cyrf6936_send(struct Cyrf6936 *cyrf, const uint8_t data[], const uint8_t length) +{ int i; /* Check if the cyrf6936 isn't busy */ - if(cyrf->status != CYRF6936_IDLE) + if (cyrf->status != CYRF6936_IDLE) { return FALSE; + } // Set the status cyrf->status = CYRF6936_SEND; // Copy the length and the data cyrf->buffer[0] = length; - for(i = 0; i < length; i++) - cyrf->buffer[i+1] = data[i]; + for (i = 0; i < length; i++) { + cyrf->buffer[i + 1] = data[i]; + } /* Try to set the packet length */ cyrf->buffer_idx = 0; diff --git a/sw/airborne/peripherals/cyrf6936.h b/sw/airborne/peripherals/cyrf6936.h index c384a9878b..111de18e0c 100644 --- a/sw/airborne/peripherals/cyrf6936.h +++ b/sw/airborne/peripherals/cyrf6936.h @@ -65,12 +65,14 @@ struct Cyrf6936 { uint8_t rx_packet[16]; /**< The last received packet */ }; -extern void cyrf6936_init(struct Cyrf6936 *cyrf, struct spi_periph *spi_p, const uint8_t slave_idx, const uint32_t rst_port, const uint16_t rst_pin); +extern void cyrf6936_init(struct Cyrf6936 *cyrf, struct spi_periph *spi_p, const uint8_t slave_idx, + const uint32_t rst_port, const uint16_t rst_pin); void cyrf6936_event(struct Cyrf6936 *cyrf); bool_t cyrf6936_write(struct Cyrf6936 *cyrf, const uint8_t addr, const uint8_t data); bool_t cyrf6936_multi_write(struct Cyrf6936 *cyrf, const uint8_t data[][2], const uint8_t length); -bool_t cyrf6936_write_chan_sop_data_crc(struct Cyrf6936 *cyrf, const uint8_t chan, const uint8_t sop_code[], const uint8_t data_code[], const uint16_t crc_seed); +bool_t cyrf6936_write_chan_sop_data_crc(struct Cyrf6936 *cyrf, const uint8_t chan, const uint8_t sop_code[], + const uint8_t data_code[], const uint16_t crc_seed); bool_t cyrf6936_read_rx_irq_status_packet(struct Cyrf6936 *cyrf); bool_t cyrf6936_send(struct Cyrf6936 *cyrf, const uint8_t data[], const uint8_t length); diff --git a/sw/airborne/peripherals/cyrf6936_regs.h b/sw/airborne/peripherals/cyrf6936_regs.h index c5926771a7..ea0a008d9d 100644 --- a/sw/airborne/peripherals/cyrf6936_regs.h +++ b/sw/airborne/peripherals/cyrf6936_regs.h @@ -85,11 +85,11 @@ enum { // CYRF_XACT_CFG enum { - CYRF_MODE_SLEEP = (0x0 <<2), - CYRF_MODE_IDLE = (0x1 <<2), - CYRF_MODE_SYNTH_TX = (0x2 <<2), - CYRF_MODE_SYNTH_RX = (0x3 <<2), - CYRF_MODE_RX = (0x4 <<2), + CYRF_MODE_SLEEP = (0x0 << 2), + CYRF_MODE_IDLE = (0x1 << 2), + CYRF_MODE_SYNTH_TX = (0x2 << 2), + CYRF_MODE_SYNTH_RX = (0x3 << 2), + CYRF_MODE_RX = (0x4 << 2), }; #define CYRF_FRC_END (1<<5) #define CYRF_ACK_EN (1<<7) @@ -201,10 +201,10 @@ enum { CYRF_PA_4 = 0x7, }; enum { - CYRF_DATA_MODE_GFSK = (0x0 <<3), - CYRF_DATA_MODE_8DR = (0x1 <<3), - CYRF_DATA_MODE_DDR = (0x2 <<3), - CYRF_DATA_MODE_SDR = (0x3 <<3), + CYRF_DATA_MODE_GFSK = (0x0 << 3), + CYRF_DATA_MODE_8DR = (0x1 << 3), + CYRF_DATA_MODE_DDR = (0x2 << 3), + CYRF_DATA_MODE_SDR = (0x3 << 3), }; #define CYRF_DATA_CODE_LENGTH (1<<5) diff --git a/sw/airborne/peripherals/hmc5843.c b/sw/airborne/peripherals/hmc5843.c index 9b7ece8aad..5ef98a5782 100644 --- a/sw/airborne/peripherals/hmc5843.c +++ b/sw/airborne/peripherals/hmc5843.c @@ -30,22 +30,22 @@ static void send_config(void) hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGA; // set to rate to 50Hz hmc5843.i2c_trans.buf[1] = 0x00 | (0x06 << 2); hmc5843.i2c_trans.len_w = 2; - i2c_submit(&HMC5843_I2C_DEV,&hmc5843.i2c_trans); - while(hmc5843.i2c_trans.status == I2CTransPending); + i2c_submit(&HMC5843_I2C_DEV, &hmc5843.i2c_trans); + while (hmc5843.i2c_trans.status == I2CTransPending); hmc5843.i2c_trans.type = I2CTransTx; hmc5843.i2c_trans.buf[0] = HMC5843_REG_CFGB; // set to gain to 1 Gauss - hmc5843.i2c_trans.buf[1] = 0x01<<5; + hmc5843.i2c_trans.buf[1] = 0x01 << 5; hmc5843.i2c_trans.len_w = 2; - i2c_submit(&HMC5843_I2C_DEV,&hmc5843.i2c_trans); - while(hmc5843.i2c_trans.status == I2CTransPending); + i2c_submit(&HMC5843_I2C_DEV, &hmc5843.i2c_trans); + while (hmc5843.i2c_trans.status == I2CTransPending); hmc5843.i2c_trans.type = I2CTransTx; hmc5843.i2c_trans.buf[0] = HMC5843_REG_MODE; // set to continuous mode hmc5843.i2c_trans.buf[1] = 0x00; hmc5843.i2c_trans.len_w = 2; - i2c_submit(&HMC5843_I2C_DEV,&hmc5843.i2c_trans); - while(hmc5843.i2c_trans.status == I2CTransPending); + i2c_submit(&HMC5843_I2C_DEV, &hmc5843.i2c_trans); + while (hmc5843.i2c_trans.status == I2CTransPending); } @@ -64,7 +64,7 @@ void hmc5843_idle_task(void) hmc5843.sent_rx = 0; } - if (hmc5843.i2c_trans.status == I2CTransRunning || hmc5843.i2c_trans.status == I2CTransPending) return; + if (hmc5843.i2c_trans.status == I2CTransRunning || hmc5843.i2c_trans.status == I2CTransPending) { return; } if (hmc5843.initialized && mag_eoc() && !hmc5843.sent_tx && !hmc5843.sent_rx) { if (HMC5843_I2C_DEV.status == I2CIdle && i2c_idle(&HMC5843_I2C_DEV)) { @@ -93,7 +93,7 @@ void hmc5843_idle_task(void) hmc5843.sent_tx = 0; hmc5843.timeout = 0; hmc5843.data_available = TRUE; - memcpy(hmc5843.data.buf, (const void*) hmc5843.i2c_trans.buf, 6); + memcpy(hmc5843.data.buf, (const void *) hmc5843.i2c_trans.buf, 6); for (int i = 0; i < 3; i++) { hmc5843.data.value[i] = bswap_16(hmc5843.data.value[i]); } @@ -105,7 +105,7 @@ void hmc5843_periodic(void) if (!hmc5843.initialized) { send_config(); hmc5843.initialized = TRUE; - } else if (hmc5843.timeout++ > HMC5843_TIMEOUT && HMC5843_I2C_DEV.status == I2CIdle && i2c_idle(&HMC5843_I2C_DEV)){ + } else if (hmc5843.timeout++ > HMC5843_TIMEOUT && HMC5843_I2C_DEV.status == I2CIdle && i2c_idle(&HMC5843_I2C_DEV)) { #ifdef USE_HMC59843_ARCH_RESET hmc5843_arch_reset(); #endif @@ -113,12 +113,12 @@ void hmc5843_periodic(void) hmc5843.i2c_trans.len_w = 1; hmc5843.i2c_trans.buf[0] = 0x3; i2c_submit(&HMC5843_I2C_DEV, &hmc5843.i2c_trans); - while(hmc5843.i2c_trans.status == I2CTransPending || hmc5843.i2c_trans.status == I2CTransRunning); + while (hmc5843.i2c_trans.status == I2CTransPending || hmc5843.i2c_trans.status == I2CTransRunning); hmc5843.i2c_trans.type = I2CTransRx; hmc5843.i2c_trans.len_r = 6; i2c_submit(&HMC5843_I2C_DEV, &hmc5843.i2c_trans); - while(hmc5843.i2c_trans.status == I2CTransPending || hmc5843.i2c_trans.status == I2CTransRunning); + while (hmc5843.i2c_trans.status == I2CTransPending || hmc5843.i2c_trans.status == I2CTransRunning); hmc5843.timeout = 0; } diff --git a/sw/airborne/peripherals/hmc5843.h b/sw/airborne/peripherals/hmc5843.h index 94cd0940cf..036e1602fe 100644 --- a/sw/airborne/peripherals/hmc5843.h +++ b/sw/airborne/peripherals/hmc5843.h @@ -26,12 +26,12 @@ #include "mcu_periph/i2c.h" struct Hmc5843 { - struct i2c_transaction i2c_trans; - uint32_t timeout; - uint8_t sent_tx; - uint8_t sent_rx; - uint8_t initialized; - uint8_t data_available; + struct i2c_transaction i2c_trans; + uint32_t timeout; + uint8_t sent_tx; + uint8_t sent_rx; + uint8_t initialized; + uint8_t data_available; union { uint8_t buf[7]; int16_t value[3]; @@ -43,8 +43,8 @@ extern struct Hmc5843 hmc5843; #ifndef HMC5843_NO_IRQ #include "peripherals/hmc5843_arch.h" -extern void hmc5843_arch_init( void ); -extern void hmc5843_arch_reset( void ); +extern void hmc5843_arch_init(void); +extern void hmc5843_arch_reset(void); #endif extern void hmc5843_init(void); diff --git a/sw/airborne/peripherals/hmc58xx.c b/sw/airborne/peripherals/hmc58xx.c index 39013ded45..65ca21631a 100644 --- a/sw/airborne/peripherals/hmc58xx.c +++ b/sw/airborne/peripherals/hmc58xx.c @@ -101,7 +101,7 @@ static void hmc58xx_send_config(struct Hmc58xx *hmc) { switch (hmc->init_status) { case HMC_CONF_CRA: - hmc58xx_i2c_tx_reg(hmc, HMC58XX_REG_CFGA, (hmc->config.rate<<2)|(hmc->config.meas)); + hmc58xx_i2c_tx_reg(hmc, HMC58XX_REG_CFGA, (hmc->config.rate << 2) | (hmc->config.meas)); hmc->init_status++; break; case HMC_CONF_CRB: @@ -137,7 +137,7 @@ void hmc58xx_start_configure(struct Hmc58xx *hmc) // Normal reading void hmc58xx_read(struct Hmc58xx *hmc) { - if (hmc->initialized && hmc->i2c_trans.status == I2CTransDone){ + if (hmc->initialized && hmc->i2c_trans.status == I2CTransDone) { hmc->i2c_trans.buf[0] = HMC58XX_REG_DATXM; hmc->i2c_trans.type = I2CTransTxRx; hmc->i2c_trans.len_r = 6; @@ -153,24 +153,22 @@ void hmc58xx_event(struct Hmc58xx *hmc) if (hmc->initialized) { if (hmc->i2c_trans.status == I2CTransFailed) { hmc->i2c_trans.status = I2CTransDone; - } - else if (hmc->i2c_trans.status == I2CTransSuccess) { + } else if (hmc->i2c_trans.status == I2CTransSuccess) { if (hmc->type == HMC_TYPE_5843) { - hmc->data.vect.x = Int16FromBuf(hmc->i2c_trans.buf,0); - hmc->data.vect.y = Int16FromBuf(hmc->i2c_trans.buf,2); - hmc->data.vect.z = Int16FromBuf(hmc->i2c_trans.buf,4); + hmc->data.vect.x = Int16FromBuf(hmc->i2c_trans.buf, 0); + hmc->data.vect.y = Int16FromBuf(hmc->i2c_trans.buf, 2); + hmc->data.vect.z = Int16FromBuf(hmc->i2c_trans.buf, 4); } /* HMC5883 has xzy order of axes in returned data */ else { - hmc->data.vect.x = Int16FromBuf(hmc->i2c_trans.buf,0); - hmc->data.vect.y = Int16FromBuf(hmc->i2c_trans.buf,4); - hmc->data.vect.z = Int16FromBuf(hmc->i2c_trans.buf,2); + hmc->data.vect.x = Int16FromBuf(hmc->i2c_trans.buf, 0); + hmc->data.vect.y = Int16FromBuf(hmc->i2c_trans.buf, 4); + hmc->data.vect.z = Int16FromBuf(hmc->i2c_trans.buf, 2); } hmc->data_available = TRUE; hmc->i2c_trans.status = I2CTransDone; } - } - else if (hmc->init_status != HMC_CONF_UNINIT) { // Configuring but not yet initialized + } else if (hmc->init_status != HMC_CONF_UNINIT) { // Configuring but not yet initialized if (hmc->i2c_trans.status == I2CTransSuccess || hmc->i2c_trans.status == I2CTransDone) { hmc->i2c_trans.status = I2CTransDone; hmc58xx_send_config(hmc); diff --git a/sw/airborne/peripherals/hmc58xx.h b/sw/airborne/peripherals/hmc58xx.h index 696e4afd8d..7c7046d0f9 100644 --- a/sw/airborne/peripherals/hmc58xx.h +++ b/sw/airborne/peripherals/hmc58xx.h @@ -81,11 +81,13 @@ extern void hmc58xx_read(struct Hmc58xx *hmc); extern void hmc58xx_event(struct Hmc58xx *hmc); /// convenience function: read or start configuration if not already initialized -static inline void hmc58xx_periodic(struct Hmc58xx *hmc) { - if (hmc->initialized) +static inline void hmc58xx_periodic(struct Hmc58xx *hmc) +{ + if (hmc->initialized) { hmc58xx_read(hmc); - else + } else { hmc58xx_start_configure(hmc); + } } #endif /* HMC58XX_H */ diff --git a/sw/airborne/peripherals/itg3200.c b/sw/airborne/peripherals/itg3200.c index f8aaf91751..758b3276d2 100644 --- a/sw/airborne/peripherals/itg3200.c +++ b/sw/airborne/peripherals/itg3200.c @@ -30,7 +30,8 @@ #include "peripherals/itg3200.h" #include "std.h" -void itg3200_set_default_config(struct Itg3200Config *c) { +void itg3200_set_default_config(struct Itg3200Config *c) +{ c->smplrt_div = ITG3200_DEFAULT_SMPLRT_DIV; c->fs_sel = ITG3200_DEFAULT_FS_SEL; c->dlpf_cfg = ITG3200_DEFAULT_DLPF_CFG; @@ -77,7 +78,7 @@ static void itg3200_send_config(struct Itg3200 *itg) itg->init_status++; break; case ITG_CONF_DF: - itg3200_i2c_tx_reg(itg, ITG3200_REG_DLPF_FS, (itg->config.fs_sel<<3)|(itg->config.dlpf_cfg)); + itg3200_i2c_tx_reg(itg, ITG3200_REG_DLPF_FS, (itg->config.fs_sel << 3) | (itg->config.dlpf_cfg)); itg->init_status++; break; case ITG_CONF_INT: @@ -127,20 +128,18 @@ void itg3200_event(struct Itg3200 *itg) if (itg->initialized) { if (itg->i2c_trans.status == I2CTransFailed) { itg->i2c_trans.status = I2CTransDone; - } - else if (itg->i2c_trans.status == I2CTransSuccess) { + } else if (itg->i2c_trans.status == I2CTransSuccess) { // Successfull reading and new data available if (itg->i2c_trans.buf[0] & 0x01) { // New data available - itg->data.rates.p = Int16FromBuf(itg->i2c_trans.buf,3); - itg->data.rates.q = Int16FromBuf(itg->i2c_trans.buf,5); - itg->data.rates.r = Int16FromBuf(itg->i2c_trans.buf,7); + itg->data.rates.p = Int16FromBuf(itg->i2c_trans.buf, 3); + itg->data.rates.q = Int16FromBuf(itg->i2c_trans.buf, 5); + itg->data.rates.r = Int16FromBuf(itg->i2c_trans.buf, 7); itg->data_available = TRUE; } itg->i2c_trans.status = I2CTransDone; } - } - else if (itg->init_status != ITG_CONF_UNINIT) { // Configuring but not yet initialized + } else if (itg->init_status != ITG_CONF_UNINIT) { // Configuring but not yet initialized if (itg->i2c_trans.status == I2CTransSuccess || itg->i2c_trans.status == I2CTransDone) { itg->i2c_trans.status = I2CTransDone; itg3200_send_config(itg); diff --git a/sw/airborne/peripherals/itg3200.h b/sw/airborne/peripherals/itg3200.h index 649948b556..4648f58124 100644 --- a/sw/airborne/peripherals/itg3200.h +++ b/sw/airborne/peripherals/itg3200.h @@ -90,11 +90,13 @@ extern void itg3200_read(struct Itg3200 *itg); extern void itg3200_event(struct Itg3200 *itg); /// convenience function: read or start configuration if not already initialized -static inline void itg3200_periodic(struct Itg3200 *itg) { - if (itg->initialized) +static inline void itg3200_periodic(struct Itg3200 *itg) +{ + if (itg->initialized) { itg3200_read(itg); - else + } else { itg3200_start_configure(itg); + } } #endif // ITG3200_H diff --git a/sw/airborne/peripherals/l3g4200.c b/sw/airborne/peripherals/l3g4200.c index 64d82302c7..f9f133bf27 100644 --- a/sw/airborne/peripherals/l3g4200.c +++ b/sw/airborne/peripherals/l3g4200.c @@ -30,7 +30,8 @@ #include "peripherals/l3g4200.h" #include "std.h" -void l3g4200_set_default_config(struct L3g4200Config *c) { +void l3g4200_set_default_config(struct L3g4200Config *c) +{ c->ctrl_reg1 = L3G4200_DEFAULT_CTRL_REG1; c->ctrl_reg4 = L3G4200_DEFAULT_CTRL_REG4; c->ctrl_reg5 = L3G4200_DEFAULT_CTRL_REG5; @@ -115,20 +116,18 @@ void l3g4200_event(struct L3g4200 *l3g) if (l3g->initialized) { if (l3g->i2c_trans.status == I2CTransFailed) { l3g->i2c_trans.status = I2CTransDone; - } - else if (l3g->i2c_trans.status == I2CTransSuccess) { + } else if (l3g->i2c_trans.status == I2CTransSuccess) { // Successfull reading and new data available if (l3g->i2c_trans.buf[0] & 0x08) { // New data available - l3g->data.rates.p = Int16FromBuf(l3g->i2c_trans.buf,1); - l3g->data.rates.q = Int16FromBuf(l3g->i2c_trans.buf,3); - l3g->data.rates.r = Int16FromBuf(l3g->i2c_trans.buf,5); + l3g->data.rates.p = Int16FromBuf(l3g->i2c_trans.buf, 1); + l3g->data.rates.q = Int16FromBuf(l3g->i2c_trans.buf, 3); + l3g->data.rates.r = Int16FromBuf(l3g->i2c_trans.buf, 5); l3g->data_available = TRUE; } l3g->i2c_trans.status = I2CTransDone; } - } - else if (l3g->init_status != L3G_CONF_UNINIT) { // Configuring but not yet initialized + } else if (l3g->init_status != L3G_CONF_UNINIT) { // Configuring but not yet initialized if (l3g->i2c_trans.status == I2CTransSuccess || l3g->i2c_trans.status == I2CTransDone) { l3g->i2c_trans.status = I2CTransDone; l3g4200_send_config(l3g); diff --git a/sw/airborne/peripherals/l3g4200.h b/sw/airborne/peripherals/l3g4200.h index ccfa6d25a8..cb94932bbf 100644 --- a/sw/airborne/peripherals/l3g4200.h +++ b/sw/airborne/peripherals/l3g4200.h @@ -85,11 +85,13 @@ extern void l3g4200_read(struct L3g4200 *l3g); extern void l3g4200_event(struct L3g4200 *l3g); /// convenience function: read or start configuration if not already initialized -static inline void l3g4200_periodic(struct L3g4200 *l3g) { - if (l3g->initialized) +static inline void l3g4200_periodic(struct L3g4200 *l3g) +{ + if (l3g->initialized) { l3g4200_read(l3g); - else + } else { l3g4200_start_configure(l3g); + } } #endif // L3G4200_H diff --git a/sw/airborne/peripherals/l3gd20_spi.c b/sw/airborne/peripherals/l3gd20_spi.c index a1e3c73d3a..c774adaa74 100644 --- a/sw/airborne/peripherals/l3gd20_spi.c +++ b/sw/airborne/peripherals/l3gd20_spi.c @@ -61,7 +61,8 @@ void l3gd20_spi_init(struct L3gd20_Spi *l3g, struct spi_periph *spi_p, uint8_t s } -static void l3gd20_spi_write_to_reg(struct L3gd20_Spi *l3g, uint8_t _reg, uint8_t _val) { +static void l3gd20_spi_write_to_reg(struct L3gd20_Spi *l3g, uint8_t _reg, uint8_t _val) +{ l3g->spi_trans.output_length = 2; l3g->spi_trans.input_length = 0; l3g->tx_buf[0] = _reg; @@ -80,9 +81,10 @@ static void l3gd20_spi_send_config(struct L3gd20_Spi *l3g) l3g->spi_trans.output_length = 1; l3g->spi_trans.input_length = 2; /* set read bit then reg address */ - l3g->tx_buf[0] = (1<<7 | L3GD20_REG_WHO_AM_I); - if (spi_submit(l3g->spi_p, &(l3g->spi_trans))) + l3g->tx_buf[0] = (1 << 7 | L3GD20_REG_WHO_AM_I); + if (spi_submit(l3g->spi_p, &(l3g->spi_trans))) { l3g->init_status++; + } break; case L3G_CONF_REG4: /* set SPI mode, Filtered Data Selection */ @@ -93,8 +95,8 @@ static void l3gd20_spi_send_config(struct L3gd20_Spi *l3g) case L3G_CONF_ENABLE: /* set data rate, range, enable measurement, is in standby after power up */ reg_val = (l3g->config.drbw << 4) | - L3GD20_PD | // Power Down Control to active mode - L3GD20_Xen | L3GD20_Yen | L3GD20_Zen; // enable z,y,x axes + L3GD20_PD | // Power Down Control to active mode + L3GD20_Xen | L3GD20_Yen | L3GD20_Zen; // enable z,y,x axes l3gd20_spi_write_to_reg(l3g, L3GD20_REG_CTRL_REG1, reg_val); l3g->init_status++; break; @@ -123,7 +125,7 @@ void l3gd20_spi_read(struct L3gd20_Spi *l3g) l3g->spi_trans.output_length = 1; l3g->spi_trans.input_length = 8; /* set read bit and multiple byte bit, then address */ - l3g->tx_buf[0] = (1<<7|1<<6|L3GD20_REG_STATUS_REG); + l3g->tx_buf[0] = (1 << 7 | 1 << 6 | L3GD20_REG_STATUS_REG); spi_submit(l3g->spi_p, &(l3g->spi_trans)); } } @@ -135,20 +137,18 @@ void l3gd20_spi_event(struct L3gd20_Spi *l3g) if (l3g->initialized) { if (l3g->spi_trans.status == SPITransFailed) { l3g->spi_trans.status = SPITransDone; - } - else if (l3g->spi_trans.status == SPITransSuccess) { + } else if (l3g->spi_trans.status == SPITransSuccess) { // Successfull reading if (bit_is_set(l3g->rx_buf[1], 3)) { // new xyz data available - l3g->data_rates.rates.p = Int16FromBuf(l3g->rx_buf,2); - l3g->data_rates.rates.q = Int16FromBuf(l3g->rx_buf,4); - l3g->data_rates.rates.r = Int16FromBuf(l3g->rx_buf,6); + l3g->data_rates.rates.p = Int16FromBuf(l3g->rx_buf, 2); + l3g->data_rates.rates.q = Int16FromBuf(l3g->rx_buf, 4); + l3g->data_rates.rates.r = Int16FromBuf(l3g->rx_buf, 6); l3g->data_available = TRUE; } l3g->spi_trans.status = SPITransDone; } - } - else if (l3g->init_status != L3G_CONF_UNINIT) { // Configuring but not yet initialized + } else if (l3g->init_status != L3G_CONF_UNINIT) { // Configuring but not yet initialized switch (l3g->spi_trans.status) { case SPITransFailed: l3g->init_status--; // Retry config (TODO max retry) @@ -156,8 +156,7 @@ void l3gd20_spi_event(struct L3gd20_Spi *l3g) if (l3g->init_status == L3G_CONF_WHO_AM_I_OK) { if (l3g->rx_buf[1] == L3GD20_WHO_AM_I) { l3g->init_status++; - } - else { + } else { l3g->init_status = L3G_CONF_WHO_AM_I; } } diff --git a/sw/airborne/peripherals/l3gd20_spi.h b/sw/airborne/peripherals/l3gd20_spi.h index c82a482288..42d50c4bb5 100644 --- a/sw/airborne/peripherals/l3gd20_spi.h +++ b/sw/airborne/peripherals/l3gd20_spi.h @@ -57,11 +57,13 @@ extern void l3gd20_spi_read(struct L3gd20_Spi *l3g); extern void l3gd20_spi_event(struct L3gd20_Spi *l3g); /// convenience function: read or start configuration if not already initialized -static inline void l3gd20_spi_periodic(struct L3gd20_Spi *l3g) { - if (l3g->initialized) +static inline void l3gd20_spi_periodic(struct L3gd20_Spi *l3g) +{ + if (l3g->initialized) { l3gd20_spi_read(l3g); - else + } else { l3gd20_spi_start_configure(l3g); + } } #endif // L3GD20_SPI_H diff --git a/sw/airborne/peripherals/lis302dl_spi.c b/sw/airborne/peripherals/lis302dl_spi.c index 55348f41e0..cc765197c6 100644 --- a/sw/airborne/peripherals/lis302dl_spi.c +++ b/sw/airborne/peripherals/lis302dl_spi.c @@ -61,7 +61,8 @@ void lis302dl_spi_init(struct Lis302dl_Spi *lis, struct spi_periph *spi_p, uint8 } -static void lis302dl_spi_write_to_reg(struct Lis302dl_Spi *lis, uint8_t _reg, uint8_t _val) { +static void lis302dl_spi_write_to_reg(struct Lis302dl_Spi *lis, uint8_t _reg, uint8_t _val) +{ lis->spi_trans.output_length = 2; lis->spi_trans.input_length = 0; lis->tx_buf[0] = _reg; @@ -80,9 +81,10 @@ static void lis302dl_spi_send_config(struct Lis302dl_Spi *lis) lis->spi_trans.output_length = 1; lis->spi_trans.input_length = 2; /* set read bit then reg address */ - lis->tx_buf[0] = (1<<7 | LIS302DL_REG_WHO_AM_I); - if (spi_submit(lis->spi_p, &(lis->spi_trans))) + lis->tx_buf[0] = (1 << 7 | LIS302DL_REG_WHO_AM_I); + if (spi_submit(lis->spi_p, &(lis->spi_trans))) { lis->init_status++; + } break; case LIS_CONF_REG2: /* set SPI mode, Filtered Data Selection */ @@ -98,9 +100,9 @@ static void lis302dl_spi_send_config(struct Lis302dl_Spi *lis) case LIS_CONF_ENABLE: /* set data rate, range, enable measurement, is in standby after power up */ reg_val = (lis->config.rate << 7) | - (1 << 6) | // Power Down Control to active mode - (lis->config.range << 5) | - 0x5; // enable z,y,x axes + (1 << 6) | // Power Down Control to active mode + (lis->config.range << 5) | + 0x5; // enable z,y,x axes lis302dl_spi_write_to_reg(lis, LIS302DL_REG_CTRL_REG1, reg_val); lis->init_status++; break; @@ -129,7 +131,7 @@ void lis302dl_spi_read(struct Lis302dl_Spi *lis) lis->spi_trans.output_length = 1; lis->spi_trans.input_length = 8; /* set read bit and multiple byte bit, then address */ - lis->tx_buf[0] = (1<<7|1<<6|LIS302DL_REG_STATUS); + lis->tx_buf[0] = (1 << 7 | 1 << 6 | LIS302DL_REG_STATUS); spi_submit(lis->spi_p, &(lis->spi_trans)); } } @@ -139,8 +141,7 @@ void lis302dl_spi_event(struct Lis302dl_Spi *lis) if (lis->initialized) { if (lis->spi_trans.status == SPITransFailed) { lis->spi_trans.status = SPITransDone; - } - else if (lis->spi_trans.status == SPITransSuccess) { + } else if (lis->spi_trans.status == SPITransSuccess) { // Successfull reading if (bit_is_set(lis->rx_buf[1], 3)) { // new xyz data available @@ -151,17 +152,17 @@ void lis302dl_spi_event(struct Lis302dl_Spi *lis) } lis->spi_trans.status = SPITransDone; } - } - else if (lis->init_status != LIS_CONF_UNINIT) { // Configuring but not yet initialized + } else if (lis->init_status != LIS_CONF_UNINIT) { // Configuring but not yet initialized switch (lis->spi_trans.status) { case SPITransFailed: lis->init_status--; // Retry config (TODO max retry) case SPITransSuccess: if (lis->init_status == LIS_CONF_WHO_AM_I_OK) { - if (lis->rx_buf[1] == LIS302DL_WHO_AM_I) + if (lis->rx_buf[1] == LIS302DL_WHO_AM_I) { lis->init_status++; - else + } else { lis->init_status = LIS_CONF_WHO_AM_I; + } } case SPITransDone: lis->spi_trans.status = SPITransDone; diff --git a/sw/airborne/peripherals/lis302dl_spi.h b/sw/airborne/peripherals/lis302dl_spi.h index 375f81ca1d..3fb9851fb9 100644 --- a/sw/airborne/peripherals/lis302dl_spi.h +++ b/sw/airborne/peripherals/lis302dl_spi.h @@ -58,11 +58,13 @@ extern void lis302dl_spi_read(struct Lis302dl_Spi *lis); extern void lis302dl_spi_event(struct Lis302dl_Spi *lis); /// convenience function: read or start configuration if not already initialized -static inline void lis302dl_spi_periodic(struct Lis302dl_Spi *lis) { - if (lis->initialized) +static inline void lis302dl_spi_periodic(struct Lis302dl_Spi *lis) +{ + if (lis->initialized) { lis302dl_spi_read(lis); - else + } else { lis302dl_spi_start_configure(lis); + } } #endif // LIS302DL_SPI_H diff --git a/sw/airborne/peripherals/lsm303dlhc.c b/sw/airborne/peripherals/lsm303dlhc.c index a3eea7b057..639737056e 100644 --- a/sw/airborne/peripherals/lsm303dlhc.c +++ b/sw/airborne/peripherals/lsm303dlhc.c @@ -186,16 +186,15 @@ void lsm303dlhc_read(struct Lsm303dlhc *lsm) { if (lsm->i2c_trans.slave_addr == LSM303DLHC_ACC_ADDR) { //if ((lsm->init_status.acc == LSM_CONF_ACC_CLR_INT_READ) && (lsm->i2c_trans.status == I2CTransDone)){ - if (!(lsm->initialized) || (lsm->initialized && lsm->i2c_trans.status == I2CTransDone)){ + if (!(lsm->initialized) || (lsm->initialized && lsm->i2c_trans.status == I2CTransDone)) { lsm->i2c_trans.buf[0] = LSM303DLHC_REG_OUT_X_L_A | 0x80; lsm->i2c_trans.type = I2CTransTxRx; lsm->i2c_trans.len_r = 6; lsm->i2c_trans.len_w = 1; i2c_submit(lsm->i2c_p, &(lsm->i2c_trans)); } - } - else { - if (lsm->initialized && lsm->i2c_trans.status == I2CTransDone){ + } else { + if (lsm->initialized && lsm->i2c_trans.status == I2CTransDone) { lsm->i2c_trans.buf[0] = LSM303DLHC_REG_OUT_X_H_M; lsm->i2c_trans.type = I2CTransTxRx; lsm->i2c_trans.len_r = 6; @@ -212,19 +211,15 @@ void lsm303dlhc_event(struct Lsm303dlhc *lsm) if (lsm->initialized) { if (lsm->i2c_trans.status == I2CTransFailed) { lsm->i2c_trans.status = I2CTransDone; - } - else if (lsm->i2c_trans.status == I2CTransSuccess) { - lsm->data.vect.x = Int16FromBuf(lsm->i2c_trans.buf,0); - lsm->data.vect.y = Int16FromBuf(lsm->i2c_trans.buf,2); - lsm->data.vect.z = Int16FromBuf(lsm->i2c_trans.buf,4); + } else if (lsm->i2c_trans.status == I2CTransSuccess) { + lsm->data.vect.x = Int16FromBuf(lsm->i2c_trans.buf, 0); + lsm->data.vect.y = Int16FromBuf(lsm->i2c_trans.buf, 2); + lsm->data.vect.z = Int16FromBuf(lsm->i2c_trans.buf, 4); lsm->data_available = TRUE; lsm->i2c_trans.status = I2CTransDone; + } else { } - else { - } - } - else - { + } else { if (lsm->i2c_trans.slave_addr == LSM303DLHC_ACC_ADDR) { if (lsm->init_status.acc != LSM_CONF_ACC_UNINIT) { // Configuring but not yet initialized if (lsm->i2c_trans.status == I2CTransSuccess || lsm->i2c_trans.status == I2CTransDone) { @@ -237,8 +232,7 @@ void lsm303dlhc_event(struct Lsm303dlhc *lsm) lsm303dlhc_send_config(lsm); // Retry config (TODO max retry) } } - } - else { + } else { if (lsm->init_status.mag != LSM_CONF_MAG_UNINIT) { // Configuring but not yet initialized if (lsm->i2c_trans.status == I2CTransSuccess || lsm->i2c_trans.status == I2CTransDone) { lsm->i2c_trans.status = I2CTransDone; diff --git a/sw/airborne/peripherals/lsm303dlhc.h b/sw/airborne/peripherals/lsm303dlhc.h index 13c598b538..bfea938f9e 100644 --- a/sw/airborne/peripherals/lsm303dlhc.h +++ b/sw/airborne/peripherals/lsm303dlhc.h @@ -97,11 +97,13 @@ extern void lsm303dlhc_read(struct Lsm303dlhc *lsm); extern void lsm303dlhc_event(struct Lsm303dlhc *lsm); /// convenience function: read or start configuration if not already initialized -static inline void lsm303dlhc_periodic(struct Lsm303dlhc *lsm) { - if (lsm->initialized) +static inline void lsm303dlhc_periodic(struct Lsm303dlhc *lsm) +{ + if (lsm->initialized) { lsm303dlhc_read(lsm); - else + } else { lsm303dlhc_start_configure(lsm); + } } #endif /* LSM303DLHC_H */ diff --git a/sw/airborne/peripherals/max1168.c b/sw/airborne/peripherals/max1168.c index 1a560345b0..20922ca6ab 100644 --- a/sw/airborne/peripherals/max1168.c +++ b/sw/airborne/peripherals/max1168.c @@ -35,15 +35,17 @@ uint16_t max1168_conv_req; /* callback function to lock the spi fifo * after the first transaction */ -void max1168_lock_cb(struct spi_transaction * t); +void max1168_lock_cb(struct spi_transaction *t); -extern void max1168_init( void ) { +extern void max1168_init(void) +{ max1168_arch_init(); uint8_t i; - for (i=0; i>4) == 0) { + if ((mcp355x_spi_trans.input_buf[0] >> 4) == 0) { mcp355x_data = (int32_t)( - ((uint32_t)mcp355x_spi_trans.input_buf[0]<<17) | - ((uint32_t)mcp355x_spi_trans.input_buf[1]<<9) | - ((uint32_t)mcp355x_spi_trans.input_buf[2]<<1) | - (mcp355x_spi_trans.input_buf[3]>>7)); + ((uint32_t)mcp355x_spi_trans.input_buf[0] << 17) | + ((uint32_t)mcp355x_spi_trans.input_buf[1] << 9) | + ((uint32_t)mcp355x_spi_trans.input_buf[2] << 1) | + (mcp355x_spi_trans.input_buf[3] >> 7)); mcp355x_data_available = TRUE; } mcp355x_spi_trans.status = SPITransDone; diff --git a/sw/airborne/peripherals/mpl3115.c b/sw/airborne/peripherals/mpl3115.c index 1ca95a416c..9132912dec 100644 --- a/sw/airborne/peripherals/mpl3115.c +++ b/sw/airborne/peripherals/mpl3115.c @@ -63,8 +63,8 @@ static void mpl3115_send_config(struct Mpl3115 *mpl) break; case MPL_CONF_CTRL1: mpl->trans.buf[0] = MPL3115_REG_CTRL_REG1; - mpl->trans.buf[1] = ((MPL3115_OVERSAMPLING<<3) | (mpl->raw_mode<<6) | - (mpl->alt_mode<<7)); + mpl->trans.buf[1] = ((MPL3115_OVERSAMPLING << 3) | (mpl->raw_mode << 6) | + (mpl->alt_mode << 7)); i2c_transmit(mpl->i2c_p, &mpl->trans, mpl->trans.slave_addr, 2); mpl->init_status++; break; @@ -97,8 +97,8 @@ void mpl3115_read(struct Mpl3115 *mpl) i2c_transceive(mpl->i2c_p, &mpl->trans, mpl->trans.slave_addr, 1, 6); if (mpl->req_trans.status == I2CTransDone) { mpl->req_trans.buf[0] = MPL3115_REG_CTRL_REG1; - mpl->req_trans.buf[1] = ((MPL3115_OVERSAMPLING<<3) | (mpl->raw_mode<<6) | - (mpl->alt_mode<<7) | MPL3115_OST_BIT); + mpl->req_trans.buf[1] = ((MPL3115_OVERSAMPLING << 3) | (mpl->raw_mode << 6) | + (mpl->alt_mode << 7) | MPL3115_OST_BIT); i2c_transmit(mpl->i2c_p, &mpl->req_trans, mpl->trans.slave_addr, 2); } } @@ -109,36 +109,32 @@ void mpl3115_event(struct Mpl3115 *mpl) if (mpl->initialized) { if (mpl->trans.status == I2CTransFailed) { mpl->trans.status = I2CTransDone; - } - else if (mpl->trans.status == I2CTransSuccess) { + } else if (mpl->trans.status == I2CTransSuccess) { // Successfull reading and new pressure data available - if (mpl->trans.buf[0] & (1<<2)) { + if (mpl->trans.buf[0] & (1 << 2)) { if (mpl->raw_mode) { // New data available - mpl->pressure = (((uint32_t)mpl->trans.buf[1]<<16) | - ((uint16_t)mpl->trans.buf[2]<<8) | + mpl->pressure = (((uint32_t)mpl->trans.buf[1] << 16) | + ((uint16_t)mpl->trans.buf[2] << 8) | mpl->trans.buf[3]); - mpl->temperature = ((int16_t)mpl->trans.buf[4]<<8) | mpl->trans.buf[5]; - } - else { // not in raw mode - uint32_t tmp = (((uint32_t)mpl->trans.buf[1]<<16) | - ((uint16_t)mpl->trans.buf[2]<<8) | + mpl->temperature = ((int16_t)mpl->trans.buf[4] << 8) | mpl->trans.buf[5]; + } else { // not in raw mode + uint32_t tmp = (((uint32_t)mpl->trans.buf[1] << 16) | + ((uint16_t)mpl->trans.buf[2] << 8) | mpl->trans.buf[3]); if (mpl->alt_mode) { - mpl->altitude = (float)(tmp>>4) / (1<<4); + mpl->altitude = (float)(tmp >> 4) / (1 << 4); + } else { // Pressure mode + mpl->pressure = (tmp >> 4); } - else { // Pressure mode - mpl->pressure = (tmp>>4); - } - tmp = ((int16_t)mpl->trans.buf[4]<<8) | mpl->trans.buf[5]; - mpl->temperature = (tmp>>4); + tmp = ((int16_t)mpl->trans.buf[4] << 8) | mpl->trans.buf[5]; + mpl->temperature = (tmp >> 4); } mpl->data_available = TRUE; } mpl->trans.status = I2CTransDone; } - } - else if (!mpl->initialized && mpl->init_status != MPL_CONF_UNINIT) { // Configuring + } else if (!mpl->initialized && mpl->init_status != MPL_CONF_UNINIT) { // Configuring if (mpl->trans.status == I2CTransSuccess || mpl->trans.status == I2CTransDone) { mpl->trans.status = I2CTransDone; mpl3115_send_config(mpl); @@ -154,9 +150,11 @@ void mpl3115_event(struct Mpl3115 *mpl) } } -void mpl3115_periodic(struct Mpl3115 *mpl) { - if (mpl->initialized) +void mpl3115_periodic(struct Mpl3115 *mpl) +{ + if (mpl->initialized) { mpl3115_read(mpl); - else + } else { mpl3115_configure(mpl); + } } diff --git a/sw/airborne/peripherals/mpu60x0.c b/sw/airborne/peripherals/mpu60x0.c index ea926b0125..70cf0e0233 100644 --- a/sw/airborne/peripherals/mpu60x0.c +++ b/sw/airborne/peripherals/mpu60x0.c @@ -48,14 +48,14 @@ void mpu60x0_set_default_config(struct Mpu60x0Config *c) c->i2c_bypass = FALSE; } -void mpu60x0_send_config(Mpu60x0ConfigSet mpu_set, void* mpu, struct Mpu60x0Config* config) +void mpu60x0_send_config(Mpu60x0ConfigSet mpu_set, void *mpu, struct Mpu60x0Config *config) { switch (config->init_status) { case MPU60X0_CONF_RESET: /* device reset, set register values to defaults */ - mpu_set(mpu, MPU60X0_REG_PWR_MGMT_1, (1<<6)); + mpu_set(mpu, MPU60X0_REG_PWR_MGMT_1, (1 << 6)); config->init_status++; - break; + break; case MPU60X0_CONF_USER_RESET: /* trigger FIFO, I2C_MST and SIG_COND resets */ mpu_set(mpu, MPU60X0_REG_USER_CTRL, ((1 << MPU60X0_FIFO_RESET) | @@ -65,7 +65,7 @@ void mpu60x0_send_config(Mpu60x0ConfigSet mpu_set, void* mpu, struct Mpu60x0Conf break; case MPU60X0_CONF_PWR: /* switch to gyroX clock by default */ - mpu_set(mpu, MPU60X0_REG_PWR_MGMT_1, ((config->clk_sel)|(0<<6))); + mpu_set(mpu, MPU60X0_REG_PWR_MGMT_1, ((config->clk_sel) | (0 << 6))); config->init_status++; break; case MPU60X0_CONF_SD: @@ -80,27 +80,28 @@ void mpu60x0_send_config(Mpu60x0ConfigSet mpu_set, void* mpu, struct Mpu60x0Conf break; case MPU60X0_CONF_GYRO: /* configure gyro range */ - mpu_set(mpu, MPU60X0_REG_GYRO_CONFIG, (config->gyro_range<<3)); + mpu_set(mpu, MPU60X0_REG_GYRO_CONFIG, (config->gyro_range << 3)); config->init_status++; break; case MPU60X0_CONF_ACCEL: /* configure accelerometer range */ - mpu_set(mpu, MPU60X0_REG_ACCEL_CONFIG, (config->accel_range<<3)); + mpu_set(mpu, MPU60X0_REG_ACCEL_CONFIG, (config->accel_range << 3)); config->init_status++; break; case MPU60X0_CONF_I2C_SLAVES: /* if any, set MPU for I2C slaves and configure them*/ if (config->nb_slaves > 0) { /* returns TRUE when all slaves are configured */ - if (mpu60x0_configure_i2c_slaves(mpu_set, mpu)) + if (mpu60x0_configure_i2c_slaves(mpu_set, mpu)) { config->init_status++; - } - else + } + } else { config->init_status++; + } break; case MPU60X0_CONF_INT_ENABLE: /* configure data ready interrupt */ - mpu_set(mpu, MPU60X0_REG_INT_ENABLE, (config->drdy_int_enable<<0)); + mpu_set(mpu, MPU60X0_REG_INT_ENABLE, (config->drdy_int_enable << 0)); config->init_status++; break; case MPU60X0_CONF_DONE: diff --git a/sw/airborne/peripherals/mpu60x0.h b/sw/airborne/peripherals/mpu60x0.h index 00df171767..419b5b59e1 100644 --- a/sw/airborne/peripherals/mpu60x0.h +++ b/sw/airborne/peripherals/mpu60x0.h @@ -61,10 +61,10 @@ enum Mpu60x0ConfStatus { }; /// Configuration function prototype -typedef void (*Mpu60x0ConfigSet)(void* mpu, uint8_t _reg, uint8_t _val); +typedef void (*Mpu60x0ConfigSet)(void *mpu, uint8_t _reg, uint8_t _val); /// function prototype for configuration of a single I2C slave -typedef bool_t (*Mpu60x0I2cSlaveConfigure)(Mpu60x0ConfigSet mpu_set, void* mpu); +typedef bool_t (*Mpu60x0I2cSlaveConfigure)(Mpu60x0ConfigSet mpu_set, void *mpu); struct Mpu60x0I2cSlave { Mpu60x0I2cSlaveConfigure configure; @@ -95,7 +95,7 @@ struct Mpu60x0Config { extern void mpu60x0_set_default_config(struct Mpu60x0Config *c); /// Configuration sequence called once before normal use -extern void mpu60x0_send_config(Mpu60x0ConfigSet mpu_set, void* mpu, struct Mpu60x0Config* config); +extern void mpu60x0_send_config(Mpu60x0ConfigSet mpu_set, void *mpu, struct Mpu60x0Config *config); /** * Configure I2C slaves of the MPU. @@ -104,6 +104,6 @@ extern void mpu60x0_send_config(Mpu60x0ConfigSet mpu_set, void* mpu, struct Mpu6 * @param mpu Mpu60x0Spi or Mpu60x0I2c peripheral * @return TRUE when all slaves are configured */ -extern bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void* mpu); +extern bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void *mpu); #endif // MPU60X0_H diff --git a/sw/airborne/peripherals/mpu60x0_i2c.c b/sw/airborne/peripherals/mpu60x0_i2c.c index 29aa3bea64..4b557ee5f0 100644 --- a/sw/airborne/peripherals/mpu60x0_i2c.c +++ b/sw/airborne/peripherals/mpu60x0_i2c.c @@ -49,8 +49,9 @@ void mpu60x0_i2c_init(struct Mpu60x0_I2c *mpu, struct i2c_periph *i2c_p, uint8_t } -static void mpu60x0_i2c_write_to_reg(void* mpu, uint8_t _reg, uint8_t _val) { - struct Mpu60x0_I2c* mpu_i2c = (struct Mpu60x0_I2c*)(mpu); +static void mpu60x0_i2c_write_to_reg(void *mpu, uint8_t _reg, uint8_t _val) +{ + struct Mpu60x0_I2c *mpu_i2c = (struct Mpu60x0_I2c *)(mpu); mpu_i2c->i2c_trans.buf[0] = _reg; mpu_i2c->i2c_trans.buf[1] = _val; i2c_transmit(mpu_i2c->i2c_p, &(mpu_i2c->i2c_trans), mpu_i2c->i2c_trans.slave_addr, 2); @@ -62,7 +63,7 @@ void mpu60x0_i2c_start_configure(struct Mpu60x0_I2c *mpu) if (mpu->config.init_status == MPU60X0_CONF_UNINIT) { mpu->config.init_status++; if (mpu->i2c_trans.status == I2CTransSuccess || mpu->i2c_trans.status == I2CTransDone) { - mpu60x0_send_config(mpu60x0_i2c_write_to_reg, (void*)mpu, &(mpu->config)); + mpu60x0_send_config(mpu60x0_i2c_write_to_reg, (void *)mpu, &(mpu->config)); } } } @@ -83,8 +84,7 @@ void mpu60x0_i2c_event(struct Mpu60x0_I2c *mpu) if (mpu->config.initialized) { if (mpu->i2c_trans.status == I2CTransFailed) { mpu->i2c_trans.status = I2CTransDone; - } - else if (mpu->i2c_trans.status == I2CTransSuccess) { + } else if (mpu->i2c_trans.status == I2CTransSuccess) { // Successfull reading if (bit_is_set(mpu->i2c_trans.buf[0], 0)) { // new data @@ -102,7 +102,7 @@ void mpu60x0_i2c_event(struct Mpu60x0_I2c *mpu) */ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wcast-qual" - memcpy(mpu->data_ext, (uint8_t*)&(mpu->i2c_trans.buf[15]), mpu->config.nb_bytes - 15); + memcpy(mpu->data_ext, (uint8_t *) & (mpu->i2c_trans.buf[15]), mpu->config.nb_bytes - 15); #pragma GCC diagnostic pop } @@ -110,16 +110,16 @@ void mpu60x0_i2c_event(struct Mpu60x0_I2c *mpu) } mpu->i2c_trans.status = I2CTransDone; } - } - else if (mpu->config.init_status != MPU60X0_CONF_UNINIT) { // Configuring but not yet initialized + } else if (mpu->config.init_status != MPU60X0_CONF_UNINIT) { // Configuring but not yet initialized switch (mpu->i2c_trans.status) { case I2CTransFailed: mpu->config.init_status--; // Retry config (TODO max retry) case I2CTransSuccess: case I2CTransDone: - mpu60x0_send_config(mpu60x0_i2c_write_to_reg, (void*)mpu, &(mpu->config)); - if (mpu->config.initialized) + mpu60x0_send_config(mpu60x0_i2c_write_to_reg, (void *)mpu, &(mpu->config)); + if (mpu->config.initialized) { mpu->i2c_trans.status = I2CTransDone; + } break; default: break; @@ -128,12 +128,13 @@ void mpu60x0_i2c_event(struct Mpu60x0_I2c *mpu) } /** @todo: only one slave so far. */ -bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void* mpu) +bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void *mpu) { - struct Mpu60x0_I2c* mpu_i2c = (struct Mpu60x0_I2c*)(mpu); + struct Mpu60x0_I2c *mpu_i2c = (struct Mpu60x0_I2c *)(mpu); - if (mpu_i2c->slave_init_status == MPU60X0_I2C_CONF_UNINIT) + if (mpu_i2c->slave_init_status == MPU60X0_I2C_CONF_UNINIT) { mpu_i2c->slave_init_status++; + } switch (mpu_i2c->slave_init_status) { case MPU60X0_I2C_CONF_I2C_MST_DIS: @@ -142,29 +143,29 @@ bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void* mpu) break; case MPU60X0_I2C_CONF_I2C_BYPASS_EN: /* switch to I2C passthrough */ - mpu_set(mpu, MPU60X0_REG_INT_PIN_CFG, (1<<1)); + mpu_set(mpu, MPU60X0_REG_INT_PIN_CFG, (1 << 1)); mpu_i2c->slave_init_status++; break; case MPU60X0_I2C_CONF_SLAVES_CONFIGURE: /* configure each slave. TODO: currently only one */ - if (mpu_i2c->config.slaves[0].configure(mpu_set, mpu)) + if (mpu_i2c->config.slaves[0].configure(mpu_set, mpu)) { mpu_i2c->slave_init_status++; + } break; case MPU60X0_I2C_CONF_I2C_BYPASS_DIS: if (mpu_i2c->config.i2c_bypass) { /* if bypassing I2C skip MPU I2C master setup */ mpu_i2c->slave_init_status = MPU60X0_I2C_CONF_DONE; - } - else { + } else { /* disable I2C passthrough again */ - mpu_set(mpu, MPU60X0_REG_INT_PIN_CFG, (0<<1)); + mpu_set(mpu, MPU60X0_REG_INT_PIN_CFG, (0 << 1)); mpu_i2c->slave_init_status++; } break; case MPU60X0_I2C_CONF_I2C_MST_CLK: /* configure MPU I2C master clock and stop/start between slave reads */ mpu_set(mpu, MPU60X0_REG_I2C_MST_CTRL, - ((1<<4) | mpu_i2c->config.i2c_mst_clk)); + ((1 << 4) | mpu_i2c->config.i2c_mst_clk)); mpu_i2c->slave_init_status++; break; case MPU60X0_I2C_CONF_I2C_MST_DELAY: diff --git a/sw/airborne/peripherals/mpu60x0_i2c.h b/sw/airborne/peripherals/mpu60x0_i2c.h index 581ad60b45..cca898c53a 100644 --- a/sw/airborne/peripherals/mpu60x0_i2c.h +++ b/sw/airborne/peripherals/mpu60x0_i2c.h @@ -75,11 +75,13 @@ extern void mpu60x0_i2c_read(struct Mpu60x0_I2c *mpu); extern void mpu60x0_i2c_event(struct Mpu60x0_I2c *mpu); /// convenience function: read or start configuration if not already initialized -static inline void mpu60x0_i2c_periodic(struct Mpu60x0_I2c *mpu) { - if (mpu->config.initialized) +static inline void mpu60x0_i2c_periodic(struct Mpu60x0_I2c *mpu) +{ + if (mpu->config.initialized) { mpu60x0_i2c_read(mpu); - else + } else { mpu60x0_i2c_start_configure(mpu); + } } #endif // MPU60X0_I2C_H diff --git a/sw/airborne/peripherals/mpu60x0_regs.h b/sw/airborne/peripherals/mpu60x0_regs.h index ac48968ab6..804cec43e9 100644 --- a/sw/airborne/peripherals/mpu60x0_regs.h +++ b/sw/airborne/peripherals/mpu60x0_regs.h @@ -35,7 +35,7 @@ #define MPU60X0_SPI_READ 0x80 // Power and Interface -#define MPU60X0_REG_AUX_VDDIO 0x01 // Must be set to 0 on MPU6000 +#define MPU60X0_REG_AUX_VDDIO 0x01 // Must be set to 0 on MPU6000 #define MPU60X0_REG_USER_CTRL 0x6A #define MPU60X0_REG_PWR_MGMT_1 0x6B #define MPU60X0_REG_PWR_MGMT_2 0x6C @@ -43,8 +43,8 @@ // FIFO #define MPU60X0_REG_FIFO_EN 0x23 #define MPU60X0_REG_FIFO_COUNT_H 0x72 -#define MPU60X0_REG_FIFO_COUNT_L 0x73 -#define MPU60X0_REG_FIFO_R_W 0x74 +#define MPU60X0_REG_FIFO_COUNT_L 0x73 +#define MPU60X0_REG_FIFO_R_W 0x74 // Measurement Settings #define MPU60X0_REG_SMPLRT_DIV 0x19 @@ -57,31 +57,31 @@ #define MPU60X0_REG_I2C_MST_STATUS 0x36 #define MPU60X0_REG_I2C_MST_DELAY 0x67 // Slave 0 -#define MPU60X0_REG_I2C_SLV0_ADDR 0X25 // i2c addr -#define MPU60X0_REG_I2C_SLV0_REG 0X26 // slave reg -#define MPU60X0_REG_I2C_SLV0_CTRL 0X27 // set-bits -#define MPU60X0_REG_I2C_SLV0_DO 0X63 // DO +#define MPU60X0_REG_I2C_SLV0_ADDR 0X25 // i2c addr +#define MPU60X0_REG_I2C_SLV0_REG 0X26 // slave reg +#define MPU60X0_REG_I2C_SLV0_CTRL 0X27 // set-bits +#define MPU60X0_REG_I2C_SLV0_DO 0X63 // DO // Slave 1 -#define MPU60X0_REG_I2C_SLV1_ADDR 0X28 // i2c addr -#define MPU60X0_REG_I2C_SLV1_REG 0X29 // slave reg -#define MPU60X0_REG_I2C_SLV1_CTRL 0X2A // set-bits -#define MPU60X0_REG_I2C_SLV1_DO 0X64 // DO +#define MPU60X0_REG_I2C_SLV1_ADDR 0X28 // i2c addr +#define MPU60X0_REG_I2C_SLV1_REG 0X29 // slave reg +#define MPU60X0_REG_I2C_SLV1_CTRL 0X2A // set-bits +#define MPU60X0_REG_I2C_SLV1_DO 0X64 // DO // Slave 2 -#define MPU60X0_REG_I2C_SLV2_ADDR 0X2B // i2c addr -#define MPU60X0_REG_I2C_SLV2_REG 0X2C // slave reg -#define MPU60X0_REG_I2C_SLV2_CTRL 0X2D // set-bits -#define MPU60X0_REG_I2C_SLV2_DO 0X65 // DO +#define MPU60X0_REG_I2C_SLV2_ADDR 0X2B // i2c addr +#define MPU60X0_REG_I2C_SLV2_REG 0X2C // slave reg +#define MPU60X0_REG_I2C_SLV2_CTRL 0X2D // set-bits +#define MPU60X0_REG_I2C_SLV2_DO 0X65 // DO // Slave 3 -#define MPU60X0_REG_I2C_SLV3_ADDR 0X2E // i2c addr -#define MPU60X0_REG_I2C_SLV3_REG 0X2F // slave reg -#define MPU60X0_REG_I2C_SLV3_CTRL 0X30 // set-bits -#define MPU60X0_REG_I2C_SLV3_DO 0X66 // DO +#define MPU60X0_REG_I2C_SLV3_ADDR 0X2E // i2c addr +#define MPU60X0_REG_I2C_SLV3_REG 0X2F // slave reg +#define MPU60X0_REG_I2C_SLV3_CTRL 0X30 // set-bits +#define MPU60X0_REG_I2C_SLV3_DO 0X66 // DO // Slave 4 - special -#define MPU60X0_REG_I2C_SLV4_ADDR 0X31 // i2c addr -#define MPU60X0_REG_I2C_SLV4_REG 0X32 // slave reg -#define MPU60X0_REG_I2C_SLV4_DO 0X33 // DO -#define MPU60X0_REG_I2C_SLV4_CTRL 0X34 // set-bits -#define MPU60X0_REG_I2C_SLV4_DI 0X35 // DI +#define MPU60X0_REG_I2C_SLV4_ADDR 0X31 // i2c addr +#define MPU60X0_REG_I2C_SLV4_REG 0X32 // slave reg +#define MPU60X0_REG_I2C_SLV4_DO 0X33 // DO +#define MPU60X0_REG_I2C_SLV4_CTRL 0X34 // set-bits +#define MPU60X0_REG_I2C_SLV4_DI 0X35 // DI // Interrupt #define MPU60X0_REG_INT_PIN_CFG 0x37 @@ -114,7 +114,7 @@ #define MPU60X0_REG_WHO_AM_I 0x75 -#define MPU60X0_WHOAMI_REPLY 0x68 +#define MPU60X0_WHOAMI_REPLY 0x68 // Bit positions #define MPU60X0_I2C_BYPASS_EN 1 @@ -124,7 +124,7 @@ #define MPU60X0_I2C_MST_RESET 1 #define MPU60X0_FIFO_RESET 2 #define MPU60X0_I2C_IF_DIS 4 -#define MPU60X0_I2C_MST_EN 5 +#define MPU60X0_I2C_MST_EN 5 #define MPU60X0_FIFO_EN 6 // in MPU60X0_REG_I2C_MST_STATUS diff --git a/sw/airborne/peripherals/mpu60x0_spi.c b/sw/airborne/peripherals/mpu60x0_spi.c index 8f166f9ab7..9d7cb1a691 100644 --- a/sw/airborne/peripherals/mpu60x0_spi.c +++ b/sw/airborne/peripherals/mpu60x0_spi.c @@ -63,8 +63,9 @@ void mpu60x0_spi_init(struct Mpu60x0_Spi *mpu, struct spi_periph *spi_p, uint8_t } -static void mpu60x0_spi_write_to_reg(void* mpu, uint8_t _reg, uint8_t _val) { - struct Mpu60x0_Spi* mpu_spi = (struct Mpu60x0_Spi*)(mpu); +static void mpu60x0_spi_write_to_reg(void *mpu, uint8_t _reg, uint8_t _val) +{ + struct Mpu60x0_Spi *mpu_spi = (struct Mpu60x0_Spi *)(mpu); mpu_spi->spi_trans.output_length = 2; mpu_spi->spi_trans.input_length = 0; mpu_spi->tx_buf[0] = _reg; @@ -78,7 +79,7 @@ void mpu60x0_spi_start_configure(struct Mpu60x0_Spi *mpu) if (mpu->config.init_status == MPU60X0_CONF_UNINIT) { mpu->config.init_status++; if (mpu->spi_trans.status == SPITransSuccess || mpu->spi_trans.status == SPITransDone) { - mpu60x0_send_config(mpu60x0_spi_write_to_reg, (void*)mpu, &(mpu->config)); + mpu60x0_send_config(mpu60x0_spi_write_to_reg, (void *)mpu, &(mpu->config)); } } } @@ -101,8 +102,7 @@ void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu) if (mpu->config.initialized) { if (mpu->spi_trans.status == SPITransFailed) { mpu->spi_trans.status = SPITransDone; - } - else if (mpu->spi_trans.status == SPITransSuccess) { + } else if (mpu->spi_trans.status == SPITransSuccess) { // Successfull reading if (bit_is_set(mpu->rx_buf[1], 0)) { // new data @@ -120,7 +120,7 @@ void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu) */ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wcast-qual" - memcpy(mpu->data_ext, (uint8_t*)&(mpu->rx_buf[16]), mpu->config.nb_bytes - 15); + memcpy(mpu->data_ext, (uint8_t *) & (mpu->rx_buf[16]), mpu->config.nb_bytes - 15); #pragma GCC diagnostic pop } @@ -128,16 +128,16 @@ void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu) } mpu->spi_trans.status = SPITransDone; } - } - else if (mpu->config.init_status != MPU60X0_CONF_UNINIT) { // Configuring but not yet initialized + } else if (mpu->config.init_status != MPU60X0_CONF_UNINIT) { // Configuring but not yet initialized switch (mpu->spi_trans.status) { case SPITransFailed: mpu->config.init_status--; // Retry config (TODO max retry) case SPITransSuccess: case SPITransDone: - mpu60x0_send_config(mpu60x0_spi_write_to_reg, (void*)mpu, &(mpu->config)); - if (mpu->config.initialized) + mpu60x0_send_config(mpu60x0_spi_write_to_reg, (void *)mpu, &(mpu->config)); + if (mpu->config.initialized) { mpu->spi_trans.status = SPITransDone; + } break; default: break; @@ -146,17 +146,18 @@ void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu) } /** @todo: only one slave so far. */ -bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void* mpu) +bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void *mpu) { - struct Mpu60x0_Spi* mpu_spi = (struct Mpu60x0_Spi*)(mpu); + struct Mpu60x0_Spi *mpu_spi = (struct Mpu60x0_Spi *)(mpu); - if (mpu_spi->slave_init_status == MPU60X0_SPI_CONF_UNINIT) + if (mpu_spi->slave_init_status == MPU60X0_SPI_CONF_UNINIT) { mpu_spi->slave_init_status++; + } switch (mpu_spi->slave_init_status) { case MPU60X0_SPI_CONF_I2C_MST_CLK: /* configure MPU I2C master clock and stop/start between slave reads */ - mpu_set(mpu, MPU60X0_REG_I2C_MST_CTRL, ((1<<4) | mpu_spi->config.i2c_mst_clk)); + mpu_set(mpu, MPU60X0_REG_I2C_MST_CTRL, ((1 << 4) | mpu_spi->config.i2c_mst_clk)); mpu_spi->slave_init_status++; break; case MPU60X0_SPI_CONF_I2C_MST_DELAY: @@ -172,8 +173,9 @@ bool_t mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void* mpu) break; case MPU60X0_SPI_CONF_SLAVES_CONFIGURE: /* configure first slave, only one slave supported so far */ - if (mpu_spi->config.slaves[0].configure(mpu_set, mpu)) + if (mpu_spi->config.slaves[0].configure(mpu_set, mpu)) { mpu_spi->slave_init_status++; + } break; case MPU60X0_SPI_CONF_DONE: return TRUE; diff --git a/sw/airborne/peripherals/mpu60x0_spi.h b/sw/airborne/peripherals/mpu60x0_spi.h index 75975f851b..f1bf3e847f 100644 --- a/sw/airborne/peripherals/mpu60x0_spi.h +++ b/sw/airborne/peripherals/mpu60x0_spi.h @@ -74,11 +74,13 @@ extern void mpu60x0_spi_read(struct Mpu60x0_Spi *mpu); extern void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu); /// convenience function: read or start configuration if not already initialized -static inline void mpu60x0_spi_periodic(struct Mpu60x0_Spi *mpu) { - if (mpu->config.initialized) +static inline void mpu60x0_spi_periodic(struct Mpu60x0_Spi *mpu) +{ + if (mpu->config.initialized) { mpu60x0_spi_read(mpu); - else + } else { mpu60x0_spi_start_configure(mpu); + } } #endif // MPU60X0_SPI_H diff --git a/sw/airborne/peripherals/mpu9250.c b/sw/airborne/peripherals/mpu9250.c index 9a3df5c779..a2326bbc2e 100644 --- a/sw/airborne/peripherals/mpu9250.c +++ b/sw/airborne/peripherals/mpu9250.c @@ -49,14 +49,14 @@ void mpu9250_set_default_config(struct Mpu9250Config *c) c->i2c_bypass = FALSE; } -void mpu9250_send_config(Mpu9250ConfigSet mpu_set, void* mpu, struct Mpu9250Config* config) +void mpu9250_send_config(Mpu9250ConfigSet mpu_set, void *mpu, struct Mpu9250Config *config) { switch (config->init_status) { case MPU9250_CONF_RESET: /* device reset, set register values to defaults */ - mpu_set(mpu, MPU9250_REG_PWR_MGMT_1, (1<<6)); + mpu_set(mpu, MPU9250_REG_PWR_MGMT_1, (1 << 6)); config->init_status++; - break; + break; case MPU9250_CONF_USER_RESET: /* trigger FIFO, I2C_MST and SIG_COND resets */ mpu_set(mpu, MPU9250_REG_USER_CTRL, ((1 << MPU9250_FIFO_RESET) | @@ -66,7 +66,7 @@ void mpu9250_send_config(Mpu9250ConfigSet mpu_set, void* mpu, struct Mpu9250Conf break; case MPU9250_CONF_PWR: /* switch to gyroX clock by default */ - mpu_set(mpu, MPU9250_REG_PWR_MGMT_1, ((config->clk_sel)|(0<<6))); + mpu_set(mpu, MPU9250_REG_PWR_MGMT_1, ((config->clk_sel) | (0 << 6))); config->init_status++; break; case MPU9250_CONF_SD: @@ -86,27 +86,28 @@ void mpu9250_send_config(Mpu9250ConfigSet mpu_set, void* mpu, struct Mpu9250Conf break; case MPU9250_CONF_GYRO: /* configure gyro range */ - mpu_set(mpu, MPU9250_REG_GYRO_CONFIG, (config->gyro_range<<3)); + mpu_set(mpu, MPU9250_REG_GYRO_CONFIG, (config->gyro_range << 3)); config->init_status++; break; case MPU9250_CONF_ACCEL: /* configure accelerometer range */ - mpu_set(mpu, MPU9250_REG_ACCEL_CONFIG, (config->accel_range<<3)); + mpu_set(mpu, MPU9250_REG_ACCEL_CONFIG, (config->accel_range << 3)); config->init_status++; break; case MPU9250_CONF_I2C_SLAVES: /* if any, set MPU for I2C slaves and configure them*/ if (config->nb_slaves > 0) { /* returns TRUE when all slaves are configured */ - if (mpu9250_configure_i2c_slaves(mpu_set, mpu)) + if (mpu9250_configure_i2c_slaves(mpu_set, mpu)) { config->init_status++; - } - else + } + } else { config->init_status++; + } break; case MPU9250_CONF_INT_ENABLE: /* configure data ready interrupt */ - mpu_set(mpu, MPU9250_REG_INT_ENABLE, (config->drdy_int_enable<<0)); + mpu_set(mpu, MPU9250_REG_INT_ENABLE, (config->drdy_int_enable << 0)); config->init_status++; break; case MPU9250_CONF_DONE: diff --git a/sw/airborne/peripherals/mpu9250.h b/sw/airborne/peripherals/mpu9250.h index 70330b3e81..81ca095cd5 100644 --- a/sw/airborne/peripherals/mpu9250.h +++ b/sw/airborne/peripherals/mpu9250.h @@ -63,10 +63,10 @@ enum Mpu9250ConfStatus { }; /// Configuration function prototype -typedef void (*Mpu9250ConfigSet)(void* mpu, uint8_t _reg, uint8_t _val); +typedef void (*Mpu9250ConfigSet)(void *mpu, uint8_t _reg, uint8_t _val); /// function prototype for configuration of a single I2C slave -typedef bool_t (*Mpu9250I2cSlaveConfigure)(Mpu9250ConfigSet mpu_set, void* mpu); +typedef bool_t (*Mpu9250I2cSlaveConfigure)(Mpu9250ConfigSet mpu_set, void *mpu); struct Mpu9250I2cSlave { Mpu9250I2cSlaveConfigure configure; @@ -98,7 +98,7 @@ struct Mpu9250Config { extern void mpu9250_set_default_config(struct Mpu9250Config *c); /// Configuration sequence called once before normal use -extern void mpu9250_send_config(Mpu9250ConfigSet mpu_set, void* mpu, struct Mpu9250Config* config); +extern void mpu9250_send_config(Mpu9250ConfigSet mpu_set, void *mpu, struct Mpu9250Config *config); /** * Configure I2C slaves of the MPU. @@ -107,6 +107,6 @@ extern void mpu9250_send_config(Mpu9250ConfigSet mpu_set, void* mpu, struct Mpu9 * @param mpu Mpu9250Spi or Mpu9250I2c peripheral * @return TRUE when all slaves are configured */ -extern bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void* mpu); +extern bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void *mpu); #endif // MPU9250_H diff --git a/sw/airborne/peripherals/mpu9250_i2c.c b/sw/airborne/peripherals/mpu9250_i2c.c index 25ae8e298b..158ebdc22e 100644 --- a/sw/airborne/peripherals/mpu9250_i2c.c +++ b/sw/airborne/peripherals/mpu9250_i2c.c @@ -27,7 +27,8 @@ #include "peripherals/mpu9250_i2c.h" -bool_t imu_mpu9250_configure_mag_slave(Mpu9250ConfigSet mpu_set __attribute__ ((unused)), void* mpu __attribute__ ((unused))); +bool_t imu_mpu9250_configure_mag_slave(Mpu9250ConfigSet mpu_set __attribute__((unused)), + void *mpu __attribute__((unused))); void mpu9250_i2c_init(struct Mpu9250_I2c *mpu, struct i2c_periph *i2c_p, uint8_t addr) { @@ -61,8 +62,9 @@ void mpu9250_i2c_init(struct Mpu9250_I2c *mpu, struct i2c_periph *i2c_p, uint8_t } -static void mpu9250_i2c_write_to_reg(void* mpu, uint8_t _reg, uint8_t _val) { - struct Mpu9250_I2c* mpu_i2c = (struct Mpu9250_I2c*)(mpu); +static void mpu9250_i2c_write_to_reg(void *mpu, uint8_t _reg, uint8_t _val) +{ + struct Mpu9250_I2c *mpu_i2c = (struct Mpu9250_I2c *)(mpu); mpu_i2c->i2c_trans.buf[0] = _reg; mpu_i2c->i2c_trans.buf[1] = _val; i2c_transmit(mpu_i2c->i2c_p, &(mpu_i2c->i2c_trans), mpu_i2c->i2c_trans.slave_addr, 2); @@ -74,7 +76,7 @@ void mpu9250_i2c_start_configure(struct Mpu9250_I2c *mpu) if (mpu->config.init_status == MPU9250_CONF_UNINIT) { mpu->config.init_status++; if (mpu->i2c_trans.status == I2CTransSuccess || mpu->i2c_trans.status == I2CTransDone) { - mpu9250_send_config(mpu9250_i2c_write_to_reg, (void*)mpu, &(mpu->config)); + mpu9250_send_config(mpu9250_i2c_write_to_reg, (void *)mpu, &(mpu->config)); } } } @@ -101,8 +103,7 @@ void mpu9250_i2c_event(struct Mpu9250_I2c *mpu) if (mpu->config.initialized) { if (mpu->i2c_trans.status == I2CTransFailed) { mpu->i2c_trans.status = I2CTransDone; - } - else if (mpu->i2c_trans.status == I2CTransSuccess) { + } else if (mpu->i2c_trans.status == I2CTransSuccess) { // Successfull reading if (bit_is_set(mpu->i2c_trans.buf[0], 0)) { // new data @@ -120,7 +121,7 @@ void mpu9250_i2c_event(struct Mpu9250_I2c *mpu) */ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wcast-qual" - memcpy(mpu->data_ext, (uint8_t *) &(mpu->i2c_trans.buf[15]), mpu->config.nb_bytes - 15); + memcpy(mpu->data_ext, (uint8_t *) & (mpu->i2c_trans.buf[15]), mpu->config.nb_bytes - 15); #pragma GCC diagnostic pop } @@ -128,16 +129,16 @@ void mpu9250_i2c_event(struct Mpu9250_I2c *mpu) } mpu->i2c_trans.status = I2CTransDone; } - } - else if (mpu->config.init_status != MPU9250_CONF_UNINIT) { // Configuring but not yet initialized + } else if (mpu->config.init_status != MPU9250_CONF_UNINIT) { // Configuring but not yet initialized switch (mpu->i2c_trans.status) { case I2CTransFailed: mpu->config.init_status--; // Retry config (TODO max retry) case I2CTransSuccess: case I2CTransDone: - mpu9250_send_config(mpu9250_i2c_write_to_reg, (void*)mpu, &(mpu->config)); - if (mpu->config.initialized) + mpu9250_send_config(mpu9250_i2c_write_to_reg, (void *)mpu, &(mpu->config)); + if (mpu->config.initialized) { mpu->i2c_trans.status = I2CTransDone; + } break; default: break; @@ -150,24 +151,26 @@ void mpu9250_i2c_event(struct Mpu9250_I2c *mpu) /** callback function to configure ak8963 mag * @return TRUE if mag configuration finished */ -bool_t imu_mpu9250_configure_mag_slave(Mpu9250ConfigSet mpu_set __attribute__ ((unused)), void* mpu) +bool_t imu_mpu9250_configure_mag_slave(Mpu9250ConfigSet mpu_set __attribute__((unused)), void *mpu) { - struct Mpu9250_I2c* mpu_i2c = (struct Mpu9250_I2c*)(mpu); + struct Mpu9250_I2c *mpu_i2c = (struct Mpu9250_I2c *)(mpu); ak8963_configure(&mpu_i2c->akm); - if (mpu_i2c->akm.initialized) + if (mpu_i2c->akm.initialized) { return TRUE; - else + } else { return FALSE; + } } /** @todo: only one slave so far. */ -bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void* mpu) +bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void *mpu) { - struct Mpu9250_I2c* mpu_i2c = (struct Mpu9250_I2c*)(mpu); + struct Mpu9250_I2c *mpu_i2c = (struct Mpu9250_I2c *)(mpu); - if (mpu_i2c->slave_init_status == MPU9250_I2C_CONF_UNINIT) + if (mpu_i2c->slave_init_status == MPU9250_I2C_CONF_UNINIT) { mpu_i2c->slave_init_status++; + } switch (mpu_i2c->slave_init_status) { case MPU9250_I2C_CONF_I2C_MST_DIS: @@ -176,29 +179,29 @@ bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void* mpu) break; case MPU9250_I2C_CONF_I2C_BYPASS_EN: /* switch to I2C passthrough */ - mpu_set(mpu, MPU9250_REG_INT_PIN_CFG, (1<<1)); + mpu_set(mpu, MPU9250_REG_INT_PIN_CFG, (1 << 1)); mpu_i2c->slave_init_status++; break; case MPU9250_I2C_CONF_SLAVES_CONFIGURE: /* configure each slave. TODO: currently only one */ - if (mpu_i2c->config.slaves[0].configure(mpu_set, mpu)) + if (mpu_i2c->config.slaves[0].configure(mpu_set, mpu)) { mpu_i2c->slave_init_status++; + } break; case MPU9250_I2C_CONF_I2C_BYPASS_DIS: if (mpu_i2c->config.i2c_bypass) { /* if bypassing I2C skip MPU I2C master setup */ mpu_i2c->slave_init_status = MPU9250_I2C_CONF_DONE; - } - else { + } else { /* disable I2C passthrough again */ - mpu_set(mpu, MPU9250_REG_INT_PIN_CFG, (0<<1)); + mpu_set(mpu, MPU9250_REG_INT_PIN_CFG, (0 << 1)); mpu_i2c->slave_init_status++; } break; case MPU9250_I2C_CONF_I2C_MST_CLK: /* configure MPU I2C master clock and stop/start between slave reads */ mpu_set(mpu, MPU9250_REG_I2C_MST_CTRL, - ((1<<4) | mpu_i2c->config.i2c_mst_clk)); + ((1 << 4) | mpu_i2c->config.i2c_mst_clk)); mpu_i2c->slave_init_status++; break; case MPU9250_I2C_CONF_I2C_MST_DELAY: diff --git a/sw/airborne/peripherals/mpu9250_i2c.h b/sw/airborne/peripherals/mpu9250_i2c.h index a826e818e1..dd22192fc3 100644 --- a/sw/airborne/peripherals/mpu9250_i2c.h +++ b/sw/airborne/peripherals/mpu9250_i2c.h @@ -77,11 +77,13 @@ extern void mpu9250_i2c_read(struct Mpu9250_I2c *mpu); extern void mpu9250_i2c_event(struct Mpu9250_I2c *mpu); /// convenience function: read or start configuration if not already initialized -static inline void mpu9250_i2c_periodic(struct Mpu9250_I2c *mpu) { - if (mpu->config.initialized) +static inline void mpu9250_i2c_periodic(struct Mpu9250_I2c *mpu) +{ + if (mpu->config.initialized) { mpu9250_i2c_read(mpu); - else + } else { mpu9250_i2c_start_configure(mpu); + } } #endif // MPU9250_I2C_H diff --git a/sw/airborne/peripherals/mpu9250_regs.h b/sw/airborne/peripherals/mpu9250_regs.h index d680e8ecf4..1c152161b9 100644 --- a/sw/airborne/peripherals/mpu9250_regs.h +++ b/sw/airborne/peripherals/mpu9250_regs.h @@ -37,7 +37,7 @@ #define MPU9250_SPI_READ 0x80 // Power and Interface -#define MPU9250_REG_AUX_VDDIO 0x01 // Must be set to 0 on MPU6000 +#define MPU9250_REG_AUX_VDDIO 0x01 // Must be set to 0 on MPU6000 #define MPU9250_REG_USER_CTRL 0x6A #define MPU9250_REG_PWR_MGMT_1 0x6B #define MPU9250_REG_PWR_MGMT_2 0x6C @@ -45,8 +45,8 @@ // FIFO #define MPU9250_REG_FIFO_EN 0x23 #define MPU9250_REG_FIFO_COUNT_H 0x72 -#define MPU9250_REG_FIFO_COUNT_L 0x73 -#define MPU9250_REG_FIFO_R_W 0x74 +#define MPU9250_REG_FIFO_COUNT_L 0x73 +#define MPU9250_REG_FIFO_R_W 0x74 // Measurement Settings #define MPU9250_REG_SMPLRT_DIV 0x19 @@ -61,31 +61,31 @@ #define MPU9250_REG_I2C_MST_STATUS 0x36 #define MPU9250_REG_I2C_MST_DELAY 0x67 // Slave 0 -#define MPU9250_REG_I2C_SLV0_ADDR 0X25 // i2c addr -#define MPU9250_REG_I2C_SLV0_REG 0X26 // slave reg -#define MPU9250_REG_I2C_SLV0_CTRL 0X27 // set-bits -#define MPU9250_REG_I2C_SLV0_DO 0X63 // DO +#define MPU9250_REG_I2C_SLV0_ADDR 0X25 // i2c addr +#define MPU9250_REG_I2C_SLV0_REG 0X26 // slave reg +#define MPU9250_REG_I2C_SLV0_CTRL 0X27 // set-bits +#define MPU9250_REG_I2C_SLV0_DO 0X63 // DO // Slave 1 -#define MPU9250_REG_I2C_SLV1_ADDR 0X28 // i2c addr -#define MPU9250_REG_I2C_SLV1_REG 0X29 // slave reg -#define MPU9250_REG_I2C_SLV1_CTRL 0X2A // set-bits -#define MPU9250_REG_I2C_SLV1_DO 0X64 // DO +#define MPU9250_REG_I2C_SLV1_ADDR 0X28 // i2c addr +#define MPU9250_REG_I2C_SLV1_REG 0X29 // slave reg +#define MPU9250_REG_I2C_SLV1_CTRL 0X2A // set-bits +#define MPU9250_REG_I2C_SLV1_DO 0X64 // DO // Slave 2 -#define MPU9250_REG_I2C_SLV2_ADDR 0X2B // i2c addr -#define MPU9250_REG_I2C_SLV2_REG 0X2C // slave reg -#define MPU9250_REG_I2C_SLV2_CTRL 0X2D // set-bits -#define MPU9250_REG_I2C_SLV2_DO 0X65 // DO +#define MPU9250_REG_I2C_SLV2_ADDR 0X2B // i2c addr +#define MPU9250_REG_I2C_SLV2_REG 0X2C // slave reg +#define MPU9250_REG_I2C_SLV2_CTRL 0X2D // set-bits +#define MPU9250_REG_I2C_SLV2_DO 0X65 // DO // Slave 3 -#define MPU9250_REG_I2C_SLV3_ADDR 0X2E // i2c addr -#define MPU9250_REG_I2C_SLV3_REG 0X2F // slave reg -#define MPU9250_REG_I2C_SLV3_CTRL 0X30 // set-bits -#define MPU9250_REG_I2C_SLV3_DO 0X66 // DO +#define MPU9250_REG_I2C_SLV3_ADDR 0X2E // i2c addr +#define MPU9250_REG_I2C_SLV3_REG 0X2F // slave reg +#define MPU9250_REG_I2C_SLV3_CTRL 0X30 // set-bits +#define MPU9250_REG_I2C_SLV3_DO 0X66 // DO // Slave 4 - special -#define MPU9250_REG_I2C_SLV4_ADDR 0X31 // i2c addr -#define MPU9250_REG_I2C_SLV4_REG 0X32 // slave reg -#define MPU9250_REG_I2C_SLV4_DO 0X33 // DO -#define MPU9250_REG_I2C_SLV4_CTRL 0X34 // set-bits -#define MPU9250_REG_I2C_SLV4_DI 0X35 // DI +#define MPU9250_REG_I2C_SLV4_ADDR 0X31 // i2c addr +#define MPU9250_REG_I2C_SLV4_REG 0X32 // slave reg +#define MPU9250_REG_I2C_SLV4_DO 0X33 // DO +#define MPU9250_REG_I2C_SLV4_CTRL 0X34 // set-bits +#define MPU9250_REG_I2C_SLV4_DI 0X35 // DI // Interrupt #define MPU9250_REG_INT_PIN_CFG 0x37 @@ -118,7 +118,7 @@ #define MPU9250_REG_WHO_AM_I 0x75 -#define MPU9250_WHOAMI_REPLY 0x68 +#define MPU9250_WHOAMI_REPLY 0x68 // Bit positions #define MPU9250_I2C_BYPASS_EN 1 @@ -128,7 +128,7 @@ #define MPU9250_I2C_MST_RESET 1 #define MPU9250_FIFO_RESET 2 #define MPU9250_I2C_IF_DIS 4 -#define MPU9250_I2C_MST_EN 5 +#define MPU9250_I2C_MST_EN 5 #define MPU9250_FIFO_EN 6 // in MPU9250_REG_I2C_MST_STATUS diff --git a/sw/airborne/peripherals/mpu9250_spi.c b/sw/airborne/peripherals/mpu9250_spi.c index 82f03067e1..6af6631f6b 100644 --- a/sw/airborne/peripherals/mpu9250_spi.c +++ b/sw/airborne/peripherals/mpu9250_spi.c @@ -63,8 +63,9 @@ void mpu9250_spi_init(struct Mpu9250_Spi *mpu, struct spi_periph *spi_p, uint8_t } -static void mpu9250_spi_write_to_reg(void* mpu, uint8_t _reg, uint8_t _val) { - struct Mpu9250_Spi* mpu_spi = (struct Mpu9250_Spi*)(mpu); +static void mpu9250_spi_write_to_reg(void *mpu, uint8_t _reg, uint8_t _val) +{ + struct Mpu9250_Spi *mpu_spi = (struct Mpu9250_Spi *)(mpu); mpu_spi->spi_trans.output_length = 2; mpu_spi->spi_trans.input_length = 0; mpu_spi->tx_buf[0] = _reg; @@ -78,7 +79,7 @@ void mpu9250_spi_start_configure(struct Mpu9250_Spi *mpu) if (mpu->config.init_status == MPU9250_CONF_UNINIT) { mpu->config.init_status++; if (mpu->spi_trans.status == SPITransSuccess || mpu->spi_trans.status == SPITransDone) { - mpu9250_send_config(mpu9250_spi_write_to_reg, (void*)mpu, &(mpu->config)); + mpu9250_send_config(mpu9250_spi_write_to_reg, (void *)mpu, &(mpu->config)); } } } @@ -101,8 +102,7 @@ void mpu9250_spi_event(struct Mpu9250_Spi *mpu) if (mpu->config.initialized) { if (mpu->spi_trans.status == SPITransFailed) { mpu->spi_trans.status = SPITransDone; - } - else if (mpu->spi_trans.status == SPITransSuccess) { + } else if (mpu->spi_trans.status == SPITransSuccess) { // Successfull reading if (bit_is_set(mpu->rx_buf[1], 0)) { // new data @@ -120,7 +120,7 @@ void mpu9250_spi_event(struct Mpu9250_Spi *mpu) */ #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wcast-qual" - memcpy(mpu->data_ext, (uint8_t *) &(mpu->rx_buf[16]), mpu->config.nb_bytes - 15); + memcpy(mpu->data_ext, (uint8_t *) & (mpu->rx_buf[16]), mpu->config.nb_bytes - 15); #pragma GCC diagnostic pop } @@ -128,16 +128,16 @@ void mpu9250_spi_event(struct Mpu9250_Spi *mpu) } mpu->spi_trans.status = SPITransDone; } - } - else if (mpu->config.init_status != MPU9250_CONF_UNINIT) { // Configuring but not yet initialized + } else if (mpu->config.init_status != MPU9250_CONF_UNINIT) { // Configuring but not yet initialized switch (mpu->spi_trans.status) { case SPITransFailed: mpu->config.init_status--; // Retry config (TODO max retry) case SPITransSuccess: case SPITransDone: - mpu9250_send_config(mpu9250_spi_write_to_reg, (void*)mpu, &(mpu->config)); - if (mpu->config.initialized) + mpu9250_send_config(mpu9250_spi_write_to_reg, (void *)mpu, &(mpu->config)); + if (mpu->config.initialized) { mpu->spi_trans.status = SPITransDone; + } break; default: break; @@ -146,17 +146,18 @@ void mpu9250_spi_event(struct Mpu9250_Spi *mpu) } /** @todo: only one slave so far. */ -bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void* mpu) +bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void *mpu) { - struct Mpu9250_Spi* mpu_spi = (struct Mpu9250_Spi*)(mpu); + struct Mpu9250_Spi *mpu_spi = (struct Mpu9250_Spi *)(mpu); - if (mpu_spi->slave_init_status == MPU9250_SPI_CONF_UNINIT) + if (mpu_spi->slave_init_status == MPU9250_SPI_CONF_UNINIT) { mpu_spi->slave_init_status++; + } switch (mpu_spi->slave_init_status) { case MPU9250_SPI_CONF_I2C_MST_CLK: /* configure MPU I2C master clock and stop/start between slave reads */ - mpu_set(mpu, MPU9250_REG_I2C_MST_CTRL, ((1<<4) | mpu_spi->config.i2c_mst_clk)); + mpu_set(mpu, MPU9250_REG_I2C_MST_CTRL, ((1 << 4) | mpu_spi->config.i2c_mst_clk)); mpu_spi->slave_init_status++; break; case MPU9250_SPI_CONF_I2C_MST_DELAY: @@ -172,8 +173,9 @@ bool_t mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void* mpu) break; case MPU9250_SPI_CONF_SLAVES_CONFIGURE: /* configure first slave, only one slave supported so far */ - if (mpu_spi->config.slaves[0].configure(mpu_set, mpu)) + if (mpu_spi->config.slaves[0].configure(mpu_set, mpu)) { mpu_spi->slave_init_status++; + } break; case MPU9250_SPI_CONF_DONE: return TRUE; diff --git a/sw/airborne/peripherals/mpu9250_spi.h b/sw/airborne/peripherals/mpu9250_spi.h index d8135ba41b..2b8866c742 100644 --- a/sw/airborne/peripherals/mpu9250_spi.h +++ b/sw/airborne/peripherals/mpu9250_spi.h @@ -74,11 +74,13 @@ extern void mpu9250_spi_read(struct Mpu9250_Spi *mpu); extern void mpu9250_spi_event(struct Mpu9250_Spi *mpu); /// convenience function: read or start configuration if not already initialized -static inline void mpu9250_spi_periodic(struct Mpu9250_Spi *mpu) { - if (mpu->config.initialized) +static inline void mpu9250_spi_periodic(struct Mpu9250_Spi *mpu) +{ + if (mpu->config.initialized) { mpu9250_spi_read(mpu); - else + } else { mpu9250_spi_start_configure(mpu); + } } #endif // MPU9250_SPI_H diff --git a/sw/airborne/peripherals/ms2100.c b/sw/airborne/peripherals/ms2100.c index dbd2b545b2..79d53df65f 100644 --- a/sw/airborne/peripherals/ms2100.c +++ b/sw/airborne/peripherals/ms2100.c @@ -44,7 +44,8 @@ struct Ms2100 ms2100; -void ms2100_init(struct Ms2100 *ms, struct spi_periph *spi_p, uint8_t slave_idx) { +void ms2100_init(struct Ms2100 *ms, struct spi_periph *spi_p, uint8_t slave_idx) +{ /* set spi_peripheral */ ms->spi_p = spi_p; @@ -92,15 +93,17 @@ void ms2100_init(struct Ms2100 *ms, struct spi_periph *spi_p, uint8_t slave_idx) } /// send request to read next axis -void ms2100_read(struct Ms2100 *ms) { - ms->req_buf[0] = (ms->cur_axe+1) << 0 | MS2100_DIVISOR << 4; +void ms2100_read(struct Ms2100 *ms) +{ + ms->req_buf[0] = (ms->cur_axe + 1) << 0 | MS2100_DIVISOR << 4; spi_submit(ms->spi_p, &(ms->req_trans)); ms->status = MS2100_SENDING_REQ; } #define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx]<<8) | _buf[_idx+1])) -void ms2100_event(struct Ms2100 *ms) { +void ms2100_event(struct Ms2100 *ms) +{ // handle request transaction if (ms->req_trans.status == SPITransDone) { if (ms->status == MS2100_GOT_EOC) { @@ -108,11 +111,9 @@ void ms2100_event(struct Ms2100 *ms) { spi_submit(ms->spi_p, &(ms->read_trans)); ms->status = MS2100_READING_RES; } - } - else if (ms->req_trans.status == SPITransSuccess) { + } else if (ms->req_trans.status == SPITransSuccess) { ms->req_trans.status = SPITransDone; - } - else if (ms->req_trans.status == SPITransFailed) { + } else if (ms->req_trans.status == SPITransFailed) { ms->status = MS2100_IDLE; ms->cur_axe = 0; ms->req_trans.status = SPITransDone; @@ -122,7 +123,7 @@ void ms2100_event(struct Ms2100 *ms) { if (ms->read_trans.status == SPITransSuccess) { if (ms->status == MS2100_READING_RES) { // store value - int16_t new_val = Int16FromBuf(ms->read_buf,0); + int16_t new_val = Int16FromBuf(ms->read_buf, 0); // what is this check about? if (abs(new_val) < 2000) { ms->data.value[ms->cur_axe] = new_val; @@ -131,14 +132,12 @@ void ms2100_event(struct Ms2100 *ms) { if (ms->cur_axe > 2) { ms->cur_axe = 0; ms->status = MS2100_DATA_AVAILABLE; - } - else { + } else { ms->status = MS2100_IDLE; } ms->read_trans.status = SPITransDone; } - } - else if (ms->read_trans.status == SPITransFailed) { + } else if (ms->read_trans.status == SPITransFailed) { ms->status = MS2100_IDLE; ms->cur_axe = 0; ms->read_trans.status = SPITransDone; diff --git a/sw/airborne/peripherals/ms2100.h b/sw/airborne/peripherals/ms2100.h index 40ffb73d9a..00b2ee3257 100644 --- a/sw/airborne/peripherals/ms2100.h +++ b/sw/airborne/peripherals/ms2100.h @@ -62,7 +62,8 @@ extern void ms2100_init(struct Ms2100 *ms, struct spi_periph *spi_p, uint8_t sla extern void ms2100_read(struct Ms2100 *ms); extern void ms2100_event(struct Ms2100 *ms); -static inline void ms2100_periodic(struct Ms2100 *ms) { +static inline void ms2100_periodic(struct Ms2100 *ms) +{ if (ms->status == MS2100_IDLE) { ms2100_read(ms); } @@ -71,6 +72,6 @@ static inline void ms2100_periodic(struct Ms2100 *ms) { /* underlying architecture */ #include "peripherals/ms2100_arch.h" /* must be implemented by underlying architecture */ -extern void ms2100_arch_init( void ); +extern void ms2100_arch_init(void); #endif /* MS2100_H */ diff --git a/sw/airborne/peripherals/ms5611.c b/sw/airborne/peripherals/ms5611.c index 5b56b45c3c..bfc8e68b99 100644 --- a/sw/airborne/peripherals/ms5611.c +++ b/sw/airborne/peripherals/ms5611.c @@ -33,52 +33,57 @@ * Check if CRC of PROM data is OK. * @return TRUE if OK, FALSE otherwise */ -bool_t ms5611_prom_crc_ok(uint16_t* prom) { +bool_t ms5611_prom_crc_ok(uint16_t *prom) +{ int32_t i, j; uint32_t res = 0; uint8_t crc = prom[7] & 0xF; prom[7] &= 0xFF00; for (i = 0; i < 16; i++) { - if (i & 1) - res ^= ((prom[i>>1]) & 0x00FF); - else - res ^= (prom[i>>1]>>8); + if (i & 1) { + res ^= ((prom[i >> 1]) & 0x00FF); + } else { + res ^= (prom[i >> 1] >> 8); + } for (j = 8; j > 0; j--) { - if (res & 0x8000) + if (res & 0x8000) { res ^= 0x1800; + } res <<= 1; } } prom[7] |= crc; - if (crc == ((res >> 12) & 0xF)) + if (crc == ((res >> 12) & 0xF)) { return TRUE; - else + } else { return FALSE; + } } /** * Calculate temperature and compensated pressure. * @return TRUE if measurement was valid, FALSE otherwise */ -bool_t ms5611_calc(struct Ms5611Data* ms) { +bool_t ms5611_calc(struct Ms5611Data *ms) +{ int64_t dt, tempms, off, sens, t2, off2, sens2; /* difference between actual and ref temperature */ - dt = ms->d2 - (int64_t)ms->c[5] * (1<<8); + dt = ms->d2 - (int64_t)ms->c[5] * (1 << 8); /* actual temperature */ - tempms = 2000 + ((int64_t)dt * ms->c[6]) / (1<<23); + tempms = 2000 + ((int64_t)dt * ms->c[6]) / (1 << 23); /* offset at actual temperature */ - off = ((int64_t)ms->c[2] * (1<<16)) + ((int64_t)ms->c[4] * dt) / (1<<7); + off = ((int64_t)ms->c[2] * (1 << 16)) + ((int64_t)ms->c[4] * dt) / (1 << 7); /* sensitivity at actual temperature */ - sens = ((int64_t)ms->c[1] * (1<<15)) + ((int64_t)ms->c[3] * dt) / (1<<8); + sens = ((int64_t)ms->c[1] * (1 << 15)) + ((int64_t)ms->c[3] * dt) / (1 << 8); /* second order temperature compensation */ if (tempms < 2000) { - t2 = (dt*dt) / (1<<31); - off2 = 5 * ((int64_t)(tempms-2000)*(tempms-2000)) / (1<<1); - sens2 = 5 * ((int64_t)(tempms-2000)*(tempms-2000)) / (1<<2); + t2 = (dt * dt) / (1 << 31); + off2 = 5 * ((int64_t)(tempms - 2000) * (tempms - 2000)) / (1 << 1); + sens2 = 5 * ((int64_t)(tempms - 2000) * (tempms - 2000)) / (1 << 2); if (tempms < -1500) { - off2 = off2 + 7 * (int64_t)(tempms+1500)*(tempms+1500); - sens2 = sens2 + 11 * ((int64_t)(tempms+1500)*(tempms+1500)) / (1<<1); + off2 = off2 + 7 * (int64_t)(tempms + 1500) * (tempms + 1500); + sens2 = sens2 + 11 * ((int64_t)(tempms + 1500) * (tempms + 1500)) / (1 << 1); } tempms = tempms - t2; off = off - off2; @@ -86,9 +91,9 @@ bool_t ms5611_calc(struct Ms5611Data* ms) { } /* temperature compensated pressure in Pascal (0.01mbar) */ - uint32_t p = (((int64_t)ms->d1 * sens) / (1<<21) - off) / (1<<15); + uint32_t p = (((int64_t)ms->d1 * sens) / (1 << 21) - off) / (1 << 15); /* if temp and pressare are in valid bounds, copy and return TRUE (valid) */ - if ((tempms > -4000) && (tempms < 8500) && (p > 1000 ) && (p < 120000)) { + if ((tempms > -4000) && (tempms < 8500) && (p > 1000) && (p < 120000)) { /* temperature in deg Celsius with 0.01 degC resolultion */ ms->temperature = (int32_t)tempms; ms->pressure = p; diff --git a/sw/airborne/peripherals/ms5611.h b/sw/airborne/peripherals/ms5611.h index 0f7990a4a7..7ab7c65eb0 100644 --- a/sw/airborne/peripherals/ms5611.h +++ b/sw/airborne/peripherals/ms5611.h @@ -56,7 +56,7 @@ struct Ms5611Data { uint32_t d2; }; -extern bool_t ms5611_prom_crc_ok(uint16_t* prom); -extern bool_t ms5611_calc(struct Ms5611Data* ms); +extern bool_t ms5611_prom_crc_ok(uint16_t *prom); +extern bool_t ms5611_calc(struct Ms5611Data *ms); #endif /* MS5611_H */ diff --git a/sw/airborne/peripherals/ms5611_i2c.c b/sw/airborne/peripherals/ms5611_i2c.c index 5e22e5968c..d328d58970 100644 --- a/sw/airborne/peripherals/ms5611_i2c.c +++ b/sw/airborne/peripherals/ms5611_i2c.c @@ -114,13 +114,13 @@ void ms5611_i2c_periodic_check(struct Ms5611_I2c *ms) } } -void ms5611_i2c_event(struct Ms5611_I2c *ms) { +void ms5611_i2c_event(struct Ms5611_I2c *ms) +{ if (ms->initialized) { if (ms->i2c_trans.status == I2CTransFailed) { ms->status = MS5611_STATUS_IDLE; ms->i2c_trans.status = I2CTransDone; - } - else if (ms->i2c_trans.status == I2CTransSuccess) { + } else if (ms->i2c_trans.status == I2CTransSuccess) { // Successfull reading switch (ms->status) { @@ -128,13 +128,12 @@ void ms5611_i2c_event(struct Ms5611_I2c *ms) { /* read D1 (pressure) */ ms->data.d1 = (ms->i2c_trans.buf[0] << 16) | (ms->i2c_trans.buf[1] << 8) | - ms->i2c_trans.buf[2]; + ms->i2c_trans.buf[2]; ms->i2c_trans.status = I2CTransDone; if (ms->data.d1 == 0) { /* if value is zero, it was read to soon and is invalid, back to idle */ ms->status = MS5611_STATUS_IDLE; - } - else { + } else { /* start D2 conversion */ ms->i2c_trans.buf[0] = MS5611_START_CONV_D2; i2c_transmit(ms->i2c_p, &(ms->i2c_trans), ms->i2c_trans.slave_addr, 1); @@ -146,16 +145,16 @@ void ms5611_i2c_event(struct Ms5611_I2c *ms) { /* read D2 (temperature) */ ms->data.d2 = (ms->i2c_trans.buf[0] << 16) | (ms->i2c_trans.buf[1] << 8) | - ms->i2c_trans.buf[2]; + ms->i2c_trans.buf[2]; ms->i2c_trans.status = I2CTransDone; if (ms->data.d2 == 0) { /* if value is zero, it was read to soon and is invalid, back to idle */ ms->status = MS5611_STATUS_IDLE; - } - else { + } else { /* calculate temp and pressure from measurements and set available if valid */ - if (ms5611_calc(&(ms->data))) + if (ms5611_calc(&(ms->data))) { ms->data_available = TRUE; + } ms->status = MS5611_STATUS_IDLE; } break; @@ -165,8 +164,7 @@ void ms5611_i2c_event(struct Ms5611_I2c *ms) { break; } } - } - else if (ms->status != MS5611_STATUS_UNINIT) { // Configuring but not yet initialized + } else if (ms->status != MS5611_STATUS_UNINIT) { // Configuring but not yet initialized switch (ms->i2c_trans.status) { case I2CTransFailed: @@ -179,20 +177,18 @@ void ms5611_i2c_event(struct Ms5611_I2c *ms) { if (ms->status == MS5611_STATUS_PROM) { /* read prom data */ ms->data.c[ms->prom_cnt++] = (ms->i2c_trans.buf[0] << 8) | - ms->i2c_trans.buf[1]; + ms->i2c_trans.buf[1]; ms->i2c_trans.status = I2CTransDone; if (ms->prom_cnt < PROM_NB) { /* get next prom data */ ms->i2c_trans.buf[0] = MS5611_PROM_READ | (ms->prom_cnt << 1); i2c_transceive(ms->i2c_p, &(ms->i2c_trans), ms->i2c_trans.slave_addr, 1, 2); - } - else { + } else { /* done reading prom, check prom crc */ if (ms5611_prom_crc_ok(ms->data.c)) { ms->initialized = TRUE; ms->status = MS5611_STATUS_IDLE; - } - else { + } else { /* checksum error, try again */ ms->status = MS5611_STATUS_UNINIT; } diff --git a/sw/airborne/peripherals/ms5611_i2c.h b/sw/airborne/peripherals/ms5611_i2c.h index 149312ff36..3cb5b5d562 100644 --- a/sw/airborne/peripherals/ms5611_i2c.h +++ b/sw/airborne/peripherals/ms5611_i2c.h @@ -44,25 +44,28 @@ struct Ms5611_I2c { }; // Functions -extern void ms5611_i2c_init(struct Ms5611_I2c* ms, struct i2c_periph* i2c_p, uint8_t addr); -extern void ms5611_i2c_start_configure(struct Ms5611_I2c* ms); -extern void ms5611_i2c_start_conversion(struct Ms5611_I2c* ms); -extern void ms5611_i2c_periodic_check(struct Ms5611_I2c* ms); -extern void ms5611_i2c_event(struct Ms5611_I2c* ms); +extern void ms5611_i2c_init(struct Ms5611_I2c *ms, struct i2c_periph *i2c_p, uint8_t addr); +extern void ms5611_i2c_start_configure(struct Ms5611_I2c *ms); +extern void ms5611_i2c_start_conversion(struct Ms5611_I2c *ms); +extern void ms5611_i2c_periodic_check(struct Ms5611_I2c *ms); +extern void ms5611_i2c_event(struct Ms5611_I2c *ms); /** convenience function to trigger new measurement. * (or start configuration if not already initialized) * Still need to regularly run ms5611_i2c_periodic_check to complete the measurement. */ -static inline void ms5611_i2c_read(struct Ms5611_I2c* ms) { - if (ms->initialized) +static inline void ms5611_i2c_read(struct Ms5611_I2c *ms) +{ + if (ms->initialized) { ms5611_i2c_start_conversion(ms); - else + } else { ms5611_i2c_start_configure(ms); + } } /// convenience function -static inline void ms5611_i2c_periodic(struct Ms5611_I2c* ms) { +static inline void ms5611_i2c_periodic(struct Ms5611_I2c *ms) +{ ms5611_i2c_read(ms); ms5611_i2c_periodic_check(ms); } diff --git a/sw/airborne/peripherals/ms5611_spi.c b/sw/airborne/peripherals/ms5611_spi.c index f015c34c7e..7640cbb068 100644 --- a/sw/airborne/peripherals/ms5611_spi.c +++ b/sw/airborne/peripherals/ms5611_spi.c @@ -128,13 +128,13 @@ void ms5611_spi_periodic_check(struct Ms5611_Spi *ms) } } -void ms5611_spi_event(struct Ms5611_Spi *ms) { +void ms5611_spi_event(struct Ms5611_Spi *ms) +{ if (ms->initialized) { if (ms->spi_trans.status == SPITransFailed) { ms->status = MS5611_STATUS_IDLE; ms->spi_trans.status = SPITransDone; - } - else if (ms->spi_trans.status == SPITransSuccess) { + } else if (ms->spi_trans.status == SPITransSuccess) { // Successfull reading switch (ms->status) { @@ -142,12 +142,11 @@ void ms5611_spi_event(struct Ms5611_Spi *ms) { /* read D1 (pressure) */ ms->data.d1 = (ms->rx_buf[1] << 16) | (ms->rx_buf[2] << 8) | - ms->rx_buf[3]; + ms->rx_buf[3]; if (ms->data.d1 == 0) { /* if value is zero, it was read to soon and is invalid, back to idle */ ms->status = MS5611_STATUS_IDLE; - } - else { + } else { /* start D2 conversion */ ms->tx_buf[0] = MS5611_START_CONV_D2; spi_submit(ms->spi_p, &(ms->spi_trans)); @@ -159,15 +158,15 @@ void ms5611_spi_event(struct Ms5611_Spi *ms) { /* read D2 (temperature) */ ms->data.d2 = (ms->rx_buf[1] << 16) | (ms->rx_buf[2] << 8) | - ms->rx_buf[3]; + ms->rx_buf[3]; if (ms->data.d2 == 0) { /* if value is zero, it was read to soon and is invalid, back to idle */ ms->status = MS5611_STATUS_IDLE; - } - else { + } else { /* calculate temp and pressure from measurements and set available if valid */ - if (ms5611_calc(&(ms->data))) + if (ms5611_calc(&(ms->data))) { ms->data_available = TRUE; + } ms->status = MS5611_STATUS_IDLE; } break; @@ -177,8 +176,7 @@ void ms5611_spi_event(struct Ms5611_Spi *ms) { } ms->spi_trans.status = SPITransDone; } - } - else if (ms->status != MS5611_STATUS_UNINIT) { // Configuring but not yet initialized + } else if (ms->status != MS5611_STATUS_UNINIT) { // Configuring but not yet initialized switch (ms->spi_trans.status) { case SPITransFailed: @@ -191,19 +189,17 @@ void ms5611_spi_event(struct Ms5611_Spi *ms) { if (ms->status == MS5611_STATUS_PROM) { /* read prom data */ ms->data.c[ms->prom_cnt++] = (ms->rx_buf[1] << 8) | - ms->rx_buf[2]; + ms->rx_buf[2]; if (ms->prom_cnt < PROM_NB) { /* get next prom data */ ms->tx_buf[0] = MS5611_PROM_READ | (ms->prom_cnt << 1); spi_submit(ms->spi_p, &(ms->spi_trans)); - } - else { + } else { /* done reading prom, check prom crc */ if (ms5611_prom_crc_ok(ms->data.c)) { ms->initialized = TRUE; ms->status = MS5611_STATUS_IDLE; - } - else { + } else { /* checksum error, try again */ ms->status = MS5611_STATUS_UNINIT; } diff --git a/sw/airborne/peripherals/ms5611_spi.h b/sw/airborne/peripherals/ms5611_spi.h index 59f8f6e8e8..b885faa79f 100644 --- a/sw/airborne/peripherals/ms5611_spi.h +++ b/sw/airborne/peripherals/ms5611_spi.h @@ -46,25 +46,28 @@ struct Ms5611_Spi { }; // Functions -extern void ms5611_spi_init(struct Ms5611_Spi* ms, struct spi_periph* spi_p, uint8_t addr); -extern void ms5611_spi_start_configure(struct Ms5611_Spi* ms); -extern void ms5611_spi_start_conversion(struct Ms5611_Spi* ms); -extern void ms5611_spi_periodic_check(struct Ms5611_Spi* ms); -extern void ms5611_spi_event(struct Ms5611_Spi* ms); +extern void ms5611_spi_init(struct Ms5611_Spi *ms, struct spi_periph *spi_p, uint8_t addr); +extern void ms5611_spi_start_configure(struct Ms5611_Spi *ms); +extern void ms5611_spi_start_conversion(struct Ms5611_Spi *ms); +extern void ms5611_spi_periodic_check(struct Ms5611_Spi *ms); +extern void ms5611_spi_event(struct Ms5611_Spi *ms); /** convenience function to trigger new measurement. * (or start configuration if not already initialized) * Still need to regularly run ms5611_spi_periodic_check to complete the measurement. */ -static inline void ms5611_spi_read(struct Ms5611_Spi* ms) { - if (ms->initialized) +static inline void ms5611_spi_read(struct Ms5611_Spi *ms) +{ + if (ms->initialized) { ms5611_spi_start_conversion(ms); - else + } else { ms5611_spi_start_configure(ms); + } } /// convenience function -static inline void ms5611_spi_periodic(struct Ms5611_Spi* ms) { +static inline void ms5611_spi_periodic(struct Ms5611_Spi *ms) +{ ms5611_spi_read(ms); ms5611_spi_periodic_check(ms); } diff --git a/sw/airborne/peripherals/sc18i600.c b/sw/airborne/peripherals/sc18i600.c index 90cdfbdebd..c54fcbe669 100644 --- a/sw/airborne/peripherals/sc18i600.c +++ b/sw/airborne/peripherals/sc18i600.c @@ -2,7 +2,8 @@ struct Sc18Is600 sc18is600; -void sc18is600_init(void) { +void sc18is600_init(void) +{ sc18is600.status = Sc18Is600Idle; sc18is600_arch_init(); diff --git a/sw/airborne/peripherals/sst25vfxxxx.c b/sw/airborne/peripherals/sst25vfxxxx.c index 60b4d05e39..bb2035f379 100644 --- a/sw/airborne/peripherals/sst25vfxxxx.c +++ b/sw/airborne/peripherals/sst25vfxxxx.c @@ -31,7 +31,7 @@ /** * Initializing the sst25vfxxxx chip */ -void sst25vfxxxx_init(struct SST25VFxxxx* sst, struct spi_periph* spi_p, const uint8_t slave_idx, SPICallback spi_cb) +void sst25vfxxxx_init(struct SST25VFxxxx *sst, struct spi_periph *spi_p, const uint8_t slave_idx, SPICallback spi_cb) { /* Set spi_peripheral and start flash address */ sst->spi_p = spi_p; @@ -61,7 +61,7 @@ void sst25vfxxxx_init(struct SST25VFxxxx* sst, struct spi_periph* spi_p, const u /** * Callback of the SPI after going one level higher for gathering the sst pointer */ -void sst25vfxxxx_after_cb(struct SST25VFxxxx* sst) +void sst25vfxxxx_after_cb(struct SST25VFxxxx *sst) { switch (sst->status) { // Enabling writing to blocks @@ -217,7 +217,7 @@ void sst25vfxxxx_after_cb(struct SST25VFxxxx* sst) /** * Read the chip identifier */ -void sst25vfxxxx_read_id(struct SST25VFxxxx* sst) +void sst25vfxxxx_read_id(struct SST25VFxxxx *sst) { if (sst->status != SST25VFXXXX_IDLE) { return; @@ -237,7 +237,7 @@ void sst25vfxxxx_read_id(struct SST25VFxxxx* sst) /** * Enable block writing */ -void sst25vfxxxx_block_write_en(struct SST25VFxxxx* sst) +void sst25vfxxxx_block_write_en(struct SST25VFxxxx *sst) { if (sst->status != SST25VFXXXX_IDLE) { return; @@ -255,7 +255,7 @@ void sst25vfxxxx_block_write_en(struct SST25VFxxxx* sst) /** * Full chip erase */ -void sst25vfxxxx_chip_erase(struct SST25VFxxxx* sst) +void sst25vfxxxx_chip_erase(struct SST25VFxxxx *sst) { if (sst->status != SST25VFXXXX_IDLE) { return; @@ -273,7 +273,7 @@ void sst25vfxxxx_chip_erase(struct SST25VFxxxx* sst) /** * Write bytes */ -void sst25vfxxxx_write(struct SST25VFxxxx* sst, uint8_t* transfer_buffer, uint8_t transfer_length) +void sst25vfxxxx_write(struct SST25VFxxxx *sst, uint8_t *transfer_buffer, uint8_t transfer_length) { if (sst->status != SST25VFXXXX_IDLE) { return; @@ -297,7 +297,7 @@ void sst25vfxxxx_write(struct SST25VFxxxx* sst, uint8_t* transfer_buffer, uint8_ * Read bytes * Need 5 more extra bytes because of SPI overhead */ -void sst25vfxxxx_read(struct SST25VFxxxx* sst, uint8_t* transfer_buffer, uint8_t transfer_length) +void sst25vfxxxx_read(struct SST25VFxxxx *sst, uint8_t *transfer_buffer, uint8_t transfer_length) { if (sst->status != SST25VFXXXX_IDLE) { return; diff --git a/sw/airborne/peripherals/sst25vfxxxx.h b/sw/airborne/peripherals/sst25vfxxxx.h index fe4938cfbd..795b57efe3 100644 --- a/sw/airborne/peripherals/sst25vfxxxx.h +++ b/sw/airborne/peripherals/sst25vfxxxx.h @@ -63,23 +63,23 @@ enum SST25VFxxxxStatus { struct SST25VFxxxx { volatile enum SST25VFxxxxStatus status; /**< The status of the SST25VFxxxx flash chip */ uint8_t status_idx; /**< The counter of substatuses */ - struct spi_periph* spi_p; /**< The SPI peripheral for the connection */ + struct spi_periph *spi_p; /**< The SPI peripheral for the connection */ struct spi_transaction spi_t; /**< The SPI transaction used for the writing and reading of registers */ uint8_t input_buf[16]; /**< The input buffer for the SPI transaction */ uint8_t output_buf[16]; /**< The output buffer for the SPI transaction */ uint32_t flash_addr; /**< The flash address to write at */ - uint8_t* transfer_buf; /**< The transfer buffer */ + uint8_t *transfer_buf; /**< The transfer buffer */ uint8_t transfer_idx; /**< The transfer idx is used for counting input/output bytes */ uint8_t transfer_length; /**< The transfer buffer length */ }; -void sst25vfxxxx_init(struct SST25VFxxxx* sst, struct spi_periph* spi_p, const uint8_t slave_idx, SPICallback spi_cb); -void sst25vfxxxx_after_cb(struct SST25VFxxxx* sst); -void sst25vfxxxx_read_id(struct SST25VFxxxx* sst); -void sst25vfxxxx_block_write_en(struct SST25VFxxxx* sst); -void sst25vfxxxx_chip_erase(struct SST25VFxxxx* sst); -void sst25vfxxxx_write(struct SST25VFxxxx* sst, uint8_t* transfer_buffer, uint8_t transfer_length); -void sst25vfxxxx_read(struct SST25VFxxxx* sst, uint8_t* transfer_buffer, uint8_t transfer_length); +void sst25vfxxxx_init(struct SST25VFxxxx *sst, struct spi_periph *spi_p, const uint8_t slave_idx, SPICallback spi_cb); +void sst25vfxxxx_after_cb(struct SST25VFxxxx *sst); +void sst25vfxxxx_read_id(struct SST25VFxxxx *sst); +void sst25vfxxxx_block_write_en(struct SST25VFxxxx *sst); +void sst25vfxxxx_chip_erase(struct SST25VFxxxx *sst); +void sst25vfxxxx_write(struct SST25VFxxxx *sst, uint8_t *transfer_buffer, uint8_t transfer_length); +void sst25vfxxxx_read(struct SST25VFxxxx *sst, uint8_t *transfer_buffer, uint8_t transfer_length); #endif /* SST25VFXXXX_H */ diff --git a/sw/airborne/pprz_debug.h b/sw/airborne/pprz_debug.h index cd35a2afa5..a1c2294337 100644 --- a/sw/airborne/pprz_debug.h +++ b/sw/airborne/pprz_debug.h @@ -22,8 +22,8 @@ #ifndef PPRZ_DEBUG_H #define PPRZ_DEBUG_H -#define MY_ASSERT(cond) { \ - if (!(cond)) while(1); \ +#define MY_ASSERT(cond) { \ + if (!(cond)) while(1); \ } #ifdef PPRZ_DEBUG @@ -46,7 +46,7 @@ extern uint8_t pprz_debug_err; if (!(cond)) { \ pprz_debug_mod = mod; \ pprz_debug_err = err; \ - DOWNLINK_SEND_PPRZ_DEBUG(DefaultChannel, DefaultDevice, &pprz_debug_mod, &pprz_debug_err); \ + DOWNLINK_SEND_PPRZ_DEBUG(DefaultChannel, DefaultDevice, &pprz_debug_mod, &pprz_debug_err); \ } \ } #else diff --git a/sw/airborne/rc_settings.c b/sw/airborne/rc_settings.c index 6e597cc6dc..1e880f9ae7 100644 --- a/sw/airborne/rc_settings.c +++ b/sw/airborne/rc_settings.c @@ -35,10 +35,10 @@ uint8_t rc_settings_mode = 0; float slider_1_val, slider_2_val; #define ParamValInt16(param_init_val, param_travel, cur_pulse, init_pulse) \ -(param_init_val + (int16_t)(((float)(cur_pulse - init_pulse)) * param_travel / (float)MAX_PPRZ)) + (param_init_val + (int16_t)(((float)(cur_pulse - init_pulse)) * param_travel / (float)MAX_PPRZ)) #define ParamValFloat(param_init_val, param_travel, cur_pulse, init_pulse) \ -(param_init_val + ((float)(cur_pulse - init_pulse)) * param_travel / (float)MAX_PPRZ) + (param_init_val + ((float)(cur_pulse - init_pulse)) * param_travel / (float)MAX_PPRZ) #define RcChannel(x) (fbw_state->channels[x]) @@ -46,6 +46,7 @@ float slider_1_val, slider_2_val; #include "generated/settings.h" -void rc_settings(bool_t mode_changed __attribute__ ((unused))) { +void rc_settings(bool_t mode_changed __attribute__((unused))) +{ RCSettings(mode_changed); } diff --git a/sw/airborne/rc_settings.h b/sw/airborne/rc_settings.h index 901eb53edb..ee03721831 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/state.c b/sw/airborne/state.c index 3919b64ada..7c6636c37f 100644 --- a/sw/airborne/state.c +++ b/sw/airborne/state.c @@ -40,7 +40,8 @@ struct State state; * @{ */ -void stateInit(void) { +void stateInit(void) +{ state.pos_status = 0; state.speed_status = 0; state.accel_status = 0; @@ -61,32 +62,29 @@ void stateInit(void) { /** @addtogroup state_position * @{ */ -void stateCalcPositionEcef_i(void) { - if (bit_is_set(state.pos_status, POS_ECEF_I)) +void stateCalcPositionEcef_i(void) +{ + if (bit_is_set(state.pos_status, POS_ECEF_I)) { return; + } if (bit_is_set(state.pos_status, POS_ECEF_F)) { ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f); - } - else if (bit_is_set(state.pos_status, POS_NED_I)) { + } else if (bit_is_set(state.pos_status, POS_NED_I)) { ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); - } - else if (bit_is_set(state.pos_status, POS_NED_F)) { + } else if (bit_is_set(state.pos_status, POS_NED_F)) { /* transform ned_f to ecef_f, set status bit, then convert to int */ ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f); SetBit(state.pos_status, POS_ECEF_F); ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f); - } - else if (bit_is_set(state.pos_status, POS_LLA_F)) { + } else if (bit_is_set(state.pos_status, POS_LLA_F)) { /* transform lla_f to ecef_f, set status bit, then convert to int */ ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f); SetBit(state.pos_status, POS_ECEF_F); ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f); - } - else if (bit_is_set(state.pos_status, POS_LLA_I)) { + } else if (bit_is_set(state.pos_status, POS_LLA_I)) { ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); - } - else { + } else { /* could not get this representation, set errno */ //struct EcefCoor_i _ecef_zero = {0}; //return _ecef_zero; @@ -95,74 +93,63 @@ void stateCalcPositionEcef_i(void) { SetBit(state.pos_status, POS_ECEF_I); } -void stateCalcPositionNed_i(void) { - if (bit_is_set(state.pos_status, POS_NED_I)) +void stateCalcPositionNed_i(void) +{ + if (bit_is_set(state.pos_status, POS_NED_I)) { return; + } int errno = 0; if (state.ned_initialized_i) { if (bit_is_set(state.pos_status, POS_NED_F)) { NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); - } - else if (bit_is_set(state.pos_status, POS_ENU_I)) { + } else if (bit_is_set(state.pos_status, POS_ENU_I)) { INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i); - } - else if (bit_is_set(state.pos_status, POS_ENU_F)) { + } else if (bit_is_set(state.pos_status, POS_ENU_F)) { ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f); SetBit(state.pos_status, POS_ENU_I); INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i); - } - else if (bit_is_set(state.pos_status, POS_ECEF_I)) { + } else if (bit_is_set(state.pos_status, POS_ECEF_I)) { ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i); - } - else if (bit_is_set(state.pos_status, POS_ECEF_F)) { + } else if (bit_is_set(state.pos_status, POS_ECEF_F)) { /* transform ecef_f -> ned_f, set status bit, then convert to int */ ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f); SetBit(state.pos_status, POS_NED_F); NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); - } - else if (bit_is_set(state.pos_status, POS_LLA_F)) { + } else if (bit_is_set(state.pos_status, POS_LLA_F)) { /* transform lla_f -> ecef_f -> ned_f, set status bits, then convert to int */ ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f); SetBit(state.pos_status, POS_ECEF_F); ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f); SetBit(state.pos_status, POS_NED_F); NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); - } - else if (bit_is_set(state.pos_status, POS_LLA_I)) { + } else if (bit_is_set(state.pos_status, POS_LLA_I)) { ned_of_lla_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.lla_pos_i); - } - else { /* could not get this representation, set errno */ + } else { /* could not get this representation, set errno */ errno = 1; } - } - else if (state.utm_initialized_f) { + } else if (state.utm_initialized_f) { if (bit_is_set(state.pos_status, POS_NED_F)) { NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); - } - else if (bit_is_set(state.pos_status, POS_ENU_I)) { + } else if (bit_is_set(state.pos_status, POS_ENU_I)) { INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i); - } - else if (bit_is_set(state.pos_status, POS_ENU_F)) { + } else if (bit_is_set(state.pos_status, POS_ENU_F)) { ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f); SetBit(state.pos_status, POS_ENU_I); INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i); - } - else if (bit_is_set(state.pos_status, POS_UTM_F)) { + } else if (bit_is_set(state.pos_status, POS_UTM_F)) { /* transform utm_f -> ned_f -> ned_i, set status bits */ NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f); SetBit(state.pos_status, POS_NED_F); NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); - } - else if (bit_is_set(state.pos_status, POS_LLA_F)) { + } else if (bit_is_set(state.pos_status, POS_LLA_F)) { /* transform lla_f -> utm_f -> ned_f -> ned_i, set status bits */ utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f); SetBit(state.pos_status, POS_UTM_F); NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f); SetBit(state.pos_status, POS_NED_F); NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); - } - else if (bit_is_set(state.pos_status, POS_LLA_I)) { + } else if (bit_is_set(state.pos_status, POS_LLA_I)) { /* transform lla_i -> lla_f -> utm_f -> ned_f -> ned_i, set status bits */ LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i); SetBit(state.pos_status, POS_LLA_F); @@ -171,12 +158,10 @@ void stateCalcPositionNed_i(void) { NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f); SetBit(state.pos_status, POS_NED_F); NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); - } - else { /* could not get this representation, set errno */ + } else { /* could not get this representation, set errno */ errno = 2; } - } - else { /* ned coordinate system not initialized, set errno */ + } else { /* ned coordinate system not initialized, set errno */ errno = 3; } if (errno) { @@ -187,74 +172,63 @@ void stateCalcPositionNed_i(void) { SetBit(state.pos_status, POS_NED_I); } -void stateCalcPositionEnu_i(void) { - if (bit_is_set(state.pos_status, POS_ENU_I)) +void stateCalcPositionEnu_i(void) +{ + if (bit_is_set(state.pos_status, POS_ENU_I)) { return; + } int errno = 0; if (state.ned_initialized_i) { if (bit_is_set(state.pos_status, POS_NED_I)) { INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i); - } - else if (bit_is_set(state.pos_status, POS_ENU_F)) { + } else if (bit_is_set(state.pos_status, POS_ENU_F)) { ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f); - } - else if (bit_is_set(state.pos_status, POS_NED_F)) { + } else if (bit_is_set(state.pos_status, POS_NED_F)) { NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); SetBit(state.pos_status, POS_NED_I); INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i); - } - else if (bit_is_set(state.pos_status, POS_ECEF_I)) { + } else if (bit_is_set(state.pos_status, POS_ECEF_I)) { enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i); - } - else if (bit_is_set(state.pos_status, POS_ECEF_F)) { + } else if (bit_is_set(state.pos_status, POS_ECEF_F)) { /* transform ecef_f -> enu_f, set status bit, then convert to int */ enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f); SetBit(state.pos_status, POS_ENU_F); ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f); - } - else if (bit_is_set(state.pos_status, POS_LLA_F)) { + } else if (bit_is_set(state.pos_status, POS_LLA_F)) { /* transform lla_f -> ecef_f -> enu_f, set status bits, then convert to int */ ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f); SetBit(state.pos_status, POS_ECEF_F); enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f); SetBit(state.pos_status, POS_ENU_F); ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f); - } - else if (bit_is_set(state.pos_status, POS_LLA_I)) { + } else if (bit_is_set(state.pos_status, POS_LLA_I)) { enu_of_lla_point_i(&state.enu_pos_i, &state.ned_origin_i, &state.lla_pos_i); - } - else { /* could not get this representation, set errno */ + } else { /* could not get this representation, set errno */ errno = 1; } - } - else if (state.utm_initialized_f) { + } else if (state.utm_initialized_f) { if (bit_is_set(state.pos_status, POS_ENU_F)) { ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f); - } - else if (bit_is_set(state.pos_status, POS_NED_I)) { + } else if (bit_is_set(state.pos_status, POS_NED_I)) { INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i); - } - else if (bit_is_set(state.pos_status, POS_NED_F)) { + } else if (bit_is_set(state.pos_status, POS_NED_F)) { NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); SetBit(state.pos_status, POS_NED_I); INT32_VECT3_ENU_OF_NED(state.enu_pos_i, state.ned_pos_i); - } - else if (bit_is_set(state.pos_status, POS_UTM_F)) { + } else if (bit_is_set(state.pos_status, POS_UTM_F)) { /* transform utm_f -> enu_f -> enu_i , set status bits */ ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f); SetBit(state.pos_status, POS_ENU_F); ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f); - } - else if (bit_is_set(state.pos_status, POS_LLA_F)) { + } else if (bit_is_set(state.pos_status, POS_LLA_F)) { /* transform lla_f -> utm_f -> enu_f -> enu_i , set status bits */ utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f); SetBit(state.pos_status, POS_UTM_F); ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f); SetBit(state.pos_status, POS_ENU_F); ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f); - } - else if (bit_is_set(state.pos_status, POS_LLA_I)) { + } else if (bit_is_set(state.pos_status, POS_LLA_I)) { /* transform lla_i -> lla_f -> utm_f -> enu_f -> enu_i , set status bits */ LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i); SetBit(state.pos_status, POS_LLA_F); @@ -263,12 +237,10 @@ void stateCalcPositionEnu_i(void) { ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f); SetBit(state.pos_status, POS_ENU_F); ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f); - } - else { /* could not get this representation, set errno */ + } else { /* could not get this representation, set errno */ errno = 2; } - } - else { /* ned coordinate system not initialized, set errno */ + } else { /* ned coordinate system not initialized, set errno */ errno = 3; } if (errno) { @@ -285,57 +257,51 @@ void stateCalcPositionEnu_i(void) { * So we mostly first convert to ECEF and then use lla_of_ecef_i * which provides higher precision but is currently using the double function internally. */ -void stateCalcPositionLla_i(void) { - if (bit_is_set(state.pos_status, POS_LLA_I)) +void stateCalcPositionLla_i(void) +{ + if (bit_is_set(state.pos_status, POS_LLA_I)) { return; + } if (bit_is_set(state.pos_status, POS_ECEF_I)) { lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i); - } - else if (bit_is_set(state.pos_status, POS_ECEF_F)) { + } else if (bit_is_set(state.pos_status, POS_ECEF_F)) { /* transform ecef_f -> ecef_i -> lla_i, set status bits */ ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f); SetBit(state.pos_status, POS_ECEF_I); lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i); - } - else if (bit_is_set(state.pos_status, POS_NED_I)) { + } else if (bit_is_set(state.pos_status, POS_NED_I)) { /* transform ned_i -> ecef_i -> lla_i, set status bits */ ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); SetBit(state.pos_status, POS_ECEF_I); lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i); - } - else if (bit_is_set(state.pos_status, POS_ENU_I)) { + } else if (bit_is_set(state.pos_status, POS_ENU_I)) { /* transform enu_i -> ecef_i -> lla_i, set status bits */ ecef_of_enu_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.enu_pos_i); SetBit(state.pos_status, POS_ECEF_I); lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i); - } - else if (bit_is_set(state.pos_status, POS_NED_F)) { + } else if (bit_is_set(state.pos_status, POS_NED_F)) { /* transform ned_f -> ned_i -> ecef_i -> lla_i, set status bits */ NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); SetBit(state.pos_status, POS_NED_I); ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); SetBit(state.pos_status, POS_ECEF_I); lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i); - } - else if (bit_is_set(state.pos_status, POS_ENU_F)) { + } else if (bit_is_set(state.pos_status, POS_ENU_F)) { /* transform enu_f -> enu_i -> ecef_i -> lla_i, set status bits */ ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f); SetBit(state.pos_status, POS_ENU_I); ecef_of_enu_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.enu_pos_i); SetBit(state.pos_status, POS_ECEF_I); lla_of_ecef_i(&state.lla_pos_i, &state.ecef_pos_i); - } - else if (bit_is_set(state.pos_status, POS_LLA_F)) { + } else if (bit_is_set(state.pos_status, POS_LLA_F)) { LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f); - } - else if (bit_is_set(state.pos_status, POS_UTM_F)) { + } else if (bit_is_set(state.pos_status, POS_UTM_F)) { /* transform utm_f -> lla_f -> lla_i, set status bits */ lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f); SetBit(state.pos_status, POS_LLA_F); LLA_BFP_OF_REAL(state.lla_pos_i, state.lla_pos_f); - } - else { + } else { /* could not get this representation, set errno */ //struct LlaCoor_i _lla_zero = {0}; //return _lla_zero; @@ -344,38 +310,34 @@ void stateCalcPositionLla_i(void) { SetBit(state.pos_status, POS_LLA_I); } -void stateCalcPositionUtm_f(void) { - if (bit_is_set(state.pos_status, POS_UTM_F)) +void stateCalcPositionUtm_f(void) +{ + if (bit_is_set(state.pos_status, POS_UTM_F)) { return; + } if (bit_is_set(state.pos_status, POS_LLA_F)) { utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f); - } - else if (bit_is_set(state.pos_status, POS_LLA_I)) { + } else if (bit_is_set(state.pos_status, POS_LLA_I)) { /* transform lla_i -> lla_f -> utm_f, set status bits */ LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i); SetBit(state.pos_status, POS_LLA_F); utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f); - } - else if (state.utm_initialized_f) { + } else if (state.utm_initialized_f) { if (bit_is_set(state.pos_status, POS_ENU_F)) { UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f); - } - else if (bit_is_set(state.pos_status, POS_ENU_I)) { + } else if (bit_is_set(state.pos_status, POS_ENU_I)) { ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i); SetBit(state.pos_status, POS_ENU_F); UTM_OF_ENU_ADD(state.utm_pos_f, state.enu_pos_f, state.utm_origin_f); - } - else if (bit_is_set(state.pos_status, POS_NED_F)) { + } else if (bit_is_set(state.pos_status, POS_NED_F)) { UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_f); - } - else if (bit_is_set(state.pos_status, POS_NED_I)) { + } else if (bit_is_set(state.pos_status, POS_NED_I)) { NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i); SetBit(state.pos_status, POS_NED_F); UTM_OF_NED_ADD(state.utm_pos_f, state.ned_pos_f, state.utm_origin_f); } - } - else { + } else { /* could not get this representation, set errno */ //struct EcefCoor_f _ecef_zero = {0.0f}; //return _ecef_zero; @@ -384,31 +346,28 @@ void stateCalcPositionUtm_f(void) { SetBit(state.pos_status, POS_UTM_F); } -void stateCalcPositionEcef_f(void) { - if (bit_is_set(state.pos_status, POS_ECEF_F)) +void stateCalcPositionEcef_f(void) +{ + if (bit_is_set(state.pos_status, POS_ECEF_F)) { return; + } if (bit_is_set(state.pos_status, POS_ECEF_I)) { ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i); - } - else if (bit_is_set(state.pos_status, POS_NED_F)) { + } else if (bit_is_set(state.pos_status, POS_NED_F)) { ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f); - } - else if (bit_is_set(state.pos_status, POS_NED_I)) { + } else if (bit_is_set(state.pos_status, POS_NED_I)) { /* transform ned_i -> ecef_i -> ecef_f, set status bits */ ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); SetBit(state.pos_status, POS_ECEF_F); ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i); - } - else if (bit_is_set(state.pos_status, POS_LLA_F)) { + } else if (bit_is_set(state.pos_status, POS_LLA_F)) { ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f); - } - else if (bit_is_set(state.pos_status, POS_LLA_I)) { + } else if (bit_is_set(state.pos_status, POS_LLA_I)) { LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i); SetBit(state.pos_status, POS_LLA_F); ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f); - } - else { + } else { /* could not get this representation, set errno */ //struct EcefCoor_f _ecef_zero = {0.0f}; //return _ecef_zero; @@ -417,73 +376,62 @@ void stateCalcPositionEcef_f(void) { SetBit(state.pos_status, POS_ECEF_F); } -void stateCalcPositionNed_f(void) { - if (bit_is_set(state.pos_status, POS_NED_F)) +void stateCalcPositionNed_f(void) +{ + if (bit_is_set(state.pos_status, POS_NED_F)) { return; + } int errno = 0; if (state.ned_initialized_f) { if (bit_is_set(state.pos_status, POS_NED_I)) { NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i); - } - else if (bit_is_set(state.pos_status, POS_ECEF_F)) { + } else if (bit_is_set(state.pos_status, POS_ECEF_F)) { ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f); - } - else if (bit_is_set(state.pos_status, POS_ECEF_I)) { + } else if (bit_is_set(state.pos_status, POS_ECEF_I)) { /* transform ecef_i -> ned_i -> ned_f, set status bits */ ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i); SetBit(state.pos_status, POS_NED_I); NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i); - } - else if (bit_is_set(state.pos_status, POS_LLA_F)) { + } else if (bit_is_set(state.pos_status, POS_LLA_F)) { ned_of_lla_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.lla_pos_f); - } - else if (bit_is_set(state.pos_status, POS_LLA_I)) { + } else if (bit_is_set(state.pos_status, POS_LLA_I)) { /* transform lla_i -> ecef_i -> ned_i -> ned_f, set status bits */ ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */ SetBit(state.pos_status, POS_ECEF_I); ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i); SetBit(state.pos_status, POS_NED_I); NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i); - } - else { /* could not get this representation, set errno */ + } else { /* could not get this representation, set errno */ errno = 1; } - } - else if (state.utm_initialized_f) { + } else if (state.utm_initialized_f) { if (bit_is_set(state.pos_status, POS_NED_I)) { NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i); - } - else if (bit_is_set(state.pos_status, POS_ENU_I)) { + } else if (bit_is_set(state.pos_status, POS_ENU_I)) { ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i); SetBit(state.pos_status, POS_ENU_F); VECT3_NED_OF_ENU(state.ned_pos_f, state.enu_pos_f); - } - else if (bit_is_set(state.pos_status, POS_ENU_F)) { + } else if (bit_is_set(state.pos_status, POS_ENU_F)) { VECT3_NED_OF_ENU(state.ned_pos_f, state.enu_pos_f); - } - else if (bit_is_set(state.pos_status, POS_UTM_F)) { + } else if (bit_is_set(state.pos_status, POS_UTM_F)) { NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f); - } - else if (bit_is_set(state.pos_status, POS_LLA_F)) { + } else if (bit_is_set(state.pos_status, POS_LLA_F)) { /* transform lla_f -> utm_f -> ned, set status bits */ utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f); SetBit(state.pos_status, POS_UTM_F); NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f); - } - else if (bit_is_set(state.pos_status, POS_LLA_I)) { + } else if (bit_is_set(state.pos_status, POS_LLA_I)) { /* transform lla_i -> lla_f -> utm_f -> ned, set status bits */ LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i); SetBit(state.pos_status, POS_LLA_F); utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f); SetBit(state.pos_status, POS_UTM_F); NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f); - } - else { /* could not get this representation, set errno */ + } else { /* could not get this representation, set errno */ errno = 2; } - } - else { /* ned coordinate system not initialized, set errno */ + } else { /* ned coordinate system not initialized, set errno */ errno = 3; } if (errno) { @@ -494,81 +442,68 @@ void stateCalcPositionNed_f(void) { SetBit(state.pos_status, POS_NED_F); } -void stateCalcPositionEnu_f(void) { - if (bit_is_set(state.pos_status, POS_ENU_F)) +void stateCalcPositionEnu_f(void) +{ + if (bit_is_set(state.pos_status, POS_ENU_F)) { return; + } int errno = 0; if (state.ned_initialized_f) { if (bit_is_set(state.pos_status, POS_NED_F)) { VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f); - } - else if (bit_is_set(state.pos_status, POS_ENU_I)) { + } else if (bit_is_set(state.pos_status, POS_ENU_I)) { ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i); - } - else if (bit_is_set(state.pos_status, POS_NED_I)) { + } else if (bit_is_set(state.pos_status, POS_NED_I)) { NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i); SetBit(state.pos_status, POS_NED_F); VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f); - } - else if (bit_is_set(state.pos_status, POS_ECEF_F)) { + } else if (bit_is_set(state.pos_status, POS_ECEF_F)) { enu_of_ecef_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.ecef_pos_f); - } - else if (bit_is_set(state.pos_status, POS_ECEF_I)) { + } else if (bit_is_set(state.pos_status, POS_ECEF_I)) { /* transform ecef_i -> enu_i -> enu_f, set status bits */ enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i); SetBit(state.pos_status, POS_ENU_I); ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i); - } - else if (bit_is_set(state.pos_status, POS_LLA_F)) { + } else if (bit_is_set(state.pos_status, POS_LLA_F)) { enu_of_lla_point_f(&state.enu_pos_f, &state.ned_origin_f, &state.lla_pos_f); - } - else if (bit_is_set(state.pos_status, POS_LLA_I)) { + } else if (bit_is_set(state.pos_status, POS_LLA_I)) { /* transform lla_i -> ecef_i -> enu_i -> enu_f, set status bits */ ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); /* converts to doubles internally */ SetBit(state.pos_status, POS_ECEF_I); enu_of_ecef_pos_i(&state.enu_pos_i, &state.ned_origin_i, &state.ecef_pos_i); SetBit(state.pos_status, POS_ENU_I); ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i); - } - else { /* could not get this representation, set errno */ + } else { /* could not get this representation, set errno */ errno = 1; } - } - else if (state.utm_initialized_f) { + } else if (state.utm_initialized_f) { if (bit_is_set(state.pos_status, POS_ENU_I)) { ENU_FLOAT_OF_BFP(state.enu_pos_f, state.enu_pos_i); - } - else if (bit_is_set(state.pos_status, POS_NED_F)) { + } else if (bit_is_set(state.pos_status, POS_NED_F)) { VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f); - } - else if (bit_is_set(state.pos_status, POS_NED_I)) { + } else if (bit_is_set(state.pos_status, POS_NED_I)) { NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i); SetBit(state.pos_status, POS_NED_F); VECT3_ENU_OF_NED(state.enu_pos_f, state.ned_pos_f); - } - else if (bit_is_set(state.pos_status, POS_UTM_F)) { + } else if (bit_is_set(state.pos_status, POS_UTM_F)) { ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f); - } - else if (bit_is_set(state.pos_status, POS_LLA_F)) { + } else if (bit_is_set(state.pos_status, POS_LLA_F)) { /* transform lla_f -> utm_f -> enu, set status bits */ utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f); SetBit(state.pos_status, POS_UTM_F); ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f); - } - else if (bit_is_set(state.pos_status, POS_LLA_I)) { + } else if (bit_is_set(state.pos_status, POS_LLA_I)) { /* transform lla_i -> lla_f -> utm_f -> enu, set status bits */ LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i); SetBit(state.pos_status, POS_LLA_F); utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f); SetBit(state.pos_status, POS_UTM_F); ENU_OF_UTM_DIFF(state.enu_pos_f, state.utm_pos_f, state.utm_origin_f); - } - else { /* could not get this representation, set errno */ + } else { /* could not get this representation, set errno */ errno = 2; } - } - else { /* ned coordinate system not initialized, set errno */ + } else { /* ned coordinate system not initialized, set errno */ errno = 3; } if (errno) { @@ -579,40 +514,36 @@ void stateCalcPositionEnu_f(void) { SetBit(state.pos_status, POS_ENU_F); } -void stateCalcPositionLla_f(void) { - if (bit_is_set(state.pos_status, POS_LLA_F)) +void stateCalcPositionLla_f(void) +{ + if (bit_is_set(state.pos_status, POS_LLA_F)) { return; + } if (bit_is_set(state.pos_status, POS_LLA_I)) { LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_f); - } - else if (bit_is_set(state.pos_status, POS_ECEF_F)) { + } else if (bit_is_set(state.pos_status, POS_ECEF_F)) { lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f); - } - else if (bit_is_set(state.pos_status, POS_ECEF_I)) { + } else if (bit_is_set(state.pos_status, POS_ECEF_I)) { /* transform ecef_i -> ecef_f -> lla_f, set status bits */ ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i); SetBit(state.pos_status, POS_ECEF_F); lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f); - } - else if (bit_is_set(state.pos_status, POS_NED_F)) { + } else if (bit_is_set(state.pos_status, POS_NED_F)) { /* transform ned_f -> ecef_f -> lla_f, set status bits */ ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f); SetBit(state.pos_status, POS_ECEF_F); lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f); - } - else if (bit_is_set(state.pos_status, POS_NED_I)) { + } else if (bit_is_set(state.pos_status, POS_NED_I)) { /* transform ned_i -> ned_f -> ecef_f -> lla_f, set status bits */ NED_FLOAT_OF_BFP(state.ned_pos_f, state.ned_pos_i); SetBit(state.pos_status, POS_NED_F); ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f); SetBit(state.pos_status, POS_ECEF_F); lla_of_ecef_f(&state.lla_pos_f, &state.ecef_pos_f); - } - else if (bit_is_set(state.pos_status, POS_UTM_F)) { + } else if (bit_is_set(state.pos_status, POS_UTM_F)) { lla_of_utm_f(&state.lla_pos_f, &state.utm_pos_f); - } - else { + } else { /* could not get this representation, set errno */ //struct LlaCoor_f _lla_zero = {0.0}; //return _lla_zero; @@ -635,53 +566,45 @@ void stateCalcPositionLla_f(void) { * @{ */ /************************ Set functions ****************************/ -void stateCalcSpeedNed_i(void) { - if (bit_is_set(state.speed_status, SPEED_NED_I)) +void stateCalcSpeedNed_i(void) +{ + if (bit_is_set(state.speed_status, SPEED_NED_I)) { return; + } int errno = 0; if (state.ned_initialized_i) { if (bit_is_set(state.speed_status, SPEED_NED_F)) { SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f); - } - else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { + } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { INT32_VECT3_NED_OF_ENU(state.ned_speed_i, state.enu_speed_i); - } - else if (bit_is_set(state.speed_status, SPEED_ENU_F)) { + } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) { SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f); SetBit(state.speed_status, SPEED_ENU_I); INT32_VECT3_NED_OF_ENU(state.ned_speed_i, state.enu_speed_i); - } - else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) { + } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) { ned_of_ecef_vect_i(&state.ned_speed_i, &state.ned_origin_i, &state.ecef_speed_i); - } - else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) { + } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) { /* transform ecef_f -> ecef_i -> ned_i , set status bits */ SPEEDS_BFP_OF_REAL(state.ecef_speed_i, state.ecef_speed_f); SetBit(state.speed_status, SPEED_ECEF_I); ned_of_ecef_vect_i(&state.ned_speed_i, &state.ned_origin_i, &state.ecef_speed_i); - } - else { /* could not get this representation, set errno */ + } else { /* could not get this representation, set errno */ errno = 1; } - } - else if (state.utm_initialized_f) { + } else if (state.utm_initialized_f) { if (bit_is_set(state.speed_status, SPEED_NED_F)) { SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f); - } - else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { + } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { INT32_VECT3_NED_OF_ENU(state.ned_speed_i, state.enu_speed_i); - } - else if (bit_is_set(state.speed_status, SPEED_ENU_F)) { + } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) { SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f); SetBit(state.speed_status, SPEED_ENU_I); INT32_VECT3_NED_OF_ENU(state.ned_speed_i, state.enu_speed_i); - } - else { /* could not get this representation, set errno */ + } else { /* could not get this representation, set errno */ errno = 2; } - } - else { /* ned coordinate system not initialized, set errno */ + } else { /* ned coordinate system not initialized, set errno */ errno = 3; } if (errno) { @@ -692,9 +615,11 @@ void stateCalcSpeedNed_i(void) { SetBit(state.speed_status, SPEED_NED_I); } -void stateCalcSpeedEnu_i(void) { - if (bit_is_set(state.speed_status, SPEED_ENU_I)) +void stateCalcSpeedEnu_i(void) +{ + if (bit_is_set(state.speed_status, SPEED_ENU_I)) { return; + } int errno = 0; if (state.ned_initialized_i) { @@ -703,42 +628,34 @@ void stateCalcSpeedEnu_i(void) { } if (bit_is_set(state.speed_status, SPEED_ENU_F)) { ENU_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f); - } - else if (bit_is_set(state.speed_status, SPEED_NED_F)) { + } else if (bit_is_set(state.speed_status, SPEED_NED_F)) { SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f); SetBit(state.pos_status, SPEED_NED_I); INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i); - } - else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) { + } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) { enu_of_ecef_vect_i(&state.enu_speed_i, &state.ned_origin_i, &state.ecef_speed_i); - } - else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) { + } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) { /* transform ecef_f -> ecef_i -> enu_i , set status bits */ SPEEDS_BFP_OF_REAL(state.ecef_speed_i, state.ecef_speed_f); SetBit(state.speed_status, SPEED_ECEF_I); enu_of_ecef_vect_i(&state.enu_speed_i, &state.ned_origin_i, &state.ecef_speed_i); - } - else { /* could not get this representation, set errno */ + } else { /* could not get this representation, set errno */ errno = 1; } - } - else if (state.utm_initialized_f) { + } else if (state.utm_initialized_f) { if (bit_is_set(state.speed_status, SPEED_NED_I)) { INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i); } if (bit_is_set(state.speed_status, SPEED_ENU_F)) { ENU_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f); - } - else if (bit_is_set(state.speed_status, SPEED_NED_F)) { + } else if (bit_is_set(state.speed_status, SPEED_NED_F)) { SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f); SetBit(state.pos_status, SPEED_NED_I); INT32_VECT3_ENU_OF_NED(state.enu_speed_i, state.ned_speed_i); - } - else { /* could not get this representation, set errno */ + } else { /* could not get this representation, set errno */ errno = 2; } - } - else { /* ned coordinate system not initialized, set errno */ + } else { /* ned coordinate system not initialized, set errno */ errno = 3; } if (errno) { @@ -749,23 +666,22 @@ void stateCalcSpeedEnu_i(void) { SetBit(state.speed_status, SPEED_ENU_I); } -void stateCalcSpeedEcef_i(void) { - if (bit_is_set(state.speed_status, SPEED_ECEF_I)) +void stateCalcSpeedEcef_i(void) +{ + if (bit_is_set(state.speed_status, SPEED_ECEF_I)) { return; + } if (bit_is_set(state.speed_status, SPEED_ECEF_F)) { SPEEDS_BFP_OF_REAL(state.ecef_speed_i, state.ecef_speed_f); - } - else if (bit_is_set(state.speed_status, SPEED_NED_I)) { + } else if (bit_is_set(state.speed_status, SPEED_NED_I)) { ecef_of_ned_vect_i(&state.ecef_speed_i, &state.ned_origin_i, &state.ned_speed_i); - } - else if (bit_is_set(state.speed_status, SPEED_NED_F)) { + } else if (bit_is_set(state.speed_status, SPEED_NED_F)) { /* transform ned_f -> ned_i -> ecef_i , set status bits */ SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f); SetBit(state.speed_status, SPEED_NED_I); ecef_of_ned_vect_i(&state.ecef_speed_i, &state.ned_origin_i, &state.ned_speed_i); - } - else { + } else { /* could not get this representation, set errno */ //struct EcefCoor_i _ecef_zero = {0}; //return _ecef_zero; @@ -774,49 +690,44 @@ void stateCalcSpeedEcef_i(void) { SetBit(state.speed_status, SPEED_ECEF_I); } -void stateCalcHorizontalSpeedNorm_i(void) { - if (bit_is_set(state.speed_status, SPEED_HNORM_I)) +void stateCalcHorizontalSpeedNorm_i(void) +{ + if (bit_is_set(state.speed_status, SPEED_HNORM_I)) { return; + } - if (bit_is_set(state.speed_status, SPEED_HNORM_F)){ + if (bit_is_set(state.speed_status, SPEED_HNORM_F)) { state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f); - } - else if (bit_is_set(state.speed_status, SPEED_NED_I)) { - uint32_t n2 = (state.ned_speed_i.x*state.ned_speed_i.x + - state.ned_speed_i.y*state.ned_speed_i.y) >> INT32_SPEED_FRAC; + } else if (bit_is_set(state.speed_status, SPEED_NED_I)) { + uint32_t n2 = (state.ned_speed_i.x * state.ned_speed_i.x + + state.ned_speed_i.y * state.ned_speed_i.y) >> INT32_SPEED_FRAC; INT32_SQRT(state.h_speed_norm_i, n2); - } - else if (bit_is_set(state.speed_status, SPEED_NED_F)) { + } else if (bit_is_set(state.speed_status, SPEED_NED_F)) { state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f); SetBit(state.speed_status, SPEED_HNORM_F); state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f); - } - else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { - uint32_t n2 = (state.enu_speed_i.x*state.enu_speed_i.x + - state.enu_speed_i.y*state.enu_speed_i.y) >> INT32_SPEED_FRAC; + } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { + uint32_t n2 = (state.enu_speed_i.x * state.enu_speed_i.x + + state.enu_speed_i.y * state.enu_speed_i.y) >> INT32_SPEED_FRAC; INT32_SQRT(state.h_speed_norm_i, n2); - } - else if (bit_is_set(state.speed_status, SPEED_ENU_F)) { + } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) { state.h_speed_norm_f = FLOAT_VECT2_NORM(state.enu_speed_f); SetBit(state.speed_status, SPEED_HNORM_F); state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f); - } - else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) { + } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) { /* transform ecef speed to ned, set status bit, then compute norm */ ned_of_ecef_vect_i(&state.ned_speed_i, &state.ned_origin_i, &state.ecef_speed_i); SetBit(state.speed_status, SPEED_NED_I); - uint32_t n2 = (state.ned_speed_i.x*state.ned_speed_i.x + - state.ned_speed_i.y*state.ned_speed_i.y) >> INT32_SPEED_FRAC; + uint32_t n2 = (state.ned_speed_i.x * state.ned_speed_i.x + + state.ned_speed_i.y * state.ned_speed_i.y) >> INT32_SPEED_FRAC; INT32_SQRT(state.h_speed_norm_i, n2); - } - else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) { + } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) { ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f); SetBit(state.speed_status, SPEED_NED_F); state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f); SetBit(state.speed_status, SPEED_HNORM_F); state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f); - } - else { + } else { //int32_t _norm_zero = 0; //return _norm_zero; } @@ -824,28 +735,26 @@ void stateCalcHorizontalSpeedNorm_i(void) { SetBit(state.speed_status, SPEED_HNORM_I); } -void stateCalcHorizontalSpeedDir_i(void) { - if (bit_is_set(state.speed_status, SPEED_HDIR_I)) +void stateCalcHorizontalSpeedDir_i(void) +{ + if (bit_is_set(state.speed_status, SPEED_HDIR_I)) { return; - - if (bit_is_set(state.speed_status, SPEED_HDIR_F)){ - state.h_speed_dir_i = SPEED_BFP_OF_REAL(state.h_speed_dir_f); } - else if (bit_is_set(state.speed_status, SPEED_NED_I)) { + + if (bit_is_set(state.speed_status, SPEED_HDIR_F)) { + state.h_speed_dir_i = SPEED_BFP_OF_REAL(state.h_speed_dir_f); + } else if (bit_is_set(state.speed_status, SPEED_NED_I)) { state.h_speed_dir_i = int32_atan2(state.ned_speed_i.y, state.ned_speed_i.x); INT32_COURSE_NORMALIZE(state.h_speed_dir_i); - } - else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { + } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { state.h_speed_dir_i = int32_atan2(state.enu_speed_i.x, state.enu_speed_i.y); INT32_COURSE_NORMALIZE(state.h_speed_dir_i); - } - else if (bit_is_set(state.speed_status, SPEED_NED_F)) { + } else if (bit_is_set(state.speed_status, SPEED_NED_F)) { SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f); SetBit(state.speed_status, SPEED_NED_I); state.h_speed_dir_i = int32_atan2(state.ned_speed_i.y, state.ned_speed_i.x); INT32_COURSE_NORMALIZE(state.h_speed_dir_i); - } - else if (bit_is_set(state.speed_status, SPEED_ENU_F)) { + } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) { SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f); SetBit(state.speed_status, SPEED_ENU_I); state.h_speed_dir_i = int32_atan2(state.enu_speed_i.x, state.enu_speed_i.y); @@ -855,53 +764,45 @@ void stateCalcHorizontalSpeedDir_i(void) { SetBit(state.speed_status, SPEED_HDIR_I); } -void stateCalcSpeedNed_f(void) { - if (bit_is_set(state.speed_status, SPEED_NED_F)) +void stateCalcSpeedNed_f(void) +{ + if (bit_is_set(state.speed_status, SPEED_NED_F)) { return; + } int errno = 0; if (state.ned_initialized_f) { if (bit_is_set(state.speed_status, SPEED_NED_I)) { SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i); - } - else if (bit_is_set(state.speed_status, SPEED_ENU_F)) { + } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) { VECT3_NED_OF_ENU(state.ned_speed_f, state.enu_speed_f); - } - else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { + } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i); SetBit(state.speed_status, SPEED_ENU_F); VECT3_NED_OF_ENU(state.ned_speed_f, state.enu_speed_f); - } - else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) { + } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) { ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f); - } - else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) { + } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) { /* transform ecef_i -> ecef_f -> ned_f , set status bits */ SPEEDS_FLOAT_OF_BFP(state.ecef_speed_f, state.ecef_speed_i); SetBit(state.speed_status, SPEED_ECEF_F); ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f); - } - else { /* could not get this representation, set errno */ + } else { /* could not get this representation, set errno */ errno = 1; } - } - else if (state.utm_initialized_f) { + } else if (state.utm_initialized_f) { if (bit_is_set(state.speed_status, SPEED_NED_I)) { SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i); - } - else if (bit_is_set(state.speed_status, SPEED_ENU_F)) { + } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) { VECT3_NED_OF_ENU(state.ned_speed_f, state.enu_speed_f); - } - else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { + } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i); SetBit(state.speed_status, SPEED_ENU_F); VECT3_NED_OF_ENU(state.ned_speed_f, state.enu_speed_f); - } - else { /* could not get this representation, set errno */ + } else { /* could not get this representation, set errno */ errno = 2; } - } - else { /* ned coordinate system not initialized, set errno */ + } else { /* ned coordinate system not initialized, set errno */ errno = 3; } if (errno) { @@ -912,53 +813,45 @@ void stateCalcSpeedNed_f(void) { SetBit(state.speed_status, SPEED_NED_F); } -void stateCalcSpeedEnu_f(void) { - if (bit_is_set(state.speed_status, SPEED_ENU_F)) +void stateCalcSpeedEnu_f(void) +{ + if (bit_is_set(state.speed_status, SPEED_ENU_F)) { return; + } int errno = 0; if (state.ned_initialized_f) { if (bit_is_set(state.speed_status, SPEED_NED_F)) { VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f); - } - else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { + } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { ENU_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i); - } - else if (bit_is_set(state.speed_status, SPEED_NED_I)) { + } else if (bit_is_set(state.speed_status, SPEED_NED_I)) { SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i); SetBit(state.pos_status, SPEED_NED_F); VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f); - } - else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) { + } else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) { enu_of_ecef_vect_f(&state.enu_speed_f, &state.ned_origin_f, &state.ecef_speed_f); - } - else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) { + } else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) { /* transform ecef_I -> ecef_f -> enu_f , set status bits */ SPEEDS_FLOAT_OF_BFP(state.ecef_speed_f, state.ecef_speed_i); SetBit(state.speed_status, SPEED_ECEF_F); enu_of_ecef_vect_f(&state.enu_speed_f, &state.ned_origin_f, &state.ecef_speed_f); - } - else { /* could not get this representation, set errno */ + } else { /* could not get this representation, set errno */ errno = 1; } - } - else if (state.utm_initialized_f) { + } else if (state.utm_initialized_f) { if (bit_is_set(state.speed_status, SPEED_NED_F)) { VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f); - } - else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { + } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { ENU_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i); - } - else if (bit_is_set(state.speed_status, SPEED_NED_I)) { + } else if (bit_is_set(state.speed_status, SPEED_NED_I)) { SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i); SetBit(state.pos_status, SPEED_NED_F); VECT3_ENU_OF_NED(state.enu_speed_f, state.ned_speed_f); - } - else { /* could not get this representation, set errno */ + } else { /* could not get this representation, set errno */ errno = 2; } - } - else { /* ned coordinate system not initialized, set errno */ + } else { /* ned coordinate system not initialized, set errno */ errno = 3; } if (errno) { @@ -969,23 +862,22 @@ void stateCalcSpeedEnu_f(void) { SetBit(state.speed_status, SPEED_ENU_F); } -void stateCalcSpeedEcef_f(void) { - if (bit_is_set(state.speed_status, SPEED_ECEF_F)) +void stateCalcSpeedEcef_f(void) +{ + if (bit_is_set(state.speed_status, SPEED_ECEF_F)) { return; + } if (bit_is_set(state.speed_status, SPEED_ECEF_I)) { SPEEDS_FLOAT_OF_BFP(state.ecef_speed_f, state.ned_speed_i); - } - else if (bit_is_set(state.speed_status, SPEED_NED_F)) { + } else if (bit_is_set(state.speed_status, SPEED_NED_F)) { ecef_of_ned_vect_f(&state.ecef_speed_f, &state.ned_origin_f, &state.ned_speed_f); - } - else if (bit_is_set(state.speed_status, SPEED_NED_I)) { + } else if (bit_is_set(state.speed_status, SPEED_NED_I)) { /* transform ned_f -> ned_i -> ecef_i , set status bits */ SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i); SetBit(state.speed_status, SPEED_NED_F); ecef_of_ned_vect_f(&state.ecef_speed_f, &state.ned_origin_f, &state.ned_speed_f); - } - else { + } else { /* could not get this representation, set errno */ //struct EcefCoor_f _ecef_zero = {0.0f}; //return _ecef_zero; @@ -994,25 +886,23 @@ void stateCalcSpeedEcef_f(void) { SetBit(state.speed_status, SPEED_ECEF_F); } -void stateCalcHorizontalSpeedNorm_f(void) { - if (bit_is_set(state.speed_status, SPEED_HNORM_F)) +void stateCalcHorizontalSpeedNorm_f(void) +{ + if (bit_is_set(state.speed_status, SPEED_HNORM_F)) { return; + } - if (bit_is_set(state.speed_status, SPEED_HNORM_I)){ + if (bit_is_set(state.speed_status, SPEED_HNORM_I)) { state.h_speed_norm_f = SPEED_FLOAT_OF_BFP(state.h_speed_norm_i); - } - else if (bit_is_set(state.speed_status, SPEED_NED_F)) { + } else if (bit_is_set(state.speed_status, SPEED_NED_F)) { state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f); - } - else if (bit_is_set(state.speed_status, SPEED_ENU_F)) { + } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) { state.h_speed_norm_f = FLOAT_VECT2_NORM(state.enu_speed_f); - } - else if (bit_is_set(state.speed_status, SPEED_NED_I)) { + } else if (bit_is_set(state.speed_status, SPEED_NED_I)) { SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i); SetBit(state.speed_status, SPEED_NED_F); state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f); - } - else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { + } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i); SetBit(state.speed_status, SPEED_ENU_F); state.h_speed_norm_f = FLOAT_VECT2_NORM(state.enu_speed_f); @@ -1021,25 +911,23 @@ void stateCalcHorizontalSpeedNorm_f(void) { SetBit(state.speed_status, SPEED_HNORM_F); } -void stateCalcHorizontalSpeedDir_f(void) { - if (bit_is_set(state.speed_status, SPEED_HDIR_F)) +void stateCalcHorizontalSpeedDir_f(void) +{ + if (bit_is_set(state.speed_status, SPEED_HDIR_F)) { return; + } - if (bit_is_set(state.speed_status, SPEED_HDIR_I)){ + if (bit_is_set(state.speed_status, SPEED_HDIR_I)) { state.h_speed_dir_f = SPEED_FLOAT_OF_BFP(state.h_speed_dir_i); - } - else if (bit_is_set(state.speed_status, SPEED_NED_F)) { + } else if (bit_is_set(state.speed_status, SPEED_NED_F)) { state.h_speed_dir_f = atan2f(state.ned_speed_f.y, state.ned_speed_f.x); - } - else if (bit_is_set(state.speed_status, SPEED_ENU_F)) { + } else if (bit_is_set(state.speed_status, SPEED_ENU_F)) { state.h_speed_dir_f = atan2f(state.enu_speed_f.x, state.enu_speed_f.y); - } - else if (bit_is_set(state.speed_status, SPEED_NED_I)) { + } else if (bit_is_set(state.speed_status, SPEED_NED_I)) { SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i); SetBit(state.speed_status, SPEED_NED_F); state.h_speed_dir_f = atan2f(state.ned_speed_f.y, state.ned_speed_f.x); - } - else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { + } else if (bit_is_set(state.speed_status, SPEED_ENU_I)) { SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i); SetBit(state.speed_status, SPEED_ENU_F); state.h_speed_dir_f = atan2f(state.enu_speed_f.x, state.enu_speed_f.y); @@ -1059,25 +947,24 @@ void stateCalcHorizontalSpeedDir_f(void) { /** @addtogroup state_acceleration * @{ */ -void stateCalcAccelNed_i(void) { - if (bit_is_set(state.accel_status, ACCEL_NED_I)) +void stateCalcAccelNed_i(void) +{ + if (bit_is_set(state.accel_status, ACCEL_NED_I)) { return; + } int errno = 0; if (state.ned_initialized_i) { if (bit_is_set(state.accel_status, ACCEL_NED_F)) { ACCELS_BFP_OF_REAL(state.ned_accel_i, state.ned_accel_f); - } - else if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) { + } else if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) { ned_of_ecef_vect_i(&state.ned_accel_i, &state.ned_origin_i, &state.ecef_accel_i); - } - else if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) { + } else if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) { /* transform ecef_f -> ecef_i -> ned_i , set status bits */ ACCELS_BFP_OF_REAL(state.ecef_accel_i, state.ecef_accel_f); SetBit(state.accel_status, ACCEL_ECEF_I); ned_of_ecef_vect_i(&state.ned_accel_i, &state.ned_origin_i, &state.ecef_accel_i); - } - else { /* could not get this representation, set errno */ + } else { /* could not get this representation, set errno */ errno = 1; } } else { /* ned coordinate system not initialized, set errno */ @@ -1091,23 +978,22 @@ void stateCalcAccelNed_i(void) { SetBit(state.accel_status, ACCEL_NED_I); } -void stateCalcAccelEcef_i(void) { - if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) +void stateCalcAccelEcef_i(void) +{ + if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) { return; + } if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) { ACCELS_BFP_OF_REAL(state.ecef_accel_i, state.ecef_accel_f); - } - else if (bit_is_set(state.accel_status, ACCEL_NED_I)) { + } else if (bit_is_set(state.accel_status, ACCEL_NED_I)) { ecef_of_ned_vect_i(&state.ecef_accel_i, &state.ned_origin_i, &state.ned_accel_i); - } - else if (bit_is_set(state.accel_status, ACCEL_NED_F)) { + } else if (bit_is_set(state.accel_status, ACCEL_NED_F)) { /* transform ned_f -> ned_i -> ecef_i , set status bits */ ACCELS_BFP_OF_REAL(state.ned_accel_i, state.ned_accel_f); SetBit(state.accel_status, ACCEL_NED_I); ecef_of_ned_vect_i(&state.ecef_accel_i, &state.ned_origin_i, &state.ned_accel_i); - } - else { + } else { /* could not get this representation, set errno */ //struct EcefCoor_i _ecef_zero = {0}; //return _ecef_zero; @@ -1116,25 +1002,24 @@ void stateCalcAccelEcef_i(void) { SetBit(state.accel_status, ACCEL_ECEF_I); } -void stateCalcAccelNed_f(void) { - if (bit_is_set(state.accel_status, ACCEL_NED_F)) +void stateCalcAccelNed_f(void) +{ + if (bit_is_set(state.accel_status, ACCEL_NED_F)) { return; + } int errno = 0; if (state.ned_initialized_f) { if (bit_is_set(state.accel_status, ACCEL_NED_I)) { ACCELS_FLOAT_OF_BFP(state.ned_accel_f, state.ned_accel_i); - } - else if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) { + } else if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) { ned_of_ecef_vect_f(&state.ned_accel_f, &state.ned_origin_f, &state.ecef_accel_f); - } - else if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) { + } else if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) { /* transform ecef_i -> ecef_f -> ned_f , set status bits */ ACCELS_FLOAT_OF_BFP(state.ecef_accel_f, state.ecef_accel_i); SetBit(state.accel_status, ACCEL_ECEF_F); ned_of_ecef_vect_f(&state.ned_accel_f, &state.ned_origin_f, &state.ecef_accel_f); - } - else { /* could not get this representation, set errno */ + } else { /* could not get this representation, set errno */ errno = 1; } } else { /* ned coordinate system not initialized, set errno */ @@ -1148,23 +1033,22 @@ void stateCalcAccelNed_f(void) { SetBit(state.accel_status, ACCEL_NED_F); } -void stateCalcAccelEcef_f(void) { - if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) +void stateCalcAccelEcef_f(void) +{ + if (bit_is_set(state.accel_status, ACCEL_ECEF_F)) { return; + } if (bit_is_set(state.accel_status, ACCEL_ECEF_I)) { ACCELS_FLOAT_OF_BFP(state.ecef_accel_f, state.ned_accel_i); - } - else if (bit_is_set(state.accel_status, ACCEL_NED_F)) { + } else if (bit_is_set(state.accel_status, ACCEL_NED_F)) { ecef_of_ned_vect_f(&state.ecef_accel_f, &state.ned_origin_f, &state.ned_accel_f); - } - else if (bit_is_set(state.accel_status, ACCEL_NED_I)) { + } else if (bit_is_set(state.accel_status, ACCEL_NED_I)) { /* transform ned_f -> ned_i -> ecef_i , set status bits */ ACCELS_FLOAT_OF_BFP(state.ned_accel_f, state.ned_accel_i); SetBit(state.accel_status, ACCEL_NED_F); ecef_of_ned_vect_f(&state.ecef_accel_f, &state.ned_origin_f, &state.ned_accel_f); - } - else { + } else { /* could not get this representation, set errno */ //struct EcefCoor_f _ecef_zero = {0.0f}; //return _ecef_zero; @@ -1182,9 +1066,11 @@ void stateCalcAccelEcef_f(void) { /** @addtogroup state_rate * @{ */ -void stateCalcBodyRates_i(void) { - if (bit_is_set(state.rate_status, RATE_I)) +void stateCalcBodyRates_i(void) +{ + if (bit_is_set(state.rate_status, RATE_I)) { return; + } if (bit_is_set(state.rate_status, RATE_F)) { RATES_BFP_OF_REAL(state.body_rates_i, state.body_rates_f); @@ -1193,9 +1079,11 @@ void stateCalcBodyRates_i(void) { SetBit(state.rate_status, RATE_I); } -void stateCalcBodyRates_f(void) { - if (bit_is_set(state.rate_status, RATE_F)) +void stateCalcBodyRates_f(void) +{ + if (bit_is_set(state.rate_status, RATE_F)) { return; + } if (bit_is_set(state.rate_status, RATE_I)) { RATES_FLOAT_OF_BFP(state.body_rates_f, state.body_rates_i); @@ -1215,9 +1103,11 @@ void stateCalcBodyRates_f(void) { /** @addtogroup state_wind_airspeed * @{ */ -void stateCalcHorizontalWindspeed_i(void) { - if (bit_is_set(state.wind_air_status, WINDSPEED_I)) +void stateCalcHorizontalWindspeed_i(void) +{ + if (bit_is_set(state.wind_air_status, WINDSPEED_I)) { return; + } if (bit_is_set(state.wind_air_status, WINDSPEED_F)) { state.h_windspeed_i.x = SPEED_BFP_OF_REAL(state.h_windspeed_f.x); @@ -1227,9 +1117,11 @@ void stateCalcHorizontalWindspeed_i(void) { SetBit(state.rate_status, WINDSPEED_I); } -void stateCalcAirspeed_i(void) { - if (bit_is_set(state.wind_air_status, AIRSPEED_I)) +void stateCalcAirspeed_i(void) +{ + if (bit_is_set(state.wind_air_status, AIRSPEED_I)) { return; + } if (bit_is_set(state.wind_air_status, AIRSPEED_F)) { state.airspeed_i = SPEED_BFP_OF_REAL(state.airspeed_f); @@ -1238,9 +1130,11 @@ void stateCalcAirspeed_i(void) { SetBit(state.wind_air_status, AIRSPEED_I); } -void stateCalcHorizontalWindspeed_f(void) { - if (bit_is_set(state.wind_air_status, WINDSPEED_F)) +void stateCalcHorizontalWindspeed_f(void) +{ + if (bit_is_set(state.wind_air_status, WINDSPEED_F)) { return; + } if (bit_is_set(state.wind_air_status, WINDSPEED_I)) { state.h_windspeed_f.x = SPEED_FLOAT_OF_BFP(state.h_windspeed_i.x); @@ -1250,9 +1144,11 @@ void stateCalcHorizontalWindspeed_f(void) { SetBit(state.rate_status, WINDSPEED_F); } -void stateCalcAirspeed_f(void) { - if (bit_is_set(state.wind_air_status, AIRSPEED_F)) +void stateCalcAirspeed_f(void) +{ + if (bit_is_set(state.wind_air_status, AIRSPEED_F)) { return; + } if (bit_is_set(state.wind_air_status, AIRSPEED_I)) { state.airspeed_f = SPEED_FLOAT_OF_BFP(state.airspeed_i); diff --git a/sw/airborne/state.h b/sw/airborne/state.h index f6c1273f42..87da9f9fa1 100644 --- a/sw/airborne/state.h +++ b/sw/airborne/state.h @@ -437,7 +437,8 @@ extern void stateInit(void); * @{ */ /// Set the local (flat earth) coordinate frame origin (int). -static inline void stateSetLocalOrigin_i(struct LtpDef_i* ltp_def) { +static inline void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def) +{ memcpy(&state.ned_origin_i, ltp_def, sizeof(struct LtpDef_i)); /* convert to float */ ECEF_FLOAT_OF_BFP(state.ned_origin_f.ecef, state.ned_origin_i.ecef); @@ -456,7 +457,8 @@ static inline void stateSetLocalOrigin_i(struct LtpDef_i* ltp_def) { } /// Set the local (flat earth) coordinate frame origin from UTM (float). -static inline void stateSetLocalUtmOrigin_f(struct UtmCoor_f* utm_def) { +static inline void stateSetLocalUtmOrigin_f(struct UtmCoor_f *utm_def) +{ memcpy(&state.utm_origin_f, utm_def, sizeof(struct UtmCoor_f)); state.utm_initialized_f = TRUE; @@ -486,40 +488,47 @@ extern void stateCalcPositionLla_f(void); /*********************** validity test functions ******************/ /// Test if local coordinates are valid. -static inline bool_t stateIsLocalCoordinateValid(void) { - return ((state.ned_initialized_i || state.ned_initialized_f || state.utm_initialized_f) && (state.pos_status & (POS_LOCAL_COORD))); +static inline bool_t stateIsLocalCoordinateValid(void) +{ + return ((state.ned_initialized_i || state.ned_initialized_f || state.utm_initialized_f) + && (state.pos_status & (POS_LOCAL_COORD))); } /// Test if global coordinates are valid. -static inline bool_t stateIsGlobalCoordinateValid(void) { +static inline bool_t stateIsGlobalCoordinateValid(void) +{ return ((state.pos_status & (POS_GLOBAL_COORD)) || stateIsLocalCoordinateValid()); } /************************ Set functions ****************************/ /// Set position from ECEF coordinates (int). -static inline void stateSetPositionEcef_i(struct EcefCoor_i* ecef_pos) { +static inline void stateSetPositionEcef_i(struct EcefCoor_i *ecef_pos) +{ VECT3_COPY(state.ecef_pos_i, *ecef_pos); /* clear bits for all position representations and only set the new one */ state.pos_status = (1 << POS_ECEF_I); } /// Set position from local NED coordinates (int). -static inline void stateSetPositionNed_i(struct NedCoor_i* ned_pos) { +static inline void stateSetPositionNed_i(struct NedCoor_i *ned_pos) +{ VECT3_COPY(state.ned_pos_i, *ned_pos); /* clear bits for all position representations and only set the new one */ state.pos_status = (1 << POS_NED_I); } /// Set position from local ENU coordinates (int). -static inline void stateSetPositionEnu_i(struct EnuCoor_i* enu_pos) { +static inline void stateSetPositionEnu_i(struct EnuCoor_i *enu_pos) +{ VECT3_COPY(state.enu_pos_i, *enu_pos); /* clear bits for all position representations and only set the new one */ state.pos_status = (1 << POS_ENU_I); } /// Set position from LLA coordinates (int). -static inline void stateSetPositionLla_i(struct LlaCoor_i* lla_pos) { +static inline void stateSetPositionLla_i(struct LlaCoor_i *lla_pos) +{ LLA_COPY(state.lla_pos_i, *lla_pos); /* clear bits for all position representations and only set the new one */ state.pos_status = (1 << POS_LLA_I); @@ -527,10 +536,11 @@ static inline void stateSetPositionLla_i(struct LlaCoor_i* lla_pos) { /// Set multiple position coordinates (int). static inline void stateSetPosition_i( - struct EcefCoor_i* ecef_pos, - struct NedCoor_i* ned_pos, - struct EnuCoor_i* enu_pos, - struct LlaCoor_i* lla_pos) { + struct EcefCoor_i *ecef_pos, + struct NedCoor_i *ned_pos, + struct EnuCoor_i *enu_pos, + struct LlaCoor_i *lla_pos) +{ /* clear all status bit */ state.pos_status = 0; if (ecef_pos != NULL) { @@ -552,35 +562,40 @@ static inline void stateSetPosition_i( } /// Set position from UTM coordinates (float). -static inline void stateSetPositionUtm_f(struct UtmCoor_f* utm_pos) { +static inline void stateSetPositionUtm_f(struct UtmCoor_f *utm_pos) +{ memcpy(&state.utm_pos_f, utm_pos, sizeof(struct UtmCoor_f)); /* clear bits for all position representations and only set the new one */ state.pos_status = (1 << POS_UTM_F); } /// Set position from ECEF coordinates (float). -static inline void stateSetPositionEcef_f(struct EcefCoor_f* ecef_pos) { +static inline void stateSetPositionEcef_f(struct EcefCoor_f *ecef_pos) +{ VECT3_COPY(state.ecef_pos_f, *ecef_pos); /* clear bits for all position representations and only set the new one */ state.pos_status = (1 << POS_ECEF_F); } /// Set position from local NED coordinates (float). -static inline void stateSetPositionNed_f(struct NedCoor_f* ned_pos) { +static inline void stateSetPositionNed_f(struct NedCoor_f *ned_pos) +{ VECT3_COPY(state.ned_pos_f, *ned_pos); /* clear bits for all position representations and only set the new one */ state.pos_status = (1 << POS_NED_F); } /// Set position from local ENU coordinates (float). -static inline void stateSetPositionEnu_f(struct EnuCoor_f* enu_pos) { +static inline void stateSetPositionEnu_f(struct EnuCoor_f *enu_pos) +{ VECT3_COPY(state.enu_pos_f, *enu_pos); /* clear bits for all position representations and only set the new one */ state.pos_status = (1 << POS_ENU_F); } /// Set position from LLA coordinates (float). -static inline void stateSetPositionLla_f(struct LlaCoor_f* lla_pos) { +static inline void stateSetPositionLla_f(struct LlaCoor_f *lla_pos) +{ LLA_COPY(state.lla_pos_f, *lla_pos); /* clear bits for all position representations and only set the new one */ state.pos_status = (1 << POS_LLA_F); @@ -588,11 +603,12 @@ static inline void stateSetPositionLla_f(struct LlaCoor_f* lla_pos) { /// Set multiple position coordinates (float). static inline void stateSetPosition_f( - struct EcefCoor_f* ecef_pos, - struct NedCoor_f* ned_pos, - struct EnuCoor_f* enu_pos, - struct LlaCoor_f* lla_pos, - struct UtmCoor_f* utm_pos) { + struct EcefCoor_f *ecef_pos, + struct NedCoor_f *ned_pos, + struct EnuCoor_f *enu_pos, + struct LlaCoor_f *lla_pos, + struct UtmCoor_f *utm_pos) +{ /* clear all status bit */ state.pos_status = 0; if (ecef_pos != NULL) { @@ -620,65 +636,83 @@ static inline void stateSetPosition_f( /************************ Get functions ****************************/ /// Get position in ECEF coordinates (int). -static inline struct EcefCoor_i* stateGetPositionEcef_i(void) { - if (!bit_is_set(state.pos_status, POS_ECEF_I)) +static inline struct EcefCoor_i *stateGetPositionEcef_i(void) +{ + if (!bit_is_set(state.pos_status, POS_ECEF_I)) { stateCalcPositionEcef_i(); + } return &state.ecef_pos_i; } /// Get position in local NED coordinates (int). -static inline struct NedCoor_i* stateGetPositionNed_i(void) { - if (!bit_is_set(state.pos_status, POS_NED_I)) +static inline struct NedCoor_i *stateGetPositionNed_i(void) +{ + if (!bit_is_set(state.pos_status, POS_NED_I)) { stateCalcPositionNed_i(); + } return &state.ned_pos_i; } /// Get position in local ENU coordinates (int). -static inline struct EnuCoor_i* stateGetPositionEnu_i(void) { - if (!bit_is_set(state.pos_status, POS_ENU_I)) +static inline struct EnuCoor_i *stateGetPositionEnu_i(void) +{ + if (!bit_is_set(state.pos_status, POS_ENU_I)) { stateCalcPositionEnu_i(); + } return &state.enu_pos_i; } /// Get position in LLA coordinates (int). -static inline struct LlaCoor_i* stateGetPositionLla_i(void) { - if (!bit_is_set(state.pos_status, POS_LLA_I)) +static inline struct LlaCoor_i *stateGetPositionLla_i(void) +{ + if (!bit_is_set(state.pos_status, POS_LLA_I)) { stateCalcPositionLla_i(); + } return &state.lla_pos_i; } /// Get position in UTM coordinates (float). -static inline struct UtmCoor_f* stateGetPositionUtm_f(void) { - if (!bit_is_set(state.pos_status, POS_UTM_F)) +static inline struct UtmCoor_f *stateGetPositionUtm_f(void) +{ + if (!bit_is_set(state.pos_status, POS_UTM_F)) { stateCalcPositionUtm_f(); + } return &state.utm_pos_f; } /// Get position in ECEF coordinates (float). -static inline struct EcefCoor_f* stateGetPositionEcef_f(void) { - if (!bit_is_set(state.pos_status, POS_ECEF_F)) +static inline struct EcefCoor_f *stateGetPositionEcef_f(void) +{ + if (!bit_is_set(state.pos_status, POS_ECEF_F)) { stateCalcPositionEcef_f(); + } return &state.ecef_pos_f; } /// Get position in local NED coordinates (float). -static inline struct NedCoor_f* stateGetPositionNed_f(void) { - if (!bit_is_set(state.pos_status, POS_NED_F)) +static inline struct NedCoor_f *stateGetPositionNed_f(void) +{ + if (!bit_is_set(state.pos_status, POS_NED_F)) { stateCalcPositionNed_f(); + } return &state.ned_pos_f; } /// Get position in local ENU coordinates (float). -static inline struct EnuCoor_f* stateGetPositionEnu_f(void) { - if (!bit_is_set(state.pos_status, POS_ENU_F)) +static inline struct EnuCoor_f *stateGetPositionEnu_f(void) +{ + if (!bit_is_set(state.pos_status, POS_ENU_F)) { stateCalcPositionEnu_f(); + } return &state.enu_pos_f; } /// Get position in LLA coordinates (float). -static inline struct LlaCoor_f* stateGetPositionLla_f(void) { - if (!bit_is_set(state.pos_status, POS_LLA_F)) +static inline struct LlaCoor_f *stateGetPositionLla_f(void) +{ + if (!bit_is_set(state.pos_status, POS_LLA_F)) { stateCalcPositionLla_f(); + } return &state.lla_pos_f; } @@ -709,21 +743,24 @@ extern void stateCalcHorizontalSpeedDir_f(void); /************************ Set functions ****************************/ /// Set ground speed in local NED coordinates (int). -static inline void stateSetSpeedNed_i(struct NedCoor_i* ned_speed) { +static inline void stateSetSpeedNed_i(struct NedCoor_i *ned_speed) +{ VECT3_COPY(state.ned_speed_i, *ned_speed); /* clear bits for all speed representations and only set the new one */ state.speed_status = (1 << SPEED_NED_I); } /// Set ground speed in local ENU coordinates (int). -static inline void stateSetSpeedEnu_i(struct EnuCoor_i* enu_speed) { +static inline void stateSetSpeedEnu_i(struct EnuCoor_i *enu_speed) +{ VECT3_COPY(state.enu_speed_i, *enu_speed); /* clear bits for all speed representations and only set the new one */ state.speed_status = (1 << SPEED_ENU_I); } /// Set ground speed in ECEF coordinates (int). -static inline void stateSetSpeedEcef_i(struct EcefCoor_i* ecef_speed) { +static inline void stateSetSpeedEcef_i(struct EcefCoor_i *ecef_speed) +{ VECT3_COPY(state.ecef_speed_i, *ecef_speed); /* clear bits for all speed representations and only set the new one */ state.speed_status = (1 << SPEED_ECEF_I); @@ -731,9 +768,10 @@ static inline void stateSetSpeedEcef_i(struct EcefCoor_i* ecef_speed) { /// Set multiple speed coordinates (int). static inline void stateSetSpeed_i( - struct EcefCoor_i* ecef_speed, - struct NedCoor_i* ned_speed, - struct EnuCoor_i* enu_speed) { + struct EcefCoor_i *ecef_speed, + struct NedCoor_i *ned_speed, + struct EnuCoor_i *enu_speed) +{ /* clear all status bit */ state.speed_status = 0; if (ecef_speed != NULL) { @@ -751,21 +789,24 @@ static inline void stateSetSpeed_i( } /// Set ground speed in local NED coordinates (float). -static inline void stateSetSpeedNed_f(struct NedCoor_f* ned_speed) { +static inline void stateSetSpeedNed_f(struct NedCoor_f *ned_speed) +{ VECT3_COPY(state.ned_speed_f, *ned_speed); /* clear bits for all speed representations and only set the new one */ state.speed_status = (1 << SPEED_NED_F); } /// Set ground speed in local ENU coordinates (float). -static inline void stateSetSpeedEnu_f(struct EnuCoor_f* enu_speed) { +static inline void stateSetSpeedEnu_f(struct EnuCoor_f *enu_speed) +{ VECT3_COPY(state.enu_speed_f, *enu_speed); /* clear bits for all speed representations and only set the new one */ state.speed_status = (1 << SPEED_ENU_F); } /// Set ground speed in ECEF coordinates (float). -static inline void stateSetSpeedEcef_f(struct EcefCoor_f* ecef_speed) { +static inline void stateSetSpeedEcef_f(struct EcefCoor_f *ecef_speed) +{ VECT3_COPY(state.ecef_speed_f, *ecef_speed); /* clear bits for all speed representations and only set the new one */ state.speed_status = (1 << SPEED_ECEF_F); @@ -773,9 +814,10 @@ static inline void stateSetSpeedEcef_f(struct EcefCoor_f* ecef_speed) { /// Set multiple speed coordinates (float). static inline void stateSetSpeed_f( - struct EcefCoor_f* ecef_speed, - struct NedCoor_f* ned_speed, - struct EnuCoor_f* enu_speed) { + struct EcefCoor_f *ecef_speed, + struct NedCoor_f *ned_speed, + struct EnuCoor_f *enu_speed) +{ /* clear all status bit */ state.speed_status = 0; if (ecef_speed != NULL) { @@ -795,72 +837,92 @@ static inline void stateSetSpeed_f( /************************ Get functions ****************************/ /// Get ground speed in local NED coordinates (int). -static inline struct NedCoor_i* stateGetSpeedNed_i(void) { - if (!bit_is_set(state.speed_status, SPEED_NED_I)) +static inline struct NedCoor_i *stateGetSpeedNed_i(void) +{ + if (!bit_is_set(state.speed_status, SPEED_NED_I)) { stateCalcSpeedNed_i(); + } return &state.ned_speed_i; } /// Get ground speed in local ENU coordinates (int). -static inline struct EnuCoor_i* stateGetSpeedEnu_i(void) { - if (!bit_is_set(state.speed_status, SPEED_ENU_I)) +static inline struct EnuCoor_i *stateGetSpeedEnu_i(void) +{ + if (!bit_is_set(state.speed_status, SPEED_ENU_I)) { stateCalcSpeedEnu_i(); + } return &state.enu_speed_i; } /// Get ground speed in ECEF coordinates (int). -static inline struct EcefCoor_i* stateGetSpeedEcef_i(void) { - if (!bit_is_set(state.speed_status, SPEED_ECEF_I)) +static inline struct EcefCoor_i *stateGetSpeedEcef_i(void) +{ + if (!bit_is_set(state.speed_status, SPEED_ECEF_I)) { stateCalcSpeedEcef_i(); + } return &state.ecef_speed_i; } /// Get norm of horizontal ground speed (int). -static inline uint32_t* stateGetHorizontalSpeedNorm_i(void) { - if (!bit_is_set(state.speed_status, SPEED_HNORM_I)) +static inline uint32_t *stateGetHorizontalSpeedNorm_i(void) +{ + if (!bit_is_set(state.speed_status, SPEED_HNORM_I)) { stateCalcHorizontalSpeedNorm_i(); + } return &state.h_speed_norm_i; } /// Get dir of horizontal ground speed (int). -static inline int32_t* stateGetHorizontalSpeedDir_i(void) { - if (!bit_is_set(state.speed_status, SPEED_HDIR_I)) +static inline int32_t *stateGetHorizontalSpeedDir_i(void) +{ + if (!bit_is_set(state.speed_status, SPEED_HDIR_I)) { stateCalcHorizontalSpeedDir_i(); + } return &state.h_speed_dir_i; } /// Get ground speed in local NED coordinates (float). -static inline struct NedCoor_f* stateGetSpeedNed_f(void) { - if (!bit_is_set(state.speed_status, SPEED_NED_F)) +static inline struct NedCoor_f *stateGetSpeedNed_f(void) +{ + if (!bit_is_set(state.speed_status, SPEED_NED_F)) { stateCalcSpeedNed_f(); + } return &state.ned_speed_f; } /// Get ground speed in local ENU coordinates (float). -static inline struct EnuCoor_f* stateGetSpeedEnu_f(void) { - if (!bit_is_set(state.speed_status, SPEED_ENU_F)) +static inline struct EnuCoor_f *stateGetSpeedEnu_f(void) +{ + if (!bit_is_set(state.speed_status, SPEED_ENU_F)) { stateCalcSpeedEnu_f(); + } return &state.enu_speed_f; } /// Get ground speed in ECEF coordinates (float). -static inline struct EcefCoor_f* stateGetSpeedEcef_f(void) { - if (!bit_is_set(state.speed_status, SPEED_ECEF_F)) +static inline struct EcefCoor_f *stateGetSpeedEcef_f(void) +{ + if (!bit_is_set(state.speed_status, SPEED_ECEF_F)) { stateCalcSpeedEcef_f(); + } return &state.ecef_speed_f; } /// Get norm of horizontal ground speed (float). -static inline float* stateGetHorizontalSpeedNorm_f(void) { - if (!bit_is_set(state.speed_status, SPEED_HNORM_F)) +static inline float *stateGetHorizontalSpeedNorm_f(void) +{ + if (!bit_is_set(state.speed_status, SPEED_HNORM_F)) { stateCalcHorizontalSpeedNorm_f(); + } return &state.h_speed_norm_f; } /// Get dir of horizontal ground speed (float). -static inline float* stateGetHorizontalSpeedDir_f(void) { - if (!bit_is_set(state.speed_status, SPEED_HDIR_F)) +static inline float *stateGetHorizontalSpeedDir_f(void) +{ + if (!bit_is_set(state.speed_status, SPEED_HDIR_F)) { stateCalcHorizontalSpeedDir_f(); + } return &state.h_speed_dir_f; } /** @}*/ @@ -884,35 +946,40 @@ extern void stateCalcAccelEcef_f(void); /*********************** validity test functions ******************/ /// Test if accelerations are valid. -static inline bool_t stateIsAccelValid(void) { +static inline bool_t stateIsAccelValid(void) +{ return (state.accel_status); } /************************ Set functions ****************************/ /// Set acceleration in NED coordinates (int). -static inline void stateSetAccelNed_i(struct NedCoor_i* ned_accel) { +static inline void stateSetAccelNed_i(struct NedCoor_i *ned_accel) +{ VECT3_COPY(state.ned_accel_i, *ned_accel); /* clear bits for all accel representations and only set the new one */ state.accel_status = (1 << ACCEL_NED_I); } /// Set acceleration in ECEF coordinates (int). -static inline void stateSetAccelEcef_i(struct EcefCoor_i* ecef_accel) { +static inline void stateSetAccelEcef_i(struct EcefCoor_i *ecef_accel) +{ VECT3_COPY(state.ecef_accel_i, *ecef_accel); /* clear bits for all accel representations and only set the new one */ state.accel_status = (1 << ACCEL_ECEF_I); } /// Set acceleration in NED coordinates (float). -static inline void stateSetAccelNed_f(struct NedCoor_f* ned_accel) { +static inline void stateSetAccelNed_f(struct NedCoor_f *ned_accel) +{ VECT3_COPY(state.ned_accel_f, *ned_accel); /* clear bits for all accel representations and only set the new one */ state.accel_status = (1 << ACCEL_NED_F); } /// Set acceleration in ECEF coordinates (float). -static inline void stateSetAccelEcef_f(struct EcefCoor_f* ecef_accel) { +static inline void stateSetAccelEcef_f(struct EcefCoor_f *ecef_accel) +{ VECT3_COPY(state.ecef_accel_f, *ecef_accel); /* clear bits for all accel representations and only set the new one */ state.accel_status = (1 << ACCEL_ECEF_F); @@ -921,30 +988,38 @@ static inline void stateSetAccelEcef_f(struct EcefCoor_f* ecef_accel) { /************************ Get functions ****************************/ /// Get acceleration in NED coordinates (int). -static inline struct NedCoor_i* stateGetAccelNed_i(void) { - if (!bit_is_set(state.accel_status, ACCEL_NED_I)) +static inline struct NedCoor_i *stateGetAccelNed_i(void) +{ + if (!bit_is_set(state.accel_status, ACCEL_NED_I)) { stateCalcAccelNed_i(); + } return &state.ned_accel_i; } /// Get acceleration in ECEF coordinates (int). -static inline struct EcefCoor_i* stateGetAccelEcef_i(void) { - if (!bit_is_set(state.accel_status, ACCEL_ECEF_I)) +static inline struct EcefCoor_i *stateGetAccelEcef_i(void) +{ + if (!bit_is_set(state.accel_status, ACCEL_ECEF_I)) { stateCalcAccelEcef_i(); + } return &state.ecef_accel_i; } /// Get acceleration in NED coordinates (float). -static inline struct NedCoor_f* stateGetAccelNed_f(void) { - if (!bit_is_set(state.accel_status, ACCEL_NED_F)) +static inline struct NedCoor_f *stateGetAccelNed_f(void) +{ + if (!bit_is_set(state.accel_status, ACCEL_NED_F)) { stateCalcAccelNed_f(); + } return &state.ned_accel_f; } /// Get acceleration in ECEF coordinates (float). -static inline struct EcefCoor_f* stateGetAccelEcef_f(void) { - if (!bit_is_set(state.accel_status, ACCEL_ECEF_F)) +static inline struct EcefCoor_f *stateGetAccelEcef_f(void) +{ + if (!bit_is_set(state.accel_status, ACCEL_ECEF_F)) { stateCalcAccelEcef_f(); + } return &state.ecef_accel_f; } /** @}*/ @@ -960,71 +1035,84 @@ static inline struct EcefCoor_f* stateGetAccelEcef_f(void) { /*********************** validity test functions ******************/ /// Test if attitudes are valid. -static inline bool_t stateIsAttitudeValid(void) { +static inline bool_t stateIsAttitudeValid(void) +{ return (orienationCheckValid(&state.ned_to_body_orientation)); } /************************ Set functions ****************************/ /// Set vehicle body attitude from quaternion (int). -static inline void stateSetNedToBodyQuat_i(struct Int32Quat* ned_to_body_quat) { - orientationSetQuat_i(&state.ned_to_body_orientation,ned_to_body_quat); +static inline void stateSetNedToBodyQuat_i(struct Int32Quat *ned_to_body_quat) +{ + orientationSetQuat_i(&state.ned_to_body_orientation, ned_to_body_quat); } /// Set vehicle body attitude from rotation matrix (int). -static inline void stateSetNedToBodyRMat_i(struct Int32RMat* ned_to_body_rmat) { - orientationSetRMat_i(&state.ned_to_body_orientation,ned_to_body_rmat); +static inline void stateSetNedToBodyRMat_i(struct Int32RMat *ned_to_body_rmat) +{ + orientationSetRMat_i(&state.ned_to_body_orientation, ned_to_body_rmat); } /// Set vehicle body attitude from euler angles (int). -static inline void stateSetNedToBodyEulers_i(struct Int32Eulers* ned_to_body_eulers) { - orientationSetEulers_i(&state.ned_to_body_orientation,ned_to_body_eulers); +static inline void stateSetNedToBodyEulers_i(struct Int32Eulers *ned_to_body_eulers) +{ + orientationSetEulers_i(&state.ned_to_body_orientation, ned_to_body_eulers); } /// Set vehicle body attitude from quaternion (float). -static inline void stateSetNedToBodyQuat_f(struct FloatQuat* ned_to_body_quat) { - orientationSetQuat_f(&state.ned_to_body_orientation,ned_to_body_quat); +static inline void stateSetNedToBodyQuat_f(struct FloatQuat *ned_to_body_quat) +{ + orientationSetQuat_f(&state.ned_to_body_orientation, ned_to_body_quat); } /// Set vehicle body attitude from rotation matrix (float). -static inline void stateSetNedToBodyRMat_f(struct FloatRMat* ned_to_body_rmat) { - orientationSetRMat_f(&state.ned_to_body_orientation,ned_to_body_rmat); +static inline void stateSetNedToBodyRMat_f(struct FloatRMat *ned_to_body_rmat) +{ + orientationSetRMat_f(&state.ned_to_body_orientation, ned_to_body_rmat); } /// Set vehicle body attitude from euler angles (float). -static inline void stateSetNedToBodyEulers_f(struct FloatEulers* ned_to_body_eulers) { - orientationSetEulers_f(&state.ned_to_body_orientation,ned_to_body_eulers); +static inline void stateSetNedToBodyEulers_f(struct FloatEulers *ned_to_body_eulers) +{ + orientationSetEulers_f(&state.ned_to_body_orientation, ned_to_body_eulers); } /************************ Get functions ****************************/ /// Get vehicle body attitude quaternion (int). -static inline struct Int32Quat* stateGetNedToBodyQuat_i(void) { +static inline struct Int32Quat *stateGetNedToBodyQuat_i(void) +{ return orientationGetQuat_i(&state.ned_to_body_orientation); } /// Get vehicle body attitude rotation matrix (int). -static inline struct Int32RMat* stateGetNedToBodyRMat_i(void) { +static inline struct Int32RMat *stateGetNedToBodyRMat_i(void) +{ return orientationGetRMat_i(&state.ned_to_body_orientation); } /// Get vehicle body attitude euler angles (int). -static inline struct Int32Eulers* stateGetNedToBodyEulers_i(void) { +static inline struct Int32Eulers *stateGetNedToBodyEulers_i(void) +{ return orientationGetEulers_i(&state.ned_to_body_orientation); } /// Get vehicle body attitude quaternion (float). -static inline struct FloatQuat* stateGetNedToBodyQuat_f(void) { +static inline struct FloatQuat *stateGetNedToBodyQuat_f(void) +{ return orientationGetQuat_f(&state.ned_to_body_orientation); } /// Get vehicle body attitude rotation matrix (float). -static inline struct FloatRMat* stateGetNedToBodyRMat_f(void) { +static inline struct FloatRMat *stateGetNedToBodyRMat_f(void) +{ return orientationGetRMat_f(&state.ned_to_body_orientation); } /// Get vehicle body attitude euler angles (float). -static inline struct FloatEulers* stateGetNedToBodyEulers_f(void) { +static inline struct FloatEulers *stateGetNedToBodyEulers_f(void) +{ return orientationGetEulers_f(&state.ned_to_body_orientation); } /** @}*/ @@ -1045,21 +1133,24 @@ extern void stateCalcBodyRates_f(void); /*********************** validity test functions ******************/ /// Test if rates are valid. -static inline bool_t stateIsRateValid(void) { +static inline bool_t stateIsRateValid(void) +{ return (state.rate_status); } /************************ Set functions ****************************/ /// Set vehicle body angular rate (int). -static inline void stateSetBodyRates_i(struct Int32Rates* body_rate) { +static inline void stateSetBodyRates_i(struct Int32Rates *body_rate) +{ RATES_COPY(state.body_rates_i, *body_rate); /* clear bits for all attitude representations and only set the new one */ state.rate_status = (1 << RATE_I); } /// Set vehicle body angular rate (float). -static inline void stateSetBodyRates_f(struct FloatRates* body_rate) { +static inline void stateSetBodyRates_f(struct FloatRates *body_rate) +{ RATES_COPY(state.body_rates_f, *body_rate); /* clear bits for all attitude representations and only set the new one */ state.rate_status = (1 << RATE_F); @@ -1068,16 +1159,20 @@ static inline void stateSetBodyRates_f(struct FloatRates* body_rate) { /************************ Get functions ****************************/ /// Get vehicle body angular rate (int). -static inline struct Int32Rates* stateGetBodyRates_i(void) { - if (!bit_is_set(state.rate_status, RATE_I)) +static inline struct Int32Rates *stateGetBodyRates_i(void) +{ + if (!bit_is_set(state.rate_status, RATE_I)) { stateCalcBodyRates_i(); + } return &state.body_rates_i; } /// Get vehicle body angular rate (float). -static inline struct FloatRates* stateGetBodyRates_f(void) { - if (!bit_is_set(state.rate_status, RATE_F)) +static inline struct FloatRates *stateGetBodyRates_f(void) +{ + if (!bit_is_set(state.rate_status, RATE_F)) { stateCalcBodyRates_f(); + } return &state.body_rates_f; } @@ -1103,29 +1198,34 @@ extern void stateCalcAirspeed_f(void); /************************ validity test function *******************/ /// test if wind speed is available. -static inline bool_t stateIsWindspeedValid(void) { - return (state.wind_air_status &= ~((1<> 24; diff --git a/sw/airborne/subsystems/actuators/actuators_mkk.c b/sw/airborne/subsystems/actuators/actuators_mkk.c index 2291a6e1a2..d1651e730a 100644 --- a/sw/airborne/subsystems/actuators/actuators_mkk.c +++ b/sw/airborne/subsystems/actuators/actuators_mkk.c @@ -34,27 +34,30 @@ PRINT_CONFIG_VAR(ACTUATORS_MKK_I2C_DEV) struct ActuatorsMkk actuators_mkk; -void actuators_mkk_init(void) { +void actuators_mkk_init(void) +{ actuators_mkk.submit_err_cnt = 0; } -void actuators_mkk_set(void) { +void actuators_mkk_set(void) +{ const uint8_t actuators_addr[ACTUATORS_MKK_NB] = ACTUATORS_MKK_ADDR; static uint8_t last_idx = ACTUATORS_MKK_NB; #if defined ACTUATORS_START_DELAY && ! defined SITL if (!actuators_delay_done) { - if (SysTimeTimer(actuators_delay_time) < USEC_OF_SEC(ACTUATORS_START_DELAY)) + if (SysTimeTimer(actuators_delay_time) < USEC_OF_SEC(ACTUATORS_START_DELAY)) { return; - else + } else { actuators_delay_done = TRUE; + } } #endif uint8_t cur_idx = last_idx; - for (uint8_t i=0; i= ACTUATORS_MKK_NB) { + for (uint8_t i = 0; i < ACTUATORS_MKK_NB; i++) { + if (cur_idx >= ACTUATORS_MKK_NB) { cur_idx = 0; } #ifdef KILL_MOTORS diff --git a/sw/airborne/subsystems/actuators/actuators_mkk_v2.c b/sw/airborne/subsystems/actuators/actuators_mkk_v2.c index 2e6f05a94a..33f6e630f6 100644 --- a/sw/airborne/subsystems/actuators/actuators_mkk_v2.c +++ b/sw/airborne/subsystems/actuators/actuators_mkk_v2.c @@ -33,10 +33,11 @@ PRINT_CONFIG_VAR(ACTUATORS_MKK_V2_I2C_DEV) struct actuators_mkk_v2_struct actuators_mkk_v2; -void actuators_mkk_v2_init(void) { +void actuators_mkk_v2_init(void) +{ const uint8_t actuators_addr[ACTUATORS_MKK_V2_NB] = ACTUATORS_MKK_V2_ADDR; - for (uint8_t i=0; i= ACTUATORS_MKK_V2_NB) + if (actuators_mkk_v2.read_number >= ACTUATORS_MKK_V2_NB) { actuators_mkk_v2.read_number = 0; + } actuators_mkk_v2.trans[actuators_mkk_v2.read_number].type = I2CTransTxRx; actuators_mkk_v2.trans[actuators_mkk_v2.read_number].len_r = 3; } -void actuators_mkk_v2_set(void) { +void actuators_mkk_v2_set(void) +{ #if defined ACTUATORS_START_DELAY && ! defined SITL if (!actuators_delay_done) { - if (SysTimeTimer(actuators_delay_time) < USEC_OF_SEC(ACTUATORS_START_DELAY)) return; - else actuators_delay_done = TRUE; + if (SysTimeTimer(actuators_delay_time) < USEC_OF_SEC(ACTUATORS_START_DELAY)) { return; } + else { actuators_delay_done = TRUE; } } #endif // Read result - for (uint8_t i=0; i i * max_counter / (MOTOR_MIXING_NB_MOTOR + MOTOR_MIXING_STARTUP_DELAY)) { if (counter > MOTOR_MIXING_NB_MOTOR * max_counter / (MOTOR_MIXING_NB_MOTOR + MOTOR_MIXING_STARTUP_DELAY)) { - motor_mixing.commands[i] = MOTOR_MIXING_MIN_MOTOR_STARTUP + (MOTOR_MIXING_MIN_MOTOR - MOTOR_MIXING_MIN_MOTOR_STARTUP) * counter / max_counter; + motor_mixing.commands[i] = MOTOR_MIXING_MIN_MOTOR_STARTUP + (MOTOR_MIXING_MIN_MOTOR - MOTOR_MIXING_MIN_MOTOR_STARTUP) * + counter / max_counter; } else { motor_mixing.commands[i] = MOTOR_MIXING_MIN_MOTOR_STARTUP; } @@ -168,7 +176,8 @@ void motor_mixing_run_spinup(uint32_t counter, uint32_t max_counter) } } -void motor_mixing_run(bool_t motors_on, bool_t override_on, pprz_t in_cmd[] ) { +void motor_mixing_run(bool_t motors_on, bool_t override_on, pprz_t in_cmd[]) +{ uint8_t i; #if !HITL if (motors_on) { @@ -178,22 +187,25 @@ void motor_mixing_run(bool_t motors_on, bool_t override_on, pprz_t in_cmd[] ) { int32_t min_cmd = INT32_MAX; int32_t max_cmd = INT32_MIN; /* do the mixing in float to avoid overflows, implicitly casted back to int32_t */ - for (i=0; i max_cmd) + } + if (motor_mixing.commands[i] > max_cmd) { max_cmd = motor_mixing.commands[i]; + } } - if (min_cmd < MOTOR_MIXING_MIN_MOTOR && max_cmd > MOTOR_MIXING_MAX_MOTOR) + if (min_cmd < MOTOR_MIXING_MIN_MOTOR && max_cmd > MOTOR_MIXING_MAX_MOTOR) { motor_mixing.nb_failure++; + } /* In case of both min and max saturation, only lower the throttle * instead of applying both. This should prevent your quad shooting up, @@ -204,8 +216,7 @@ void motor_mixing_run(bool_t motors_on, bool_t override_on, pprz_t in_cmd[] ) { BoundAbs(saturation_offset, MOTOR_MIXING_MAX_SATURATION_OFFSET); offset_commands(saturation_offset); motor_mixing.nb_saturation++; - } - else if (min_cmd < MOTOR_MIXING_MIN_MOTOR) { + } else if (min_cmd < MOTOR_MIXING_MIN_MOTOR) { int32_t saturation_offset = MOTOR_MIXING_MIN_MOTOR - min_cmd; BoundAbs(saturation_offset, MOTOR_MIXING_MAX_SATURATION_OFFSET); offset_commands(saturation_offset); @@ -215,15 +226,15 @@ void motor_mixing_run(bool_t motors_on, bool_t override_on, pprz_t in_cmd[] ) { /* For testing motor failure */ if (motors_on && override_on) { for (i = 0; i < MOTOR_MIXING_NB_MOTOR; i++) { - if (motor_mixing.override_enabled[i]) + if (motor_mixing.override_enabled[i]) { motor_mixing.commands[i] = motor_mixing.override_value[i]; + } } } bound_commands(); bound_commands_step(); - } - else { - for (i=0; i= TX_BUF_SIZE) tx_head = 0; \ -} +#define UPDATE_HEAD() { \ + tx_head++; \ + if (tx_head >= TX_BUF_SIZE) tx_head = 0; \ + } #endif #define AudioTelemetryCheckFreeSpace(_space) (tx_head>=tx_tail? _space < (TX_BUF_SIZE - (tx_head - tx_tail)) : _space < (tx_tail - tx_head)) #define AudioTelemetryPut1Byte(_byte) { \ - tx_buf[tx_head] = _byte; \ - UPDATE_HEAD(); \ -} + tx_buf[tx_head] = _byte; \ + UPDATE_HEAD(); \ + } #define AUDIO_TELEMETRY_LOAD_NEXT_BYTE() { \ - tx_byte = tx_buf[tx_tail]; \ - tx_byte_idx = 0; \ - tx_tail++; \ - if( tx_tail >= TX_BUF_SIZE ) \ - tx_tail = 0; \ -} + tx_byte = tx_buf[tx_tail]; \ + tx_byte_idx = 0; \ + tx_tail++; \ + if( tx_tail >= TX_BUF_SIZE ) \ + tx_tail = 0; \ + } #define AudioTelemetryTransmit(_x) Audio_TelemetryPut1Byte(_x) diff --git a/sw/airborne/subsystems/datalink/datalink.h b/sw/airborne/subsystems/datalink/datalink.h index e8fe6bfef6..8a332064db 100644 --- a/sw/airborne/subsystems/datalink/datalink.h +++ b/sw/airborne/subsystems/datalink/datalink.h @@ -54,18 +54,18 @@ EXTERN bool_t dl_msg_available; EXTERN uint16_t datalink_time; #define MSG_SIZE 128 -EXTERN uint8_t dl_buffer[MSG_SIZE] __attribute__ ((aligned)); +EXTERN uint8_t dl_buffer[MSG_SIZE] __attribute__((aligned)); EXTERN void dl_parse_msg(void); /** Should be called when chars are available in dl_buffer */ /** Check for new message and parse */ #define DlCheckAndParse() { \ - if (dl_msg_available) { \ - dl_parse_msg(); \ - dl_msg_available = FALSE; \ - } \ -} + if (dl_msg_available) { \ + dl_parse_msg(); \ + dl_msg_available = FALSE; \ + } \ + } #if defined DATALINK && DATALINK == PPRZ diff --git a/sw/airborne/subsystems/datalink/downlink.c b/sw/airborne/subsystems/datalink/downlink.c index 58d5f8a73c..b3a33cef16 100644 --- a/sw/airborne/subsystems/datalink/downlink.c +++ b/sw/airborne/subsystems/datalink/downlink.c @@ -35,7 +35,8 @@ struct downlink downlink; #include "subsystems/datalink/datalink.h" #include "mcu_periph/sys_time.h" -static void send_downlink(struct transport_tx *trans, struct link_device *dev) { +static void send_downlink(struct transport_tx *trans, struct link_device *dev) +{ static uint32_t last_nb_bytes = 0; // timestamp in usec when last message was send static uint32_t last_ts = 0.; @@ -50,8 +51,8 @@ static void send_downlink(struct transport_tx *trans, struct link_device *dev) { // TODO uplink nb received msg uint16_t uplink_nb_msgs = 0; pprz_msg_send_DATALINK_REPORT(trans, dev, AC_ID, - &datalink_time, &uplink_nb_msgs, - &downlink.nb_msgs, &rate, &downlink.nb_ovrn); + &datalink_time, &uplink_nb_msgs, + &downlink.nb_msgs, &rate, &downlink.nb_ovrn); } } #endif diff --git a/sw/airborne/subsystems/datalink/downlink_transport.h b/sw/airborne/subsystems/datalink/downlink_transport.h index 5c1d6c1f5e..e532af245a 100644 --- a/sw/airborne/subsystems/datalink/downlink_transport.h +++ b/sw/airborne/subsystems/datalink/downlink_transport.h @@ -45,8 +45,7 @@ enum DownlinkDataType { DL_TYPE_TIMESTAMP, }; -struct DownlinkTransport -{ +struct DownlinkTransport { uint8_t (*SizeOf)(void *impl, uint8_t size); int (*CheckFreeSpace)(void *impl, uint8_t size); diff --git a/sw/airborne/subsystems/datalink/ivy_transport.c b/sw/airborne/subsystems/datalink/ivy_transport.c index 69dd85477d..6b98d470ef 100644 --- a/sw/airborne/subsystems/datalink/ivy_transport.c +++ b/sw/airborne/subsystems/datalink/ivy_transport.c @@ -37,7 +37,9 @@ struct ivy_transport ivy_tp; -static void put_bytes(struct ivy_transport *trans, struct link_device *dev __attribute__((unused)), enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), uint8_t len, const void *bytes) +static void put_bytes(struct ivy_transport *trans, struct link_device *dev __attribute__((unused)), + enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), + uint8_t len, const void *bytes) { const uint8_t *b = (const uint8_t *) bytes; @@ -51,7 +53,7 @@ static void put_bytes(struct ivy_transport *trans, struct link_device *dev __att // print data with correct type switch (type) { case DL_TYPE_CHAR: - trans->ivy_p += sprintf(trans->ivy_p, "%c", (char)(*((char*)(b+i)))); + trans->ivy_p += sprintf(trans->ivy_p, "%c", (char)(*((char *)(b + i)))); i++; break; case DL_TYPE_UINT8: @@ -59,48 +61,48 @@ static void put_bytes(struct ivy_transport *trans, struct link_device *dev __att i++; break; case DL_TYPE_UINT16: - trans->ivy_p += sprintf(trans->ivy_p, "%u", (uint16_t)(*((uint16_t*)(b+i)))); + trans->ivy_p += sprintf(trans->ivy_p, "%u", (uint16_t)(*((uint16_t *)(b + i)))); i += 2; break; case DL_TYPE_UINT32: case DL_TYPE_TIMESTAMP: - trans->ivy_p += sprintf(trans->ivy_p, "%u", (uint32_t)(*((uint32_t*)(b+i)))); + trans->ivy_p += sprintf(trans->ivy_p, "%u", (uint32_t)(*((uint32_t *)(b + i)))); i += 4; break; case DL_TYPE_UINT64: #if __WORDSIZE == 64 - trans->ivy_p += sprintf(trans->ivy_p, "%lu", (uint64_t)(*((uint64_t*)(b+i)))); + trans->ivy_p += sprintf(trans->ivy_p, "%lu", (uint64_t)(*((uint64_t *)(b + i)))); #else - trans->ivy_p += sprintf(trans->ivy_p, "%llu", (uint64_t)(*((uint64_t*)(b+i)))); + trans->ivy_p += sprintf(trans->ivy_p, "%llu", (uint64_t)(*((uint64_t *)(b + i)))); #endif i += 8; break; case DL_TYPE_INT8: - trans->ivy_p += sprintf(trans->ivy_p, "%d", (int8_t)(*((int8_t*)(b+i)))); + trans->ivy_p += sprintf(trans->ivy_p, "%d", (int8_t)(*((int8_t *)(b + i)))); i++; break; case DL_TYPE_INT16: - trans->ivy_p += sprintf(trans->ivy_p, "%d", (int16_t)(*((int16_t*)(b+i)))); + trans->ivy_p += sprintf(trans->ivy_p, "%d", (int16_t)(*((int16_t *)(b + i)))); i += 2; break; case DL_TYPE_INT32: - trans->ivy_p += sprintf(trans->ivy_p, "%d", (int32_t)(*((int32_t*)(b+i)))); + trans->ivy_p += sprintf(trans->ivy_p, "%d", (int32_t)(*((int32_t *)(b + i)))); i += 4; break; case DL_TYPE_INT64: #if __WORDSIZE == 64 - trans->ivy_p += sprintf(trans->ivy_p, "%ld", (uint64_t)(*((uint64_t*)(b+i)))); + trans->ivy_p += sprintf(trans->ivy_p, "%ld", (uint64_t)(*((uint64_t *)(b + i)))); #else - trans->ivy_p += sprintf(trans->ivy_p, "%lld", (uint64_t)(*((uint64_t*)(b+i)))); + trans->ivy_p += sprintf(trans->ivy_p, "%lld", (uint64_t)(*((uint64_t *)(b + i)))); #endif i += 8; break; case DL_TYPE_FLOAT: - trans->ivy_p += sprintf(trans->ivy_p, "%f", (float)(*((float*)(b+i)))); + trans->ivy_p += sprintf(trans->ivy_p, "%f", (float)(*((float *)(b + i)))); i += 4; break; case DL_TYPE_DOUBLE: - trans->ivy_p += sprintf(trans->ivy_p, "%f", (double)(*((double*)(b+i)))); + trans->ivy_p += sprintf(trans->ivy_p, "%f", (double)(*((double *)(b + i)))); i += 8; break; case DL_TYPE_ARRAY_LENGTH: @@ -123,14 +125,15 @@ static void put_bytes(struct ivy_transport *trans, struct link_device *dev __att if (format == DL_FORMAT_ARRAY) { if (type == DL_TYPE_CHAR) { trans->ivy_p += sprintf(trans->ivy_p, "\" "); - } - else { + } else { trans->ivy_p += sprintf(trans->ivy_p, " "); } } } -static void put_named_byte(struct ivy_transport *trans, struct link_device *dev __attribute__((unused)), enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), uint8_t byte __attribute__((unused)), const char * name __attribute__((unused))) +static void put_named_byte(struct ivy_transport *trans, struct link_device *dev __attribute__((unused)), + enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), + uint8_t byte __attribute__((unused)), const char *name __attribute__((unused))) { trans->ivy_p += sprintf(trans->ivy_p, "%s ", name); } @@ -140,7 +143,8 @@ static uint8_t size_of(struct ivy_transport *trans __attribute__((unused)), uint return len; } -static void start_message(struct ivy_transport *trans, struct link_device *dev __attribute__((unused)), uint8_t payload_len __attribute__((unused))) +static void start_message(struct ivy_transport *trans, struct link_device *dev __attribute__((unused)), + uint8_t payload_len __attribute__((unused))) { trans->ivy_p = trans->ivy_buf; } @@ -154,24 +158,27 @@ static void end_message(struct ivy_transport *trans, struct link_device *dev __a } } -static void overrun(struct ivy_transport *trans __attribute__((unused)), struct link_device *dev __attribute__((unused))) +static void overrun(struct ivy_transport *trans __attribute__((unused)), + struct link_device *dev __attribute__((unused))) { downlink.nb_ovrn++; } -static void count_bytes(struct ivy_transport *trans __attribute__((unused)), struct link_device *dev __attribute__((unused)), uint8_t bytes) +static void count_bytes(struct ivy_transport *trans __attribute__((unused)), + struct link_device *dev __attribute__((unused)), uint8_t bytes) { downlink.nb_bytes += bytes; } -static int check_available_space(struct ivy_transport *trans __attribute__((unused)), struct link_device *dev __attribute__((unused)), uint8_t bytes __attribute__((unused))) +static int check_available_space(struct ivy_transport *trans __attribute__((unused)), + struct link_device *dev __attribute__((unused)), uint8_t bytes __attribute__((unused))) { return TRUE; } -static int check_free_space(struct ivy_transport* p __attribute__((unused)), uint8_t len __attribute__((unused))) { return TRUE; } -static void transmit(struct ivy_transport* p __attribute__((unused)), uint8_t byte __attribute__((unused))) {} -static void send_message(struct ivy_transport* p __attribute__((unused))) {} +static int check_free_space(struct ivy_transport *p __attribute__((unused)), uint8_t len __attribute__((unused))) { return TRUE; } +static void transmit(struct ivy_transport *p __attribute__((unused)), uint8_t byte __attribute__((unused))) {} +static void send_message(struct ivy_transport *p __attribute__((unused))) {} void ivy_transport_init(void) { diff --git a/sw/airborne/subsystems/datalink/ivy_transport.h b/sw/airborne/subsystems/datalink/ivy_transport.h index dae014a7ed..cc1dd7be8f 100644 --- a/sw/airborne/subsystems/datalink/ivy_transport.h +++ b/sw/airborne/subsystems/datalink/ivy_transport.h @@ -36,7 +36,7 @@ // IVY transport struct ivy_transport { char ivy_buf[256]; - char* ivy_p; + char *ivy_p; int ivy_dl_enabled; // generic transmission interface struct transport_tx trans_tx; diff --git a/sw/airborne/subsystems/datalink/pprz_transport.c b/sw/airborne/subsystems/datalink/pprz_transport.c index 02ed012868..75b19458ca 100644 --- a/sw/airborne/subsystems/datalink/pprz_transport.c +++ b/sw/airborne/subsystems/datalink/pprz_transport.c @@ -55,7 +55,9 @@ static void put_1byte(struct pprz_transport *trans, struct link_device *dev, con dev->transmit(dev->periph, byte); } -static void put_bytes(struct pprz_transport *trans, struct link_device *dev, enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), uint8_t len, const void *bytes) +static void put_bytes(struct pprz_transport *trans, struct link_device *dev, + enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), + uint8_t len, const void *bytes) { const uint8_t *b = (const uint8_t *) bytes; int i; @@ -64,7 +66,9 @@ static void put_bytes(struct pprz_transport *trans, struct link_device *dev, enu } } -static void put_named_byte(struct pprz_transport *trans, struct link_device *dev, enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), uint8_t byte, const char * name __attribute__((unused))) +static void put_named_byte(struct pprz_transport *trans, struct link_device *dev, + enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), + uint8_t byte, const char *name __attribute__((unused))) { put_1byte(trans, dev, byte); } @@ -92,17 +96,20 @@ static void end_message(struct pprz_transport *trans, struct link_device *dev) dev->send_message(dev->periph); } -static void overrun(struct pprz_transport *trans __attribute__((unused)), struct link_device *dev __attribute__((unused))) +static void overrun(struct pprz_transport *trans __attribute__((unused)), + struct link_device *dev __attribute__((unused))) { downlink.nb_ovrn++; } -static void count_bytes(struct pprz_transport *trans __attribute__((unused)), struct link_device *dev __attribute__((unused)), uint8_t bytes) +static void count_bytes(struct pprz_transport *trans __attribute__((unused)), + struct link_device *dev __attribute__((unused)), uint8_t bytes) { downlink.nb_bytes += bytes; } -static int check_available_space(struct pprz_transport *trans __attribute__((unused)), struct link_device *dev, uint8_t bytes) +static int check_available_space(struct pprz_transport *trans __attribute__((unused)), struct link_device *dev, + uint8_t bytes) { return dev->check_free_space(dev->periph, bytes); } diff --git a/sw/airborne/subsystems/datalink/pprz_transport.h b/sw/airborne/subsystems/datalink/pprz_transport.h index 7a97da6600..a505bd7413 100644 --- a/sw/airborne/subsystems/datalink/pprz_transport.h +++ b/sw/airborne/subsystems/datalink/pprz_transport.h @@ -81,54 +81,61 @@ extern struct pprz_transport pprz_tp; // Init function extern void pprz_transport_init(void); -static inline void parse_pprz(struct pprz_transport * t, uint8_t c ) { +static inline void parse_pprz(struct pprz_transport *t, uint8_t c) +{ switch (t->status) { - case UNINIT: - if (c == STX) + case UNINIT: + if (c == STX) { + t->status++; + } + break; + case GOT_STX: + if (t->trans_rx.msg_received) { + t->trans_rx.ovrn++; + goto error; + } + t->trans_rx.payload_len = c - 4; /* Counting STX, LENGTH and CRC1 and CRC2 */ + t->ck_a_rx = t->ck_b_rx = c; t->status++; - break; - case GOT_STX: - if (t->trans_rx.msg_received) { - t->trans_rx.ovrn++; - goto error; - } - t->trans_rx.payload_len = c-4; /* Counting STX, LENGTH and CRC1 and CRC2 */ - t->ck_a_rx = t->ck_b_rx = c; - t->status++; - t->payload_idx = 0; - break; - case GOT_LENGTH: - t->trans_rx.payload[t->payload_idx] = c; - t->ck_a_rx += c; t->ck_b_rx += t->ck_a_rx; - t->payload_idx++; - if (t->payload_idx == t->trans_rx.payload_len) + t->payload_idx = 0; + break; + case GOT_LENGTH: + t->trans_rx.payload[t->payload_idx] = c; + t->ck_a_rx += c; t->ck_b_rx += t->ck_a_rx; + t->payload_idx++; + if (t->payload_idx == t->trans_rx.payload_len) { + t->status++; + } + break; + case GOT_PAYLOAD: + if (c != t->ck_a_rx) { + goto error; + } t->status++; - break; - case GOT_PAYLOAD: - if (c != t->ck_a_rx) + break; + case GOT_CRC1: + if (c != t->ck_b_rx) { + goto error; + } + t->trans_rx.msg_received = TRUE; + goto restart; + default: goto error; - t->status++; - break; - case GOT_CRC1: - if (c != t->ck_b_rx) - goto error; - t->trans_rx.msg_received = TRUE; - goto restart; - default: - goto error; } return; - error: +error: t->trans_rx.error++; - restart: +restart: t->status = UNINIT; return; } -static inline void pprz_parse_payload(struct pprz_transport * t) { +static inline void pprz_parse_payload(struct pprz_transport *t) +{ uint8_t i; - for(i = 0; i < t->trans_rx.payload_len; i++) + for (i = 0; i < t->trans_rx.payload_len; i++) { dl_buffer[i] = t->trans_rx.payload[i]; + } dl_msg_available = TRUE; } @@ -136,20 +143,20 @@ static inline void pprz_parse_payload(struct pprz_transport * t) { #define PprzBuffer(_dev) TransportLink(_dev,ChAvailable()) #define ReadPprzBuffer(_dev,_trans) { \ - while (TransportLink(_dev,ChAvailable()) && !(_trans.trans_rx.msg_received)) { \ - parse_pprz(&(_trans),TransportLink(_dev,Getch())); \ - } \ -} + while (TransportLink(_dev,ChAvailable()) && !(_trans.trans_rx.msg_received)) { \ + parse_pprz(&(_trans),TransportLink(_dev,Getch())); \ + } \ + } #define PprzCheckAndParse(_dev,_trans) { \ - if (PprzBuffer(_dev)) { \ - ReadPprzBuffer(_dev,_trans); \ - if (_trans.trans_rx.msg_received) { \ - pprz_parse_payload(&(_trans)); \ - _trans.trans_rx.msg_received = FALSE; \ - } \ - } \ -} + if (PprzBuffer(_dev)) { \ + ReadPprzBuffer(_dev,_trans); \ + if (_trans.trans_rx.msg_received) { \ + pprz_parse_payload(&(_trans)); \ + _trans.trans_rx.msg_received = FALSE; \ + } \ + } \ + } #endif /* PPRZ_TRANSPORT_H */ diff --git a/sw/airborne/subsystems/datalink/pprzlog_transport.c b/sw/airborne/subsystems/datalink/pprzlog_transport.c index 820eb17742..f6993bc4ad 100644 --- a/sw/airborne/subsystems/datalink/pprzlog_transport.c +++ b/sw/airborne/subsystems/datalink/pprzlog_transport.c @@ -54,7 +54,9 @@ static void put_1byte(struct pprzlog_transport *trans, struct link_device *dev, dev->transmit(dev->periph, byte); } -static void put_bytes(struct pprzlog_transport *trans, struct link_device *dev, enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), uint8_t len, const void *bytes) +static void put_bytes(struct pprzlog_transport *trans, struct link_device *dev, + enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), + uint8_t len, const void *bytes) { const uint8_t *b = (const uint8_t *) bytes; int i; @@ -63,7 +65,9 @@ static void put_bytes(struct pprzlog_transport *trans, struct link_device *dev, } } -static void put_named_byte(struct pprzlog_transport *trans, struct link_device *dev, enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), uint8_t byte, const char * name __attribute__((unused))) +static void put_named_byte(struct pprzlog_transport *trans, struct link_device *dev, + enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), + uint8_t byte, const char *name __attribute__((unused))) { put_1byte(trans, dev, byte); } @@ -79,8 +83,8 @@ static void start_message(struct pprzlog_transport *trans, struct link_device *d const uint8_t msg_len = size_of(trans, payload_len); trans->ck = 0; put_1byte(trans, dev, msg_len); - uint32_t ts = get_sys_time_usec()/100; - put_bytes(trans, dev, DL_TYPE_TIMESTAMP, DL_FORMAT_SCALAR, 4, (uint8_t*)(&ts)); + uint32_t ts = get_sys_time_usec() / 100; + put_bytes(trans, dev, DL_TYPE_TIMESTAMP, DL_FORMAT_SCALAR, 4, (uint8_t *)(&ts)); } static void end_message(struct pprzlog_transport *trans, struct link_device *dev) @@ -89,15 +93,18 @@ static void end_message(struct pprzlog_transport *trans, struct link_device *dev dev->send_message(dev->periph); } -static void overrun(struct pprzlog_transport *trans __attribute__((unused)), struct link_device *dev __attribute__((unused))) +static void overrun(struct pprzlog_transport *trans __attribute__((unused)), + struct link_device *dev __attribute__((unused))) { } -static void count_bytes(struct pprzlog_transport *trans __attribute__((unused)), struct link_device *dev __attribute__((unused)), uint8_t bytes __attribute__((unused))) +static void count_bytes(struct pprzlog_transport *trans __attribute__((unused)), + struct link_device *dev __attribute__((unused)), uint8_t bytes __attribute__((unused))) { } -static int check_available_space(struct pprzlog_transport *trans __attribute__((unused)), struct link_device *dev, uint8_t bytes) +static int check_available_space(struct pprzlog_transport *trans __attribute__((unused)), struct link_device *dev, + uint8_t bytes) { return dev->check_free_space(dev->periph, bytes); } diff --git a/sw/airborne/subsystems/datalink/superbitrf.c b/sw/airborne/subsystems/datalink/superbitrf.c index 2cd58c2ee3..5edba4b0fe 100644 --- a/sw/airborne/subsystems/datalink/superbitrf.c +++ b/sw/airborne/subsystems/datalink/superbitrf.c @@ -70,7 +70,7 @@ PRINT_CONFIG_VAR(SUPERBITRF_FORCE_DSM2); struct SuperbitRF superbitrf; /* The internal functions */ -static inline void superbitrf_radio_to_channels(uint8_t* data, uint8_t nb_channels, bool_t is_11bit, int16_t* channels); +static inline void superbitrf_radio_to_channels(uint8_t *data, uint8_t nb_channels, bool_t is_11bit, int16_t *channels); static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, uint8_t packet[]); static inline void superbitrf_send_packet_cb(bool_t error); static inline void superbitrf_gen_dsmx_channels(void); @@ -117,109 +117,112 @@ const uint8_t cyrf_start_receive[][2] = { /* The PN codes used for DSM2 and DSMX channel hopping */ static const uint8_t pn_codes[5][9][8] = { -{ /* Row 0 */ - /* Col 0 */ {0x03, 0xBC, 0x6E, 0x8A, 0xEF, 0xBD, 0xFE, 0xF8}, - /* Col 1 */ {0x88, 0x17, 0x13, 0x3B, 0x2D, 0xBF, 0x06, 0xD6}, - /* Col 2 */ {0xF1, 0x94, 0x30, 0x21, 0xA1, 0x1C, 0x88, 0xA9}, - /* Col 3 */ {0xD0, 0xD2, 0x8E, 0xBC, 0x82, 0x2F, 0xE3, 0xB4}, - /* Col 4 */ {0x8C, 0xFA, 0x47, 0x9B, 0x83, 0xA5, 0x66, 0xD0}, - /* Col 5 */ {0x07, 0xBD, 0x9F, 0x26, 0xC8, 0x31, 0x0F, 0xB8}, - /* Col 6 */ {0xEF, 0x03, 0x95, 0x89, 0xB4, 0x71, 0x61, 0x9D}, - /* Col 7 */ {0x40, 0xBA, 0x97, 0xD5, 0x86, 0x4F, 0xCC, 0xD1}, - /* Col 8 */ {0xD7, 0xA1, 0x54, 0xB1, 0x5E, 0x89, 0xAE, 0x86} -}, -{ /* Row 1 */ - /* Col 0 */ {0x83, 0xF7, 0xA8, 0x2D, 0x7A, 0x44, 0x64, 0xD3}, - /* Col 1 */ {0x3F, 0x2C, 0x4E, 0xAA, 0x71, 0x48, 0x7A, 0xC9}, - /* Col 2 */ {0x17, 0xFF, 0x9E, 0x21, 0x36, 0x90, 0xC7, 0x82}, - /* Col 3 */ {0xBC, 0x5D, 0x9A, 0x5B, 0xEE, 0x7F, 0x42, 0xEB}, - /* Col 4 */ {0x24, 0xF5, 0xDD, 0xF8, 0x7A, 0x77, 0x74, 0xE7}, - /* Col 5 */ {0x3D, 0x70, 0x7C, 0x94, 0xDC, 0x84, 0xAD, 0x95}, - /* Col 6 */ {0x1E, 0x6A, 0xF0, 0x37, 0x52, 0x7B, 0x11, 0xD4}, - /* Col 7 */ {0x62, 0xF5, 0x2B, 0xAA, 0xFC, 0x33, 0xBF, 0xAF}, - /* Col 8 */ {0x40, 0x56, 0x32, 0xD9, 0x0F, 0xD9, 0x5D, 0x97} -}, -{ /* Row 2 */ - /* Col 0 */ {0x40, 0x56, 0x32, 0xD9, 0x0F, 0xD9, 0x5D, 0x97}, - /* Col 1 */ {0x8E, 0x4A, 0xD0, 0xA9, 0xA7, 0xFF, 0x20, 0xCA}, - /* Col 2 */ {0x4C, 0x97, 0x9D, 0xBF, 0xB8, 0x3D, 0xB5, 0xBE}, - /* Col 3 */ {0x0C, 0x5D, 0x24, 0x30, 0x9F, 0xCA, 0x6D, 0xBD}, - /* Col 4 */ {0x50, 0x14, 0x33, 0xDE, 0xF1, 0x78, 0x95, 0xAD}, - /* Col 5 */ {0x0C, 0x3C, 0xFA, 0xF9, 0xF0, 0xF2, 0x10, 0xC9}, - /* Col 6 */ {0xF4, 0xDA, 0x06, 0xDB, 0xBF, 0x4E, 0x6F, 0xB3}, - /* Col 7 */ {0x9E, 0x08, 0xD1, 0xAE, 0x59, 0x5E, 0xE8, 0xF0}, - /* Col 8 */ {0xC0, 0x90, 0x8F, 0xBB, 0x7C, 0x8E, 0x2B, 0x8E} -}, -{ /* Row 3 */ - /* Col 0 */ {0xC0, 0x90, 0x8F, 0xBB, 0x7C, 0x8E, 0x2B, 0x8E}, - /* Col 1 */ {0x80, 0x69, 0x26, 0x80, 0x08, 0xF8, 0x49, 0xE7}, - /* Col 2 */ {0x7D, 0x2D, 0x49, 0x54, 0xD0, 0x80, 0x40, 0xC1}, - /* Col 3 */ {0xB6, 0xF2, 0xE6, 0x1B, 0x80, 0x5A, 0x36, 0xB4}, - /* Col 4 */ {0x42, 0xAE, 0x9C, 0x1C, 0xDA, 0x67, 0x05, 0xF6}, - /* Col 5 */ {0x9B, 0x75, 0xF7, 0xE0, 0x14, 0x8D, 0xB5, 0x80}, - /* Col 6 */ {0xBF, 0x54, 0x98, 0xB9, 0xB7, 0x30, 0x5A, 0x88}, - /* Col 7 */ {0x35, 0xD1, 0xFC, 0x97, 0x23, 0xD4, 0xC9, 0x88}, - /* Col 8 */ {0x88, 0xE1, 0xD6, 0x31, 0x26, 0x5F, 0xBD, 0x40} -}, -{ /* Row 4 */ - /* Col 0 */ {0xE1, 0xD6, 0x31, 0x26, 0x5F, 0xBD, 0x40, 0x93}, - /* Col 1 */ {0xDC, 0x68, 0x08, 0x99, 0x97, 0xAE, 0xAF, 0x8C}, - /* Col 2 */ {0xC3, 0x0E, 0x01, 0x16, 0x0E, 0x32, 0x06, 0xBA}, - /* Col 3 */ {0xE0, 0x83, 0x01, 0xFA, 0xAB, 0x3E, 0x8F, 0xAC}, - /* Col 4 */ {0x5C, 0xD5, 0x9C, 0xB8, 0x46, 0x9C, 0x7D, 0x84}, - /* Col 5 */ {0xF1, 0xC6, 0xFE, 0x5C, 0x9D, 0xA5, 0x4F, 0xB7}, - /* Col 6 */ {0x58, 0xB5, 0xB3, 0xDD, 0x0E, 0x28, 0xF1, 0xB0}, - /* Col 7 */ {0x5F, 0x30, 0x3B, 0x56, 0x96, 0x45, 0xF4, 0xA1}, - /* Col 8 */ {0x03, 0xBC, 0x6E, 0x8A, 0xEF, 0xBD, 0xFE, 0xF8} -}, + { /* Row 0 */ + /* Col 0 */ {0x03, 0xBC, 0x6E, 0x8A, 0xEF, 0xBD, 0xFE, 0xF8}, + /* Col 1 */ {0x88, 0x17, 0x13, 0x3B, 0x2D, 0xBF, 0x06, 0xD6}, + /* Col 2 */ {0xF1, 0x94, 0x30, 0x21, 0xA1, 0x1C, 0x88, 0xA9}, + /* Col 3 */ {0xD0, 0xD2, 0x8E, 0xBC, 0x82, 0x2F, 0xE3, 0xB4}, + /* Col 4 */ {0x8C, 0xFA, 0x47, 0x9B, 0x83, 0xA5, 0x66, 0xD0}, + /* Col 5 */ {0x07, 0xBD, 0x9F, 0x26, 0xC8, 0x31, 0x0F, 0xB8}, + /* Col 6 */ {0xEF, 0x03, 0x95, 0x89, 0xB4, 0x71, 0x61, 0x9D}, + /* Col 7 */ {0x40, 0xBA, 0x97, 0xD5, 0x86, 0x4F, 0xCC, 0xD1}, + /* Col 8 */ {0xD7, 0xA1, 0x54, 0xB1, 0x5E, 0x89, 0xAE, 0x86} + }, + { /* Row 1 */ + /* Col 0 */ {0x83, 0xF7, 0xA8, 0x2D, 0x7A, 0x44, 0x64, 0xD3}, + /* Col 1 */ {0x3F, 0x2C, 0x4E, 0xAA, 0x71, 0x48, 0x7A, 0xC9}, + /* Col 2 */ {0x17, 0xFF, 0x9E, 0x21, 0x36, 0x90, 0xC7, 0x82}, + /* Col 3 */ {0xBC, 0x5D, 0x9A, 0x5B, 0xEE, 0x7F, 0x42, 0xEB}, + /* Col 4 */ {0x24, 0xF5, 0xDD, 0xF8, 0x7A, 0x77, 0x74, 0xE7}, + /* Col 5 */ {0x3D, 0x70, 0x7C, 0x94, 0xDC, 0x84, 0xAD, 0x95}, + /* Col 6 */ {0x1E, 0x6A, 0xF0, 0x37, 0x52, 0x7B, 0x11, 0xD4}, + /* Col 7 */ {0x62, 0xF5, 0x2B, 0xAA, 0xFC, 0x33, 0xBF, 0xAF}, + /* Col 8 */ {0x40, 0x56, 0x32, 0xD9, 0x0F, 0xD9, 0x5D, 0x97} + }, + { /* Row 2 */ + /* Col 0 */ {0x40, 0x56, 0x32, 0xD9, 0x0F, 0xD9, 0x5D, 0x97}, + /* Col 1 */ {0x8E, 0x4A, 0xD0, 0xA9, 0xA7, 0xFF, 0x20, 0xCA}, + /* Col 2 */ {0x4C, 0x97, 0x9D, 0xBF, 0xB8, 0x3D, 0xB5, 0xBE}, + /* Col 3 */ {0x0C, 0x5D, 0x24, 0x30, 0x9F, 0xCA, 0x6D, 0xBD}, + /* Col 4 */ {0x50, 0x14, 0x33, 0xDE, 0xF1, 0x78, 0x95, 0xAD}, + /* Col 5 */ {0x0C, 0x3C, 0xFA, 0xF9, 0xF0, 0xF2, 0x10, 0xC9}, + /* Col 6 */ {0xF4, 0xDA, 0x06, 0xDB, 0xBF, 0x4E, 0x6F, 0xB3}, + /* Col 7 */ {0x9E, 0x08, 0xD1, 0xAE, 0x59, 0x5E, 0xE8, 0xF0}, + /* Col 8 */ {0xC0, 0x90, 0x8F, 0xBB, 0x7C, 0x8E, 0x2B, 0x8E} + }, + { /* Row 3 */ + /* Col 0 */ {0xC0, 0x90, 0x8F, 0xBB, 0x7C, 0x8E, 0x2B, 0x8E}, + /* Col 1 */ {0x80, 0x69, 0x26, 0x80, 0x08, 0xF8, 0x49, 0xE7}, + /* Col 2 */ {0x7D, 0x2D, 0x49, 0x54, 0xD0, 0x80, 0x40, 0xC1}, + /* Col 3 */ {0xB6, 0xF2, 0xE6, 0x1B, 0x80, 0x5A, 0x36, 0xB4}, + /* Col 4 */ {0x42, 0xAE, 0x9C, 0x1C, 0xDA, 0x67, 0x05, 0xF6}, + /* Col 5 */ {0x9B, 0x75, 0xF7, 0xE0, 0x14, 0x8D, 0xB5, 0x80}, + /* Col 6 */ {0xBF, 0x54, 0x98, 0xB9, 0xB7, 0x30, 0x5A, 0x88}, + /* Col 7 */ {0x35, 0xD1, 0xFC, 0x97, 0x23, 0xD4, 0xC9, 0x88}, + /* Col 8 */ {0x88, 0xE1, 0xD6, 0x31, 0x26, 0x5F, 0xBD, 0x40} + }, + { /* Row 4 */ + /* Col 0 */ {0xE1, 0xD6, 0x31, 0x26, 0x5F, 0xBD, 0x40, 0x93}, + /* Col 1 */ {0xDC, 0x68, 0x08, 0x99, 0x97, 0xAE, 0xAF, 0x8C}, + /* Col 2 */ {0xC3, 0x0E, 0x01, 0x16, 0x0E, 0x32, 0x06, 0xBA}, + /* Col 3 */ {0xE0, 0x83, 0x01, 0xFA, 0xAB, 0x3E, 0x8F, 0xAC}, + /* Col 4 */ {0x5C, 0xD5, 0x9C, 0xB8, 0x46, 0x9C, 0x7D, 0x84}, + /* Col 5 */ {0xF1, 0xC6, 0xFE, 0x5C, 0x9D, 0xA5, 0x4F, 0xB7}, + /* Col 6 */ {0x58, 0xB5, 0xB3, 0xDD, 0x0E, 0x28, 0xF1, 0xB0}, + /* Col 7 */ {0x5F, 0x30, 0x3B, 0x56, 0x96, 0x45, 0xF4, 0xA1}, + /* Col 8 */ {0x03, 0xBC, 0x6E, 0x8A, 0xEF, 0xBD, 0xFE, 0xF8} + }, }; static const uint8_t pn_bind[] = { 0x98, 0x88, 0x1B, 0xE4, 0x30, 0x79, 0x03, 0x84 }; #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_superbit(struct transport_tx *trans, struct link_device *dev) { +static void send_superbit(struct transport_tx *trans, struct link_device *dev) +{ uint8_t status = superbitrf.status; uint8_t cyrf6936_status = superbitrf.cyrf6936.status; pprz_msg_send_SUPERBITRF(trans, dev, AC_ID, - &status, - &cyrf6936_status, - &superbitrf.irq_count, - &superbitrf.rx_packet_count, - &superbitrf.tx_packet_count, - &superbitrf.transfer_timeouts, - &superbitrf.resync_count, - &superbitrf.uplink_count, - &superbitrf.rc_count, - &superbitrf.timing1, - &superbitrf.timing2, - &superbitrf.bind_mfg_id32, - 6, - superbitrf.cyrf6936.mfg_id); + &status, + &cyrf6936_status, + &superbitrf.irq_count, + &superbitrf.rx_packet_count, + &superbitrf.tx_packet_count, + &superbitrf.transfer_timeouts, + &superbitrf.resync_count, + &superbitrf.uplink_count, + &superbitrf.rc_count, + &superbitrf.timing1, + &superbitrf.timing2, + &superbitrf.bind_mfg_id32, + 6, + superbitrf.cyrf6936.mfg_id); } #endif // Functions for the generic device API -static bool_t superbitrf_check_free_space(struct SuperbitRF* p, uint8_t len) +static bool_t superbitrf_check_free_space(struct SuperbitRF *p, uint8_t len) { int16_t space = p->tx_extract_idx - p->tx_insert_idx; - if (space <= 0) + if (space <= 0) { space += SUPERBITRF_TX_BUFFER_SIZE; + } return (uint16_t)(space - 1) >= len; } -static void superbitrf_transmit(struct SuperbitRF* p, uint8_t byte) +static void superbitrf_transmit(struct SuperbitRF *p, uint8_t byte) { p->tx_buffer[p->tx_insert_idx] = byte; - p->tx_insert_idx = (p->tx_insert_idx+1) %SUPERBITRF_TX_BUFFER_SIZE; + p->tx_insert_idx = (p->tx_insert_idx + 1) % SUPERBITRF_TX_BUFFER_SIZE; } -static void superbitrf_send(struct SuperbitRF* p __attribute__((unused))) { } +static void superbitrf_send(struct SuperbitRF *p __attribute__((unused))) { } /** * Initialize the superbitrf */ -void superbitrf_init(void) { +void superbitrf_init(void) +{ // Set the status to uninitialized and set the timer to 0 superbitrf.status = SUPERBITRF_UNINIT; superbitrf.state = 0; @@ -255,12 +258,13 @@ void superbitrf_init(void) { #endif } -void superbitrf_set_mfg_id(uint32_t id) { +void superbitrf_set_mfg_id(uint32_t id) +{ superbitrf.bind_mfg_id32 = id; - superbitrf.bind_mfg_id[0] = (superbitrf.bind_mfg_id32 &0xFF); - superbitrf.bind_mfg_id[1] = (superbitrf.bind_mfg_id32 >>8 &0xFF); - superbitrf.bind_mfg_id[2] = (superbitrf.bind_mfg_id32 >>16 &0xFF); - superbitrf.bind_mfg_id[3] = (superbitrf.bind_mfg_id32 >>24 &0xFF); + superbitrf.bind_mfg_id[0] = (superbitrf.bind_mfg_id32 & 0xFF); + superbitrf.bind_mfg_id[1] = (superbitrf.bind_mfg_id32 >> 8 & 0xFF); + superbitrf.bind_mfg_id[2] = (superbitrf.bind_mfg_id32 >> 16 & 0xFF); + superbitrf.bind_mfg_id[3] = (superbitrf.bind_mfg_id32 >> 24 & 0xFF); // Calculate some values based on the bind MFG id superbitrf.crc_seed = ~((superbitrf.bind_mfg_id[0] << 8) + superbitrf.bind_mfg_id[1]); @@ -268,15 +272,17 @@ void superbitrf_set_mfg_id(uint32_t id) { superbitrf.data_col = 7 - superbitrf.sop_col; } -void superbitrf_set_protocol(uint8_t protocol) { - superbitrf.protocol = protocol; - superbitrf.resolution = (superbitrf.protocol & 0x10)>>4; +void superbitrf_set_protocol(uint8_t protocol) +{ + superbitrf.protocol = protocol; + superbitrf.resolution = (superbitrf.protocol & 0x10) >> 4; } /** * The superbitrf on event call */ -void superbitrf_event(void) { +void superbitrf_event(void) +{ uint8_t i, pn_row, data_code[16]; static uint8_t packet_size, tx_packet[16]; static bool_t start_transfer = TRUE; @@ -286,22 +292,24 @@ void superbitrf_event(void) { #endif // Check if the cyrf6936 isn't busy and the uperbitrf is initialized - if(superbitrf.cyrf6936.status != CYRF6936_IDLE) + if (superbitrf.cyrf6936.status != CYRF6936_IDLE) { return; + } // When the device is initialized handle the IRQ - if(superbitrf.status != SUPERBITRF_UNINIT) { + if (superbitrf.status != SUPERBITRF_UNINIT) { // First handle the IRQ - if(gpio_get(SUPERBITRF_DRDY_PORT, SUPERBITRF_DRDY_PIN) == 0) { + if (gpio_get(SUPERBITRF_DRDY_PORT, SUPERBITRF_DRDY_PIN) == 0) { // Receive the packet cyrf6936_read_rx_irq_status_packet(&superbitrf.cyrf6936); superbitrf.irq_count++; } /* Check if it is a valid receive */ - if(superbitrf.cyrf6936.has_irq && (superbitrf.cyrf6936.rx_irq_status & CYRF_RXC_IRQ)) { + if (superbitrf.cyrf6936.has_irq && (superbitrf.cyrf6936.rx_irq_status & CYRF_RXC_IRQ)) { // Handle the packet received - superbitrf_receive_packet_cb((superbitrf.cyrf6936.rx_irq_status & CYRF_RXE_IRQ), superbitrf.cyrf6936.rx_status, superbitrf.cyrf6936.rx_packet); + superbitrf_receive_packet_cb((superbitrf.cyrf6936.rx_irq_status & CYRF_RXE_IRQ), superbitrf.cyrf6936.rx_status, + superbitrf.cyrf6936.rx_packet); superbitrf.rx_packet_count++; // Reset the packet receiving @@ -309,7 +317,7 @@ void superbitrf_event(void) { } /* Check if it has a valid send */ - if(superbitrf.cyrf6936.has_irq && (superbitrf.cyrf6936.tx_irq_status & CYRF_TXC_IRQ)) { + if (superbitrf.cyrf6936.has_irq && (superbitrf.cyrf6936.tx_irq_status & CYRF_TXC_IRQ)) { // Handle the send packet superbitrf_send_packet_cb((superbitrf.cyrf6936.rx_irq_status & CYRF_TXE_IRQ)); superbitrf.tx_packet_count++; @@ -320,757 +328,800 @@ void superbitrf_event(void) { } // Check the status of the superbitrf - switch(superbitrf.status) { + switch (superbitrf.status) { - /* When the superbitrf isn't initialized */ - case SUPERBITRF_UNINIT: - // Try to write the startup config - if(cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_stratup_config, 11)) { - // Check if need to go to bind or transfer - if(gpio_get(SPEKTRUM_BIND_PIN_PORT, SPEKTRUM_BIND_PIN) == 0) - start_transfer = FALSE; - - superbitrf.status = SUPERBITRF_INIT_BINDING; - } - break; - - /* When the superbitrf is initializing binding */ - case SUPERBITRF_INIT_BINDING: - /* Switch the different states */ - switch (superbitrf.state) { - case 0: - // Try to write the binding config - cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_bind_config, 5); - superbitrf.state++; - break; - case 1: - // Set the data code and channel - memcpy(data_code, pn_codes[0][8], 8); - memcpy(data_code + 8, pn_bind, 8); - cyrf6936_write_chan_sop_data_crc(&superbitrf.cyrf6936, 1, pn_codes[0][0], data_code, 0x0000); - - superbitrf.status = SUPERBITRF_BINDING; - break; - default: - // Should not happen - superbitrf.state = 0; - break; - } - break; - - /* When the superbitrf is initializing transfer */ - case SUPERBITRF_INIT_TRANSFER: - // Generate the DSMX channels - superbitrf_gen_dsmx_channels(); - - // Try to write the transfer config - if(cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_transfer_config, 4)) { - superbitrf.resync_count = 0; - superbitrf.packet_loss = FALSE; - superbitrf.packet_loss_bit = 0; - superbitrf.status = SUPERBITRF_SYNCING_A; - superbitrf.state = 1; - } - break; - - /* When the superbitrf is in binding mode */ - case SUPERBITRF_BINDING: - -#ifdef RADIO_CONTROL_LED - slowLedCpt++; - if(slowLedCpt>100000){ - - LED_TOGGLE(RADIO_CONTROL_LED); - slowLedCpt = 0; - } -#endif - - /* Switch the different states */ - switch (superbitrf.state) { - case 0: - // When there is a timeout - if (superbitrf.timer < get_sys_time_usec()) - superbitrf.state++; - break; - case 1: - // Abort the receive - cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2); - - superbitrf.state++; - break; - case 2: - // Switch channel - superbitrf.channel = (superbitrf.channel + 2) % 0x4F; //TODO fix define - cyrf6936_write(&superbitrf.cyrf6936, CYRF_CHANNEL, superbitrf.channel); - - superbitrf.state += 2; // Already aborted - break; - case 3: - // Abort the receive - cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2); - - superbitrf.state++; - break; - case 4: - // Start receiving - cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); - superbitrf.state++; - break; - default: - // Check if need to go to transfer - if(start_transfer) { - // Initialize the binding values - // set values based on mfg id - // if bind_mfg_id32 is loaded from persistent settings use that, - if (superbitrf.bind_mfg_id32) { - superbitrf_set_mfg_id(superbitrf.bind_mfg_id32); + /* When the superbitrf isn't initialized */ + case SUPERBITRF_UNINIT: + // Try to write the startup config + if (cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_stratup_config, 11)) { + // Check if need to go to bind or transfer + if (gpio_get(SPEKTRUM_BIND_PIN_PORT, SPEKTRUM_BIND_PIN) == 0) { + start_transfer = FALSE; } - #ifdef RADIO_TRANSMITTER_ID - // otherwise load airframe file value - else { - PRINT_CONFIG_VAR(RADIO_TRANSMITTER_ID); - superbitrf_set_mfg_id(RADIO_TRANSMITTER_ID); - } - #endif - #ifdef RADIO_TRANSMITTER_CHAN - PRINT_CONFIG_VAR(RADIO_TRANSMITTER_CHAN); - if (superbitrf.num_channels == 0) { - superbitrf.num_channels = RADIO_TRANSMITTER_CHAN; - } - #endif - if (superbitrf.protocol == 0) { - superbitrf_set_protocol(superbitrf.protocol); - } - #ifdef RADIO_TRANSMITTER_PROTOCOL - else { - PRINT_CONFIG_VAR(RADIO_TRANSMITTER_PROTOCOL); - superbitrf_set_protocol(RADIO_TRANSMITTER_PROTOCOL); - } - #endif - // Start transfer - superbitrf.state = 0; - superbitrf.status = SUPERBITRF_INIT_TRANSFER; - break; + superbitrf.status = SUPERBITRF_INIT_BINDING; } - - // Set the timer - superbitrf.timer = (get_sys_time_usec() + SUPERBITRF_BIND_RECV_TIME) % 0xFFFFFFFF; - superbitrf.state = 0; break; - } - break; - /* When the receiver is synchronizing with the transmitter */ - case SUPERBITRF_SYNCING_A: - case SUPERBITRF_SYNCING_B: + /* When the superbitrf is initializing binding */ + case SUPERBITRF_INIT_BINDING: + /* Switch the different states */ + switch (superbitrf.state) { + case 0: + // Try to write the binding config + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_bind_config, 5); + superbitrf.state++; + break; + case 1: + // Set the data code and channel + memcpy(data_code, pn_codes[0][8], 8); + memcpy(data_code + 8, pn_bind, 8); + cyrf6936_write_chan_sop_data_crc(&superbitrf.cyrf6936, 1, pn_codes[0][0], data_code, 0x0000); -#ifdef RADIO_CONTROL_LED - slowLedCpt++; - if(slowLedCpt>5000){ - - LED_TOGGLE(RADIO_CONTROL_LED); - slowLedCpt = 0; - } -#endif - - /* Switch the different states */ - switch (superbitrf.state) { - case 0: - // When there is a timeout - if (superbitrf.timer < get_sys_time_usec()) - superbitrf.state++; - break; - case 1: - // Abort the receive - cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2); - superbitrf.state++; - break; - case 2: - // Switch channel, sop code, data code and crc - superbitrf.channel_idx = (IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2)? (superbitrf.channel_idx + 1) %2 : (superbitrf.channel_idx + 1) %23; - pn_row = (IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2)? superbitrf.channels[superbitrf.channel_idx] % 5 : (superbitrf.channels[superbitrf.channel_idx]-2) % 5; - - cyrf6936_write_chan_sop_data_crc(&superbitrf.cyrf6936, superbitrf.channels[superbitrf.channel_idx], - pn_codes[pn_row][superbitrf.sop_col], - pn_codes[pn_row][superbitrf.data_col], - superbitrf.crc_seed); - superbitrf.state++; - break; - case 3: - // Create a new packet when no packet loss - if(!superbitrf.packet_loss) { - superbitrf.packet_loss_bit = !superbitrf.packet_loss_bit; - if(IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) { - tx_packet[0] = ~superbitrf.bind_mfg_id[2]; - tx_packet[1] = (~superbitrf.bind_mfg_id[3])+1+superbitrf.packet_loss_bit; - } else { - tx_packet[0] = superbitrf.bind_mfg_id[2]; - tx_packet[1] = (superbitrf.bind_mfg_id[3])+1+superbitrf.packet_loss_bit; - } - - packet_size = (superbitrf.tx_insert_idx-superbitrf.tx_extract_idx+SUPERBITRF_TX_BUFFER_SIZE %SUPERBITRF_TX_BUFFER_SIZE); - if(packet_size > 14) - packet_size = 14; - - for(i = 0; i < packet_size; i++) - tx_packet[i+2] = superbitrf.tx_buffer[(superbitrf.tx_extract_idx+i) %SUPERBITRF_TX_BUFFER_SIZE]; + superbitrf.status = SUPERBITRF_BINDING; + break; + default: + // Should not happen + superbitrf.state = 0; + break; } - - // Send a packet - cyrf6936_send(&superbitrf.cyrf6936, tx_packet, packet_size+2); - - // Update the packet extraction - if(!superbitrf.packet_loss) - superbitrf.tx_extract_idx = (superbitrf.tx_extract_idx+packet_size) %SUPERBITRF_TX_BUFFER_SIZE; - - superbitrf.state++; break; - case 4: - //TODO: check timeout? (Waiting for send) - break; - case 5: - superbitrf.state = 7; - break; - // Start receiving - cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); - superbitrf.timer = (get_sys_time_usec() + SUPERBITRF_DATARECVB_TIME) % 0xFFFFFFFF; - superbitrf.state++; - break; - case 6: - // Wait for telemetry data - if (superbitrf.timer < get_sys_time_usec()) - superbitrf.state++; - break; - case 7: - // When DSMX we don't need to switch - if(IS_DSMX(superbitrf.protocol) && !SUPERBITRF_FORCE_DSM2) { - superbitrf.state++; - superbitrf.channel = superbitrf.channels[superbitrf.channel_idx]; - break; - } - // Switch channel, sop code, data code and crc - superbitrf.channel = (superbitrf.channel + 2) % 0x4F; //TODO fix define - superbitrf.crc_seed = ~superbitrf.crc_seed; - pn_row = (IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2)? superbitrf.channel % 5 : (superbitrf.channel-2) % 5; + /* When the superbitrf is initializing transfer */ + case SUPERBITRF_INIT_TRANSFER: + // Generate the DSMX channels + superbitrf_gen_dsmx_channels(); - cyrf6936_write_chan_sop_data_crc(&superbitrf.cyrf6936, superbitrf.channel, - pn_codes[pn_row][superbitrf.sop_col], - pn_codes[pn_row][superbitrf.data_col], - superbitrf.crc_seed); - - superbitrf.state++; - break; - case 8: - // Start receiving - cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); - superbitrf.state++; - break; - default: - // Set the timer - superbitrf.timer = (get_sys_time_usec() + SUPERBITRF_SYNC_RECV_TIME) % 0xFFFFFFFF; - superbitrf.state = 0; - break; - } - break; - - /* Normal transfer mode */ - case SUPERBITRF_TRANSFER: - -#ifdef RADIO_CONTROL_LED - LED_ON(RADIO_CONTROL_LED); -#endif - - /* Switch the different states */ - switch (superbitrf.state) { - case 0: - // Fixing timer overflow - if(superbitrf.timer_overflow && get_sys_time_usec() <= superbitrf.timer) - superbitrf.timer_overflow = FALSE; - - // When there is a timeout - if(superbitrf.timer < get_sys_time_usec() && !superbitrf.timer_overflow) { - superbitrf.transfer_timeouts++; - superbitrf.timeouts++; - superbitrf.state++; - } - - // We really lost the communication - if(superbitrf.timeouts > 100) { - superbitrf.state = 0; - superbitrf.resync_count++; + // Try to write the transfer config + if (cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_transfer_config, 4)) { + superbitrf.resync_count = 0; + superbitrf.packet_loss = FALSE; + superbitrf.packet_loss_bit = 0; superbitrf.status = SUPERBITRF_SYNCING_A; + superbitrf.state = 1; } break; - case 1: - // Abort the receive - cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2); - superbitrf.state++; - // Set the timer - superbitrf.timer = (get_sys_time_usec() + SUPERBITRF_DATARECV_TIME) % 0xFFFFFFFF; - if(superbitrf.timer < get_sys_time_usec()) - superbitrf.timer_overflow = TRUE; - else - superbitrf.timer_overflow = FALSE; + /* When the superbitrf is in binding mode */ + case SUPERBITRF_BINDING: - // Only send on channel 2 - if(superbitrf.crc_seed != ((superbitrf.bind_mfg_id[0] << 8) + superbitrf.bind_mfg_id[1])) - superbitrf.state = 8; - break; - case 2: - // Wait before sending (FIXME??) - superbitrf.state++; - break; - case 3: - // Create a new packet when no packet loss - if(!superbitrf.packet_loss) { - superbitrf.packet_loss_bit = !superbitrf.packet_loss_bit; - if(IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) { - tx_packet[0] = ~superbitrf.bind_mfg_id[2]; - tx_packet[1] = ((~superbitrf.bind_mfg_id[3])+1+superbitrf.packet_loss_bit) % 0xFF; - } else { - tx_packet[0] = superbitrf.bind_mfg_id[2]; - tx_packet[1] = ((superbitrf.bind_mfg_id[3])+1+superbitrf.packet_loss_bit) % 0xFF; - } +#ifdef RADIO_CONTROL_LED + slowLedCpt++; + if (slowLedCpt > 100000) { - packet_size = (superbitrf.tx_insert_idx-superbitrf.tx_extract_idx+SUPERBITRF_TX_BUFFER_SIZE %SUPERBITRF_TX_BUFFER_SIZE); - if(packet_size > 14) - packet_size = 14; - - for(i = 0; i < packet_size; i++) - tx_packet[i+2] = superbitrf.tx_buffer[(superbitrf.tx_extract_idx+i) %SUPERBITRF_TX_BUFFER_SIZE]; + LED_TOGGLE(RADIO_CONTROL_LED); + slowLedCpt = 0; } +#endif - // Send a packet - cyrf6936_send(&superbitrf.cyrf6936, tx_packet, packet_size+2); + /* Switch the different states */ + switch (superbitrf.state) { + case 0: + // When there is a timeout + if (superbitrf.timer < get_sys_time_usec()) { + superbitrf.state++; + } + break; + case 1: + // Abort the receive + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2); - // Update the packet extraction - if(!superbitrf.packet_loss) - superbitrf.tx_extract_idx = (superbitrf.tx_extract_idx+packet_size) %SUPERBITRF_TX_BUFFER_SIZE; + superbitrf.state++; + break; + case 2: + // Switch channel + superbitrf.channel = (superbitrf.channel + 2) % 0x4F; //TODO fix define + cyrf6936_write(&superbitrf.cyrf6936, CYRF_CHANNEL, superbitrf.channel); - superbitrf.state++; - break; - case 4: - //TODO: check timeout? (Waiting for send) - break; - case 5: - // Start receiving - cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); - superbitrf.state++; - break; - case 6: - // Fixing timer overflow - if(superbitrf.timer_overflow && get_sys_time_usec() <= superbitrf.timer) - superbitrf.timer_overflow = FALSE; + superbitrf.state += 2; // Already aborted + break; + case 3: + // Abort the receive + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2); - // Waiting for data receive - if (superbitrf.timer < get_sys_time_usec() && !superbitrf.timer_overflow) - superbitrf.state++; - break; - case 7: - // Abort the receive - cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2); - superbitrf.state++; - break; - case 8: - // Switch channel, sop code, data code and crc - superbitrf.channel_idx = (IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2)? (superbitrf.channel_idx + 1) %2 : (superbitrf.channel_idx + 1) %23; - superbitrf.channel = superbitrf.channels[superbitrf.channel_idx]; - superbitrf.crc_seed = ~superbitrf.crc_seed; - pn_row = (IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2)? superbitrf.channel % 5 : (superbitrf.channel-2) % 5; + superbitrf.state++; + break; + case 4: + // Start receiving + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + superbitrf.state++; + break; + default: + // Check if need to go to transfer + if (start_transfer) { + // Initialize the binding values + // set values based on mfg id + // if bind_mfg_id32 is loaded from persistent settings use that, + if (superbitrf.bind_mfg_id32) { + superbitrf_set_mfg_id(superbitrf.bind_mfg_id32); + } +#ifdef RADIO_TRANSMITTER_ID + // otherwise load airframe file value + else { + PRINT_CONFIG_VAR(RADIO_TRANSMITTER_ID); + superbitrf_set_mfg_id(RADIO_TRANSMITTER_ID); + } +#endif +#ifdef RADIO_TRANSMITTER_CHAN + PRINT_CONFIG_VAR(RADIO_TRANSMITTER_CHAN); + if (superbitrf.num_channels == 0) { + superbitrf.num_channels = RADIO_TRANSMITTER_CHAN; + } +#endif + if (superbitrf.protocol == 0) { + superbitrf_set_protocol(superbitrf.protocol); + } +#ifdef RADIO_TRANSMITTER_PROTOCOL + else { + PRINT_CONFIG_VAR(RADIO_TRANSMITTER_PROTOCOL); + superbitrf_set_protocol(RADIO_TRANSMITTER_PROTOCOL); + } +#endif - cyrf6936_write_chan_sop_data_crc(&superbitrf.cyrf6936, superbitrf.channel, - pn_codes[pn_row][superbitrf.sop_col], - pn_codes[pn_row][superbitrf.data_col], - superbitrf.crc_seed); + // Start transfer + superbitrf.state = 0; + superbitrf.status = SUPERBITRF_INIT_TRANSFER; + break; + } - superbitrf.state++; + // Set the timer + superbitrf.timer = (get_sys_time_usec() + SUPERBITRF_BIND_RECV_TIME) % 0xFFFFFFFF; + superbitrf.state = 0; + break; + } break; - case 9: - // Start receiving - cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); - superbitrf.state++; + + /* When the receiver is synchronizing with the transmitter */ + case SUPERBITRF_SYNCING_A: + case SUPERBITRF_SYNCING_B: + +#ifdef RADIO_CONTROL_LED + slowLedCpt++; + if (slowLedCpt > 5000) { + + LED_TOGGLE(RADIO_CONTROL_LED); + slowLedCpt = 0; + } +#endif + + /* Switch the different states */ + switch (superbitrf.state) { + case 0: + // When there is a timeout + if (superbitrf.timer < get_sys_time_usec()) { + superbitrf.state++; + } + break; + case 1: + // Abort the receive + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2); + superbitrf.state++; + break; + case 2: + // Switch channel, sop code, data code and crc + superbitrf.channel_idx = (IS_DSM2(superbitrf.protocol) + || SUPERBITRF_FORCE_DSM2) ? (superbitrf.channel_idx + 1) % 2 : (superbitrf.channel_idx + 1) % 23; + pn_row = (IS_DSM2(superbitrf.protocol) + || SUPERBITRF_FORCE_DSM2) ? superbitrf.channels[superbitrf.channel_idx] % 5 : + (superbitrf.channels[superbitrf.channel_idx] - 2) % 5; + + cyrf6936_write_chan_sop_data_crc(&superbitrf.cyrf6936, superbitrf.channels[superbitrf.channel_idx], + pn_codes[pn_row][superbitrf.sop_col], + pn_codes[pn_row][superbitrf.data_col], + superbitrf.crc_seed); + superbitrf.state++; + break; + case 3: + // Create a new packet when no packet loss + if (!superbitrf.packet_loss) { + superbitrf.packet_loss_bit = !superbitrf.packet_loss_bit; + if (IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) { + tx_packet[0] = ~superbitrf.bind_mfg_id[2]; + tx_packet[1] = (~superbitrf.bind_mfg_id[3]) + 1 + superbitrf.packet_loss_bit; + } else { + tx_packet[0] = superbitrf.bind_mfg_id[2]; + tx_packet[1] = (superbitrf.bind_mfg_id[3]) + 1 + superbitrf.packet_loss_bit; + } + + packet_size = (superbitrf.tx_insert_idx - superbitrf.tx_extract_idx + SUPERBITRF_TX_BUFFER_SIZE % + SUPERBITRF_TX_BUFFER_SIZE); + if (packet_size > 14) { + packet_size = 14; + } + + for (i = 0; i < packet_size; i++) { + tx_packet[i + 2] = superbitrf.tx_buffer[(superbitrf.tx_extract_idx + i) % SUPERBITRF_TX_BUFFER_SIZE]; + } + } + + // Send a packet + cyrf6936_send(&superbitrf.cyrf6936, tx_packet, packet_size + 2); + + // Update the packet extraction + if (!superbitrf.packet_loss) { + superbitrf.tx_extract_idx = (superbitrf.tx_extract_idx + packet_size) % SUPERBITRF_TX_BUFFER_SIZE; + } + + superbitrf.state++; + break; + case 4: + //TODO: check timeout? (Waiting for send) + break; + case 5: + superbitrf.state = 7; + break; + // Start receiving + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + superbitrf.timer = (get_sys_time_usec() + SUPERBITRF_DATARECVB_TIME) % 0xFFFFFFFF; + superbitrf.state++; + break; + case 6: + // Wait for telemetry data + if (superbitrf.timer < get_sys_time_usec()) { + superbitrf.state++; + } + break; + case 7: + // When DSMX we don't need to switch + if (IS_DSMX(superbitrf.protocol) && !SUPERBITRF_FORCE_DSM2) { + superbitrf.state++; + superbitrf.channel = superbitrf.channels[superbitrf.channel_idx]; + break; + } + + // Switch channel, sop code, data code and crc + superbitrf.channel = (superbitrf.channel + 2) % 0x4F; //TODO fix define + superbitrf.crc_seed = ~superbitrf.crc_seed; + pn_row = (IS_DSM2(superbitrf.protocol) + || SUPERBITRF_FORCE_DSM2) ? superbitrf.channel % 5 : (superbitrf.channel - 2) % 5; + + cyrf6936_write_chan_sop_data_crc(&superbitrf.cyrf6936, superbitrf.channel, + pn_codes[pn_row][superbitrf.sop_col], + pn_codes[pn_row][superbitrf.data_col], + superbitrf.crc_seed); + + superbitrf.state++; + break; + case 8: + // Start receiving + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + superbitrf.state++; + break; + default: + // Set the timer + superbitrf.timer = (get_sys_time_usec() + SUPERBITRF_SYNC_RECV_TIME) % 0xFFFFFFFF; + superbitrf.state = 0; + break; + } break; + + /* Normal transfer mode */ + case SUPERBITRF_TRANSFER: + +#ifdef RADIO_CONTROL_LED + LED_ON(RADIO_CONTROL_LED); +#endif + + /* Switch the different states */ + switch (superbitrf.state) { + case 0: + // Fixing timer overflow + if (superbitrf.timer_overflow && get_sys_time_usec() <= superbitrf.timer) { + superbitrf.timer_overflow = FALSE; + } + + // When there is a timeout + if (superbitrf.timer < get_sys_time_usec() && !superbitrf.timer_overflow) { + superbitrf.transfer_timeouts++; + superbitrf.timeouts++; + superbitrf.state++; + } + + // We really lost the communication + if (superbitrf.timeouts > 100) { + superbitrf.state = 0; + superbitrf.resync_count++; + superbitrf.status = SUPERBITRF_SYNCING_A; + } + break; + case 1: + // Abort the receive + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2); + superbitrf.state++; + + // Set the timer + superbitrf.timer = (get_sys_time_usec() + SUPERBITRF_DATARECV_TIME) % 0xFFFFFFFF; + if (superbitrf.timer < get_sys_time_usec()) { + superbitrf.timer_overflow = TRUE; + } else { + superbitrf.timer_overflow = FALSE; + } + + // Only send on channel 2 + if (superbitrf.crc_seed != ((superbitrf.bind_mfg_id[0] << 8) + superbitrf.bind_mfg_id[1])) { + superbitrf.state = 8; + } + break; + case 2: + // Wait before sending (FIXME??) + superbitrf.state++; + break; + case 3: + // Create a new packet when no packet loss + if (!superbitrf.packet_loss) { + superbitrf.packet_loss_bit = !superbitrf.packet_loss_bit; + if (IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) { + tx_packet[0] = ~superbitrf.bind_mfg_id[2]; + tx_packet[1] = ((~superbitrf.bind_mfg_id[3]) + 1 + superbitrf.packet_loss_bit) % 0xFF; + } else { + tx_packet[0] = superbitrf.bind_mfg_id[2]; + tx_packet[1] = ((superbitrf.bind_mfg_id[3]) + 1 + superbitrf.packet_loss_bit) % 0xFF; + } + + packet_size = (superbitrf.tx_insert_idx - superbitrf.tx_extract_idx + SUPERBITRF_TX_BUFFER_SIZE % + SUPERBITRF_TX_BUFFER_SIZE); + if (packet_size > 14) { + packet_size = 14; + } + + for (i = 0; i < packet_size; i++) { + tx_packet[i + 2] = superbitrf.tx_buffer[(superbitrf.tx_extract_idx + i) % SUPERBITRF_TX_BUFFER_SIZE]; + } + } + + // Send a packet + cyrf6936_send(&superbitrf.cyrf6936, tx_packet, packet_size + 2); + + // Update the packet extraction + if (!superbitrf.packet_loss) { + superbitrf.tx_extract_idx = (superbitrf.tx_extract_idx + packet_size) % SUPERBITRF_TX_BUFFER_SIZE; + } + + superbitrf.state++; + break; + case 4: + //TODO: check timeout? (Waiting for send) + break; + case 5: + // Start receiving + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + superbitrf.state++; + break; + case 6: + // Fixing timer overflow + if (superbitrf.timer_overflow && get_sys_time_usec() <= superbitrf.timer) { + superbitrf.timer_overflow = FALSE; + } + + // Waiting for data receive + if (superbitrf.timer < get_sys_time_usec() && !superbitrf.timer_overflow) { + superbitrf.state++; + } + break; + case 7: + // Abort the receive + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_abort_receive, 2); + superbitrf.state++; + break; + case 8: + // Switch channel, sop code, data code and crc + superbitrf.channel_idx = (IS_DSM2(superbitrf.protocol) + || SUPERBITRF_FORCE_DSM2) ? (superbitrf.channel_idx + 1) % 2 : (superbitrf.channel_idx + 1) % 23; + superbitrf.channel = superbitrf.channels[superbitrf.channel_idx]; + superbitrf.crc_seed = ~superbitrf.crc_seed; + pn_row = (IS_DSM2(superbitrf.protocol) + || SUPERBITRF_FORCE_DSM2) ? superbitrf.channel % 5 : (superbitrf.channel - 2) % 5; + + cyrf6936_write_chan_sop_data_crc(&superbitrf.cyrf6936, superbitrf.channel, + pn_codes[pn_row][superbitrf.sop_col], + pn_codes[pn_row][superbitrf.data_col], + superbitrf.crc_seed); + + superbitrf.state++; + break; + case 9: + // Start receiving + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + superbitrf.state++; + break; + default: + // Set the timer + if (superbitrf.crc_seed != ((superbitrf.bind_mfg_id[0] << 8) + superbitrf.bind_mfg_id[1])) { + superbitrf.timer = (superbitrf.timer - SUPERBITRF_DATARECV_TIME + SUPERBITRF_RECV_TIME) % 0xFFFFFFFF; + } else { + superbitrf.timer = (superbitrf.timer - SUPERBITRF_DATARECV_TIME + SUPERBITRF_RECV_SHORT_TIME) % 0xFFFFFFFF; + } + if (superbitrf.timer < get_sys_time_usec()) { + superbitrf.timer_overflow = TRUE; + } else { + superbitrf.timer_overflow = FALSE; + } + + superbitrf.state = 0; + break; + } + break; + + /* Should not come here */ default: - // Set the timer - if(superbitrf.crc_seed != ((superbitrf.bind_mfg_id[0] << 8) + superbitrf.bind_mfg_id[1])) - superbitrf.timer = (superbitrf.timer - SUPERBITRF_DATARECV_TIME + SUPERBITRF_RECV_TIME) % 0xFFFFFFFF; - else - superbitrf.timer = (superbitrf.timer - SUPERBITRF_DATARECV_TIME + SUPERBITRF_RECV_SHORT_TIME) % 0xFFFFFFFF; - if(superbitrf.timer < get_sys_time_usec()) - superbitrf.timer_overflow = TRUE; - else - superbitrf.timer_overflow = FALSE; - - superbitrf.state = 0; break; - } - break; - - /* Should not come here */ - default: - break; } } /** * When we receive a packet this callback is called */ -static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, uint8_t packet[]) { +static inline void superbitrf_receive_packet_cb(bool_t error, uint8_t status, uint8_t packet[]) +{ int i; uint16_t sum; /* Switch on the status of the superbitRF */ switch (superbitrf.status) { - /* When we are in binding mode */ - case SUPERBITRF_BINDING: - // Check if the MFG id is exactly the same - if (packet[0] != packet[4] || packet[1] != packet[5] || packet[2] != packet[6] || packet[3] != packet[7]) { - // Start receiving without changing channel - superbitrf.state = 3; + /* When we are in binding mode */ + case SUPERBITRF_BINDING: + // Check if the MFG id is exactly the same + if (packet[0] != packet[4] || packet[1] != packet[5] || packet[2] != packet[6] || packet[3] != packet[7]) { + // Start receiving without changing channel + superbitrf.state = 3; + break; + } + + // Calculate the first sum + sum = 384 - 0x10; + for (i = 0; i < 8; i++) { + sum += packet[i]; + } + + // Check the first sum + if (packet[8] != sum >> 8 || packet[9] != (sum & 0xFF)) { + // Start receiving without changing channel + superbitrf.state = 3; + break; + } + + // Calculate second sum + for (i = 8; i < 14; i++) { + sum += packet[i]; + } + + // Check the second sum + if (packet[14] != sum >> 8 || packet[15] != (sum & 0xFF)) { + // Start receiving without changing channel + superbitrf.state = 3; + break; + } + + // Update the mfg id, number of channels and protocol + uint32_t mfg_id = ((~packet[3] & 0xFF) << 24 | (~packet[2] & 0xFF) << 16 | + (~packet[1] & 0xFF) << 8 | (~packet[0] & 0xFF)); + superbitrf_set_mfg_id(mfg_id); + + superbitrf.num_channels = packet[11]; + superbitrf_set_protocol(packet[12]); + + // Store all the persistent settings. + // In case we have the superbit setting file loaded and persistent settings + // enabled in the airframe file this will store our binding information and + // survive a reboot. + settings_StoreSettings(1); + + // Update the status of the receiver + superbitrf.state = 0; + superbitrf.status = SUPERBITRF_INIT_TRANSFER; break; - } - // Calculate the first sum - sum = 384 - 0x10; - for (i = 0; i < 8; i++) - sum += packet[i]; + /* When we receive a packet during syncing first channel A */ + case SUPERBITRF_SYNCING_A: + // Check the MFG id + if (error && !(status & CYRF_BAD_CRC)) { + // Start receiving TODO: Fix nicely + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + break; + } + if ((IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) && + (packet[0] != (~superbitrf.bind_mfg_id[2] & 0xFF) || (packet[1] != (~superbitrf.bind_mfg_id[3] & 0xFF) && + packet[1] != (~superbitrf.bind_mfg_id[3] & 0xFF) + 1))) { + // Start receiving TODO: Fix nicely + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + break; + } + if ((IS_DSMX(superbitrf.protocol) && !SUPERBITRF_FORCE_DSM2) && + (packet[0] != (superbitrf.bind_mfg_id[2] & 0xFF) || (packet[1] != (superbitrf.bind_mfg_id[3] & 0xFF) && + packet[1] != (superbitrf.bind_mfg_id[3] & 0xFF) + 1))) { + // Start receiving TODO: Fix nicely + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + break; + } - // Check the first sum - if (packet[8] != sum >> 8 || packet[9] != (sum & 0xFF)) { - // Start receiving without changing channel - superbitrf.state = 3; + // If the CRC is wrong invert + if (error && (status & CYRF_BAD_CRC)) { + superbitrf.crc_seed = ~superbitrf.crc_seed; + } + + // When we receive a data packet + if (packet[1] != (~superbitrf.bind_mfg_id[3] & 0xFF) && packet[1] != (superbitrf.bind_mfg_id[3] & 0xFF)) { + superbitrf.uplink_count++; + + // Check if it is a data loss packet + if (packet[1] != (~superbitrf.bind_mfg_id[3] + 1 + superbitrf.packet_loss_bit) % 0xFF + && packet[1] != (superbitrf.bind_mfg_id[3] + 1 + superbitrf.packet_loss_bit) % 0xFF) { + superbitrf.packet_loss = TRUE; + } else { + superbitrf.packet_loss = FALSE; + } + + // When it is a data packet, parse the packet if not busy already + if (!dl_msg_available && !superbitrf.packet_loss) { + for (i = 2; i < superbitrf.cyrf6936.rx_count; i++) { + parse_pprz(&superbitrf.rx_transport, packet[i]); + + // When we have a full message + if (superbitrf.rx_transport.trans_rx.msg_received) { + pprz_parse_payload(&superbitrf.rx_transport); + superbitrf.rx_transport.trans_rx.msg_received = FALSE; + } + } + } + break; + } + + if (IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) { + superbitrf.channels[0] = superbitrf.channel; + superbitrf.channels[1] = superbitrf.channel; + + superbitrf.state = 1; + superbitrf.status = SUPERBITRF_SYNCING_B; + } else { + superbitrf.timeouts = 0; + superbitrf.state = 1; + superbitrf.status = SUPERBITRF_TRANSFER; + } break; - } - // Calculate second sum - for (i = 8; i < 14; i++) - sum += packet[i]; + /* When we receive a packet during syncing second channel B */ + case SUPERBITRF_SYNCING_B: + // Check the MFG id + if (error && !(status & CYRF_BAD_CRC)) { + // Start receiving TODO: Fix nicely + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + break; + } + if ((IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) && + (packet[0] != (~superbitrf.bind_mfg_id[2] & 0xFF) || (packet[1] != (~superbitrf.bind_mfg_id[3] & 0xFF) && + packet[1] != (~superbitrf.bind_mfg_id[3] & 0xFF) + 1 && packet[1] != (~superbitrf.bind_mfg_id[3] & 0xFF) + 2))) { + // Start receiving TODO: Fix nicely + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + break; + } + if ((IS_DSMX(superbitrf.protocol) && !SUPERBITRF_FORCE_DSM2) && + (packet[0] != (superbitrf.bind_mfg_id[2] & 0xFF) || (packet[1] != (superbitrf.bind_mfg_id[3] & 0xFF) && + packet[1] != (superbitrf.bind_mfg_id[3] & 0xFF) + 1 && packet[1] != (superbitrf.bind_mfg_id[3] & 0xFF) + 2))) { + // Start receiving TODO: Fix nicely + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + break; + } - // Check the second sum - if (packet[14] != sum >> 8 || packet[15] != (sum & 0xFF)) { - // Start receiving without changing channel - superbitrf.state = 3; + // If the CRC is wrong invert + if (error && (status & CYRF_BAD_CRC)) { + superbitrf.crc_seed = ~superbitrf.crc_seed; + } + + // When we receive a data packet + if (packet[1] != (~superbitrf.bind_mfg_id[3] & 0xFF) && packet[1] != (superbitrf.bind_mfg_id[3] & 0xFF)) { + superbitrf.uplink_count++; + + // When it is a data packet, parse the packet if not busy already + if (!dl_msg_available) { + for (i = 2; i < superbitrf.cyrf6936.rx_count; i++) { + parse_pprz(&superbitrf.rx_transport, packet[i]); + + // When we have a full message + if (superbitrf.rx_transport.trans_rx.msg_received) { + pprz_parse_payload(&superbitrf.rx_transport); + superbitrf.rx_transport.trans_rx.msg_received = FALSE; + } + } + } + break; + } + + // Set the channel + if (superbitrf.channels[0] != superbitrf.channel) { + superbitrf.channels[0] = superbitrf.channel; + superbitrf.channel_idx = 0; + } else { + superbitrf.channels[1] = superbitrf.channel; + superbitrf.channel_idx = 1; + } + + // When the channels aren't the same go to transfer mode + if (superbitrf.channels[1] != superbitrf.channels[0]) { + superbitrf.state = 1; + superbitrf.status = SUPERBITRF_TRANSFER; + superbitrf.timeouts = 0; + } break; - } - // Update the mfg id, number of channels and protocol - uint32_t mfg_id = ((~packet[3] &0xFF) << 24 | (~packet[2] &0xFF) << 16 | - (~packet[1] &0xFF) << 8 | (~packet[0] &0xFF)); - superbitrf_set_mfg_id(mfg_id); + /* When we receive a packet during transfer */ + case SUPERBITRF_TRANSFER: + // Check the MFG id + if (error) { + // Start receiving TODO: Fix nicely + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + break; + } + if ((IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) && + (packet[0] != (~superbitrf.bind_mfg_id[2] & 0xFF) || (packet[1] != (~superbitrf.bind_mfg_id[3] & 0xFF) && + packet[1] != (~superbitrf.bind_mfg_id[3] & 0xFF) + 1 && packet[1] != (~superbitrf.bind_mfg_id[3] & 0xFF) + 2))) { + // Start receiving TODO: Fix nicely + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + break; + } + if ((IS_DSMX(superbitrf.protocol) && !SUPERBITRF_FORCE_DSM2) && + (packet[0] != (superbitrf.bind_mfg_id[2] & 0xFF) || (packet[1] != (superbitrf.bind_mfg_id[3] & 0xFF) && + packet[1] != (superbitrf.bind_mfg_id[3] & 0xFF) + 1 && packet[1] != (superbitrf.bind_mfg_id[3] & 0xFF) + 2))) { + // Start receiving TODO: Fix nicely + cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + break; + } - superbitrf.num_channels = packet[11]; - superbitrf_set_protocol(packet[12]); + // Check if it is a RC packet + if (packet[1] == (~superbitrf.bind_mfg_id[3] & 0xFF) || packet[1] == (superbitrf.bind_mfg_id[3] & 0xFF)) { + superbitrf.rc_count++; - // Store all the persistent settings. - // In case we have the superbit setting file loaded and persistent settings - // enabled in the airframe file this will store our binding information and - // survive a reboot. - settings_StoreSettings(1); + // Parse the packet + superbitrf_radio_to_channels(&packet[2], superbitrf.num_channels, superbitrf.resolution, superbitrf.rc_values); + superbitrf.rc_frame_available = TRUE; - // Update the status of the receiver - superbitrf.state = 0; - superbitrf.status = SUPERBITRF_INIT_TRANSFER; - break; + // Calculate the timing (seperately for the channel switches) + if (superbitrf.crc_seed != ((superbitrf.bind_mfg_id[0] << 8) + superbitrf.bind_mfg_id[1])) { + superbitrf.timing2 = get_sys_time_usec() - (superbitrf.timer - SUPERBITRF_RECV_TIME); + } else { + superbitrf.timing1 = get_sys_time_usec() - (superbitrf.timer - SUPERBITRF_RECV_SHORT_TIME); + } - /* When we receive a packet during syncing first channel A */ - case SUPERBITRF_SYNCING_A: - // Check the MFG id - if(error && !(status & CYRF_BAD_CRC)) { - // Start receiving TODO: Fix nicely - cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); - break; - } - if((IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) && - (packet[0] != (~superbitrf.bind_mfg_id[2]&0xFF) || (packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF) && - packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)+1))) { - // Start receiving TODO: Fix nicely - cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); - break; - } - if((IS_DSMX(superbitrf.protocol) && !SUPERBITRF_FORCE_DSM2) && - (packet[0] != (superbitrf.bind_mfg_id[2]&0xFF) || (packet[1] != (superbitrf.bind_mfg_id[3]&0xFF) && - packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)+1))) { - // Start receiving TODO: Fix nicely - cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); - break; - } + // Go to next receive + superbitrf.state = 1; + superbitrf.timeouts = 0; + } else { + superbitrf.uplink_count++; - // If the CRC is wrong invert - if (error && (status & CYRF_BAD_CRC)) - superbitrf.crc_seed = ~superbitrf.crc_seed; + // Check if it is a data loss packet + if (packet[1] != (~superbitrf.bind_mfg_id[3] + 1 + superbitrf.packet_loss_bit) + && packet[1] != (superbitrf.bind_mfg_id[3] + 1 + superbitrf.packet_loss_bit)) { + superbitrf.packet_loss = TRUE; + } else { + superbitrf.packet_loss = FALSE; + } - // When we receive a data packet - if(packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF) && packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)) { - superbitrf.uplink_count++; - - // Check if it is a data loss packet - if(packet[1] != (~superbitrf.bind_mfg_id[3] + 1 + superbitrf.packet_loss_bit)%0xFF && packet[1] != (superbitrf.bind_mfg_id[3] + 1 + superbitrf.packet_loss_bit)%0xFF) - superbitrf.packet_loss = TRUE; - else superbitrf.packet_loss = FALSE; - // When it is a data packet, parse the packet if not busy already - if(!dl_msg_available && !superbitrf.packet_loss) { - for(i = 2; i < superbitrf.cyrf6936.rx_count; i++) { - parse_pprz(&superbitrf.rx_transport, packet[i]); + // When it is a data packet, parse the packet if not busy already + if (!dl_msg_available && !superbitrf.packet_loss) { + for (i = 2; i < superbitrf.cyrf6936.rx_count; i++) { + parse_pprz(&superbitrf.rx_transport, packet[i]); - // When we have a full message - if (superbitrf.rx_transport.trans_rx.msg_received) { - pprz_parse_payload(&superbitrf.rx_transport); - superbitrf.rx_transport.trans_rx.msg_received = FALSE; + // When we have a full message + if (superbitrf.rx_transport.trans_rx.msg_received) { + pprz_parse_payload(&superbitrf.rx_transport); + superbitrf.rx_transport.trans_rx.msg_received = FALSE; + } } } + + // Update the state + superbitrf.state = 7; } break; - } - if(IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) { - superbitrf.channels[0] = superbitrf.channel; - superbitrf.channels[1] = superbitrf.channel; - - superbitrf.state = 1; - superbitrf.status = SUPERBITRF_SYNCING_B; - } else { - superbitrf.timeouts = 0; - superbitrf.state = 1; - superbitrf.status = SUPERBITRF_TRANSFER; - } - break; - - /* When we receive a packet during syncing second channel B */ - case SUPERBITRF_SYNCING_B: - // Check the MFG id - if(error && !(status & CYRF_BAD_CRC)) { - // Start receiving TODO: Fix nicely - cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); + /* Should not come here */ + default: break; - } - if((IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) && - (packet[0] != (~superbitrf.bind_mfg_id[2]&0xFF) || (packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF) && - packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)+1 && packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)+2))) { - // Start receiving TODO: Fix nicely - cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); - break; - } - if((IS_DSMX(superbitrf.protocol) && !SUPERBITRF_FORCE_DSM2) && - (packet[0] != (superbitrf.bind_mfg_id[2]&0xFF) || (packet[1] != (superbitrf.bind_mfg_id[3]&0xFF) && - packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)+1 && packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)+2))) { - // Start receiving TODO: Fix nicely - cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); - break; - } - - // If the CRC is wrong invert - if (error && (status & CYRF_BAD_CRC)) - superbitrf.crc_seed = ~superbitrf.crc_seed; - - // When we receive a data packet - if(packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF) && packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)) { - superbitrf.uplink_count++; - - // When it is a data packet, parse the packet if not busy already - if(!dl_msg_available) { - for(i = 2; i < superbitrf.cyrf6936.rx_count; i++) { - parse_pprz(&superbitrf.rx_transport, packet[i]); - - // When we have a full message - if (superbitrf.rx_transport.trans_rx.msg_received) { - pprz_parse_payload(&superbitrf.rx_transport); - superbitrf.rx_transport.trans_rx.msg_received = FALSE; - } - } - } - break; - } - - // Set the channel - if(superbitrf.channels[0] != superbitrf.channel) { - superbitrf.channels[0] = superbitrf.channel; - superbitrf.channel_idx = 0; - } - else { - superbitrf.channels[1] = superbitrf.channel; - superbitrf.channel_idx = 1; - } - - // When the channels aren't the same go to transfer mode - if(superbitrf.channels[1] != superbitrf.channels[0]) { - superbitrf.state = 1; - superbitrf.status = SUPERBITRF_TRANSFER; - superbitrf.timeouts = 0; - } - break; - - /* When we receive a packet during transfer */ - case SUPERBITRF_TRANSFER: - // Check the MFG id - if(error) { - // Start receiving TODO: Fix nicely - cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); - break; - } - if((IS_DSM2(superbitrf.protocol) || SUPERBITRF_FORCE_DSM2) && - (packet[0] != (~superbitrf.bind_mfg_id[2]&0xFF) || (packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF) && - packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)+1 && packet[1] != (~superbitrf.bind_mfg_id[3]&0xFF)+2))) { - // Start receiving TODO: Fix nicely - cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); - break; - } - if((IS_DSMX(superbitrf.protocol) && !SUPERBITRF_FORCE_DSM2) && - (packet[0] != (superbitrf.bind_mfg_id[2]&0xFF) || (packet[1] != (superbitrf.bind_mfg_id[3]&0xFF) && - packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)+1 && packet[1] != (superbitrf.bind_mfg_id[3]&0xFF)+2))) { - // Start receiving TODO: Fix nicely - cyrf6936_multi_write(&superbitrf.cyrf6936, cyrf_start_receive, 2); - break; - } - - // Check if it is a RC packet - if(packet[1] == (~superbitrf.bind_mfg_id[3]&0xFF) || packet[1] == (superbitrf.bind_mfg_id[3]&0xFF)) { - superbitrf.rc_count++; - - // Parse the packet - superbitrf_radio_to_channels(&packet[2], superbitrf.num_channels, superbitrf.resolution, superbitrf.rc_values); - superbitrf.rc_frame_available = TRUE; - - // Calculate the timing (seperately for the channel switches) - if(superbitrf.crc_seed != ((superbitrf.bind_mfg_id[0] << 8) + superbitrf.bind_mfg_id[1])) - superbitrf.timing2 = get_sys_time_usec() - (superbitrf.timer - SUPERBITRF_RECV_TIME); - else - superbitrf.timing1 = get_sys_time_usec() - (superbitrf.timer - SUPERBITRF_RECV_SHORT_TIME); - - // Go to next receive - superbitrf.state = 1; - superbitrf.timeouts = 0; - } else { - superbitrf.uplink_count++; - - // Check if it is a data loss packet - if(packet[1] != (~superbitrf.bind_mfg_id[3] + 1 + superbitrf.packet_loss_bit) && packet[1] != (superbitrf.bind_mfg_id[3] + 1 + superbitrf.packet_loss_bit)) - superbitrf.packet_loss = TRUE; - else - superbitrf.packet_loss = FALSE; - - superbitrf.packet_loss = FALSE; - - // When it is a data packet, parse the packet if not busy already - if(!dl_msg_available && !superbitrf.packet_loss) { - for(i = 2; i < superbitrf.cyrf6936.rx_count; i++) { - parse_pprz(&superbitrf.rx_transport, packet[i]); - - // When we have a full message - if (superbitrf.rx_transport.trans_rx.msg_received) { - pprz_parse_payload(&superbitrf.rx_transport); - superbitrf.rx_transport.trans_rx.msg_received = FALSE; - } - } - } - - // Update the state - superbitrf.state = 7; - } - break; - - /* Should not come here */ - default: - break; } } -static inline void superbitrf_send_packet_cb(bool_t error __attribute__((unused))) { +static inline void superbitrf_send_packet_cb(bool_t error __attribute__((unused))) +{ /* Switch on the status of the superbitRF */ switch (superbitrf.status) { - /* When we are synchronizing */ - case SUPERBITRF_SYNCING_A: - case SUPERBITRF_SYNCING_B: - // When we successfully or unsuccessfully send a data packet - if(superbitrf.state == 4) - superbitrf.state++; - break; + /* When we are synchronizing */ + case SUPERBITRF_SYNCING_A: + case SUPERBITRF_SYNCING_B: + // When we successfully or unsuccessfully send a data packet + if (superbitrf.state == 4) { + superbitrf.state++; + } + break; - /* When we are in transfer mode */ - case SUPERBITRF_TRANSFER: - // When we successfully or unsuccessfully send a packet - if(superbitrf.state == 4) - superbitrf.state++; - break; + /* When we are in transfer mode */ + case SUPERBITRF_TRANSFER: + // When we successfully or unsuccessfully send a packet + if (superbitrf.state == 4) { + superbitrf.state++; + } + break; - /* Should not come here */ - default: - break; + /* Should not come here */ + default: + break; } } /** * Parse a radio channel packet */ -static inline void superbitrf_radio_to_channels(uint8_t* data, uint8_t nb_channels, bool_t is_11bit, int16_t* channels) { - int i; - uint8_t bit_shift = (is_11bit)? 11:10; - int16_t value_max = (is_11bit)? 0x07FF: 0x03FF; +static inline void superbitrf_radio_to_channels(uint8_t *data, uint8_t nb_channels, bool_t is_11bit, int16_t *channels) +{ + int i; + uint8_t bit_shift = (is_11bit) ? 11 : 10; + int16_t value_max = (is_11bit) ? 0x07FF : 0x03FF; - for (i=0; i<7; i++) { - const int16_t tmp = ((data[2*i]<<8) + data[2*i+1]) & 0x7FFF; - const uint8_t chan = (tmp >> bit_shift) & 0x0F; - const int16_t val = (tmp&value_max); + for (i = 0; i < 7; i++) { + const int16_t tmp = ((data[2 * i] << 8) + data[2 * i + 1]) & 0x7FFF; + const uint8_t chan = (tmp >> bit_shift) & 0x0F; + const int16_t val = (tmp & value_max); - if(chan < nb_channels) { - channels[chan] = val; + if (chan < nb_channels) { + channels[chan] = val; - // Scale the channel - if(is_11bit) { - channels[chan] -= 0x400; - channels[chan] *= MAX_PPRZ/0x2AC; - } else { - channels[chan] -= 0x200; - channels[chan] *= MAX_PPRZ/0x156; - } + // Scale the channel + if (is_11bit) { + channels[chan] -= 0x400; + channels[chan] *= MAX_PPRZ / 0x2AC; + } else { + channels[chan] -= 0x200; + channels[chan] *= MAX_PPRZ / 0x156; } } + } } /** * Generate the channels */ -static inline void superbitrf_gen_dsmx_channels(void) { +static inline void superbitrf_gen_dsmx_channels(void) +{ // Calculate the DSMX channels int idx = 0; uint32_t id = ~((superbitrf.bind_mfg_id[0] << 24) | (superbitrf.bind_mfg_id[1] << 16) | - (superbitrf.bind_mfg_id[2] << 8) | (superbitrf.bind_mfg_id[3] << 0)); + (superbitrf.bind_mfg_id[2] << 8) | (superbitrf.bind_mfg_id[3] << 0)); uint32_t id_tmp = id; // While not all channels are set - while(idx < 23) { + while (idx < 23) { int i; int count_3_27 = 0, count_28_51 = 0, count_52_76 = 0; id_tmp = id_tmp * 0x0019660D + 0x3C6EF35F; // Randomization uint8_t next_ch = ((id_tmp >> 8) % 0x49) + 3; // Use least-significant byte and must be larger than 3 - if (((next_ch ^ id) & 0x01 ) == 0) + if (((next_ch ^ id) & 0x01) == 0) { continue; + } // Go trough all already set channels for (i = 0; i < idx; i++) { // Channel is already used - if(superbitrf.channels[i] == next_ch) + if (superbitrf.channels[i] == next_ch) { break; + } // Count the channel groups - if(superbitrf.channels[i] <= 27) + if (superbitrf.channels[i] <= 27) { count_3_27++; - else if (superbitrf.channels[i] <= 51) + } else if (superbitrf.channels[i] <= 51) { count_28_51++; - else + } else { count_52_76++; + } } // When channel is already used continue - if (i != idx) + if (i != idx) { continue; + } // Set the channel when channel groups aren't full if ((next_ch < 28 && count_3_27 < 8) // Channels 3-27: max 8 diff --git a/sw/airborne/subsystems/datalink/superbitrf.h b/sw/airborne/subsystems/datalink/superbitrf.h index afb73e5482..46ecfdc1a3 100644 --- a/sw/airborne/subsystems/datalink/superbitrf.h +++ b/sw/airborne/subsystems/datalink/superbitrf.h @@ -54,18 +54,18 @@ enum SuperbitRFStatus { /* The different resolutions a transmitter can be in */ enum dsm_resolution { - SUPERBITRF_10_BIT_RESOLUTION = 0x00, /**< The transmitter has a 10 bit resolution */ - SUPERBITRF_11_BIT_RESOLUTION = 0x01, /**< The transmitter has a 11 bit resolution */ + SUPERBITRF_10_BIT_RESOLUTION = 0x00, /**< The transmitter has a 10 bit resolution */ + SUPERBITRF_11_BIT_RESOLUTION = 0x01, /**< The transmitter has a 11 bit resolution */ }; /* The different protocols a transmitter can send */ enum dsm_protocol { - DSM_DSM2_1 = 0x01, /**< The original DSM2 protocol with 1 packet of data */ - DSM_DSM2_2 = 0x02, /**< The original DSM2 protocol with 2 packets of data */ - DSM_DSM2P = 0x10, /**< Our own DSM2 Paparazzi protocol */ - DSM_DSMXP = 0x11, /**< Our own DSMX Paparazzi protocol */ - DSM_DSMX_1 = 0xA2, /**< The original DSMX protocol with 1 packet of data */ - DSM_DSMX_2 = 0xB2, /**< The original DSMX protocol with 2 packets of data */ + DSM_DSM2_1 = 0x01, /**< The original DSM2 protocol with 1 packet of data */ + DSM_DSM2_2 = 0x02, /**< The original DSM2 protocol with 2 packets of data */ + DSM_DSM2P = 0x10, /**< Our own DSM2 Paparazzi protocol */ + DSM_DSMXP = 0x11, /**< Our own DSMX Paparazzi protocol */ + DSM_DSMX_1 = 0xA2, /**< The original DSMX protocol with 1 packet of data */ + DSM_DSMX_2 = 0xB2, /**< The original DSMX protocol with 2 packets of data */ }; #define IS_DSM2(x) (x == DSM_DSM2P || x == DSM_DSM2_1 || x == DSM_DSM2_2) #define IS_DSMX(x) (!IS_DSM2(x)) diff --git a/sw/airborne/subsystems/datalink/telemetry.c b/sw/airborne/subsystems/datalink/telemetry.c index 9f28188370..d7f512af7d 100644 --- a/sw/airborne/subsystems/datalink/telemetry.c +++ b/sw/airborne/subsystems/datalink/telemetry.c @@ -35,9 +35,10 @@ * @param _cb callback function, called according to telemetry mode and specified period * @return TRUE if message registered with success, FALSE otherwise */ -bool_t register_periodic_telemetry(struct pprz_telemetry * _pt, const char * _msg, telemetry_cb _cb) { +bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb) +{ // return FALSE if NULL is passed as pprz_telemetry - if (_pt == NULL) return FALSE; + if (_pt == NULL) { return FALSE; } // look for message name uint8_t i; for (i = 0; i < _pt->nb; i++) { @@ -46,8 +47,7 @@ bool_t register_periodic_telemetry(struct pprz_telemetry * _pt, const char * _ms if (_pt->msgs[i].cb == NULL) { _pt->msgs[i].cb = _cb; return TRUE; - } - else { return FALSE; } + } else { return FALSE; } } } // message name is not in telemetry file @@ -63,7 +63,8 @@ bool_t register_periodic_telemetry(struct pprz_telemetry * _pt, const char * _ms * @param _mode telemetry mode * @param _id id of the message in telemetry system (see var//generated/periodic_telemetry.h) */ -void periodic_telemetry_err_report(uint8_t _process, uint8_t _mode, uint8_t _id) { +void periodic_telemetry_err_report(uint8_t _process, uint8_t _mode, uint8_t _id) +{ uint8_t process = _process; uint8_t mode = _mode; uint8_t id = _id; diff --git a/sw/airborne/subsystems/datalink/telemetry_common.h b/sw/airborne/subsystems/datalink/telemetry_common.h index 30227b7768..c547d788c9 100644 --- a/sw/airborne/subsystems/datalink/telemetry_common.h +++ b/sw/airborne/subsystems/datalink/telemetry_common.h @@ -57,7 +57,7 @@ struct telemetry_msg { */ struct pprz_telemetry { uint8_t nb; ///< number of messages - struct telemetry_msg* msgs; ///< the list of (msg name, callbacks) + struct telemetry_msg *msgs; ///< the list of (msg name, callbacks) }; /** Register a telemetry callback function. @@ -68,9 +68,10 @@ struct pprz_telemetry { * @return TRUE if message registered with success, FALSE otherwise */ #if PERIODIC_TELEMETRY -extern bool_t register_periodic_telemetry(struct pprz_telemetry * _pt, const char * _msg, telemetry_cb _cb); +extern bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb); #else -static inline bool_t register_periodic_telemetry(struct pprz_telemetry * _pt __attribute__((unused)), const char * _msg __attribute__((unused)), telemetry_cb _cb __attribute__((unused))) { return FALSE; } +static inline bool_t register_periodic_telemetry(struct pprz_telemetry *_pt __attribute__((unused)), + const char *_msg __attribute__((unused)), telemetry_cb _cb __attribute__((unused))) { return FALSE; } #endif #if USE_PERIODIC_TELEMETRY_REPORT diff --git a/sw/airborne/subsystems/datalink/transport.h b/sw/airborne/subsystems/datalink/transport.h index d3dc1a4da0..8887fc5e21 100644 --- a/sw/airborne/subsystems/datalink/transport.h +++ b/sw/airborne/subsystems/datalink/transport.h @@ -85,8 +85,10 @@ enum TransportDataFormat { */ typedef uint8_t (*size_of_t)(void *, uint8_t); typedef int (*check_available_space_t)(void *, struct link_device *, uint8_t); -typedef void (*put_bytes_t)(void *, struct link_device *, enum TransportDataType, enum TransportDataFormat, uint8_t, const void *); -typedef void (*put_named_byte_t)(void *, struct link_device *, enum TransportDataType, enum TransportDataFormat, uint8_t, const char *); +typedef void (*put_bytes_t)(void *, struct link_device *, enum TransportDataType, enum TransportDataFormat, uint8_t, + const void *); +typedef void (*put_named_byte_t)(void *, struct link_device *, enum TransportDataType, enum TransportDataFormat, + uint8_t, const char *); typedef void (*start_message_t)(void *, struct link_device *, uint8_t); typedef void (*end_message_t)(void *, struct link_device *); typedef void (*overrun_t)(void *, struct link_device *); diff --git a/sw/airborne/subsystems/datalink/uart_print.h b/sw/airborne/subsystems/datalink/uart_print.h index e61c7e4491..20928dcf7b 100644 --- a/sw/airborne/subsystems/datalink/uart_print.h +++ b/sw/airborne/subsystems/datalink/uart_print.h @@ -26,35 +26,35 @@ #include "mcu_periph/usb_serial.h" #define _PrintString(out_fun, s) { \ - uint8_t i = 0; \ - while (s[i]) { \ - out_fun(s[i]); \ - i++; \ - } \ -} + uint8_t i = 0; \ + while (s[i]) { \ + out_fun(s[i]); \ + i++; \ + } \ + } -#define _PrintHex(out_fun, c) { \ +#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' }; \ - uint8_t high = (c & 0xF0)>>4; \ - uint8_t low = c & 0x0F; \ - out_fun(hex[high]); \ - out_fun(hex[low]); \ -} \ + '8', '9', 'A', 'B', 'C', 'D', 'E', 'F' }; \ + uint8_t high = (c & 0xF0)>>4; \ + uint8_t low = c & 0x0F; \ + out_fun(hex[high]); \ + out_fun(hex[low]); \ + } \ + +#define _PrintHex16(out_fun, c ) { \ + uint8_t high16 = (uint8_t)(c>>8); \ + uint8_t low16 = (uint8_t)(c); \ + _PrintHex(out_fun, high16); \ + _PrintHex(out_fun, low16); \ + } -#define _PrintHex16(out_fun, c ) { \ - uint8_t high16 = (uint8_t)(c>>8); \ - uint8_t low16 = (uint8_t)(c); \ - _PrintHex(out_fun, high16); \ - _PrintHex(out_fun, low16); \ -} - -#define _PrintHex32(out_fun, c ) { \ - uint16_t high32 = (uint16_t)(c>>16); \ - uint16_t low32 = (uint16_t)(c); \ - _PrintHex16(out_fun, high32); \ - _PrintHex16(out_fun, low32); \ -} +#define _PrintHex32(out_fun, c ) { \ + uint16_t high32 = (uint16_t)(c>>16); \ + uint16_t low32 = (uint16_t)(c); \ + _PrintHex16(out_fun, high32); \ + _PrintHex16(out_fun, low32); \ + } #if USE_UART0 diff --git a/sw/airborne/subsystems/datalink/w5100.c b/sw/airborne/subsystems/datalink/w5100.c index 7fd5d2925e..1b4e38772a 100644 --- a/sw/airborne/subsystems/datalink/w5100.c +++ b/sw/airborne/subsystems/datalink/w5100.c @@ -108,9 +108,9 @@ static const uint16_t RSIZE = 2048; // Max Rx buffer size struct spi_transaction w5100_spi; -static void w5100_close_socket( uint8_t _s ); -static void configure_socket( uint8_t _s, uint8_t _flag, uint16_t _lport, uint16_t _dport, uint8_t *_dest ); -static void w5100_read_data( uint8_t s, volatile uint8_t *src, volatile uint8_t *dst, uint16_t len ); +static void w5100_close_socket(uint8_t _s); +static void configure_socket(uint8_t _s, uint8_t _flag, uint16_t _lport, uint16_t _dport, uint8_t *_dest); +static void w5100_read_data(uint8_t s, volatile uint8_t *src, volatile uint8_t *dst, uint16_t len); static uint16_t w5100_read(uint16_t _addr, uint8_t *_buf, uint16_t _len); static inline void w5100_set(uint16_t _reg, uint8_t _val) @@ -120,10 +120,10 @@ static inline void w5100_set(uint16_t _reg, uint8_t _val) w5100_spi.output_buf[2] = _reg & 0xFF; w5100_spi.output_buf[3] = _val; - spi_submit( &(W5100_SPI_DEV), &w5100_spi ); + spi_submit(&(W5100_SPI_DEV), &w5100_spi); // FIXME: no busy waiting! if really needed add a timeout!!!! - while(w5100_spi.status != SPITransSuccess); + while (w5100_spi.status != SPITransSuccess); } static inline uint8_t w5100_get(uint16_t _reg) @@ -132,45 +132,49 @@ static inline uint8_t w5100_get(uint16_t _reg) w5100_spi.output_buf[1] = _reg >> 8; w5100_spi.output_buf[2] = _reg & 0xFF; - spi_submit( &(W5100_SPI_DEV), &w5100_spi ); + spi_submit(&(W5100_SPI_DEV), &w5100_spi); // FIXME: no busy waiting! if really needed add a timeout!!!! - while(w5100_spi.status != SPITransSuccess); + while (w5100_spi.status != SPITransSuccess); return w5100_spi.input_buf[3]; } static inline void w5100_set_buffer(uint16_t _reg, volatile uint8_t *_buf, uint16_t _len) { - for (int i=0; i<_len; i++) { - w5100_set( _reg, _buf[ i ] ); + for (int i = 0; i < _len; i++) { + w5100_set(_reg, _buf[ i ]); _reg++; } } -static inline void w5100_sock_set(uint8_t _sock, uint16_t _reg, uint8_t _val) { - w5100_set( CH_BASE + _sock * CH_SIZE + _reg, _val ); +static inline void w5100_sock_set(uint8_t _sock, uint16_t _reg, uint8_t _val) +{ + w5100_set(CH_BASE + _sock * CH_SIZE + _reg, _val); } -static inline uint8_t w5100_sock_get(uint8_t _sock, uint16_t _reg) { - return w5100_get( CH_BASE + _sock * CH_SIZE + _reg ); +static inline uint8_t w5100_sock_get(uint8_t _sock, uint16_t _reg) +{ + return w5100_get(CH_BASE + _sock * CH_SIZE + _reg); } -static inline uint16_t w5100_sock_get16( uint8_t _sock, uint16_t _reg) { - uint16_t res = w5100_sock_get(_sock, _reg); - uint16_t res2 = w5100_sock_get(_sock, _reg + 1); - res = res << 8; - res2 = res2 & 0xFF; - res = res | res2; - return res; +static inline uint16_t w5100_sock_get16(uint8_t _sock, uint16_t _reg) +{ + uint16_t res = w5100_sock_get(_sock, _reg); + uint16_t res2 = w5100_sock_get(_sock, _reg + 1); + res = res << 8; + res2 = res2 & 0xFF; + res = res | res2; + return res; } // Functions for the generic device API -static int true_function(struct w5100_periph* p __attribute__((unused)), uint8_t len __attribute__((unused))) { return TRUE; } -static void dev_transmit(struct w5100_periph* p __attribute__((unused)), uint8_t byte) { w5100_transmit(byte); } -static void dev_send(struct w5100_periph* p __attribute__((unused))) { w5100_send(); } +static int true_function(struct w5100_periph *p __attribute__((unused)), uint8_t len __attribute__((unused))) { return TRUE; } +static void dev_transmit(struct w5100_periph *p __attribute__((unused)), uint8_t byte) { w5100_transmit(byte); } +static void dev_send(struct w5100_periph *p __attribute__((unused))) { w5100_send(); } -void w5100_init( void ) { +void w5100_init(void) +{ // configure the SPI bus. w5100_spi.slave_idx = W5100_SLAVE_IDX; @@ -201,19 +205,19 @@ void w5100_init( void ) { sys_time_usleep(20000); // write reset bit into mode register - w5100_set( REG_MR, 1<= W5100_BUFFER_NUM ) { + if (chip0.curbuf >= W5100_BUFFER_NUM) { chip0.curbuf = 0; } - uint16_t ptr = w5100_sock_get16( TELEM_SOCKET, SOCK_TX_WR ); + uint16_t ptr = w5100_sock_get16(TELEM_SOCKET, SOCK_TX_WR); uint16_t offset = ptr & SMASK; uint16_t dstAddr = offset + SBASE[ TELEM_SOCKET ]; @@ -279,104 +285,110 @@ void w5100_send() { if (offset + len > SSIZE) { // Wrap around circular buffer uint16_t size = SSIZE - offset; - w5100_set_buffer( dstAddr, &chip0.tx_buf[curbuf][0], size ); - w5100_set_buffer( SBASE[ TELEM_SOCKET ], &chip0.tx_buf[curbuf][0] + size, len - size); - } - else { - w5100_set_buffer( dstAddr, &chip0.tx_buf[curbuf][0], len); + w5100_set_buffer(dstAddr, &chip0.tx_buf[curbuf][0], size); + w5100_set_buffer(SBASE[ TELEM_SOCKET ], &chip0.tx_buf[curbuf][0] + size, len - size); + } else { + w5100_set_buffer(dstAddr, &chip0.tx_buf[curbuf][0], len); } // Reset write pointer ptr += len; - w5100_sock_set( TELEM_SOCKET, SOCK_TX_WR, ptr >> 8 ); - w5100_sock_set( TELEM_SOCKET, SOCK_TX_WR+1, ptr & 0xFF ); + w5100_sock_set(TELEM_SOCKET, SOCK_TX_WR, ptr >> 8); + w5100_sock_set(TELEM_SOCKET, SOCK_TX_WR + 1, ptr & 0xFF); // send - w5100_sock_set( TELEM_SOCKET, SOCK_CR, SOCK_SEND ); + w5100_sock_set(TELEM_SOCKET, SOCK_CR, SOCK_SEND); - uint8_t complete = w5100_sock_get( TELEM_SOCKET, SOCK_CR); - while ( complete != 0x00 ) { - complete = w5100_sock_get( TELEM_SOCKET, SOCK_CR); + uint8_t complete = w5100_sock_get(TELEM_SOCKET, SOCK_CR); + while (complete != 0x00) { + complete = w5100_sock_get(TELEM_SOCKET, SOCK_CR); } } -uint16_t w5100_rx_size( uint8_t _s ) { - uint16_t val=0,val1=0; +uint16_t w5100_rx_size(uint8_t _s) +{ + uint16_t val = 0, val1 = 0; do { - val1 = w5100_sock_get16( _s, SOCK_RSR ); - if (val1 != 0) - val = w5100_sock_get16( _s, SOCK_RSR ); - } - while (val != val1); + val1 = w5100_sock_get16(_s, SOCK_RSR); + if (val1 != 0) { + val = w5100_sock_get16(_s, SOCK_RSR); + } + } while (val != val1); return val; } -static void w5100_close_socket( uint8_t _s ) { +static void w5100_close_socket(uint8_t _s) +{ // send command to close socket - w5100_sock_set( _s, SOCK_CR, SOCK_CLOSE ); + w5100_sock_set(_s, SOCK_CR, SOCK_CLOSE); // clear interrupt registers - w5100_sock_set( _s, SOCK_IR, 0xFF ); + w5100_sock_set(_s, SOCK_IR, 0xFF); } -static void configure_socket( uint8_t _s, uint8_t _flag, uint16_t _lport, uint16_t _dport, uint8_t *_dest ) { +static void configure_socket(uint8_t _s, uint8_t _flag, uint16_t _lport, uint16_t _dport, uint8_t *_dest) +{ // Configure socket that receives data on 1234 - w5100_close_socket( _s ); + w5100_close_socket(_s); // configure type of socket - w5100_sock_set( _s, SOCK_MR, SNMR_UDP | _flag ); + w5100_sock_set(_s, SOCK_MR, SNMR_UDP | _flag); // configure MSB+LSB of local port number - w5100_sock_set( _s, SOCK_PORT, _lport >> 8 ); - w5100_sock_set( _s, SOCK_PORT+1, _lport & 0xFF ); - if ( _dest[0] != 0x00 ) { + w5100_sock_set(_s, SOCK_PORT, _lport >> 8); + w5100_sock_set(_s, SOCK_PORT + 1, _lport & 0xFF); + if (_dest[0] != 0x00) { // configure destination to send stuff to. - w5100_set_buffer( CH_BASE + _s * CH_SIZE + SOCK_DIPR, _dest, 4 ); + w5100_set_buffer(CH_BASE + _s * CH_SIZE + SOCK_DIPR, _dest, 4); } // this mac corresponds to a mac for multicasting.... uint8_t mac_multi[] = { 0x01, 0x00, 0x5E, 0x01, 0x01, 0x0B }; - w5100_set_buffer( CH_BASE + _s * CH_SIZE + SOCK_DHAR, mac_multi, 6 ); + w5100_set_buffer(CH_BASE + _s * CH_SIZE + SOCK_DHAR, mac_multi, 6); // configure destination port - w5100_sock_set( _s, SOCK_DPORT, _dport >> 8 ); - w5100_sock_set( _s, SOCK_DPORT+1, _dport & 0xFF ); + w5100_sock_set(_s, SOCK_DPORT, _dport >> 8); + w5100_sock_set(_s, SOCK_DPORT + 1, _dport & 0xFF); // send command to open the socket - w5100_sock_set( _s, SOCK_CR, SOCK_OPEN ); + w5100_sock_set(_s, SOCK_CR, SOCK_OPEN); } -bool_t w5100_ch_available() { - if ( w5100_rx_size( CMD_SOCKET ) > 0 ) { +bool_t w5100_ch_available() +{ + if (w5100_rx_size(CMD_SOCKET) > 0) { return TRUE; } return FALSE; } -uint16_t w5100_receive( uint8_t *buf, uint16_t len __attribute__((unused))) { +uint16_t w5100_receive(uint8_t *buf, uint16_t len __attribute__((unused))) +{ uint8_t head[8]; - uint16_t data_len=0; - uint16_t ptr=0; + uint16_t data_len = 0; + uint16_t ptr = 0; // Get socket read pointer - ptr = w5100_sock_get16( CMD_SOCKET, SOCK_RXRD ); - w5100_read_data( CMD_SOCKET, (uint8_t *)ptr, head, 0x08); + ptr = w5100_sock_get16(CMD_SOCKET, SOCK_RXRD); + w5100_read_data(CMD_SOCKET, (uint8_t *)ptr, head, 0x08); ptr += 8; data_len = head[6]; data_len = (data_len << 8) + head[7]; // read data from buffer. - w5100_read_data( CMD_SOCKET, (uint8_t *)ptr, buf, data_len); // data copy. + w5100_read_data(CMD_SOCKET, (uint8_t *)ptr, buf, data_len); // data copy. ptr += data_len; // update read pointer - w5100_sock_set( CMD_SOCKET, SOCK_RXRD, ptr >> 8); - w5100_sock_set( CMD_SOCKET, SOCK_RXRD+1, ptr & 0xFF ); + w5100_sock_set(CMD_SOCKET, SOCK_RXRD, ptr >> 8); + w5100_sock_set(CMD_SOCKET, SOCK_RXRD + 1, ptr & 0xFF); // finalize read. - w5100_sock_set( CMD_SOCKET, SOCK_CR, SOCK_RECV); + w5100_sock_set(CMD_SOCKET, SOCK_CR, SOCK_RECV); return data_len; } -static void w5100_read_data( uint8_t s __attribute__((unused)), volatile uint8_t *src, volatile uint8_t *dst, uint16_t len ) { +static void w5100_read_data(uint8_t s __attribute__((unused)), volatile uint8_t *src, volatile uint8_t *dst, + uint16_t len) +{ uint16_t size; uint16_t src_mask; uint16_t src_ptr; @@ -384,7 +396,7 @@ static void w5100_read_data( uint8_t s __attribute__((unused)), volatile uint8_t src_mask = (uint16_t)src & RMASK; src_ptr = RBASE[CMD_SOCKET] + src_mask; - if( (src_mask + len) > RSIZE ) { + if ((src_mask + len) > RSIZE) { size = RSIZE - src_mask; w5100_read(src_ptr, (uint8_t *)dst, size); dst += size; @@ -396,8 +408,8 @@ static void w5100_read_data( uint8_t s __attribute__((unused)), volatile uint8_t static uint16_t w5100_read(uint16_t _addr, uint8_t *_buf, uint16_t _len) { - for (int i=0; i<_len; i++) { - _buf[i] = w5100_get( _addr ); + for (int i = 0; i < _len; i++) { + _buf[i] = w5100_get(_addr); _addr++; } return _len; diff --git a/sw/airborne/subsystems/datalink/w5100.h b/sw/airborne/subsystems/datalink/w5100.h index 4ee53fcee6..2664fa5a33 100644 --- a/sw/airborne/subsystems/datalink/w5100.h +++ b/sw/airborne/subsystems/datalink/w5100.h @@ -63,12 +63,12 @@ extern uint8_t w5100_rx_buf[W5100_RX_BUFFER_SIZE]; extern struct w5100_periph chip0; -void w5100_init( void ); -void w5100_transmit( uint8_t data ); -uint16_t w5100_receive( uint8_t *buf, uint16_t len ); -void w5100_send( void ); -uint16_t w5100_rx_size( uint8_t _s ); -bool_t w5100_ch_available( void ); +void w5100_init(void); +void w5100_transmit(uint8_t data); +uint16_t w5100_receive(uint8_t *buf, uint16_t len); +void w5100_send(void); +uint16_t w5100_rx_size(uint8_t _s); +bool_t w5100_ch_available(void); // Defines that are done in mcu_periph on behalf of uart. // We need to do these here... @@ -87,13 +87,14 @@ bool_t w5100_ch_available( void ); // W5100 needs a specific read_buffer function #include "subsystems/datalink/pprz_transport.h" -static inline void w5100_read_buffer( struct pprz_transport *t ) { - while ( w5100_ch_available() ) { - w5100_receive( w5100_rx_buf, W5100_RX_BUFFER_SIZE ); +static inline void w5100_read_buffer(struct pprz_transport *t) +{ + while (w5100_ch_available()) { + w5100_receive(w5100_rx_buf, W5100_RX_BUFFER_SIZE); int c = 0; do { - parse_pprz( t, w5100_rx_buf[ c++ ] ); - } while ( ( t->status != UNINIT ) && !(t->trans_rx.msg_received) ); + parse_pprz(t, w5100_rx_buf[ c++ ]); + } while ((t->status != UNINIT) && !(t->trans_rx.msg_received)); } } diff --git a/sw/airborne/subsystems/datalink/xbee.c b/sw/airborne/subsystems/datalink/xbee.c index f8be5f65b5..459e415204 100644 --- a/sw/airborne/subsystems/datalink/xbee.c +++ b/sw/airborne/subsystems/datalink/xbee.c @@ -59,7 +59,9 @@ static void put_1byte(struct xbee_transport *trans, struct link_device *dev, con dev->transmit(dev->periph, byte); } -static void put_bytes(struct xbee_transport *trans, struct link_device *dev, enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), uint8_t len, const void *bytes) +static void put_bytes(struct xbee_transport *trans, struct link_device *dev, + enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), + uint8_t len, const void *bytes) { const uint8_t *b = (const uint8_t *) bytes; int i; @@ -68,7 +70,9 @@ static void put_bytes(struct xbee_transport *trans, struct link_device *dev, enu } } -static void put_named_byte(struct xbee_transport *trans, struct link_device *dev, enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), uint8_t byte, const char * name __attribute__((unused))) +static void put_named_byte(struct xbee_transport *trans, struct link_device *dev, + enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), + uint8_t byte, const char *name __attribute__((unused))) { put_1byte(trans, dev, byte); } @@ -98,17 +102,20 @@ static void end_message(struct xbee_transport *trans, struct link_device *dev) dev->send_message(dev->periph); } -static void overrun(struct xbee_transport *trans __attribute__((unused)), struct link_device *dev __attribute__((unused))) +static void overrun(struct xbee_transport *trans __attribute__((unused)), + struct link_device *dev __attribute__((unused))) { downlink.nb_ovrn++; } -static void count_bytes(struct xbee_transport *trans __attribute__((unused)), struct link_device *dev __attribute__((unused)), uint8_t bytes) +static void count_bytes(struct xbee_transport *trans __attribute__((unused)), + struct link_device *dev __attribute__((unused)), uint8_t bytes) { downlink.nb_bytes += bytes; } -static int check_available_space(struct xbee_transport *trans __attribute__((unused)), struct link_device *dev, uint8_t bytes) +static int check_available_space(struct xbee_transport *trans __attribute__((unused)), struct link_device *dev, + uint8_t bytes) { return dev->check_free_space(dev->periph, bytes); } @@ -118,24 +125,26 @@ static uint8_t xbee_text_reply_is_ok(void) char c[2]; int count = 0; - while (TransportLink(XBEE_UART,ChAvailable())) - { - char cc = TransportLink(XBEE_UART,Getch()); - if (count < 2) + while (TransportLink(XBEE_UART, ChAvailable())) { + char cc = TransportLink(XBEE_UART, Getch()); + if (count < 2) { c[count] = cc; + } count++; } - if ((count > 2) && (c[0] == 'O') && (c[1] == 'K')) + if ((count > 2) && (c[0] == 'O') && (c[1] == 'K')) { return TRUE; + } return FALSE; } -static uint8_t xbee_try_to_enter_api(void) { +static uint8_t xbee_try_to_enter_api(void) +{ /** Switching to AT mode (FIXME: busy waiting) */ - XBeePrintString(XBEE_UART,AT_COMMAND_SEQUENCE); + XBeePrintString(XBEE_UART, AT_COMMAND_SEQUENCE); /** - busy wait 1.25s */ sys_time_usleep(1250000); @@ -147,19 +156,20 @@ static uint8_t xbee_try_to_enter_api(void) { #if XBEE_BAUD == B9600 - #define XBEE_BAUD_ALTERNATE B57600 - #define XBEE_ATBD_CODE "ATBD3\rATWR\r" - #pragma message "Experimental: XBEE-API@9k6 auto-baudrate 57k6 -> 9k6 (stop ground link for correct operation)" +#define XBEE_BAUD_ALTERNATE B57600 +#define XBEE_ATBD_CODE "ATBD3\rATWR\r" +#pragma message "Experimental: XBEE-API@9k6 auto-baudrate 57k6 -> 9k6 (stop ground link for correct operation)" #elif XBEE_BAUD == B57600 - #define XBEE_BAUD_ALTERNATE B9600 - #define XBEE_ATBD_CODE "ATBD6\rATWR\r" - #pragma message "Experimental: XBEE-API@57k6 auto-baudrate 9k6 -> 57k6 (stop ground link for correct operation)" +#define XBEE_BAUD_ALTERNATE B9600 +#define XBEE_ATBD_CODE "ATBD6\rATWR\r" +#pragma message "Experimental: XBEE-API@57k6 auto-baudrate 9k6 -> 57k6 (stop ground link for correct operation)" #else - #warning XBEE-API Non default baudrate: auto-baud disabled +#warning XBEE-API Non default baudrate: auto-baud disabled #endif -void xbee_init( void ) { +void xbee_init(void) +{ xbee_tp.status = XBEE_UNINIT; xbee_tp.trans_rx.msg_received = FALSE; xbee_tp.trans_tx.size_of = (size_of_t) size_of; @@ -173,52 +183,49 @@ void xbee_init( void ) { xbee_tp.trans_tx.impl = (void *)(&xbee_tp); // Empty buffer before init process - while (TransportLink(XBEE_UART,ChAvailable())) - TransportLink(XBEE_UART,Getch()); + while (TransportLink(XBEE_UART, ChAvailable())) { + TransportLink(XBEE_UART, Getch()); + } #ifndef NO_XBEE_API_INIT /** - busy wait 1.25s */ sys_time_usleep(1250000); - if (! xbee_try_to_enter_api() ) - { - #ifdef XBEE_BAUD_ALTERNATE + if (! xbee_try_to_enter_api()) { +#ifdef XBEE_BAUD_ALTERNATE - // Badly configured... try the alternate baudrate: - XBeeUartSetBaudrate(XBEE_BAUD_ALTERNATE); - if ( xbee_try_to_enter_api() ) - { - // The alternate baudrate worked, - XBeePrintString(XBEE_UART,XBEE_ATBD_CODE); - } - else - { - // Complete failure, none of the 2 baudrates result in any reply - // TODO: set LED? + // Badly configured... try the alternate baudrate: + XBeeUartSetBaudrate(XBEE_BAUD_ALTERNATE); + if (xbee_try_to_enter_api()) { + // The alternate baudrate worked, + XBeePrintString(XBEE_UART, XBEE_ATBD_CODE); + } else { + // Complete failure, none of the 2 baudrates result in any reply + // TODO: set LED? - // Set the default baudrate, just in case everything is right - XBeeUartSetBaudrate(XBEE_BAUD); - XBeePrintString(XBEE_UART,"\r"); - } + // Set the default baudrate, just in case everything is right + XBeeUartSetBaudrate(XBEE_BAUD); + XBeePrintString(XBEE_UART, "\r"); + } - #endif +#endif // Continue changing settings until the EXIT is issued. } /** Setting my address */ - XBeePrintString(XBEE_UART,AT_SET_MY); + XBeePrintString(XBEE_UART, AT_SET_MY); uint16_t addr = XBEE_MY_ADDR; - XBeePrintHex16(XBEE_UART,addr); - XBeePrintString(XBEE_UART,"\r"); + XBeePrintHex16(XBEE_UART, addr); + XBeePrintString(XBEE_UART, "\r"); - XBeePrintString(XBEE_UART,AT_AP_MODE); + XBeePrintString(XBEE_UART, AT_AP_MODE); #ifdef XBEE_INIT - XBeePrintString(XBEE_UART,XBEE_INIT); + XBeePrintString(XBEE_UART, XBEE_INIT); #endif /** Switching back to normal mode */ - XBeePrintString(XBEE_UART,AT_EXIT); + XBeePrintString(XBEE_UART, AT_EXIT); XBeeUartSetBaudrate(XBEE_BAUD); diff --git a/sw/airborne/subsystems/datalink/xbee.h b/sw/airborne/subsystems/datalink/xbee.h index 5dba67f93c..759fdc70e0 100644 --- a/sw/airborne/subsystems/datalink/xbee.h +++ b/sw/airborne/subsystems/datalink/xbee.h @@ -42,7 +42,7 @@ /** Initialisation in API mode and setting of the local address * FIXME: busy wait */ -void xbee_init( void ); +void xbee_init(void); /** Status of the API packet receiver automata */ #define XBEE_UNINIT 0 @@ -68,77 +68,83 @@ struct xbee_transport { extern struct xbee_transport xbee_tp; /** Parsing a XBee API frame */ -static inline void parse_xbee( struct xbee_transport * t, uint8_t c ) { +static inline void parse_xbee(struct xbee_transport *t, uint8_t c) +{ switch (t->status) { - case XBEE_UNINIT: - if (c == XBEE_START) + case XBEE_UNINIT: + if (c == XBEE_START) { + t->status++; + } + break; + case XBEE_GOT_START: + if (t->trans_rx.msg_received) { + t->trans_rx.ovrn++; + goto error; + } + t->trans_rx.payload_len = c << 8; t->status++; - break; - case XBEE_GOT_START: - if (t->trans_rx.msg_received) { - t->trans_rx.ovrn++; - goto error; - } - t->trans_rx.payload_len = c<<8; - t->status++; - break; - case XBEE_GOT_LENGTH_MSB: - t->trans_rx.payload_len |= c; - t->status++; - t->payload_idx = 0; - t->cs_rx=0; - break; - case XBEE_GOT_LENGTH_LSB: - t->trans_rx.payload[t->payload_idx] = c; - t->cs_rx += c; - t->payload_idx++; - if (t->payload_idx == t->trans_rx.payload_len) + break; + case XBEE_GOT_LENGTH_MSB: + t->trans_rx.payload_len |= c; t->status++; - break; - case XBEE_GOT_PAYLOAD: - if (c + t->cs_rx != 0xff) + t->payload_idx = 0; + t->cs_rx = 0; + break; + case XBEE_GOT_LENGTH_LSB: + t->trans_rx.payload[t->payload_idx] = c; + t->cs_rx += c; + t->payload_idx++; + if (t->payload_idx == t->trans_rx.payload_len) { + t->status++; + } + break; + case XBEE_GOT_PAYLOAD: + if (c + t->cs_rx != 0xff) { + goto error; + } + t->trans_rx.msg_received = TRUE; + goto restart; + break; + default: goto error; - t->trans_rx.msg_received = TRUE; - goto restart; - break; - default: - goto error; } return; - error: +error: t->trans_rx.error++; - restart: +restart: t->status = XBEE_UNINIT; return; } /** Parsing a frame data and copy the payload to the datalink buffer */ -static inline void xbee_parse_payload(struct xbee_transport * t) { +static inline void xbee_parse_payload(struct xbee_transport *t) +{ switch (t->trans_rx.payload[0]) { - case XBEE_RX_ID: - case XBEE_TX_ID: /* Useful if A/C is connected to the PC with a cable */ - XbeeGetRSSI(t->trans_rx.payload); - uint8_t i; - for(i = XBEE_RFDATA_OFFSET; i < t->trans_rx.payload_len; i++) - dl_buffer[i-XBEE_RFDATA_OFFSET] = t->trans_rx.payload[i]; - dl_msg_available = TRUE; - break; - default: - return; + case XBEE_RX_ID: + case XBEE_TX_ID: /* Useful if A/C is connected to the PC with a cable */ + XbeeGetRSSI(t->trans_rx.payload); + uint8_t i; + for (i = XBEE_RFDATA_OFFSET; i < t->trans_rx.payload_len; i++) { + dl_buffer[i - XBEE_RFDATA_OFFSET] = t->trans_rx.payload[i]; + } + dl_msg_available = TRUE; + break; + default: + return; } } #define XBeeBuffer(_dev) TransportLink(_dev,ChAvailable()) #define ReadXBeeBuffer(_dev,_trans) { while (TransportLink(_dev,ChAvailable())&&!(_trans.trans_rx.msg_received)) parse_xbee(&(_trans),TransportLink(_dev,Getch())); } #define XBeeCheckAndParse(_dev,_trans) { \ - if (XBeeBuffer(_dev)) { \ - ReadXBeeBuffer(_dev,_trans); \ - if (_trans.trans_rx.msg_received) { \ - xbee_parse_payload(&(_trans)); \ - _trans.trans_rx.msg_received = FALSE; \ - } \ - } \ -} + if (XBeeBuffer(_dev)) { \ + ReadXBeeBuffer(_dev,_trans); \ + if (_trans.trans_rx.msg_received) { \ + xbee_parse_payload(&(_trans)); \ + _trans.trans_rx.msg_received = FALSE; \ + } \ + } \ + } #define XBeePrintString(_dev, s) TransportLink(_dev,PrintString(s)) #define XBeePrintHex16(_dev, x) TransportLink(_dev,PrintHex16(x)) diff --git a/sw/airborne/subsystems/datalink/xbee24.h b/sw/airborne/subsystems/datalink/xbee24.h index 50daa6a7cf..dbe947f5b6 100644 --- a/sw/airborne/subsystems/datalink/xbee24.h +++ b/sw/airborne/subsystems/datalink/xbee24.h @@ -34,12 +34,12 @@ #define XBEE_TX_OVERHEAD 4 #define XBEE_TX_HEADER { \ - XBEE_TX_ID, \ - NO_FRAME_ID, \ - (GROUND_STATION_ADDR >> 8), \ - (GROUND_STATION_ADDR & 0xff), \ - TX_OPTIONS \ -} + XBEE_TX_ID, \ + NO_FRAME_ID, \ + (GROUND_STATION_ADDR >> 8), \ + (GROUND_STATION_ADDR & 0xff), \ + TX_OPTIONS \ + } #define XbeeGetRSSI(_payload) { xbee_tp.rssi = _payload[3]; } diff --git a/sw/airborne/subsystems/datalink/xbee868.h b/sw/airborne/subsystems/datalink/xbee868.h index ad199aa5d7..d77ef559c0 100644 --- a/sw/airborne/subsystems/datalink/xbee868.h +++ b/sw/airborne/subsystems/datalink/xbee868.h @@ -34,21 +34,21 @@ #define XBEE_TX_OVERHEAD 13 #define XBEE_TX_HEADER { \ - XBEE_TX_ID, \ - NO_FRAME_ID, \ - 0x00, \ - 0x00, \ - 0x00, \ - 0x00, \ - 0x00, \ - 0x00, \ - (GROUND_STATION_ADDR >> 8), \ - (GROUND_STATION_ADDR & 0xff), \ - 0xff, \ - 0xfe, \ - 0x00, \ - TX_OPTIONS \ -} + XBEE_TX_ID, \ + NO_FRAME_ID, \ + 0x00, \ + 0x00, \ + 0x00, \ + 0x00, \ + 0x00, \ + 0x00, \ + (GROUND_STATION_ADDR >> 8), \ + (GROUND_STATION_ADDR & 0xff), \ + 0xff, \ + 0xfe, \ + 0x00, \ + TX_OPTIONS \ + } #define XbeeGetRSSI(_payload) {} diff --git a/sw/airborne/subsystems/electrical.c b/sw/airborne/subsystems/electrical.c index 239a4ef839..2d73be829b 100644 --- a/sw/airborne/subsystems/electrical.c +++ b/sw/airborne/subsystems/electrical.c @@ -87,7 +87,8 @@ static struct { #define CURRENT_ESTIMATION_NONLINEARITY 1.2 #endif -void electrical_init(void) { +void electrical_init(void) +{ electrical.vsupply = 0; electrical.current = 0; electrical.energy = 0; @@ -103,23 +104,25 @@ void electrical_init(void) { #if defined ADC_CHANNEL_CURRENT && !defined SITL adc_buf_channel(ADC_CHANNEL_CURRENT, &electrical_priv.current_adc_buf, DEFAULT_AV_NB_SAMPLE); #elif defined MILLIAMP_AT_FULL_THROTTLE -PRINT_CONFIG_VAR(CURRENT_ESTIMATION_NONLINEARITY) + PRINT_CONFIG_VAR(CURRENT_ESTIMATION_NONLINEARITY) electrical_priv.nonlin_factor = CURRENT_ESTIMATION_NONLINEARITY; #endif } -void electrical_periodic(void) { +void electrical_periodic(void) +{ static uint32_t bat_low_counter = 0; static uint32_t bat_critical_counter = 0; static bool_t vsupply_check_started = FALSE; #if defined(ADC_CHANNEL_VSUPPLY) && !defined(SITL) - electrical.vsupply = 10 * VoltageOfAdc((electrical_priv.vsupply_adc_buf.sum/electrical_priv.vsupply_adc_buf.av_nb_sample)); + electrical.vsupply = 10 * VoltageOfAdc((electrical_priv.vsupply_adc_buf.sum / + electrical_priv.vsupply_adc_buf.av_nb_sample)); #endif #ifdef ADC_CHANNEL_CURRENT #ifndef SITL - int32_t current_adc = electrical_priv.current_adc_buf.sum/electrical_priv.current_adc_buf.av_nb_sample; + int32_t current_adc = electrical_priv.current_adc_buf.sum / electrical_priv.current_adc_buf.av_nb_sample; electrical.current = MilliAmpereOfAdc(current_adc); /* Prevent an overflow on high current spikes when using the motor brake */ BoundAbs(electrical.current, 65000); @@ -140,7 +143,8 @@ void electrical_periodic(void) { /* electrical.current y = ( b^n - (b* x/a)^n )^1/n * a=1, n = electrical_priv.nonlin_factor */ - electrical.current = b - pow((pow(b,electrical_priv.nonlin_factor)-pow((b*x),electrical_priv.nonlin_factor)), (1./electrical_priv.nonlin_factor)); + electrical.current = b - pow((pow(b, electrical_priv.nonlin_factor) - pow((b * x), electrical_priv.nonlin_factor)), + (1. / electrical_priv.nonlin_factor)); #endif /* ADC_CHANNEL_CURRENT */ // mAh = mA * dt (10Hz -> hours) @@ -153,24 +157,26 @@ void electrical_periodic(void) { if (vsupply_check_started) { if (electrical.vsupply < LOW_BAT_LEVEL * 10) { - if (bat_low_counter > 0) + if (bat_low_counter > 0) { bat_low_counter--; - if (bat_low_counter == 0) + } + if (bat_low_counter == 0) { electrical.bat_low = TRUE; - } - else { + } + } else { // reset battery low status and counter bat_low_counter = BAT_CHECKER_DELAY * ELECTRICAL_PERIODIC_FREQ; electrical.bat_low = FALSE; } if (electrical.vsupply < CRITIC_BAT_LEVEL * 10) { - if (bat_critical_counter > 0) + if (bat_critical_counter > 0) { bat_critical_counter--; - if (bat_critical_counter == 0) + } + if (bat_critical_counter == 0) { electrical.bat_critical = TRUE; - } - else { + } + } else { // reset battery critical status and counter bat_critical_counter = BAT_CHECKER_DELAY * ELECTRICAL_PERIODIC_FREQ; electrical.bat_critical = FALSE; diff --git a/sw/airborne/subsystems/gps.c b/sw/airborne/subsystems/gps.c index 3c8a083a24..7d306f595e 100644 --- a/sw/airborne/subsystems/gps.c +++ b/sw/airborne/subsystems/gps.c @@ -46,7 +46,8 @@ struct GpsTimeSync gps_time_sync; #include "subsystems/datalink/telemetry.h" static void send_svinfo_id(struct transport_tx *trans, struct link_device *dev, - uint8_t svid) { + uint8_t svid) +{ if (svid < GPS_NB_CHANNELS) { pprz_msg_send_SVINFO(trans, dev, AC_ID, &svid, &gps.svinfos[svid].svid, &gps.svinfos[svid].flags, @@ -56,9 +57,10 @@ static void send_svinfo_id(struct transport_tx *trans, struct link_device *dev, } /** send SVINFO message (regardless of state) */ -static void send_svinfo(struct transport_tx *trans, struct link_device *dev) { +static void send_svinfo(struct transport_tx *trans, struct link_device *dev) +{ static uint8_t i = 0; - if (i == gps.nb_channels) i = 0; + if (i == gps.nb_channels) { i = 0; } send_svinfo_id(trans, dev, i); i++; } @@ -67,66 +69,71 @@ static void send_svinfo(struct transport_tx *trans, struct link_device *dev) { * send SVINFO for all satellites while no GPS fix, * after 3D fix, send avialable sats only when there is new information */ -static inline void send_svinfo_available(struct transport_tx *trans, struct link_device *dev) { +static inline void send_svinfo_available(struct transport_tx *trans, struct link_device *dev) +{ static uint8_t i = 0; static uint8_t last_cnos[GPS_NB_CHANNELS]; - if (i >= gps.nb_channels) i = 0; + if (i >= gps.nb_channels) { i = 0; } // send SVINFO for all satellites while no GPS fix, // after 3D fix, send avialable sats if they were updated if (gps.fix != GPS_FIX_3D) { send_svinfo_id(trans, dev, i); - } - else if (gps.svinfos[i].cno != last_cnos[i]) { + } else if (gps.svinfos[i].cno != last_cnos[i]) { send_svinfo_id(trans, dev, i); last_cnos[i] = gps.svinfos[i].cno; } i++; } -static void send_gps(struct transport_tx *trans, struct link_device *dev) { +static void send_gps(struct transport_tx *trans, struct link_device *dev) +{ uint8_t zero = 0; int16_t climb = -gps.ned_vel.z; - int16_t course = (DegOfRad(gps.course)/((int32_t)1e6)); + int16_t course = (DegOfRad(gps.course) / ((int32_t)1e6)); pprz_msg_send_GPS(trans, dev, AC_ID, &gps.fix, - &gps.utm_pos.east, &gps.utm_pos.north, - &course, &gps.hmsl, &gps.gspeed, &climb, - &gps.week, &gps.tow, &gps.utm_pos.zone, &zero); + &gps.utm_pos.east, &gps.utm_pos.north, + &course, &gps.hmsl, &gps.gspeed, &climb, + &gps.week, &gps.tow, &gps.utm_pos.zone, &zero); // send SVINFO for available satellites that have new data send_svinfo_available(trans, dev); } -static void send_gps_int(struct transport_tx *trans, struct link_device *dev) { +static void send_gps_int(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_GPS_INT(trans, dev, AC_ID, - &gps.ecef_pos.x, &gps.ecef_pos.y, &gps.ecef_pos.z, - &gps.lla_pos.lat, &gps.lla_pos.lon, &gps.lla_pos.alt, - &gps.hmsl, - &gps.ecef_vel.x, &gps.ecef_vel.y, &gps.ecef_vel.z, - &gps.pacc, &gps.sacc, - &gps.tow, - &gps.pdop, - &gps.num_sv, - &gps.fix); + &gps.ecef_pos.x, &gps.ecef_pos.y, &gps.ecef_pos.z, + &gps.lla_pos.lat, &gps.lla_pos.lon, &gps.lla_pos.alt, + &gps.hmsl, + &gps.ecef_vel.x, &gps.ecef_vel.y, &gps.ecef_vel.z, + &gps.pacc, &gps.sacc, + &gps.tow, + &gps.pdop, + &gps.num_sv, + &gps.fix); // send SVINFO for available satellites that have new data send_svinfo_available(trans, dev); } -static void send_gps_lla(struct transport_tx *trans, struct link_device *dev) { +static void send_gps_lla(struct transport_tx *trans, struct link_device *dev) +{ uint8_t err = 0; int16_t climb = -gps.ned_vel.z; - int16_t course = (DegOfRad(gps.course)/((int32_t)1e6)); + int16_t course = (DegOfRad(gps.course) / ((int32_t)1e6)); pprz_msg_send_GPS_LLA(trans, dev, AC_ID, - &gps.lla_pos.lat, &gps.lla_pos.lon, &gps.lla_pos.alt, - &course, &gps.gspeed, &climb, - &gps.week, &gps.tow, - &gps.fix, &err); + &gps.lla_pos.lat, &gps.lla_pos.lon, &gps.lla_pos.alt, + &course, &gps.gspeed, &climb, + &gps.week, &gps.tow, + &gps.fix, &err); } -static void send_gps_sol(struct transport_tx *trans, struct link_device *dev) { +static void send_gps_sol(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_GPS_SOL(trans, dev, AC_ID, &gps.pacc, &gps.sacc, &gps.pdop, &gps.num_sv); } #endif -void gps_init(void) { +void gps_init(void) +{ gps.fix = GPS_FIX_NONE; gps.week = 0; gps.tow = 0; @@ -156,7 +163,8 @@ void gps_init(void) { #endif } -void gps_periodic_check(void) { +void gps_periodic_check(void) +{ if (sys_time.nb_sec - gps.last_msg_time > GPS_TIMEOUT) { gps.fix = GPS_FIX_NONE; } diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h index dd388fba25..06e50c5de8 100644 --- a/sw/airborne/subsystems/gps.h +++ b/sw/airborne/subsystems/gps.h @@ -111,14 +111,16 @@ extern void gps_impl_init(void); #define GPS_TIMEOUT 2 #endif -static inline bool_t GpsIsLost(void) { +static inline bool_t GpsIsLost(void) +{ if (gps.fix == GPS_FIX_3D) { return FALSE; } return TRUE; } -static inline bool_t gps_has_been_good(void) { +static inline bool_t gps_has_been_good(void) +{ static bool_t gps_had_valid_fix = FALSE; if (GpsFixValid()) { gps_had_valid_fix = TRUE; @@ -137,7 +139,7 @@ extern void gps_periodic_check(void); * @todo this still needs to call gps specific stuff */ #define gps_Reset(_val) { \ -} + } /* diff --git a/sw/airborne/subsystems/gps/gps_ardrone2.c b/sw/airborne/subsystems/gps/gps_ardrone2.c index 2feba4c765..f1006c69ef 100644 --- a/sw/airborne/subsystems/gps/gps_ardrone2.c +++ b/sw/airborne/subsystems/gps/gps_ardrone2.c @@ -35,11 +35,13 @@ bool_t gps_ardrone2_available; -void gps_impl_init( void ) { +void gps_impl_init(void) +{ gps_ardrone2_available = FALSE; } -void gps_ardrone2_parse(navdata_gps_t *navdata_gps) { +void gps_ardrone2_parse(navdata_gps_t *navdata_gps) +{ int i; #ifdef ARDRONE2_DEBUG @@ -62,16 +64,17 @@ void gps_ardrone2_parse(navdata_gps_t *navdata_gps) { // TODO: parse other stuff gps.nb_channels = GPS_NB_CHANNELS; - for(i = 0; i < GPS_NB_CHANNELS; i++) { + for (i = 0; i < GPS_NB_CHANNELS; i++) { gps.svinfos[i].svid = navdata_gps->channels[i].sat; gps.svinfos[i].cno = navdata_gps->channels[i].cn0; } // Check if we have a fix TODO: check if 2D or 3D fix? - if (navdata_gps->gps_state == 1) + if (navdata_gps->gps_state == 1) { gps.fix = GPS_FIX_3D; - else + } else { gps.fix = GPS_FIX_NONE; + } // Set that there is a packet gps_ardrone2_available = TRUE; diff --git a/sw/airborne/subsystems/gps/gps_datalink.c b/sw/airborne/subsystems/gps/gps_datalink.c index 31dfcbe6c3..ffdc51a763 100644 --- a/sw/airborne/subsystems/gps/gps_datalink.c +++ b/sw/airborne/subsystems/gps/gps_datalink.c @@ -32,7 +32,8 @@ bool_t gps_available; ///< Is set to TRUE when a new REMOTE_GPS packet is received and parsed /** GPS initialization */ -void gps_impl_init(void) { +void gps_impl_init(void) +{ gps.fix = GPS_FIX_NONE; gps_available = FALSE; gps.gspeed = 700; // To enable course setting @@ -40,8 +41,10 @@ void gps_impl_init(void) { } /** Parse the REMOTE_GPS datalink packet */ -void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t ecef_z, int32_t lat, int32_t lon, int32_t alt, - int32_t hmsl, int32_t ecef_xd, int32_t ecef_yd, int32_t ecef_zd, uint32_t tow, int32_t course) { +void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t ecef_z, int32_t lat, int32_t lon, + int32_t alt, + int32_t hmsl, int32_t ecef_xd, int32_t ecef_yd, int32_t ecef_zd, uint32_t tow, int32_t course) +{ gps.lla_pos.lat = lat; gps.lla_pos.lon = lon; @@ -71,8 +74,8 @@ void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t e // convert to utm utm_of_lla_f(&utm_f, &lla_f); // copy results of utm conversion - gps.utm_pos.east = utm_f.east*100; - gps.utm_pos.north = utm_f.north*100; + gps.utm_pos.east = utm_f.east * 100; + gps.utm_pos.north = utm_f.north * 100; gps.utm_pos.alt = gps.lla_pos.alt; gps.utm_pos.zone = nav_utm_zone0; #endif diff --git a/sw/airborne/subsystems/gps/gps_datalink.h b/sw/airborne/subsystems/gps/gps_datalink.h index 3eb1800844..24726af4d2 100644 --- a/sw/airborne/subsystems/gps/gps_datalink.h +++ b/sw/airborne/subsystems/gps/gps_datalink.h @@ -34,8 +34,9 @@ #define GPS_NB_CHANNELS 0 extern bool_t gps_available; -extern void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t ecef_z, int32_t lat, int32_t lon, int32_t alt, - int32_t hmsl, int32_t ecef_xd, int32_t ecef_yd, int32_t ecef_zd, uint32_t tow, int32_t course); +extern void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t ecef_z, int32_t lat, int32_t lon, + int32_t alt, + int32_t hmsl, int32_t ecef_xd, int32_t ecef_yd, int32_t ecef_zd, uint32_t tow, int32_t course); #define GpsEvent(_sol_available_callback) { \ diff --git a/sw/airborne/subsystems/gps/gps_mtk.c b/sw/airborne/subsystems/gps/gps_mtk.c index e1097b567e..826e1ce127 100644 --- a/sw/airborne/subsystems/gps/gps_mtk.c +++ b/sw/airborne/subsystems/gps/gps_mtk.c @@ -41,8 +41,8 @@ #include "mcu_periph/sys_time.h" -#define MTK_DIY_OUTPUT_RATE MTK_DIY_OUTPUT_4HZ -#define OUTPUT_RATE 4 +#define MTK_DIY_OUTPUT_RATE MTK_DIY_OUTPUT_4HZ +#define OUTPUT_RATE 4 /* parser status */ #define UNINIT 0 @@ -103,19 +103,21 @@ bool_t gps_configuring; static uint8_t gps_status_config; #endif -void gps_impl_init(void) { - gps_mtk.status = UNINIT; - gps_mtk.msg_available = FALSE; - gps_mtk.error_cnt = 0; - gps_mtk.error_last = GPS_MTK_ERR_NONE; +void gps_impl_init(void) +{ + gps_mtk.status = UNINIT; + gps_mtk.msg_available = FALSE; + gps_mtk.error_cnt = 0; + gps_mtk.error_last = GPS_MTK_ERR_NONE; #ifdef GPS_CONFIGURE - gps_status_config = 0; - gps_configuring = TRUE; + gps_status_config = 0; + gps_configuring = TRUE; #endif } static void gps_mtk_time2itow(uint32_t gps_date, uint32_t gps_time, - int16_t* gps_week, uint32_t* gps_itow) { + int16_t *gps_week, uint32_t *gps_itow) +{ /* convert UTC date/time to GPS week/itow, we have no idea about GPS leap seconds for now */ uint16_t gps_msecond = gps_time % 1000; @@ -131,63 +133,64 @@ static void gps_mtk_time2itow(uint32_t gps_date, uint32_t gps_time, *gps_itow = 0; /* sanity checks */ - if (gps_month > 12) return; + if (gps_month > 12) { return; } if (gps_day > (DAYS_MONTH[gps_month] + - ((gps_month == 1) ? isleap(gps_year):0))) return; - if (gps_hour > 23) return; - if (gps_minute > 59) return; - if (gps_second > 59) return; + ((gps_month == 1) ? isleap(gps_year) : 0))) { return; } + if (gps_hour > 23) { return; } + if (gps_minute > 59) { return; } + if (gps_second > 59) { return; } /* days since 6-JAN-1980 */ days = -6; - for (i = 1980; i < gps_year; i++) days += (365 + isleap(i)); + for (i = 1980; i < gps_year; i++) { days += (365 + isleap(i)); } /* add days in gps_year */ - for (i = 0; i < gps_month-1; i++) { - days += DAYS_MONTH[i] + ((i == 1) ? isleap(gps_year):0); + for (i = 0; i < gps_month - 1; i++) { + days += DAYS_MONTH[i] + ((i == 1) ? isleap(gps_year) : 0); } days += gps_day; /* convert */ - *gps_week = (uint16_t) (days / 7); + *gps_week = (uint16_t)(days / 7); *gps_itow = ((days % 7) * SECS_DAY + - gps_hour * SECS_HOUR + - gps_minute * SECS_MINUTE + - gps_second) * 1000 + - gps_msecond; + gps_hour * SECS_HOUR + + gps_minute * SECS_MINUTE + + gps_second) * 1000 + + gps_msecond; } -void gps_mtk_read_message(void) { +void gps_mtk_read_message(void) +{ if (gps_mtk.msg_class == MTK_DIY14_ID) { if (gps_mtk.msg_id == MTK_DIY14_NAV_ID) { /* get hardware clock ticks */ gps_time_sync.t0_ticks = sys_time.nb_tick; gps_time_sync.t0_tow = MTK_DIY14_NAV_ITOW(gps_mtk.msg_buf); gps_time_sync.t0_tow_frac = 0; - gps.lla_pos.lat = MTK_DIY14_NAV_LAT(gps_mtk.msg_buf)*10; - gps.lla_pos.lon = MTK_DIY14_NAV_LON(gps_mtk.msg_buf)*10; + gps.lla_pos.lat = MTK_DIY14_NAV_LAT(gps_mtk.msg_buf) * 10; + gps.lla_pos.lon = MTK_DIY14_NAV_LON(gps_mtk.msg_buf) * 10; // FIXME: with MTK you do not receive vertical speed if (sys_time.nb_sec - gps.last_3dfix_time < 2) { gps.ned_vel.z = ((gps.hmsl - - MTK_DIY14_NAV_HEIGHT(gps_mtk.msg_buf)*10)*OUTPUT_RATE)/10; - } else gps.ned_vel.z = 0; - gps.hmsl = MTK_DIY14_NAV_HEIGHT(gps_mtk.msg_buf)*10; + MTK_DIY14_NAV_HEIGHT(gps_mtk.msg_buf) * 10) * OUTPUT_RATE) / 10; + } else { gps.ned_vel.z = 0; } + gps.hmsl = MTK_DIY14_NAV_HEIGHT(gps_mtk.msg_buf) * 10; // FIXME: with MTK you do not receive ellipsoid altitude gps.lla_pos.alt = gps.hmsl; gps.gspeed = MTK_DIY14_NAV_GSpeed(gps_mtk.msg_buf); // FIXME: with MTK you do not receive speed 3D gps.speed_3d = gps.gspeed; - gps.course = (RadOfDeg(MTK_DIY14_NAV_Heading(gps_mtk.msg_buf)))*10; + gps.course = (RadOfDeg(MTK_DIY14_NAV_Heading(gps_mtk.msg_buf))) * 10; gps.num_sv = MTK_DIY14_NAV_numSV(gps_mtk.msg_buf); switch (MTK_DIY14_NAV_GPSfix(gps_mtk.msg_buf)) { - case MTK_DIY_FIX_3D: - gps.fix = GPS_FIX_3D; - break; - case MTK_DIY_FIX_2D: - gps.fix = GPS_FIX_2D; - break; - default: - gps.fix = GPS_FIX_NONE; + case MTK_DIY_FIX_3D: + gps.fix = GPS_FIX_3D; + break; + case MTK_DIY_FIX_2D: + gps.fix = GPS_FIX_2D; + break; + default: + gps.fix = GPS_FIX_NONE; } gps.tow = MTK_DIY14_NAV_ITOW(gps_mtk.msg_buf);; // FIXME: with MTK DIY 1.4 you do not receive GPS week @@ -200,15 +203,14 @@ void gps_mtk_read_message(void) { /* convert to utm */ utm_of_lla_f(&utm_f, &lla_f); /* copy results of utm conversion */ - gps.utm_pos.east = utm_f.east*100; - gps.utm_pos.north = utm_f.north*100; + gps.utm_pos.east = utm_f.east * 100; + gps.utm_pos.north = utm_f.north * 100; gps.utm_pos.alt = gps.lla_pos.alt; gps.utm_pos.zone = nav_utm_zone0; #ifdef GPS_LED if (gps.fix == GPS_FIX_3D) { LED_ON(GPS_LED); - } - else { + } else { LED_TOGGLE(GPS_LED); } #endif @@ -227,30 +229,30 @@ void gps_mtk_read_message(void) { gps.t0_tow = gps.tow; gps.t0_tow_frac = 0; #endif - gps.lla_pos.lat = MTK_DIY16_NAV_LAT(gps_mtk.msg_buf)*10; - gps.lla_pos.lon = MTK_DIY16_NAV_LON(gps_mtk.msg_buf)*10; + gps.lla_pos.lat = MTK_DIY16_NAV_LAT(gps_mtk.msg_buf) * 10; + gps.lla_pos.lon = MTK_DIY16_NAV_LON(gps_mtk.msg_buf) * 10; // FIXME: with MTK you do not receive vertical speed if (sys_time.nb_sec - gps.last_3dfix_time < 2) { gps.ned_vel.z = ((gps.hmsl - - MTK_DIY16_NAV_HEIGHT(gps_mtk.msg_buf)*10)*OUTPUT_RATE)/10; - } else gps.ned_vel.z = 0; - gps.hmsl = MTK_DIY16_NAV_HEIGHT(gps_mtk.msg_buf)*10; + MTK_DIY16_NAV_HEIGHT(gps_mtk.msg_buf) * 10) * OUTPUT_RATE) / 10; + } else { gps.ned_vel.z = 0; } + gps.hmsl = MTK_DIY16_NAV_HEIGHT(gps_mtk.msg_buf) * 10; // FIXME: with MTK you do not receive ellipsoid altitude gps.lla_pos.alt = gps.hmsl; gps.gspeed = MTK_DIY16_NAV_GSpeed(gps_mtk.msg_buf); // FIXME: with MTK you do not receive speed 3D gps.speed_3d = gps.gspeed; - gps.course = (RadOfDeg(MTK_DIY16_NAV_Heading(gps_mtk.msg_buf)*10000)) * 10; + gps.course = (RadOfDeg(MTK_DIY16_NAV_Heading(gps_mtk.msg_buf) * 10000)) * 10; gps.num_sv = MTK_DIY16_NAV_numSV(gps_mtk.msg_buf); switch (MTK_DIY16_NAV_GPSfix(gps_mtk.msg_buf)) { - case MTK_DIY_FIX_3D: - gps.fix = GPS_FIX_3D; - break; - case MTK_DIY_FIX_2D: - gps.fix = GPS_FIX_2D; - break; - default: - gps.fix = GPS_FIX_NONE; + case MTK_DIY_FIX_3D: + gps.fix = GPS_FIX_3D; + break; + case MTK_DIY_FIX_2D: + gps.fix = GPS_FIX_2D; + break; + default: + gps.fix = GPS_FIX_NONE; } /* HDOP? */ /* Computes from (lat, long) in the referenced UTM zone */ @@ -261,15 +263,14 @@ void gps_mtk_read_message(void) { /* convert to utm */ utm_of_lla_f(&utm_f, &lla_f); /* copy results of utm conversion */ - gps.utm_pos.east = utm_f.east*100; - gps.utm_pos.north = utm_f.north*100; + gps.utm_pos.east = utm_f.east * 100; + gps.utm_pos.north = utm_f.north * 100; gps.utm_pos.alt = gps.lla_pos.alt; gps.utm_pos.zone = nav_utm_zone0; #ifdef GPS_LED if (gps.fix == GPS_FIX_3D) { LED_ON(GPS_LED); - } - else { + } else { LED_TOGGLE(GPS_LED); } #endif @@ -278,103 +279,106 @@ void gps_mtk_read_message(void) { } /* byte parsing */ -void gps_mtk_parse( uint8_t c ) { +void gps_mtk_parse(uint8_t c) +{ if (gps_mtk.status < GOT_PAYLOAD) { gps_mtk.ck_a += c; gps_mtk.ck_b += gps_mtk.ck_a; } switch (gps_mtk.status) { - case UNINIT: - if (c == MTK_DIY14_SYNC1) - gps_mtk.status = GOT_SYNC1_14; - if (c == MTK_DIY16_ID) - gps_mtk.msg_class = c; + case UNINIT: + if (c == MTK_DIY14_SYNC1) { + gps_mtk.status = GOT_SYNC1_14; + } + if (c == MTK_DIY16_ID) { + gps_mtk.msg_class = c; + } gps_mtk.status = GOT_SYNC1_16; - break; - /* MTK_DIY_VER_14 */ - case GOT_SYNC1_14: - if (c != MTK_DIY14_SYNC2) { - gps_mtk.error_last = GPS_MTK_ERR_OUT_OF_SYNC; - goto error; - } - if (gps_mtk.msg_available) { - /* Previous message has not yet been parsed: discard this one */ - gps_mtk.error_last = GPS_MTK_ERR_OVERRUN; - goto error; - } - gps_mtk.ck_a = 0; - gps_mtk.ck_b = 0; - gps_mtk.status++; - gps_mtk.len = MTK_DIY14_NAV_LENGTH; - break; - case GOT_SYNC2_14: - if (c != MTK_DIY14_ID) { - gps_mtk.error_last = GPS_MTK_ERR_OUT_OF_SYNC; - goto error; - } - gps_mtk.msg_class = c; - gps_mtk.msg_idx = 0; - gps_mtk.status++; - break; - case GOT_CLASS_14: - if (c != MTK_DIY14_NAV_ID) { - gps_mtk.error_last = GPS_MTK_ERR_OUT_OF_SYNC; - goto error; - } - gps_mtk.msg_id = c; - gps_mtk.status = GOT_ID; - break; - /* MTK_DIY_VER_16 */ - case GOT_SYNC1_16: - if (c != MTK_DIY16_NAV_ID) { - gps_mtk.error_last = GPS_MTK_ERR_OUT_OF_SYNC; - goto error; - } - if (gps_mtk.msg_available) { - /* Previous message has not yet been parsed: discard this one */ - gps_mtk.error_last = GPS_MTK_ERR_OVERRUN; - goto error; - } - gps_mtk.msg_id = c; - gps_mtk.ck_a = 0; - gps_mtk.ck_b = 0; - gps_mtk.status++; - break; - case GOT_SYNC2_16: - gps_mtk.len = c; - gps_mtk.msg_idx = 0; - gps_mtk.status = GOT_ID; - break; - case GOT_ID: - gps_mtk.msg_buf[gps_mtk.msg_idx] = c; - gps_mtk.msg_idx++; - if (gps_mtk.msg_idx >= gps_mtk.len) { + break; + /* MTK_DIY_VER_14 */ + case GOT_SYNC1_14: + if (c != MTK_DIY14_SYNC2) { + gps_mtk.error_last = GPS_MTK_ERR_OUT_OF_SYNC; + goto error; + } + if (gps_mtk.msg_available) { + /* Previous message has not yet been parsed: discard this one */ + gps_mtk.error_last = GPS_MTK_ERR_OVERRUN; + goto error; + } + gps_mtk.ck_a = 0; + gps_mtk.ck_b = 0; gps_mtk.status++; - } - break; - case GOT_PAYLOAD: - if (c != gps_mtk.ck_a) { - gps_mtk.error_last = GPS_MTK_ERR_CHECKSUM; + gps_mtk.len = MTK_DIY14_NAV_LENGTH; + break; + case GOT_SYNC2_14: + if (c != MTK_DIY14_ID) { + gps_mtk.error_last = GPS_MTK_ERR_OUT_OF_SYNC; + goto error; + } + gps_mtk.msg_class = c; + gps_mtk.msg_idx = 0; + gps_mtk.status++; + break; + case GOT_CLASS_14: + if (c != MTK_DIY14_NAV_ID) { + gps_mtk.error_last = GPS_MTK_ERR_OUT_OF_SYNC; + goto error; + } + gps_mtk.msg_id = c; + gps_mtk.status = GOT_ID; + break; + /* MTK_DIY_VER_16 */ + case GOT_SYNC1_16: + if (c != MTK_DIY16_NAV_ID) { + gps_mtk.error_last = GPS_MTK_ERR_OUT_OF_SYNC; + goto error; + } + if (gps_mtk.msg_available) { + /* Previous message has not yet been parsed: discard this one */ + gps_mtk.error_last = GPS_MTK_ERR_OVERRUN; + goto error; + } + gps_mtk.msg_id = c; + gps_mtk.ck_a = 0; + gps_mtk.ck_b = 0; + gps_mtk.status++; + break; + case GOT_SYNC2_16: + gps_mtk.len = c; + gps_mtk.msg_idx = 0; + gps_mtk.status = GOT_ID; + break; + case GOT_ID: + gps_mtk.msg_buf[gps_mtk.msg_idx] = c; + gps_mtk.msg_idx++; + if (gps_mtk.msg_idx >= gps_mtk.len) { + gps_mtk.status++; + } + break; + case GOT_PAYLOAD: + if (c != gps_mtk.ck_a) { + gps_mtk.error_last = GPS_MTK_ERR_CHECKSUM; + goto error; + } + gps_mtk.status++; + break; + case GOT_CHECKSUM1: + if (c != gps_mtk.ck_b) { + gps_mtk.error_last = GPS_MTK_ERR_CHECKSUM; + goto error; + } + gps_mtk.msg_available = TRUE; + goto restart; + break; + default: + gps_mtk.error_last = GPS_MTK_ERR_UNEXPECTED; goto error; - } - gps_mtk.status++; - break; - case GOT_CHECKSUM1: - if (c != gps_mtk.ck_b) { - gps_mtk.error_last = GPS_MTK_ERR_CHECKSUM; - goto error; - } - gps_mtk.msg_available = TRUE; - goto restart; - break; - default: - gps_mtk.error_last = GPS_MTK_ERR_UNEXPECTED; - goto error; } return; - error: +error: gps_mtk.error_cnt++; - restart: +restart: gps_mtk.status = UNINIT; return; } @@ -389,36 +393,41 @@ void gps_mtk_parse( uint8_t c ) { */ #ifdef GPS_CONFIGURE -static void MtkSend_CFG(char* dat) { - while (*dat != 0) GpsLink(Transmit(*dat++)); +static void MtkSend_CFG(char *dat) +{ + while (*dat != 0) { GpsLink(Transmit(*dat++)); } } -void gps_configure_uart(void) { +void gps_configure_uart(void) +{ } #ifdef USER_GPS_CONFIGURE #include USER_GPS_CONFIGURE #else -static bool_t user_gps_configure(bool_t cpt) { +static bool_t user_gps_configure(bool_t cpt) +{ switch (cpt) { - case 0: - MtkSend_CFG(MTK_DIY_SET_BINARY); - break; - case 1: - MtkSend_CFG(MTK_DIY_OUTPUT_RATE); - return FALSE; - default: - break; + case 0: + MtkSend_CFG(MTK_DIY_SET_BINARY); + break; + case 1: + MtkSend_CFG(MTK_DIY_OUTPUT_RATE); + return FALSE; + default: + break; } return TRUE; /* Continue, except for the last case */ } #endif // ! USER_GPS_CONFIGURE -void gps_configure( void ) { - static uint32_t count=0; +void gps_configure(void) +{ + static uint32_t count = 0; /* start configuring after having received 50 bytes */ - if (count++ > 50) + if (count++ > 50) { gps_configuring = user_gps_configure(gps_status_config++); + } } #endif /* GPS_CONFIGURE */ diff --git a/sw/airborne/subsystems/gps/gps_mtk.h b/sw/airborne/subsystems/gps/gps_mtk.h index 887778cad5..fe6c400cab 100644 --- a/sw/airborne/subsystems/gps/gps_mtk.h +++ b/sw/airborne/subsystems/gps/gps_mtk.h @@ -32,7 +32,7 @@ struct GpsMtk { bool_t msg_available; - uint8_t msg_buf[GPS_MTK_MAX_PAYLOAD] __attribute__ ((aligned)); + uint8_t msg_buf[GPS_MTK_MAX_PAYLOAD] __attribute__((aligned)); uint8_t msg_id; uint8_t msg_class; @@ -99,9 +99,9 @@ extern bool_t gps_configuring; } \ } -#define ReadGpsBuffer() { \ - while (GpsLink(ChAvailable())&&!gps_mtk.msg_available) \ - gps_mtk_parse(GpsLink(Getch())); \ +#define ReadGpsBuffer() { \ + while (GpsLink(ChAvailable())&&!gps_mtk.msg_available) \ + gps_mtk_parse(GpsLink(Getch())); \ } diff --git a/sw/airborne/subsystems/gps/gps_sim.c b/sw/airborne/subsystems/gps/gps_sim.c index 6ac8669964..4bff546650 100644 --- a/sw/airborne/subsystems/gps/gps_sim.c +++ b/sw/airborne/subsystems/gps/gps_sim.c @@ -25,20 +25,22 @@ bool_t gps_available; #if 0 -void gps_feed_values(double utm_north, double utm_east, double utm_alt, double gspeed, double course, double climb) { +void gps_feed_values(double utm_north, double utm_east, double utm_alt, double gspeed, double course, double climb) +{ gps.utm_pos.north = CM_OF_M(utm_north); gps.utm_pos.east = CM_OF_M(utm_east); //TODO set height above ellipsoid properly gps.hmsl = utm_alt * 1000.; gps.gspeed = CM_OF_M(gspeed); - gps.course = EM7RAD_OF_RAD(RadOfDeg(course/10.)); - gps.ned_vel.z = -climb*100.; + gps.course = EM7RAD_OF_RAD(RadOfDeg(course / 10.)); + gps.ned_vel.z = -climb * 100.; gps.fix = GPS_FIX_3D; gps_available = TRUE; } #endif -void gps_impl_init(void) { +void gps_impl_init(void) +{ gps.fix = GPS_FIX_NONE; gps_available = FALSE; } diff --git a/sw/airborne/subsystems/gps/gps_sim_hitl.c b/sw/airborne/subsystems/gps/gps_sim_hitl.c index 87ed9082a9..bc546df60e 100644 --- a/sw/airborne/subsystems/gps/gps_sim_hitl.c +++ b/sw/airborne/subsystems/gps/gps_sim_hitl.c @@ -29,7 +29,8 @@ bool_t gps_available; uint32_t gps_sim_hitl_timer; -void gps_impl_init(void) { +void gps_impl_init(void) +{ gps.fix = GPS_FIX_NONE; gps_available = FALSE; } diff --git a/sw/airborne/subsystems/gps/gps_sim_nps.c b/sw/airborne/subsystems/gps/gps_sim_nps.c index 5e70503bbd..ca231e7027 100644 --- a/sw/airborne/subsystems/gps/gps_sim_nps.c +++ b/sw/airborne/subsystems/gps/gps_sim_nps.c @@ -34,7 +34,8 @@ bool_t gps_available; bool_t gps_has_fix; -void gps_feed_value() { +void gps_feed_value() +{ // FIXME, set proper time instead of hardcoded to May 2014 gps.week = 1794; gps.tow = fdm.time * 1000; @@ -76,20 +77,22 @@ void gps_feed_value() { /* convert to utm */ utm_of_lla_f(&utm_f, &lla_f); /* copy results of utm conversion */ - gps.utm_pos.east = utm_f.east*100; - gps.utm_pos.north = utm_f.north*100; + gps.utm_pos.east = utm_f.east * 100; + gps.utm_pos.north = utm_f.north * 100; gps.utm_pos.alt = gps.lla_pos.alt; gps.utm_pos.zone = nav_utm_zone0; #endif - if (gps_has_fix) + if (gps_has_fix) { gps.fix = GPS_FIX_3D; - else + } else { gps.fix = GPS_FIX_NONE; + } gps_available = TRUE; } -void gps_impl_init() { +void gps_impl_init() +{ gps_available = FALSE; gps_has_fix = TRUE; } diff --git a/sw/airborne/subsystems/gps/gps_sirf.c b/sw/airborne/subsystems/gps/gps_sirf.c index b832cac79b..d6d6f58f8d 100644 --- a/sw/airborne/subsystems/gps/gps_sirf.c +++ b/sw/airborne/subsystems/gps/gps_sirf.c @@ -40,53 +40,56 @@ struct GpsSirf gps_sirf; void sirf_parse_2(void); void sirf_parse_41(void); -void gps_impl_init( void ) { +void gps_impl_init(void) +{ gps_sirf.msg_available = FALSE; gps_sirf.pos_available = FALSE; gps_sirf.msg_len = 0; gps_sirf.read_state = 0; } -void sirf_parse_char(uint8_t c) { - switch(gps_sirf.read_state) { - case UNINIT: - if(c == 0xA0) { +void sirf_parse_char(uint8_t c) +{ + switch (gps_sirf.read_state) { + case UNINIT: + if (c == 0xA0) { gps_sirf.msg_len = 0; gps_sirf.msg_buf[gps_sirf.msg_len] = c; gps_sirf.msg_len++; gps_sirf.read_state = GOT_A0; } break; - case GOT_A0: - if(c == 0xA2) { + case GOT_A0: + if (c == 0xA2) { gps_sirf.msg_buf[gps_sirf.msg_len] = c; gps_sirf.msg_len++; gps_sirf.read_state = GOT_A2; - } - else + } 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) + if (c == 0xB0) { gps_sirf.read_state = GOT_B0; + } break; - case GOT_B0: - if(c == 0xB3) { + case GOT_B0: + if (c == 0xB3) { gps_sirf.msg_buf[gps_sirf.msg_len] = c; gps_sirf.msg_len++; gps_sirf.msg_available = TRUE; - } - else + } else { goto restart; + } break; default: break; } return; - restart: +restart: gps_sirf.read_state = UNINIT; } @@ -95,11 +98,12 @@ int ticks = 0; int start_time2 = 0; int ticks2 = 0; -void sirf_parse_41(void) { - struct sirf_msg_41* p = (struct sirf_msg_41*)&gps_sirf.msg_buf[4]; +void sirf_parse_41(void) +{ + struct sirf_msg_41 *p = (struct sirf_msg_41 *)&gps_sirf.msg_buf[4]; gps.tow = Invert4Bytes(p->tow); - gps.hmsl = Invert4Bytes(p->alt_msl)*10; + gps.hmsl = Invert4Bytes(p->alt_msl) * 10; gps.num_sv = p->num_sat; gps.nb_channels = p ->num_sat; @@ -117,33 +121,35 @@ void sirf_parse_41(void) { utm_of_lla_f(&utm_f, &lla_f); /* copy results of utm conversion */ - gps.utm_pos.east = utm_f.east*100; - gps.utm_pos.north = utm_f.north*100; + gps.utm_pos.east = utm_f.east * 100; + gps.utm_pos.north = utm_f.north * 100; gps.utm_pos.alt = gps.lla_pos.alt; gps.utm_pos.zone = nav_utm_zone0; #endif - gps.sacc = (Invert2Bytes(p->ehve)>>16); - gps.course = RadOfDeg(Invert2Bytes(p->cog))*pow(10, 5); - gps.gspeed = RadOfDeg(Invert2Bytes(p->sog))*pow(10, 5); - gps.cacc = RadOfDeg(Invert2Bytes(p->heading_err))*pow(10, 5); + gps.sacc = (Invert2Bytes(p->ehve) >> 16); + gps.course = RadOfDeg(Invert2Bytes(p->cog)) * pow(10, 5); + gps.gspeed = RadOfDeg(Invert2Bytes(p->sog)) * pow(10, 5); + gps.cacc = RadOfDeg(Invert2Bytes(p->heading_err)) * pow(10, 5); gps.pacc = Invert4Bytes(p->ehpe); gps.pdop = p->hdop * 20; - if ((p->nav_type >> 8 & 0x7) >= 0x4) + if ((p->nav_type >> 8 & 0x7) >= 0x4) { gps.fix = GPS_FIX_3D; - else if((p->nav_type >> 8 & 0x7) >= 0x1) + } else if ((p->nav_type >> 8 & 0x7) >= 0x1) { gps.fix = GPS_FIX_2D; - else + } else { gps.fix = GPS_FIX_NONE; + } //Let gps_sirf know we have a position update gps_sirf.pos_available = TRUE; } -void sirf_parse_2(void) { - struct sirf_msg_2* p = (struct sirf_msg_2*)&gps_sirf.msg_buf[4]; +void sirf_parse_2(void) +{ + struct sirf_msg_2 *p = (struct sirf_msg_2 *)&gps_sirf.msg_buf[4]; gps.week = Invert2Bytes(p->week); @@ -151,40 +157,42 @@ void sirf_parse_2(void) { gps.ecef_pos.y = Invert4Bytes(p->y_pos) * 100; gps.ecef_pos.z = Invert4Bytes(p->z_pos) * 100; - gps.ecef_vel.x = (Invert2Bytes(p->vx)>>16)*100/8; - gps.ecef_vel.y = (Invert2Bytes(p->vy)>>16)*100/8; - gps.ecef_vel.z = (Invert2Bytes(p->vz)>>16)*100/8; + gps.ecef_vel.x = (Invert2Bytes(p->vx) >> 16) * 100 / 8; + gps.ecef_vel.y = (Invert2Bytes(p->vy) >> 16) * 100 / 8; + gps.ecef_vel.z = (Invert2Bytes(p->vz) >> 16) * 100 / 8; - if(gps.fix == GPS_FIX_3D) { + if (gps.fix == GPS_FIX_3D) { ticks++; #if DEBUG_SIRF printf("GPS %i %i %i %i\n", ticks, (sys_time.nb_sec - start_time), ticks2, (sys_time.nb_sec - start_time2)); #endif - } - else if(sys_time.nb_sec - gps.last_3dfix_time > 10) { + } else if (sys_time.nb_sec - gps.last_3dfix_time > 10) { start_time = sys_time.nb_sec; ticks = 0; } } -void sirf_parse_msg(void) { +void sirf_parse_msg(void) +{ //Set position available to false and check if it is a valid message gps_sirf.pos_available = FALSE; - if(gps_sirf.msg_len < 8) + if (gps_sirf.msg_len < 8) { return; + } - if(start_time2 == 0) + if (start_time2 == 0) { start_time2 = sys_time.nb_sec; + } ticks2++; //Check the message id and parse the message uint8_t message_id = gps_sirf.msg_buf[4]; - switch(message_id) { - case 0x29: + switch (message_id) { + case 0x29: sirf_parse_41(); break; - case 0x02: + case 0x02: sirf_parse_2(); break; } diff --git a/sw/airborne/subsystems/gps/gps_sirf.h b/sw/airborne/subsystems/gps/gps_sirf.h index 02d9fde3f5..db270a4fe6 100644 --- a/sw/airborne/subsystems/gps/gps_sirf.h +++ b/sw/airborne/subsystems/gps/gps_sirf.h @@ -35,10 +35,10 @@ #define SIRF_MAXLEN 255 //Read states -#define UNINIT 0 -#define GOT_A0 1 -#define GOT_A2 2 -#define GOT_B0 3 +#define UNINIT 0 +#define GOT_A0 1 +#define GOT_A2 2 +#define GOT_B0 3 struct GpsSirf { bool_t msg_available; @@ -56,20 +56,20 @@ extern struct GpsSirf gps_sirf; /** Message ID 2 from GPS. Total payload length should be 41 bytes. */ struct sirf_msg_2 { - uint8_t msg_id; ///< hex value 0x02 ( = decimal 2) - int32_t x_pos; ///< x-position in m - int32_t y_pos; ///< y-position in m - int32_t z_pos; ///< z-position in m - int16_t vx; ///< x-velocity * 8 in m/s - int16_t vy; ///< y-velocity * 8 in m/s - int16_t vz; ///< z-velocity * 8 in m/s + uint8_t msg_id; ///< hex value 0x02 ( = decimal 2) + int32_t x_pos; ///< x-position in m + int32_t y_pos; ///< y-position in m + int32_t z_pos; ///< z-position in m + int16_t vx; ///< x-velocity * 8 in m/s + int16_t vy; ///< y-velocity * 8 in m/s + int16_t vz; ///< z-velocity * 8 in m/s uint8_t mode1; - uint8_t hdop; ///< horizontal dilution of precision *5 (0.2 precision) + uint8_t hdop; ///< horizontal dilution of precision *5 (0.2 precision) uint8_t mode2; uint16_t week; - uint32_t tow; ///< time of week in seconds * 10^2 - uint8_t num_sat; ///< Number of satellites in fix - uint8_t ch1prn; ///< pseudo-random noise, 12 channels + uint32_t tow; ///< time of week in seconds * 10^2 + uint8_t num_sat; ///< Number of satellites in fix + uint8_t ch1prn; ///< pseudo-random noise, 12 channels uint8_t ch2prn; uint8_t ch3prn; uint8_t ch4prn; @@ -81,48 +81,48 @@ struct sirf_msg_2 { uint8_t ch10prn; uint8_t ch11prn; uint8_t ch12prn; -} __attribute__ ((packed)); +} __attribute__((packed)); /** Message ID 41 from GPS. Total payload length should be 91 bytes. */ struct sirf_msg_41 { - uint8_t msg_id; ///< hex value 0x29 (= decimal 41) - uint16_t nav_valid; ///< if equal to 0x0000, then navigation solution is valid + uint8_t msg_id; ///< hex value 0x29 (= decimal 41) + uint16_t nav_valid; ///< if equal to 0x0000, then navigation solution is valid uint16_t nav_type; uint16_t extended_week_number; - uint32_t tow; ///< time of week in seconds *10^3] + uint32_t tow; ///< time of week in seconds *10^3] uint16_t year; uint8_t month; uint8_t day; uint8_t hour; uint8_t minute; uint16_t second; - uint32_t sat_id; ///< satellites used in solution. Each satellite corresponds with a bit, e.g. bit 1 ON = SV 1 is used in solution - int32_t latitude; ///< in degrees (+= North) *10^7 - int32_t longitude; ///< in degrees (+= East) *10*7 - int32_t alt_ellipsoid; ///< in meters *10^2 - int32_t alt_msl; ///< in meters *10^2 + uint32_t sat_id; ///< satellites used in solution. Each satellite corresponds with a bit, e.g. bit 1 ON = SV 1 is used in solution + int32_t latitude; ///< in degrees (+= North) *10^7 + int32_t longitude; ///< in degrees (+= East) *10*7 + int32_t alt_ellipsoid; ///< in meters *10^2 + int32_t alt_msl; ///< in meters *10^2 int8_t map_datum; - uint16_t sog; ///< speed over ground, in m/s * 10^2 - uint16_t cog; ///< course over ground, in degrees clockwise from true north * 10^2 - int16_t mag_var; ///< not implemented - int16_t climb_rate; ///< in m/s * 10^2 - int16_t heading_rate; ///< in deg/s * 10^2 - uint32_t ehpe; ///< estimated horizontal position error, in meters * 10^2 - uint32_t evpe; ///< estimated vertical position error, in meters * 10^2 - uint32_t ete; ///< estimated time error, in seconds * 10^2 - uint16_t ehve; ///< estimated horizontal velocity error in m/s * 10^2 - int32_t clock_bias; ///< in m * 10^2 - uint32_t clock_bias_err; ///< in m * 10^2 - int32_t clock_drift; ///< in m/s * 10^2 + uint16_t sog; ///< speed over ground, in m/s * 10^2 + uint16_t cog; ///< course over ground, in degrees clockwise from true north * 10^2 + int16_t mag_var; ///< not implemented + int16_t climb_rate; ///< in m/s * 10^2 + int16_t heading_rate; ///< in deg/s * 10^2 + uint32_t ehpe; ///< estimated horizontal position error, in meters * 10^2 + uint32_t evpe; ///< estimated vertical position error, in meters * 10^2 + uint32_t ete; ///< estimated time error, in seconds * 10^2 + uint16_t ehve; ///< estimated horizontal velocity error in m/s * 10^2 + int32_t clock_bias; ///< in m * 10^2 + uint32_t clock_bias_err; ///< in m * 10^2 + int32_t clock_drift; ///< in m/s * 10^2 uint32_t clock_drift_err; ///< in m/s * 10^2 - uint32_t distance; ///< Distance traveled since reset in m - uint16_t distance_err; ///< in meters - uint16_t heading_err; ///< in degrees * 10^2 - uint8_t num_sat; ///< Number of satellites used for solution - uint8_t hdop; ///< Horizontal dilution of precision x 5 (0.2 precision) - uint8_t add_info; ///< Additional mode info -} __attribute__ ((packed)); + uint32_t distance; ///< Distance traveled since reset in m + uint16_t distance_err; ///< in meters + uint16_t heading_err; ///< in degrees * 10^2 + uint8_t num_sat; ///< Number of satellites used for solution + uint8_t hdop; ///< Horizontal dilution of precision x 5 (0.2 precision) + uint8_t add_info; ///< Additional mode info +} __attribute__((packed)); /* * This part is used by the autopilot to read data from a uart @@ -153,7 +153,7 @@ struct sirf_msg_41 { } #define ReadGpsBuffer() { \ - while (GpsLink(ChAvailable())&&!gps_sirf.msg_available) \ + while (GpsLink(ChAvailable())&&!gps_sirf.msg_available) \ sirf_parse_char(GpsLink(Getch())); \ } diff --git a/sw/airborne/subsystems/gps/gps_skytraq.c b/sw/airborne/subsystems/gps/gps_skytraq.c index d3fedc36ea..e46606c697 100644 --- a/sw/airborne/subsystems/gps/gps_skytraq.c +++ b/sw/airborne/subsystems/gps/gps_skytraq.c @@ -54,8 +54,9 @@ struct GpsSkytraq gps_skytraq; #define SKYTRAQ_SYNC4 0x0A -static inline uint16_t bswap16(uint16_t a) { - return (a<<8)|(a>>8); +static inline uint16_t bswap16(uint16_t a) +{ + return (a << 8) | (a >> 8); } #define SKYTRAQ_NAVIGATION_DATA_FixMode(_payload) (uint8_t) (*((uint8_t*)_payload+2-2)) @@ -89,14 +90,16 @@ static inline uint16_t bswap16(uint16_t a) { static int distance_too_great(struct EcefCoor_i *ecef_ref, struct EcefCoor_i *ecef_pos); -void gps_impl_init(void) { +void gps_impl_init(void) +{ gps_skytraq.status = UNINIT; } -void gps_skytraq_read_message(void) { +void gps_skytraq_read_message(void) +{ if (gps_skytraq.msg_id == SKYTRAQ_ID_NAVIGATION_DATA) { gps.ecef_pos.x = SKYTRAQ_NAVIGATION_DATA_ECEFX(gps_skytraq.msg_buf); @@ -107,13 +110,13 @@ void gps_skytraq_read_message(void) { gps.ecef_vel.z = SKYTRAQ_NAVIGATION_DATA_ECEFVZ(gps_skytraq.msg_buf); gps.lla_pos.lat = SKYTRAQ_NAVIGATION_DATA_LAT(gps_skytraq.msg_buf); gps.lla_pos.lon = SKYTRAQ_NAVIGATION_DATA_LON(gps_skytraq.msg_buf); - gps.lla_pos.alt = SKYTRAQ_NAVIGATION_DATA_AEL(gps_skytraq.msg_buf)*10; - gps.hmsl = SKYTRAQ_NAVIGATION_DATA_ASL(gps_skytraq.msg_buf)*10; + gps.lla_pos.alt = SKYTRAQ_NAVIGATION_DATA_AEL(gps_skytraq.msg_buf) * 10; + gps.hmsl = SKYTRAQ_NAVIGATION_DATA_ASL(gps_skytraq.msg_buf) * 10; // pacc; // sacc; gps.pdop = SKYTRAQ_NAVIGATION_DATA_PDOP(gps_skytraq.msg_buf); gps.num_sv = SKYTRAQ_NAVIGATION_DATA_NumSV(gps_skytraq.msg_buf); - gps.tow = SKYTRAQ_NAVIGATION_DATA_TOW(gps_skytraq.msg_buf)*10; + gps.tow = SKYTRAQ_NAVIGATION_DATA_TOW(gps_skytraq.msg_buf) * 10; switch (SKYTRAQ_NAVIGATION_DATA_FixMode(gps_skytraq.msg_buf)) { case SKYTRAQ_FIX_3D_DGPS: @@ -136,14 +139,14 @@ void gps_skytraq_read_message(void) { /* convert to utm */ utm_of_lla_f(&utm_f, &lla_f); /* copy results of utm conversion */ - gps.utm_pos.east = utm_f.east*100; - gps.utm_pos.north = utm_f.north*100; + gps.utm_pos.east = utm_f.east * 100; + gps.utm_pos.north = utm_f.north * 100; gps.utm_pos.alt = gps.lla_pos.alt; gps.utm_pos.zone = nav_utm_zone0; #endif if (gps.fix == GPS_FIX_3D) { - if ( distance_too_great(&gps_skytraq.ref_ltp.ecef, &gps.ecef_pos)) { + if (distance_too_great(&gps_skytraq.ref_ltp.ecef, &gps.ecef_pos)) { // just grab current ecef_pos as reference. ltp_def_from_ecef_i(&gps_skytraq.ref_ltp, &gps.ecef_pos); } @@ -151,7 +154,7 @@ void gps_skytraq_read_message(void) { ned_of_ecef_vect_i(&gps.ned_vel, &gps_skytraq.ref_ltp, &gps.ecef_vel); // ground course in radians - gps.course = (atan2f( (float)gps.ned_vel.y, (float)gps.ned_vel.x )) * 1e7; + gps.course = (atan2f((float)gps.ned_vel.y, (float)gps.ned_vel.x)) * 1e7; // GT: gps.cacc = ... ? what should course accuracy be? // ground speed @@ -166,8 +169,7 @@ void gps_skytraq_read_message(void) { #ifdef GPS_LED if (gps.fix == GPS_FIX_3D) { LED_ON(GPS_LED); - } - else { + } else { LED_TOGGLE(GPS_LED); } #endif @@ -175,14 +177,16 @@ void gps_skytraq_read_message(void) { } -void gps_skytraq_parse(uint8_t c) { +void gps_skytraq_parse(uint8_t c) +{ if (gps_skytraq.status < GOT_PAYLOAD) { gps_skytraq.checksum ^= c; } switch (gps_skytraq.status) { case UNINIT: - if (c == SKYTRAQ_SYNC1) + if (c == SKYTRAQ_SYNC1) { gps_skytraq.status = GOT_SYNC1; + } break; case GOT_SYNC1: if (c != SKYTRAQ_SYNC2) { @@ -192,7 +196,7 @@ void gps_skytraq_parse(uint8_t c) { gps_skytraq.status = GOT_SYNC2; break; case GOT_SYNC2: - gps_skytraq.len = c<<8; + gps_skytraq.len = c << 8; gps_skytraq.status = GOT_LEN1; break; case GOT_LEN1: @@ -212,7 +216,7 @@ void gps_skytraq_parse(uint8_t c) { case GOT_ID: gps_skytraq.msg_buf[gps_skytraq.msg_idx] = c; gps_skytraq.msg_idx++; - if (gps_skytraq.msg_idx >= gps_skytraq.len-1) { + if (gps_skytraq.msg_idx >= gps_skytraq.len - 1) { gps_skytraq.status = GOT_PAYLOAD; } break; @@ -238,14 +242,15 @@ void gps_skytraq_parse(uint8_t c) { goto error; } return; - error: +error: gps_skytraq.error_cnt++; - restart: +restart: gps_skytraq.status = UNINIT; return; } -static int distance_too_great(struct EcefCoor_i *ecef_ref, struct EcefCoor_i *ecef_pos) { +static int distance_too_great(struct EcefCoor_i *ecef_ref, struct EcefCoor_i *ecef_pos) +{ int32_t xdiff = abs(ecef_ref->x - ecef_pos->x); if (xdiff > MAX_DISTANCE) { return TRUE; diff --git a/sw/airborne/subsystems/gps/gps_skytraq.h b/sw/airborne/subsystems/gps/gps_skytraq.h index ee95b8e233..d136dfc9b3 100644 --- a/sw/airborne/subsystems/gps/gps_skytraq.h +++ b/sw/airborne/subsystems/gps/gps_skytraq.h @@ -72,7 +72,7 @@ extern struct GpsSkytraq gps_skytraq; gps.last_msg_ticks = sys_time.nb_sec_rem; \ gps.last_msg_time = sys_time.nb_sec; \ gps_skytraq_read_message(); \ - if (gps_skytraq.msg_id == SKYTRAQ_ID_NAVIGATION_DATA) { \ + if (gps_skytraq.msg_id == SKYTRAQ_ID_NAVIGATION_DATA) { \ if (gps.fix == GPS_FIX_3D) { \ gps.last_3dfix_ticks = sys_time.nb_sec_rem; \ gps.last_3dfix_time = sys_time.nb_sec; \ @@ -83,9 +83,9 @@ extern struct GpsSkytraq gps_skytraq; } \ } -#define ReadGpsBuffer() { \ - while (GpsLink(ChAvailable())&&!gps_skytraq.msg_available) \ - gps_skytraq_parse(GpsLink(Getch())); \ +#define ReadGpsBuffer() { \ + while (GpsLink(ChAvailable())&&!gps_skytraq.msg_available) \ + gps_skytraq_parse(GpsLink(Getch())); \ } diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c index 03183eb2cb..c29c1f6b62 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.c +++ b/sw/airborne/subsystems/gps/gps_ubx.c @@ -61,16 +61,18 @@ struct GpsUbx gps_ubx; struct GpsUbxRaw gps_ubx_raw; #endif -void gps_impl_init(void) { - gps_ubx.status = UNINIT; - gps_ubx.msg_available = FALSE; - gps_ubx.error_cnt = 0; - gps_ubx.error_last = GPS_UBX_ERR_NONE; - gps_ubx.have_velned = 0; +void gps_impl_init(void) +{ + gps_ubx.status = UNINIT; + gps_ubx.msg_available = FALSE; + gps_ubx.error_cnt = 0; + gps_ubx.error_last = GPS_UBX_ERR_NONE; + gps_ubx.have_velned = 0; } -void gps_ubx_read_message(void) { +void gps_ubx_read_message(void) +{ if (gps_ubx.msg_class == UBX_NAV_ID) { if (gps_ubx.msg_id == UBX_NAV_SOL_ID) { @@ -94,8 +96,7 @@ void gps_ubx_read_message(void) { #ifdef GPS_LED if (gps.fix == GPS_FIX_3D) { LED_ON(GPS_LED); - } - else { + } else { LED_TOGGLE(GPS_LED); } #endif @@ -113,25 +114,24 @@ void gps_ubx_read_message(void) { /* convert to utm */ utm_of_lla_f(&utm_f, &lla_f); /* copy results of utm conversion */ - gps.utm_pos.east = utm_f.east*100; - gps.utm_pos.north = utm_f.north*100; + gps.utm_pos.east = utm_f.east * 100; + gps.utm_pos.north = utm_f.north * 100; gps.utm_pos.alt = gps.lla_pos.alt; gps.utm_pos.zone = nav_utm_zone0; #else - } - else if (gps_ubx.msg_id == UBX_NAV_POSUTM_ID) { + } else if (gps_ubx.msg_id == UBX_NAV_POSUTM_ID) { gps.utm_pos.east = UBX_NAV_POSUTM_EAST(gps_ubx.msg_buf); gps.utm_pos.north = UBX_NAV_POSUTM_NORTH(gps_ubx.msg_buf); uint8_t hem = UBX_NAV_POSUTM_HEM(gps_ubx.msg_buf); - if (hem == UTM_HEM_SOUTH) - gps.utm_pos.north -= 1000000000; /* Subtract false northing: -10000km */ - gps.utm_pos.alt = UBX_NAV_POSUTM_ALT(gps_ubx.msg_buf)*10; + if (hem == UTM_HEM_SOUTH) { + gps.utm_pos.north -= 1000000000; /* Subtract false northing: -10000km */ + } + gps.utm_pos.alt = UBX_NAV_POSUTM_ALT(gps_ubx.msg_buf) * 10; gps.hmsl = gps.utm_pos.alt; gps.lla_pos.alt = gps.utm_pos.alt; // FIXME: with UTM only you do not receive ellipsoid altitude gps.utm_pos.zone = UBX_NAV_POSUTM_ZONE(gps_ubx.msg_buf); #endif - } - else if (gps_ubx.msg_id == UBX_NAV_VELNED_ID) { + } else if (gps_ubx.msg_id == UBX_NAV_VELNED_ID) { gps.speed_3d = UBX_NAV_VELNED_Speed(gps_ubx.msg_buf); gps.gspeed = UBX_NAV_VELNED_GSpeed(gps_ubx.msg_buf); gps.ned_vel.x = UBX_NAV_VELNED_VEL_N(gps_ubx.msg_buf); @@ -141,15 +141,14 @@ void gps_ubx_read_message(void) { // I4 max = 2^31 = 214 * 1e5 * 100 < 360 * 1e7: overflow on angles over 214 deg -> casted to -214 deg // solution: First to radians, and then scale to 1e-7 radians // First x 10 for loosing less resolution, then to radians, then multiply x 10 again - gps.course = (RadOfDeg(UBX_NAV_VELNED_Heading(gps_ubx.msg_buf)*10)) * 10; - gps.cacc = (RadOfDeg(UBX_NAV_VELNED_CAcc(gps_ubx.msg_buf)*10)) * 10; + gps.course = (RadOfDeg(UBX_NAV_VELNED_Heading(gps_ubx.msg_buf) * 10)) * 10; + gps.cacc = (RadOfDeg(UBX_NAV_VELNED_CAcc(gps_ubx.msg_buf) * 10)) * 10; gps.tow = UBX_NAV_VELNED_ITOW(gps_ubx.msg_buf); gps_ubx.have_velned = 1; - } - else if (gps_ubx.msg_id == UBX_NAV_SVINFO_ID) { + } else if (gps_ubx.msg_id == UBX_NAV_SVINFO_ID) { gps.nb_channels = Min(UBX_NAV_SVINFO_NCH(gps_ubx.msg_buf), GPS_NB_CHANNELS); uint8_t i; - for(i = 0; i < gps.nb_channels; i++) { + for (i = 0; i < gps.nb_channels; i++) { gps.svinfos[i].svid = UBX_NAV_SVINFO_SVID(gps_ubx.msg_buf, i); gps.svinfos[i].flags = UBX_NAV_SVINFO_Flags(gps_ubx.msg_buf, i); gps.svinfos[i].qi = UBX_NAV_SVINFO_QI(gps_ubx.msg_buf, i); @@ -157,8 +156,7 @@ void gps_ubx_read_message(void) { gps.svinfos[i].elev = UBX_NAV_SVINFO_Elev(gps_ubx.msg_buf, i); gps.svinfos[i].azim = UBX_NAV_SVINFO_Azim(gps_ubx.msg_buf, i); } - } - else if (gps_ubx.msg_id == UBX_NAV_STATUS_ID) { + } else if (gps_ubx.msg_id == UBX_NAV_STATUS_ID) { gps.fix = UBX_NAV_STATUS_GPSfix(gps_ubx.msg_buf); gps_ubx.status_flags = UBX_NAV_STATUS_Flags(gps_ubx.msg_buf); gps_ubx.sol_flags = UBX_NAV_SOL_Flags(gps_ubx.msg_buf); @@ -191,7 +189,8 @@ void gps_ubx_read_message(void) { #endif /* UBX parsing */ -void gps_ubx_parse( uint8_t c ) { +void gps_ubx_parse(uint8_t c) +{ #if LOG_RAW_GPS sdLogWriteByte(&pprzLogFile, c); #endif @@ -200,80 +199,82 @@ void gps_ubx_parse( uint8_t c ) { gps_ubx.ck_b += gps_ubx.ck_a; } switch (gps_ubx.status) { - case UNINIT: - if (c == UBX_SYNC1) + case UNINIT: + if (c == UBX_SYNC1) { + gps_ubx.status++; + } + break; + case GOT_SYNC1: + if (c != UBX_SYNC2) { + gps_ubx.error_last = GPS_UBX_ERR_OUT_OF_SYNC; + goto error; + } + gps_ubx.ck_a = 0; + gps_ubx.ck_b = 0; gps_ubx.status++; - break; - case GOT_SYNC1: - if (c != UBX_SYNC2) { - gps_ubx.error_last = GPS_UBX_ERR_OUT_OF_SYNC; - goto error; - } - gps_ubx.ck_a = 0; - gps_ubx.ck_b = 0; - gps_ubx.status++; - break; - case GOT_SYNC2: - if (gps_ubx.msg_available) { - /* Previous message has not yet been parsed: discard this one */ - gps_ubx.error_last = GPS_UBX_ERR_OVERRUN; - goto error; - } - gps_ubx.msg_class = c; - gps_ubx.status++; - break; - case GOT_CLASS: - gps_ubx.msg_id = c; - gps_ubx.status++; - break; - case GOT_ID: - gps_ubx.len = c; - gps_ubx.status++; - break; - case GOT_LEN1: - gps_ubx.len |= (c<<8); - if (gps_ubx.len > GPS_UBX_MAX_PAYLOAD) { - gps_ubx.error_last = GPS_UBX_ERR_MSG_TOO_LONG; - goto error; - } - gps_ubx.msg_idx = 0; - gps_ubx.status++; - break; - case GOT_LEN2: - gps_ubx.msg_buf[gps_ubx.msg_idx] = c; - gps_ubx.msg_idx++; - if (gps_ubx.msg_idx >= gps_ubx.len) { + break; + case GOT_SYNC2: + if (gps_ubx.msg_available) { + /* Previous message has not yet been parsed: discard this one */ + gps_ubx.error_last = GPS_UBX_ERR_OVERRUN; + goto error; + } + gps_ubx.msg_class = c; gps_ubx.status++; - } - break; - case GOT_PAYLOAD: - if (c != gps_ubx.ck_a) { - gps_ubx.error_last = GPS_UBX_ERR_CHECKSUM; + break; + case GOT_CLASS: + gps_ubx.msg_id = c; + gps_ubx.status++; + break; + case GOT_ID: + gps_ubx.len = c; + gps_ubx.status++; + break; + case GOT_LEN1: + gps_ubx.len |= (c << 8); + if (gps_ubx.len > GPS_UBX_MAX_PAYLOAD) { + gps_ubx.error_last = GPS_UBX_ERR_MSG_TOO_LONG; + goto error; + } + gps_ubx.msg_idx = 0; + gps_ubx.status++; + break; + case GOT_LEN2: + gps_ubx.msg_buf[gps_ubx.msg_idx] = c; + gps_ubx.msg_idx++; + if (gps_ubx.msg_idx >= gps_ubx.len) { + gps_ubx.status++; + } + break; + case GOT_PAYLOAD: + if (c != gps_ubx.ck_a) { + gps_ubx.error_last = GPS_UBX_ERR_CHECKSUM; + goto error; + } + gps_ubx.status++; + break; + case GOT_CHECKSUM1: + if (c != gps_ubx.ck_b) { + gps_ubx.error_last = GPS_UBX_ERR_CHECKSUM; + goto error; + } + gps_ubx.msg_available = TRUE; + goto restart; + break; + default: + gps_ubx.error_last = GPS_UBX_ERR_UNEXPECTED; goto error; - } - gps_ubx.status++; - break; - case GOT_CHECKSUM1: - if (c != gps_ubx.ck_b) { - gps_ubx.error_last = GPS_UBX_ERR_CHECKSUM; - goto error; - } - gps_ubx.msg_available = TRUE; - goto restart; - break; - default: - gps_ubx.error_last = GPS_UBX_ERR_UNEXPECTED; - goto error; } return; - error: +error: gps_ubx.error_cnt++; - restart: +restart: gps_ubx.status = UNINIT; return; } -void ubxsend_cfg_rst(uint16_t bbr , uint8_t reset_mode) { +void ubxsend_cfg_rst(uint16_t bbr , uint8_t reset_mode) +{ #ifdef GPS_LINK UbxSend_CFG_RST(bbr, reset_mode, 0x00); #endif /* else less harmful for HITL */ diff --git a/sw/airborne/subsystems/gps/gps_ubx.h b/sw/airborne/subsystems/gps/gps_ubx.h index 722925ddd6..8ff670e005 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.h +++ b/sw/airborne/subsystems/gps/gps_ubx.h @@ -41,7 +41,7 @@ #define GPS_UBX_MAX_PAYLOAD 255 struct GpsUbx { bool_t msg_available; - uint8_t msg_buf[GPS_UBX_MAX_PAYLOAD] __attribute__ ((aligned)); + uint8_t msg_buf[GPS_UBX_MAX_PAYLOAD] __attribute__((aligned)); uint8_t msg_id; uint8_t msg_class; @@ -123,9 +123,9 @@ extern struct GpsUbxRaw gps_ubx_raw; } \ } -#define ReadGpsBuffer() { \ - while (GpsLink(ChAvailable())&&!gps_ubx.msg_available) \ - gps_ubx_parse(GpsLink(Getch())); \ +#define ReadGpsBuffer() { \ + while (GpsLink(ChAvailable())&&!gps_ubx.msg_available) \ + gps_ubx_parse(GpsLink(Getch())); \ } diff --git a/sw/airborne/subsystems/gps/gps_udp.c b/sw/airborne/subsystems/gps/gps_udp.c index 64430feed3..5941414676 100644 --- a/sw/airborne/subsystems/gps/gps_udp.c +++ b/sw/airborne/subsystems/gps/gps_udp.c @@ -42,9 +42,10 @@ bool_t gps_available; //Define the buffer, check bytes and FmsNetwork unsigned char gps_udp_read_buffer[256]; -struct FmsNetwork* gps_network = NULL; +struct FmsNetwork *gps_network = NULL; -void gps_impl_init(void) { +void gps_impl_init(void) +{ gps.fix = GPS_FIX_NONE; gps_available = FALSE; gps_network = network_new(GPS_UDP_HOST, 6000 /*out*/, 7000 /*in*/, TRUE); @@ -54,29 +55,28 @@ void gps_impl_init(void) { #define UDP_GPS_INT(_udp_gps_payload) (int32_t)(*((uint8_t*)_udp_gps_payload)|*((uint8_t*)_udp_gps_payload+1)<<8|((int32_t)*((uint8_t*)_udp_gps_payload+2))<<16|((int32_t)*((uint8_t*)_udp_gps_payload+3))<<24) -void gps_parse(void) { - if (gps_network == NULL) return; +void gps_parse(void) +{ + if (gps_network == NULL) { return; } //Read from the network - int size = network_read( gps_network, &gps_udp_read_buffer[0], 256); + int size = network_read(gps_network, &gps_udp_read_buffer[0], 256); - if(size > 0) - { + if (size > 0) { // Correct MSG - if ((size == GPS_UDP_MSG_LEN) && (gps_udp_read_buffer[0] == STX)) - { - gps.lla_pos.lat = UDP_GPS_INT(gps_udp_read_buffer+4); - gps.lla_pos.lon = UDP_GPS_INT(gps_udp_read_buffer+8); - gps.lla_pos.alt = UDP_GPS_INT(gps_udp_read_buffer+12); - gps.hmsl = UDP_GPS_INT(gps_udp_read_buffer+16); + if ((size == GPS_UDP_MSG_LEN) && (gps_udp_read_buffer[0] == STX)) { + gps.lla_pos.lat = UDP_GPS_INT(gps_udp_read_buffer + 4); + gps.lla_pos.lon = UDP_GPS_INT(gps_udp_read_buffer + 8); + gps.lla_pos.alt = UDP_GPS_INT(gps_udp_read_buffer + 12); + gps.hmsl = UDP_GPS_INT(gps_udp_read_buffer + 16); - gps.ecef_pos.x = UDP_GPS_INT(gps_udp_read_buffer+20); - gps.ecef_pos.y = UDP_GPS_INT(gps_udp_read_buffer+24); - gps.ecef_pos.z = UDP_GPS_INT(gps_udp_read_buffer+28); + gps.ecef_pos.x = UDP_GPS_INT(gps_udp_read_buffer + 20); + gps.ecef_pos.y = UDP_GPS_INT(gps_udp_read_buffer + 24); + gps.ecef_pos.z = UDP_GPS_INT(gps_udp_read_buffer + 28); - gps.ecef_vel.x = UDP_GPS_INT(gps_udp_read_buffer+32); - gps.ecef_vel.y = UDP_GPS_INT(gps_udp_read_buffer+36); - gps.ecef_vel.z = UDP_GPS_INT(gps_udp_read_buffer+40); + gps.ecef_vel.x = UDP_GPS_INT(gps_udp_read_buffer + 32); + gps.ecef_vel.y = UDP_GPS_INT(gps_udp_read_buffer + 36); + gps.ecef_vel.z = UDP_GPS_INT(gps_udp_read_buffer + 40); gps.fix = GPS_FIX_3D; gps_available = TRUE; @@ -90,16 +90,14 @@ void gps_parse(void) { // convert to utm utm_of_lla_f(&utm_f, &lla_f); // copy results of utm conversion - gps.utm_pos.east = utm_f.east*100; - gps.utm_pos.north = utm_f.north*100; + gps.utm_pos.east = utm_f.east * 100; + gps.utm_pos.north = utm_f.north * 100; gps.utm_pos.alt = gps.lla_pos.alt; gps.utm_pos.zone = nav_utm_zone0; #endif - } - else - { - printf("gps_udp error: msg len invalid %d bytes\n",size); + } else { + printf("gps_udp error: msg len invalid %d bytes\n", size); } memset(&gps_udp_read_buffer[0], 0, sizeof(gps_udp_read_buffer)); } diff --git a/sw/airborne/subsystems/imu.c b/sw/airborne/subsystems/imu.c index c7dcb09f72..b1f6880db9 100644 --- a/sw/airborne/subsystems/imu.c +++ b/sw/airborne/subsystems/imu.c @@ -44,67 +44,78 @@ #if USE_IMU_FLOAT -static void send_accel(struct transport_tx *trans, struct link_device *dev) { +static void send_accel(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_IMU_ACCEL(trans, dev, AC_ID, - &imuf.accel.x, &imuf.accel.y, &imuf.accel.z); + &imuf.accel.x, &imuf.accel.y, &imuf.accel.z); } -static void send_gyro(struct transport_tx *trans, struct link_device *dev) { +static void send_gyro(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_IMU_GYRO(trans, dev, AC_ID, - &imuf.gyro.p, &imuf.gyro.q, &imuf.gyro.r); + &imuf.gyro.p, &imuf.gyro.q, &imuf.gyro.r); } #else // !USE_IMU_FLOAT -static void send_accel_raw(struct transport_tx *trans, struct link_device *dev) { +static void send_accel_raw(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_IMU_ACCEL_RAW(trans, dev, AC_ID, - &imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z); + &imu.accel_unscaled.x, &imu.accel_unscaled.y, &imu.accel_unscaled.z); } -static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev) { +static void send_accel_scaled(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_IMU_ACCEL_SCALED(trans, dev, AC_ID, - &imu.accel.x, &imu.accel.y, &imu.accel.z); + &imu.accel.x, &imu.accel.y, &imu.accel.z); } -static void send_accel(struct transport_tx *trans, struct link_device *dev) { +static void send_accel(struct transport_tx *trans, struct link_device *dev) +{ struct FloatVect3 accel_float; ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); pprz_msg_send_IMU_ACCEL(trans, dev, AC_ID, - &accel_float.x, &accel_float.y, &accel_float.z); + &accel_float.x, &accel_float.y, &accel_float.z); } -static void send_gyro_raw(struct transport_tx *trans, struct link_device *dev) { +static void send_gyro_raw(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_IMU_GYRO_RAW(trans, dev, AC_ID, - &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r); + &imu.gyro_unscaled.p, &imu.gyro_unscaled.q, &imu.gyro_unscaled.r); } -static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev) { +static void send_gyro_scaled(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_IMU_GYRO_SCALED(trans, dev, AC_ID, - &imu.gyro.p, &imu.gyro.q, &imu.gyro.r); + &imu.gyro.p, &imu.gyro.q, &imu.gyro.r); } -static void send_gyro(struct transport_tx *trans, struct link_device *dev) { +static void send_gyro(struct transport_tx *trans, struct link_device *dev) +{ struct FloatRates gyro_float; RATES_FLOAT_OF_BFP(gyro_float, imu.gyro); pprz_msg_send_IMU_GYRO(trans, dev, AC_ID, - &gyro_float.p, &gyro_float.q, &gyro_float.r); + &gyro_float.p, &gyro_float.q, &gyro_float.r); } -static void send_mag_raw(struct transport_tx *trans, struct link_device *dev) { +static void send_mag_raw(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_IMU_MAG_RAW(trans, dev, AC_ID, - &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z); + &imu.mag_unscaled.x, &imu.mag_unscaled.y, &imu.mag_unscaled.z); } -static void send_mag_scaled(struct transport_tx *trans, struct link_device *dev) { +static void send_mag_scaled(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_IMU_MAG_SCALED(trans, dev, AC_ID, - &imu.mag.x, &imu.mag.y, &imu.mag.z); + &imu.mag.x, &imu.mag.y, &imu.mag.z); } -static void send_mag(struct transport_tx *trans, struct link_device *dev) { +static void send_mag(struct transport_tx *trans, struct link_device *dev) +{ struct FloatVect3 mag_float; MAGS_FLOAT_OF_BFP(mag_float, imu.mag); pprz_msg_send_IMU_MAG(trans, dev, AC_ID, - &mag_float.x, &mag_float.y, &mag_float.z); + &mag_float.x, &mag_float.y, &mag_float.z); } #endif // !USE_IMU_FLOAT @@ -113,7 +124,8 @@ static void send_mag(struct transport_tx *trans, struct link_device *dev) { struct Imu imu; struct ImuFloat imuf; -void imu_init(void) { +void imu_init(void) +{ #ifdef IMU_POWER_GPIO gpio_setup_output(IMU_POWER_GPIO); @@ -129,13 +141,13 @@ void imu_init(void) { VECT3_ASSIGN(imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL); #else #if USE_MAGNETOMETER -INFO("Magnetometer neutrals are set to zero, you should calibrate!") + INFO("Magnetometer neutrals are set to zero, you should calibrate!") #endif INT_VECT3_ZERO(imu.mag_neutral); #endif struct FloatEulers 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}; orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers); #if USE_IMU_FLOAT orientationSetEulers_f(&imuf.body_to_imu, &body_to_imu_eulers); @@ -161,7 +173,8 @@ INFO("Magnetometer neutrals are set to zero, you should calibrate!") } -void imu_SetBodyToImuPhi(float phi) { +void imu_SetBodyToImuPhi(float phi) +{ struct FloatEulers body_to_imu_eulers; memcpy(&body_to_imu_eulers, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers)); body_to_imu_eulers.phi = phi; @@ -171,7 +184,8 @@ void imu_SetBodyToImuPhi(float phi) { #endif } -void imu_SetBodyToImuTheta(float theta) { +void imu_SetBodyToImuTheta(float theta) +{ struct FloatEulers body_to_imu_eulers; memcpy(&body_to_imu_eulers, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers)); body_to_imu_eulers.theta = theta; @@ -181,7 +195,8 @@ void imu_SetBodyToImuTheta(float theta) { #endif } -void imu_SetBodyToImuPsi(float psi) { +void imu_SetBodyToImuPsi(float psi) +{ struct FloatEulers body_to_imu_eulers; memcpy(&body_to_imu_eulers, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers)); body_to_imu_eulers.psi = psi; @@ -191,7 +206,8 @@ void imu_SetBodyToImuPsi(float psi) { #endif } -void imu_SetBodyToImuCurrent(float set) { +void imu_SetBodyToImuCurrent(float set) +{ imu.b2i_set_current = set; if (imu.b2i_set_current) { @@ -206,16 +222,14 @@ void imu_SetBodyToImuCurrent(float set) { #if USE_IMU_FLOAT orientationSetEulers_f(&imuf.body_to_imu, &body_to_imu_eulers); #endif - } - else { + } else { // indicate that we couldn't set to current roll/pitch imu.b2i_set_current = FALSE; } - } - else { + } else { // reset to BODY_TO_IMU as defined in airframe file struct FloatEulers 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}; orientationSetEulers_f(&imu.body_to_imu, &body_to_imu_eulers); #if USE_IMU_FLOAT orientationSetEulers_f(&imuf.body_to_imu, &body_to_imu_eulers); @@ -230,7 +244,7 @@ void WEAK imu_periodic(void) { } -void WEAK imu_scale_gyro(struct Imu* _imu) +void WEAK imu_scale_gyro(struct Imu *_imu) { RATES_COPY(_imu->gyro_prev, _imu->gyro); _imu->gyro.p = ((_imu->gyro_unscaled.p - _imu->gyro_neutral.p) * IMU_GYRO_P_SIGN * @@ -241,7 +255,7 @@ void WEAK imu_scale_gyro(struct Imu* _imu) IMU_GYRO_R_SENS_NUM) / IMU_GYRO_R_SENS_DEN; } -void WEAK imu_scale_accel(struct Imu* _imu) +void WEAK imu_scale_accel(struct Imu *_imu) { VECT3_COPY(_imu->accel_prev, _imu->accel); _imu->accel.x = ((_imu->accel_unscaled.x - _imu->accel_neutral.x) * IMU_ACCEL_X_SIGN * @@ -254,7 +268,7 @@ void WEAK imu_scale_accel(struct Imu* _imu) #if defined IMU_MAG_X_CURRENT_COEF && defined IMU_MAG_Y_CURRENT_COEF && defined IMU_MAG_Z_CURRENT_COEF #include "subsystems/electrical.h" -void WEAK imu_scale_mag(struct Imu* _imu) +void WEAK imu_scale_mag(struct Imu *_imu) { struct Int32Vect3 mag_correction; mag_correction.x = (int32_t)(IMU_MAG_X_CURRENT_COEF * (float) electrical.current); @@ -268,7 +282,7 @@ void WEAK imu_scale_mag(struct Imu* _imu) IMU_MAG_Z_SIGN * IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN; } #elif USE_MAGNETOMETER -void WEAK imu_scale_mag(struct Imu* _imu) +void WEAK imu_scale_mag(struct Imu *_imu) { _imu->mag.x = ((_imu->mag_unscaled.x - _imu->mag_neutral.x) * IMU_MAG_X_SIGN * IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN; @@ -278,5 +292,5 @@ void WEAK imu_scale_mag(struct Imu* _imu) IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN; } #else -void WEAK imu_scale_mag(struct Imu* _imu __attribute__((unused))) {} +void WEAK imu_scale_mag(struct Imu *_imu __attribute__((unused))) {} #endif /* MAG_x_CURRENT_COEF */ diff --git a/sw/airborne/subsystems/imu.h b/sw/airborne/subsystems/imu.h index 8ccd518d94..1604f42ad1 100644 --- a/sw/airborne/subsystems/imu.h +++ b/sw/airborne/subsystems/imu.h @@ -87,9 +87,9 @@ extern void imu_SetBodyToImuCurrent(float set); extern void imu_ResetBodyToImu(float reset); /* can be provided implementation */ -extern void imu_scale_gyro(struct Imu* _imu); -extern void imu_scale_accel(struct Imu* _imu); -extern void imu_scale_mag(struct Imu* _imu); +extern void imu_scale_gyro(struct Imu *_imu); +extern void imu_scale_accel(struct Imu *_imu); +extern void imu_scale_mag(struct Imu *_imu); #if !defined IMU_BODY_TO_IMU_PHI && !defined IMU_BODY_TO_IMU_THETA && !defined IMU_BODY_TO_IMU_PSI #define IMU_BODY_TO_IMU_PHI 0 diff --git a/sw/airborne/subsystems/imu/imu_analog.c b/sw/airborne/subsystems/imu/imu_analog.c index 164aa3708d..357c145141 100644 --- a/sw/airborne/subsystems/imu/imu_analog.c +++ b/sw/airborne/subsystems/imu/imu_analog.c @@ -27,7 +27,8 @@ int imu_overrun; static struct adc_buf analog_imu_adc_buf[NB_ANALOG_IMU_ADC]; -void imu_impl_init(void) { +void imu_impl_init(void) +{ analog_imu_available = FALSE; imu_overrun = 0; @@ -53,13 +54,15 @@ void imu_impl_init(void) { } -void imu_periodic(void) { +void imu_periodic(void) +{ // Actual Nr of ADC measurements per channel per periodic loop static int last_head = 0; imu_overrun = analog_imu_adc_buf[0].head - last_head; - if (imu_overrun < 0) + if (imu_overrun < 0) { imu_overrun += ADC_CHANNEL_GYRO_NB_SAMPLES; + } last_head = analog_imu_adc_buf[0].head; // Read All Measurements @@ -88,22 +91,25 @@ void imu_periodic(void) { // if not all gyros are used, override the imu_scale_gyro handler #if defined ADC_CHANNEL_GYRO_P && defined ADC_CHANNEL_GYRO_Q && ! defined ADC_CHANNEL_GYRO_R -void imu_scale_gyro(struct Imu* _imu) +void imu_scale_gyro(struct Imu *_imu) { - _imu->gyro.p = ((_imu->gyro_unscaled.p - _imu->gyro_neutral.p)*IMU_GYRO_P_SIGN*IMU_GYRO_P_SENS_NUM)/IMU_GYRO_P_SENS_DEN; - _imu->gyro.q = ((_imu->gyro_unscaled.q - _imu->gyro_neutral.q)*IMU_GYRO_Q_SIGN*IMU_GYRO_Q_SENS_NUM)/IMU_GYRO_Q_SENS_DEN; + _imu->gyro.p = ((_imu->gyro_unscaled.p - _imu->gyro_neutral.p) * IMU_GYRO_P_SIGN * IMU_GYRO_P_SENS_NUM) / + IMU_GYRO_P_SENS_DEN; + _imu->gyro.q = ((_imu->gyro_unscaled.q - _imu->gyro_neutral.q) * IMU_GYRO_Q_SIGN * IMU_GYRO_Q_SENS_NUM) / + IMU_GYRO_Q_SENS_DEN; } #elif defined ADC_CHANNEL_GYRO_P && ! defined ADC_CHANNEL_GYRO_Q && ! defined ADC_CHANNEL_GYRO_R -void imu_scale_gyro(struct Imu* _imu) +void imu_scale_gyro(struct Imu *_imu) { - _imu->gyro.p = ((_imu->gyro_unscaled.p - _imu->gyro_neutral.p)*IMU_GYRO_P_SIGN*IMU_GYRO_P_SENS_NUM)/IMU_GYRO_P_SENS_DEN; + _imu->gyro.p = ((_imu->gyro_unscaled.p - _imu->gyro_neutral.p) * IMU_GYRO_P_SIGN * IMU_GYRO_P_SENS_NUM) / + IMU_GYRO_P_SENS_DEN; } #endif // if we don't have any accelerometers, set an empty imu_scale_accel handler #if ! defined ADC_CHANNEL_ACCEL_X && ! defined ADC_CHANNEL_ACCEL_Z && ! defined ADC_CHANNEL_ACCEL_Z -void imu_scale_accel(struct Imu* _imu __attribute__((unused))) {} +void imu_scale_accel(struct Imu *_imu __attribute__((unused))) {} #endif diff --git a/sw/airborne/subsystems/imu/imu_ardrone2_raw.c b/sw/airborne/subsystems/imu/imu_ardrone2_raw.c index 8fc8dc719b..4e7312e214 100644 --- a/sw/airborne/subsystems/imu/imu_ardrone2_raw.c +++ b/sw/airborne/subsystems/imu/imu_ardrone2_raw.c @@ -29,6 +29,7 @@ #include "imu_ardrone2_raw.h" #include "mcu_periph/uart.h" -void imu_impl_init(void) { +void imu_impl_init(void) +{ navdata_init(); } diff --git a/sw/airborne/subsystems/imu/imu_ardrone2_raw.h b/sw/airborne/subsystems/imu/imu_ardrone2_raw.h index 3662ac6bce..69975d54ab 100644 --- a/sw/airborne/subsystems/imu/imu_ardrone2_raw.h +++ b/sw/airborne/subsystems/imu/imu_ardrone2_raw.h @@ -84,14 +84,15 @@ */ #include "subsystems/imu.h" -static inline void imu_ardrone2_event ( void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) +static inline void imu_ardrone2_event(void (* _gyro_handler)(void), void (* _accel_handler)(void), + void (* _mag_handler)(void)) { navdata_update(); //checks if the navboard has a new dataset ready if (navdata_imu_available == TRUE) { navdata_imu_available = FALSE; RATES_ASSIGN(imu.gyro_unscaled, navdata.vx, -navdata.vy, -navdata.vz); - VECT3_ASSIGN(imu.accel_unscaled, navdata.ax, 4096-navdata.ay, 4096-navdata.az); + VECT3_ASSIGN(imu.accel_unscaled, navdata.ax, 4096 - navdata.ay, 4096 - navdata.az); VECT3_ASSIGN(imu.mag_unscaled, -navdata.mx, -navdata.my, -navdata.mz); _gyro_handler(); @@ -102,6 +103,6 @@ static inline void imu_ardrone2_event ( void (* _gyro_handler)(void), void (* _a #define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \ imu_ardrone2_event(_gyro_handler, _accel_handler, _mag_handler); \ -} + } #endif /* IMU_ARDRONE2_RAW_H_ */ diff --git a/sw/airborne/subsystems/imu/imu_ardrone2_sdk.c b/sw/airborne/subsystems/imu/imu_ardrone2_sdk.c index 837020778d..19a1c264a0 100644 --- a/sw/airborne/subsystems/imu/imu_ardrone2_sdk.c +++ b/sw/airborne/subsystems/imu/imu_ardrone2_sdk.c @@ -28,10 +28,12 @@ #include "imu_ardrone2_sdk.h" -void imu_impl_init(void) { +void imu_impl_init(void) +{ } -void imu_periodic(void) { +void imu_periodic(void) +{ } diff --git a/sw/airborne/subsystems/imu/imu_ardrone2_sdk.h b/sw/airborne/subsystems/imu/imu_ardrone2_sdk.h index 14fab1c40e..bef17f4c49 100644 --- a/sw/airborne/subsystems/imu/imu_ardrone2_sdk.h +++ b/sw/airborne/subsystems/imu/imu_ardrone2_sdk.h @@ -30,7 +30,8 @@ #include "subsystems/imu.h" #include "generated/airframe.h" -static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) { +static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) +{ _gyro_handler(); _accel_handler(); _mag_handler(); diff --git a/sw/airborne/subsystems/imu/imu_aspirin.c b/sw/airborne/subsystems/imu/imu_aspirin.c index c68fed5c44..be343bf2ef 100644 --- a/sw/airborne/subsystems/imu/imu_aspirin.c +++ b/sw/airborne/subsystems/imu/imu_aspirin.c @@ -97,7 +97,7 @@ void imu_impl_init(void) #endif #if ASPIRIN_ARCH_INDEP -TODO("Arch dependent functions (accel and gyro eoc interrupt) not used for aspirin!") + TODO("Arch dependent functions (accel and gyro eoc interrupt) not used for aspirin!") #else imu_aspirin_arch_init(); #endif diff --git a/sw/airborne/subsystems/imu/imu_aspirin.h b/sw/airborne/subsystems/imu/imu_aspirin.h index f213ace9c4..cee2c588aa 100644 --- a/sw/airborne/subsystems/imu/imu_aspirin.h +++ b/sw/airborne/subsystems/imu/imu_aspirin.h @@ -60,7 +60,8 @@ extern void imu_aspirin_arch_init(void); #endif -static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) { +static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) +{ imu_aspirin_event(); if (imu_aspirin.gyro_valid) { imu_aspirin.gyro_valid = FALSE; diff --git a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c index 18fff6b991..fdaebc187b 100644 --- a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c +++ b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.c @@ -99,8 +99,8 @@ PRINT_CONFIG_VAR(ASPIRIN_2_ACCEL_RANGE) struct ImuAspirin2Spi imu_aspirin2; void mpu_wait_slave4_ready(void); -void mpu_wait_slave4_ready_cb(struct spi_transaction * t); -bool_t imu_aspirin2_configure_mag_slave(Mpu60x0ConfigSet mpu_set, void* mpu); +void mpu_wait_slave4_ready_cb(struct spi_transaction *t); +bool_t imu_aspirin2_configure_mag_slave(Mpu60x0ConfigSet mpu_set, void *mpu); void imu_impl_init(void) { @@ -169,7 +169,7 @@ void imu_aspirin2_event(void) mag.z = Int16FromBuf(imu_aspirin2.mpu.data_ext, 2); mag.y = Int16FromBuf(imu_aspirin2.mpu.data_ext, 4); -/* Handle axis assignement for Lisa/S integrated Aspirin like IMU. */ + /* Handle axis assignement for Lisa/S integrated Aspirin like IMU. */ #ifdef LISA_S #ifdef LISA_S_UPSIDE_DOWN RATES_ASSIGN(imu.gyro_unscaled, @@ -188,9 +188,9 @@ void imu_aspirin2_event(void) #endif #else -/* Handle axis assignement for Lisa/M or Lisa/MX V2.1 integrated Aspirin like - * IMU. - */ + /* Handle axis assignement for Lisa/M or Lisa/MX V2.1 integrated Aspirin like + * IMU. + */ #ifdef LISA_M_OR_MX_21 RATES_ASSIGN(imu.gyro_unscaled, -imu_aspirin2.mpu.data_rates.rates.q, @@ -203,7 +203,7 @@ void imu_aspirin2_event(void) VECT3_ASSIGN(imu.mag_unscaled, -mag.y, mag.x, mag.z); #else -/* Handle real Aspirin IMU axis assignement. */ + /* Handle real Aspirin IMU axis assignement. */ #ifdef LISA_M_LONGITUDINAL_X RATES_ASSIGN(imu.gyro_unscaled, imu_aspirin2.mpu.data_rates.rates.q, @@ -230,21 +230,22 @@ void imu_aspirin2_event(void) } // hack with waiting to avoid creating another event loop to check the mag config status -static inline void mpu_set_and_wait(Mpu60x0ConfigSet mpu_set, void* mpu, uint8_t _reg, uint8_t _val) +static inline void mpu_set_and_wait(Mpu60x0ConfigSet mpu_set, void *mpu, uint8_t _reg, uint8_t _val) { mpu_set(mpu, _reg, _val); - while(imu_aspirin2.mpu.spi_trans.status != SPITransSuccess); + while (imu_aspirin2.mpu.spi_trans.status != SPITransSuccess); } /** function to configure hmc5883 mag * @return TRUE if mag configuration finished */ -bool_t imu_aspirin2_configure_mag_slave(Mpu60x0ConfigSet mpu_set, void* mpu) +bool_t imu_aspirin2_configure_mag_slave(Mpu60x0ConfigSet mpu_set, void *mpu) { // wait before starting the configuration of the HMC58xx mag // doing to early may void the mode configuration - if (get_sys_time_float() < ASPIRIN_2_MAG_STARTUP_DELAY) + if (get_sys_time_float() < ASPIRIN_2_MAG_STARTUP_DELAY) { return FALSE; + } mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_ADDR, (HMC58XX_ADDR >> 1)); mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV4_REG, HMC58XX_REG_CFGA); @@ -269,8 +270,8 @@ bool_t imu_aspirin2_configure_mag_slave(Mpu60x0ConfigSet mpu_set, void* mpu) mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV0_REG, HMC58XX_REG_DATXM); // Put the enable command as last. mpu_set_and_wait(mpu_set, mpu, MPU60X0_REG_I2C_SLV0_CTRL, - (1 << 7) | // Slave 0 enable - (6 << 0) ); // Read 6 bytes + (1 << 7) | // Slave 0 enable + (6 << 0)); // Read 6 bytes return TRUE; } @@ -285,11 +286,12 @@ void mpu_wait_slave4_ready(void) } } -void mpu_wait_slave4_ready_cb(struct spi_transaction * t) +void mpu_wait_slave4_ready_cb(struct spi_transaction *t) { - if (bit_is_set(t->input_buf[1], MPU60X0_I2C_SLV4_DONE)) + if (bit_is_set(t->input_buf[1], MPU60X0_I2C_SLV4_DONE)) { imu_aspirin2.slave4_ready = TRUE; - else + } else { imu_aspirin2.slave4_ready = FALSE; + } t->status = SPITransDone; } diff --git a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.h b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.h index b005aca42d..e92139a642 100644 --- a/sw/airborne/subsystems/imu/imu_aspirin_2_spi.h +++ b/sw/airborne/subsystems/imu/imu_aspirin_2_spi.h @@ -52,7 +52,8 @@ extern struct ImuAspirin2Spi imu_aspirin2; extern void imu_aspirin2_event(void); -static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) { +static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) +{ imu_aspirin2_event(); if (imu_aspirin2.gyro_valid) { imu_aspirin2.gyro_valid = FALSE; diff --git a/sw/airborne/subsystems/imu/imu_aspirin_i2c.c b/sw/airborne/subsystems/imu/imu_aspirin_i2c.c index a94ce32a77..348abda000 100644 --- a/sw/airborne/subsystems/imu/imu_aspirin_i2c.c +++ b/sw/airborne/subsystems/imu/imu_aspirin_i2c.c @@ -69,10 +69,10 @@ PRINT_CONFIG_VAR(ASPIRIN_GYRO_LOWPASS) #ifndef ASPIRIN_GYRO_SMPLRT_DIV # if PERIODIC_FREQUENCY <= 60 # define ASPIRIN_GYRO_SMPLRT_DIV 19 - PRINT_CONFIG_MSG("Gyro output rate is 50Hz") +PRINT_CONFIG_MSG("Gyro output rate is 50Hz") # else # define ASPIRIN_GYRO_SMPLRT_DIV 9 - PRINT_CONFIG_MSG("Gyro output rate is 100Hz") +PRINT_CONFIG_MSG("Gyro output rate is 100Hz") # endif #endif PRINT_CONFIG_VAR(ASPIRIN_GYRO_SMPLRT_DIV) diff --git a/sw/airborne/subsystems/imu/imu_aspirin_i2c.h b/sw/airborne/subsystems/imu/imu_aspirin_i2c.h index 282f7eb937..29115d1ff7 100644 --- a/sw/airborne/subsystems/imu/imu_aspirin_i2c.h +++ b/sw/airborne/subsystems/imu/imu_aspirin_i2c.h @@ -53,7 +53,8 @@ extern struct ImuAspirinI2c imu_aspirin; extern void imu_aspirin_i2c_event(void); -static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) { +static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) +{ imu_aspirin_i2c_event(); if (imu_aspirin.gyro_valid) { imu_aspirin.gyro_valid = FALSE; diff --git a/sw/airborne/subsystems/imu/imu_b2.c b/sw/airborne/subsystems/imu/imu_b2.c index f986a1d4c1..fbbfc14f4f 100644 --- a/sw/airborne/subsystems/imu/imu_b2.c +++ b/sw/airborne/subsystems/imu/imu_b2.c @@ -35,7 +35,8 @@ struct ImuBooz2 imu_b2; PRINT_CONFIG_VAR(IMU_B2_MAG_TYPE) -void imu_impl_init(void) { +void imu_impl_init(void) +{ max1168_init(); #if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2100 @@ -51,7 +52,8 @@ void imu_impl_init(void) { } #include "led.h" -void imu_periodic(void) { +void imu_periodic(void) +{ // read adc Max1168Periodic(); @@ -67,7 +69,7 @@ void imu_periodic(void) { } #if defined IMU_MAG_45_HACK -void imu_scale_mag(struct Imu* _imu) +void imu_scale_mag(struct Imu *_imu) { int32_t msx = ((_imu->mag_unscaled.x - _imu->mag_neutral.x) * IMU_MAG_X_SIGN * IMU_MAG_X_SENS_NUM) / IMU_MAG_X_SENS_DEN; int32_t msy = ((_imu->mag_unscaled.y - _imu->mag_neutral.y) * IMU_MAG_Y_SIGN * IMU_MAG_Y_SENS_NUM) / IMU_MAG_Y_SENS_DEN; @@ -76,5 +78,5 @@ void imu_scale_mag(struct Imu* _imu) _imu->mag.z = ((_imu->mag_unscaled.z - _imu->mag_neutral.z) * IMU_MAG_Z_SIGN * IMU_MAG_Z_SENS_NUM) / IMU_MAG_Z_SENS_DEN; } #elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_NONE -void imu_scale_mag(struct Imu* _imu __attribute__((unused))) {} +void imu_scale_mag(struct Imu *_imu __attribute__((unused))) {} #endif diff --git a/sw/airborne/subsystems/imu/imu_b2.h b/sw/airborne/subsystems/imu/imu_b2.h index c46f96c108..4074879fff 100644 --- a/sw/airborne/subsystems/imu/imu_b2.h +++ b/sw/airborne/subsystems/imu/imu_b2.h @@ -170,7 +170,8 @@ extern struct ImuBooz2 imu_b2; #if defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_MS2100 #include "peripherals/ms2100.h" -static inline void ImuMagEvent(void (* _mag_handler)(void)) { +static inline void ImuMagEvent(void (* _mag_handler)(void)) +{ ms2100_event(&ms2100); if (ms2100.status == MS2100_DATA_AVAILABLE) { imu.mag_unscaled.x = ms2100.data.value[IMU_MAG_X_CHAN]; @@ -183,7 +184,8 @@ static inline void ImuMagEvent(void (* _mag_handler)(void)) { #elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_AMI601 #include "peripherals/ami601.h" static inline void foo_handler(void) {} -static inline void ImuMagEvent(void (* _mag_handler)(void)) { +static inline void ImuMagEvent(void (* _mag_handler)(void)) +{ AMI601Event(foo_handler); if (ami601_status == AMI601_DATA_AVAILABLE) { imu.mag_unscaled.x = ami601_values[IMU_MAG_X_CHAN]; @@ -196,7 +198,8 @@ static inline void ImuMagEvent(void (* _mag_handler)(void)) { #elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_HMC5843 #include "peripherals/hmc5843.h" static inline void foo_handler(void) {} -static inline void ImuMagEvent(void (* _mag_handler)(void)) { +static inline void ImuMagEvent(void (* _mag_handler)(void)) +{ MagEvent(foo_handler); if (hmc5843.data_available) { imu.mag_unscaled.x = hmc5843.data.value[IMU_MAG_X_CHAN]; @@ -207,7 +210,8 @@ static inline void ImuMagEvent(void (* _mag_handler)(void)) { } } #elif defined IMU_B2_MAG_TYPE && IMU_B2_MAG_TYPE == IMU_B2_MAG_HMC58XX -static inline void ImuMagEvent(void (* _mag_handler)(void)) { +static inline void ImuMagEvent(void (* _mag_handler)(void)) +{ hmc58xx_event(&imu_b2.mag_hmc); if (imu_b2.mag_hmc.data_available) { imu.mag_unscaled.x = imu_b2.mag_hmc.data.value[IMU_MAG_X_CHAN]; diff --git a/sw/airborne/subsystems/imu/imu_bebop.c b/sw/airborne/subsystems/imu/imu_bebop.c index f48b729238..78b28a357b 100644 --- a/sw/airborne/subsystems/imu/imu_bebop.c +++ b/sw/airborne/subsystems/imu/imu_bebop.c @@ -116,8 +116,10 @@ void imu_bebop_event(void) if (imu_bebop.mpu.data_available) { /* default orientation of the MPU is upside down sor corrigate this here */ - RATES_ASSIGN(imu.gyro_unscaled, imu_bebop.mpu.data_rates.rates.p, -imu_bebop.mpu.data_rates.rates.q, -imu_bebop.mpu.data_rates.rates.r); - VECT3_ASSIGN(imu.accel_unscaled, imu_bebop.mpu.data_accel.vect.x, -imu_bebop.mpu.data_accel.vect.y, -imu_bebop.mpu.data_accel.vect.z); + RATES_ASSIGN(imu.gyro_unscaled, imu_bebop.mpu.data_rates.rates.p, -imu_bebop.mpu.data_rates.rates.q, + -imu_bebop.mpu.data_rates.rates.r); + VECT3_ASSIGN(imu.accel_unscaled, imu_bebop.mpu.data_accel.vect.x, -imu_bebop.mpu.data_accel.vect.y, + -imu_bebop.mpu.data_accel.vect.z); imu_bebop.mpu.data_available = FALSE; imu_bebop.gyro_valid = TRUE; diff --git a/sw/airborne/subsystems/imu/imu_crista.c b/sw/airborne/subsystems/imu/imu_crista.c index 5374307037..e863a48266 100644 --- a/sw/airborne/subsystems/imu/imu_crista.c +++ b/sw/airborne/subsystems/imu/imu_crista.c @@ -24,7 +24,8 @@ volatile bool_t ADS8344_available; uint16_t ADS8344_values[ADS8344_NB_CHANNELS]; -void imu_impl_init(void) { +void imu_impl_init(void) +{ ADS8344_available = FALSE; @@ -39,7 +40,8 @@ void imu_impl_init(void) { } -void imu_periodic(void) { +void imu_periodic(void) +{ ImuCristaArchPeriodic(); #ifdef USE_AMI601 diff --git a/sw/airborne/subsystems/imu/imu_crista.h b/sw/airborne/subsystems/imu/imu_crista.h index 4cb3b60939..93f441d1fc 100644 --- a/sw/airborne/subsystems/imu/imu_crista.h +++ b/sw/airborne/subsystems/imu/imu_crista.h @@ -29,47 +29,47 @@ extern uint16_t ADS8344_values[ADS8344_NB_CHANNELS]; extern volatile bool_t ADS8344_available; -#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \ - if (ADS8344_available) { \ - ADS8344_available = FALSE; \ - imu.gyro_unscaled.p = ADS8344_values[IMU_GYRO_P_CHAN]; \ - imu.gyro_unscaled.q = ADS8344_values[IMU_GYRO_Q_CHAN]; \ - imu.gyro_unscaled.r = ADS8344_values[IMU_GYRO_R_CHAN]; \ - imu.accel_unscaled.x = ADS8344_values[IMU_ACCEL_X_CHAN]; \ - imu.accel_unscaled.y = ADS8344_values[IMU_ACCEL_Y_CHAN]; \ - imu.accel_unscaled.z = ADS8344_values[IMU_ACCEL_Z_CHAN]; \ - /* spare 3, temp 7 */ \ - _gyro_handler(); \ - _accel_handler(); \ - } \ - ImuMagEvent(_mag_handler); \ +#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \ + if (ADS8344_available) { \ + ADS8344_available = FALSE; \ + imu.gyro_unscaled.p = ADS8344_values[IMU_GYRO_P_CHAN]; \ + imu.gyro_unscaled.q = ADS8344_values[IMU_GYRO_Q_CHAN]; \ + imu.gyro_unscaled.r = ADS8344_values[IMU_GYRO_R_CHAN]; \ + imu.accel_unscaled.x = ADS8344_values[IMU_ACCEL_X_CHAN]; \ + imu.accel_unscaled.y = ADS8344_values[IMU_ACCEL_Y_CHAN]; \ + imu.accel_unscaled.z = ADS8344_values[IMU_ACCEL_Z_CHAN]; \ + /* spare 3, temp 7 */ \ + _gyro_handler(); \ + _accel_handler(); \ + } \ + ImuMagEvent(_mag_handler); \ } #ifdef USE_AMI601 #include "peripherals/ami601.h" #define foo_handler() {} -#define ImuMagEvent(_mag_handler) { \ - AMI601Event(foo_handler); \ - if (ami601_status == AMI601_DATA_AVAILABLE) { \ - imu.mag_unscaled.x = ami601_values[IMU_MAG_X_CHAN]; \ - imu.mag_unscaled.y = ami601_values[IMU_MAG_Y_CHAN]; \ - imu.mag_unscaled.z = ami601_values[IMU_MAG_Z_CHAN]; \ - ami601_status = AMI601_IDLE; \ - _mag_handler(); \ - } \ +#define ImuMagEvent(_mag_handler) { \ + AMI601Event(foo_handler); \ + if (ami601_status == AMI601_DATA_AVAILABLE) { \ + imu.mag_unscaled.x = ami601_values[IMU_MAG_X_CHAN]; \ + imu.mag_unscaled.y = ami601_values[IMU_MAG_Y_CHAN]; \ + imu.mag_unscaled.z = ami601_values[IMU_MAG_Z_CHAN]; \ + ami601_status = AMI601_IDLE; \ + _mag_handler(); \ + } \ } #elif defined USE_HMC5843 #include "peripherals/hmc5843.h" #define foo_handler() {} -#define ImuMagEvent(_mag_handler) { \ - MagEvent(foo_handler); \ - if (hmc5843.data_available) { \ - imu.mag_unscaled.x = hmc5843.data.value[IMU_MAG_X_CHAN]; \ - imu.mag_unscaled.y = hmc5843.data.value[IMU_MAG_Y_CHAN]; \ - imu.mag_unscaled.z = hmc5843.data.value[IMU_MAG_Z_CHAN]; \ - _mag_handler(); \ - hmc5843.data_available = FALSE; \ - } \ +#define ImuMagEvent(_mag_handler) { \ + MagEvent(foo_handler); \ + if (hmc5843.data_available) { \ + imu.mag_unscaled.x = hmc5843.data.value[IMU_MAG_X_CHAN]; \ + imu.mag_unscaled.y = hmc5843.data.value[IMU_MAG_Y_CHAN]; \ + imu.mag_unscaled.z = hmc5843.data.value[IMU_MAG_Z_CHAN]; \ + _mag_handler(); \ + hmc5843.data_available = FALSE; \ + } \ } #else #define ImuMagEvent(_mag_handler) {} diff --git a/sw/airborne/subsystems/imu/imu_drotek_10dof_v2.c b/sw/airborne/subsystems/imu/imu_drotek_10dof_v2.c index 256eb96266..2598bb4fb1 100644 --- a/sw/airborne/subsystems/imu/imu_drotek_10dof_v2.c +++ b/sw/airborne/subsystems/imu/imu_drotek_10dof_v2.c @@ -161,11 +161,13 @@ void imu_drotek2_event(void) /** callback function to configure hmc5883 mag * @return TRUE if mag configuration finished */ -bool_t imu_drotek2_configure_mag_slave(Mpu60x0ConfigSet mpu_set __attribute__ ((unused)), void* mpu __attribute__ ((unused))) +bool_t imu_drotek2_configure_mag_slave(Mpu60x0ConfigSet mpu_set __attribute__((unused)), + void *mpu __attribute__((unused))) { hmc58xx_start_configure(&imu_drotek2.hmc); - if (imu_drotek2.hmc.initialized) + if (imu_drotek2.hmc.initialized) { return TRUE; - else + } else { return FALSE; + } } diff --git a/sw/airborne/subsystems/imu/imu_drotek_10dof_v2.h b/sw/airborne/subsystems/imu/imu_drotek_10dof_v2.h index 2ebe9d0034..eb0243b161 100644 --- a/sw/airborne/subsystems/imu/imu_drotek_10dof_v2.h +++ b/sw/airborne/subsystems/imu/imu_drotek_10dof_v2.h @@ -84,10 +84,11 @@ struct ImuDrotek2 { extern struct ImuDrotek2 imu_drotek2; extern void imu_drotek2_event(void); -extern bool_t imu_drotek2_configure_mag_slave(Mpu60x0ConfigSet mpu_set, void* mpu); +extern bool_t imu_drotek2_configure_mag_slave(Mpu60x0ConfigSet mpu_set, void *mpu); -static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) { +static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) +{ imu_drotek2_event(); if (imu_drotek2.gyro_valid) { imu_drotek2.gyro_valid = FALSE; diff --git a/sw/airborne/subsystems/imu/imu_dummy.c b/sw/airborne/subsystems/imu/imu_dummy.c index 8ef21d644c..bcffaa7bd6 100644 --- a/sw/airborne/subsystems/imu/imu_dummy.c +++ b/sw/airborne/subsystems/imu/imu_dummy.c @@ -1,6 +1,7 @@ -void imu_impl_init(void) { +void imu_impl_init(void) +{ } diff --git a/sw/airborne/subsystems/imu/imu_gl1.c b/sw/airborne/subsystems/imu/imu_gl1.c index eb035d6704..8c7281fc2e 100644 --- a/sw/airborne/subsystems/imu/imu_gl1.c +++ b/sw/airborne/subsystems/imu/imu_gl1.c @@ -94,9 +94,9 @@ void imu_impl_init(void) l3g4200_init(&imu_gl1.gyro_l3g, &(GL1_I2C_DEV), L3G4200_ADDR_ALT); /* change the default config */ // output data rate, bandwidth, enable axis (0x1f = 100 ODR, 25hz) (0x5f = 200hz ODR, 25hz) - imu_gl1.gyro_l3g.config.ctrl_reg1 = ((GL1_GYRO_SMPLRT<<6) | (GL1_GYRO_LOWPASS<<4) | 0xf); + imu_gl1.gyro_l3g.config.ctrl_reg1 = ((GL1_GYRO_SMPLRT << 6) | (GL1_GYRO_LOWPASS << 4) | 0xf); // senstivity - imu_gl1.gyro_l3g.config.ctrl_reg4 = (L3G4200_SCALE<<4) | 0x00; + imu_gl1.gyro_l3g.config.ctrl_reg4 = (L3G4200_SCALE << 4) | 0x00; // filter config imu_gl1.gyro_l3g.config.ctrl_reg5 = 0x00; // only first LPF active diff --git a/sw/airborne/subsystems/imu/imu_gl1.h b/sw/airborne/subsystems/imu/imu_gl1.h index a222ca6459..bb28d6c3a9 100644 --- a/sw/airborne/subsystems/imu/imu_gl1.h +++ b/sw/airborne/subsystems/imu/imu_gl1.h @@ -53,7 +53,8 @@ extern struct ImuGL1I2c imu_gl1; extern void imu_gl1_i2c_event(void); -static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) { +static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) +{ imu_gl1_i2c_event(); if (imu_gl1.gyro_valid) { imu_gl1.gyro_valid = FALSE; diff --git a/sw/airborne/subsystems/imu/imu_mpu6000_hmc5883.h b/sw/airborne/subsystems/imu/imu_mpu6000_hmc5883.h index 8cbc4415f2..563d612d80 100644 --- a/sw/airborne/subsystems/imu/imu_mpu6000_hmc5883.h +++ b/sw/airborne/subsystems/imu/imu_mpu6000_hmc5883.h @@ -50,7 +50,8 @@ extern struct ImuMpu6000Hmc5883 imu_mpu_hmc; extern void imu_mpu_hmc_event(void); -static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) { +static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) +{ imu_mpu_hmc_event(); if (imu_mpu_hmc.gyro_valid) { imu_mpu_hmc.gyro_valid = FALSE; diff --git a/sw/airborne/subsystems/imu/imu_mpu9250_i2c.h b/sw/airborne/subsystems/imu/imu_mpu9250_i2c.h index 2a39b804f7..94e41d04cb 100644 --- a/sw/airborne/subsystems/imu/imu_mpu9250_i2c.h +++ b/sw/airborne/subsystems/imu/imu_mpu9250_i2c.h @@ -82,7 +82,8 @@ extern struct ImuMpu9250 imu_mpu9250; extern void imu_mpu9250_event(void); -static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) { +static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) +{ imu_mpu9250_event(); if (imu_mpu9250.accel_valid) { imu_mpu9250.accel_valid = FALSE; diff --git a/sw/airborne/subsystems/imu/imu_navstik.h b/sw/airborne/subsystems/imu/imu_navstik.h index e08f15f138..5bab1ea261 100644 --- a/sw/airborne/subsystems/imu/imu_navstik.h +++ b/sw/airborne/subsystems/imu/imu_navstik.h @@ -82,7 +82,8 @@ extern struct ImuNavstik imu_navstik; extern void imu_navstik_event(void); -static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) { +static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) +{ imu_navstik_event(); if (imu_navstik.gyro_valid) { imu_navstik.gyro_valid = FALSE; diff --git a/sw/airborne/subsystems/imu/imu_nps.c b/sw/airborne/subsystems/imu/imu_nps.c index 5cc65e8ae8..be4d75dd6a 100644 --- a/sw/airborne/subsystems/imu/imu_nps.c +++ b/sw/airborne/subsystems/imu/imu_nps.c @@ -27,7 +27,8 @@ struct ImuNps imu_nps; -void imu_impl_init(void) { +void imu_impl_init(void) +{ imu_nps.gyro_available = FALSE; imu_nps.mag_available = FALSE; @@ -35,11 +36,13 @@ void imu_impl_init(void) { } -void imu_periodic(void) { +void imu_periodic(void) +{ } -void imu_feed_gyro_accel(void) { +void imu_feed_gyro_accel(void) +{ RATES_ASSIGN(imu.gyro_unscaled, sensors.gyro.value.x, sensors.gyro.value.y, sensors.gyro.value.z); VECT3_ASSIGN(imu.accel_unscaled, sensors.accel.value.x, sensors.accel.value.y, sensors.accel.value.z); @@ -51,7 +54,8 @@ void imu_feed_gyro_accel(void) { } -void imu_feed_mag(void) { +void imu_feed_mag(void) +{ VECT3_ASSIGN(imu.mag_unscaled, sensors.mag.value.x, sensors.mag.value.y, sensors.mag.value.z); imu_nps.mag_available = TRUE; diff --git a/sw/airborne/subsystems/imu/imu_ppzuav.c b/sw/airborne/subsystems/imu/imu_ppzuav.c index 8d79810446..83262743bc 100644 --- a/sw/airborne/subsystems/imu/imu_ppzuav.c +++ b/sw/airborne/subsystems/imu/imu_ppzuav.c @@ -66,13 +66,13 @@ PRINT_CONFIG_VAR(IMU_PPZUAV_GYRO_LOWPASS) #ifndef IMU_PPZUAV_GYRO_SMPLRT_DIV # if PERIODIC_FREQUENCY <= 60 # define IMU_PPZUAV_GYRO_SMPLRT_DIV 19 - PRINT_CONFIG_MSG("Gyro output rate is 50Hz") +PRINT_CONFIG_MSG("Gyro output rate is 50Hz") # else # define IMU_PPZUAV_GYRO_SMPLRT_DIV 9 - PRINT_CONFIG_MSG("Gyro output rate is 100Hz") +PRINT_CONFIG_MSG("Gyro output rate is 100Hz") # endif #endif - PRINT_CONFIG_VAR(IMU_PPZUAV_GYRO_SMPLRT_DIV) +PRINT_CONFIG_VAR(IMU_PPZUAV_GYRO_SMPLRT_DIV) struct ImuPpzuav imu_ppzuav; diff --git a/sw/airborne/subsystems/imu/imu_ppzuav.h b/sw/airborne/subsystems/imu/imu_ppzuav.h index 0774008d80..4971f48cac 100644 --- a/sw/airborne/subsystems/imu/imu_ppzuav.h +++ b/sw/airborne/subsystems/imu/imu_ppzuav.h @@ -92,7 +92,8 @@ extern struct ImuPpzuav imu_ppzuav; extern void imu_ppzuav_event(void); -static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) { +static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) +{ imu_ppzuav_event(); if (imu_ppzuav.gyro_valid) { imu_ppzuav.gyro_valid = FALSE; diff --git a/sw/airborne/subsystems/imu/imu_px4fmu.h b/sw/airborne/subsystems/imu/imu_px4fmu.h index 3c79b49495..085540653b 100644 --- a/sw/airborne/subsystems/imu/imu_px4fmu.h +++ b/sw/airborne/subsystems/imu/imu_px4fmu.h @@ -48,7 +48,8 @@ extern struct ImuPx4fmu imu_px4fmu; extern void imu_px4fmu_event(void); -static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) { +static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void)) +{ imu_px4fmu_event(); if (imu_px4fmu.gyro_valid) { imu_px4fmu.gyro_valid = FALSE; diff --git a/sw/airborne/subsystems/imu/imu_um6.c b/sw/airborne/subsystems/imu/imu_um6.c index 93c0bc6a5d..eb4ccf4f88 100644 --- a/sw/airborne/subsystems/imu/imu_um6.c +++ b/sw/airborne/subsystems/imu/imu_um6.c @@ -19,17 +19,17 @@ * the Free Software Foundation, 59 Temple Place - Suite 330, * Boston, MA 02111-1307, USA. */ - /** - * @file imu_um6.c - * - * Driver for CH Robotics UM6 IMU/AHRS subsystem - * - * Takes care of configuration of the IMU, communication and parsing - * the received packets. See UM6 datasheet for configuration options. - * Should be used with ahrs_extern_euler AHRS subsystem. - * - * @author Michal Podhradsky - */ +/** +* @file imu_um6.c +* +* Driver for CH Robotics UM6 IMU/AHRS subsystem +* +* Takes care of configuration of the IMU, communication and parsing +* the received packets. See UM6 datasheet for configuration options. +* Should be used with ahrs_extern_euler AHRS subsystem. +* +* @author Michal Podhradsky +*/ #include "subsystems/imu/imu_um6.h" #include "subsystems/imu.h" #include "mcu_periph/sys_time.h" @@ -57,77 +57,82 @@ inline void UM6_send_packet(uint8_t *packet_buffer, uint8_t packet_length); inline uint16_t UM6_calculate_checksum(uint8_t packet_buffer[], uint8_t packet_length); inline bool_t UM6_verify_chk(uint8_t packet_buffer[], uint8_t packet_length); -inline bool_t UM6_verify_chk(uint8_t packet_buffer[], uint8_t packet_length) { - chk_rec = (packet_buffer[packet_length-2] << 8) | packet_buffer[packet_length-1]; - chk_calc = UM6_calculate_checksum(packet_buffer, packet_length-2); - return (chk_calc == chk_rec); +inline bool_t UM6_verify_chk(uint8_t packet_buffer[], uint8_t packet_length) +{ + chk_rec = (packet_buffer[packet_length - 2] << 8) | packet_buffer[packet_length - 1]; + chk_calc = UM6_calculate_checksum(packet_buffer, packet_length - 2); + return (chk_calc == chk_rec); } -inline uint16_t UM6_calculate_checksum(uint8_t packet_buffer[], uint8_t packet_length) { +inline uint16_t UM6_calculate_checksum(uint8_t packet_buffer[], uint8_t packet_length) +{ uint16_t chk = 0; - for (int i=0; i> 8); - buf_out[6] = (uint8_t)data_chk; - UM6_send_packet(buf_out, 7); + // Acceleration vector realign + buf_out[0] = 's'; + buf_out[1] = 'n'; + buf_out[2] = 'p'; + buf_out[3] = 0; + buf_out[4] = IMU_UM6_SET_MAG_REF; + data_chk = UM6_calculate_checksum(buf_out, 5); + buf_out[5] = (uint8_t)(data_chk >> 8); + buf_out[6] = (uint8_t)data_chk; + UM6_send_packet(buf_out, 7); - // Magnetic realign - buf_out[0] = 's'; - buf_out[1] = 'n'; - buf_out[2] = 'p'; - buf_out[3] = 0; - buf_out[4] = IMU_UM6_SET_ACCEL_REF; - data_chk = UM6_calculate_checksum(buf_out, 5); - buf_out[5] = (uint8_t)(data_chk >> 8); - buf_out[6] = (uint8_t)data_chk; - UM6_send_packet(buf_out, 7); + // Magnetic realign + buf_out[0] = 's'; + buf_out[1] = 'n'; + buf_out[2] = 'p'; + buf_out[3] = 0; + buf_out[4] = IMU_UM6_SET_ACCEL_REF; + data_chk = UM6_calculate_checksum(buf_out, 5); + buf_out[5] = (uint8_t)(data_chk >> 8); + buf_out[6] = (uint8_t)data_chk; + UM6_send_packet(buf_out, 7); - // Zero gyros 0xAC, takes around 3s - buf_out[0] = 's'; - buf_out[1] = 'n'; - buf_out[2] = 'p'; - buf_out[3] = 0; - buf_out[4] = IMU_UM6_ZERO_GYROS_CMD; - data_chk = UM6_calculate_checksum(buf_out, 5); - buf_out[5] = (uint8_t)(data_chk >> 8); - buf_out[6] = (uint8_t)data_chk; - UM6_send_packet(buf_out, 7); + // Zero gyros 0xAC, takes around 3s + buf_out[0] = 's'; + buf_out[1] = 'n'; + buf_out[2] = 'p'; + buf_out[3] = 0; + buf_out[4] = IMU_UM6_ZERO_GYROS_CMD; + data_chk = UM6_calculate_checksum(buf_out, 5); + buf_out[5] = (uint8_t)(data_chk >> 8); + buf_out[6] = (uint8_t)data_chk; + UM6_send_packet(buf_out, 7); - // Reset EKF 0xAD - buf_out[0] = 's'; - buf_out[1] = 'n'; - buf_out[2] = 'p'; - buf_out[3] = 0; - buf_out[4] = IMU_UM6_RESET_EKF_CMD; - data_chk = UM6_calculate_checksum(buf_out, 5); - buf_out[5] = (uint8_t)(data_chk >> 8); - buf_out[6] = (uint8_t)data_chk; - UM6_send_packet(buf_out, 7); + // Reset EKF 0xAD + buf_out[0] = 's'; + buf_out[1] = 'n'; + buf_out[2] = 'p'; + buf_out[3] = 0; + buf_out[4] = IMU_UM6_RESET_EKF_CMD; + data_chk = UM6_calculate_checksum(buf_out, 5); + buf_out[5] = (uint8_t)(data_chk >> 8); + buf_out[6] = (uint8_t)data_chk; + UM6_send_packet(buf_out, 7); - UM6_status = UM6Running; + UM6_status = UM6Running; } -void imu_impl_init(void) { +void imu_impl_init(void) +{ // Initialize variables UM6_status = UM6Uninit; @@ -186,7 +191,8 @@ void imu_impl_init(void) { -void imu_periodic(void) { +void imu_periodic(void) +{ /* We would request for data here - optional //GET_DATA command 0xAE buf_out[0] = 's'; @@ -201,159 +207,159 @@ void imu_periodic(void) { */ } -void UM6_packet_read_message(void) { +void UM6_packet_read_message(void) +{ if ((UM6_status == UM6Running) && PacketLength > 11) { - switch (PacketAddr) { - case IMU_UM6_GYRO_PROC: - UM6_rate.p = - ((float)((int16_t) - ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+0]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+1])))*0.0610352; - UM6_rate.q = - ((float)((int16_t) - ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+2]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+3])))*0.0610352; - UM6_rate.r = - ((float)((int16_t) - ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+4]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+5])))*0.0610352; - RATES_SMUL(UM6_rate, UM6_rate, 0.0174532925); //Convert deg/sec to rad/sec - RATES_BFP_OF_REAL(imu.gyro, UM6_rate); - break; - case IMU_UM6_ACCEL_PROC: - UM6_accel.x = - ((float)((int16_t) - ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+0]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+1])))*0.000183105; - UM6_accel.y = - ((float)((int16_t) - ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+2]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+3])))*0.000183105; - UM6_accel.z = - ((float)((int16_t) - ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+4]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+5])))*0.000183105; - VECT3_SMUL(UM6_accel, UM6_accel, 9.80665); //Convert g into m/s2 - ACCELS_BFP_OF_REAL(imu.accel, UM6_accel); // float to int - break; - case IMU_UM6_MAG_PROC: - UM6_mag.x = - ((float)((int16_t) - ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+0]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+1])))*0.000305176; - UM6_mag.y = - ((float)((int16_t) - ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+2]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+3])))*0.000305176; - UM6_mag.z = - ((float)((int16_t) - ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+4]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+5])))*0.000305176; - // Assume the same units for magnetic field - // Magnitude at the Earth's surface ranges from 25 to 65 microteslas (0.25 to 0.65 gauss). - MAGS_BFP_OF_REAL(imu.mag, UM6_mag); - break; - case IMU_UM6_EULER: - UM6_eulers.phi = - ((float)((int16_t) - ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+0]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+1])))*0.0109863; - UM6_eulers.theta = - ((float)((int16_t) - ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+2]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+3])))*0.0109863; - UM6_eulers.psi = - ((float)((int16_t) - ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+4]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+5])))*0.0109863; - EULERS_SMUL(UM6_eulers,UM6_eulers, 0.0174532925); //Convert deg to rad - EULERS_BFP_OF_REAL(ahrs_impl.ltp_to_imu_euler, UM6_eulers); - break; - case IMU_UM6_QUAT: - UM6_quat.qi = - ((float)((int16_t) - ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+0]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+1])))*0.0000335693; - UM6_quat.qx = - ((float)((int16_t) - ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+2]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+3])))*0.0000335693; - UM6_quat.qy = - ((float)((int16_t) - ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+4]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+5])))*0.0000335693; - UM6_quat.qz = - ((float)((int16_t) - ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+6]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+7])))*0.0000335693; - QUAT_BFP_OF_REAL(ahrs_impl.ltp_to_imu_quat, UM6_quat); - break; - default: - break; - } + switch (PacketAddr) { + case IMU_UM6_GYRO_PROC: + UM6_rate.p = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.0610352; + UM6_rate.q = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.0610352; + UM6_rate.r = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.0610352; + RATES_SMUL(UM6_rate, UM6_rate, 0.0174532925); //Convert deg/sec to rad/sec + RATES_BFP_OF_REAL(imu.gyro, UM6_rate); + break; + case IMU_UM6_ACCEL_PROC: + UM6_accel.x = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.000183105; + UM6_accel.y = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.000183105; + UM6_accel.z = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.000183105; + VECT3_SMUL(UM6_accel, UM6_accel, 9.80665); //Convert g into m/s2 + ACCELS_BFP_OF_REAL(imu.accel, UM6_accel); // float to int + break; + case IMU_UM6_MAG_PROC: + UM6_mag.x = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.000305176; + UM6_mag.y = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.000305176; + UM6_mag.z = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.000305176; + // Assume the same units for magnetic field + // Magnitude at the Earth's surface ranges from 25 to 65 microteslas (0.25 to 0.65 gauss). + MAGS_BFP_OF_REAL(imu.mag, UM6_mag); + break; + case IMU_UM6_EULER: + UM6_eulers.phi = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.0109863; + UM6_eulers.theta = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.0109863; + UM6_eulers.psi = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.0109863; + EULERS_SMUL(UM6_eulers, UM6_eulers, 0.0174532925); //Convert deg to rad + EULERS_BFP_OF_REAL(ahrs_impl.ltp_to_imu_euler, UM6_eulers); + break; + case IMU_UM6_QUAT: + UM6_quat.qi = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.0000335693; + UM6_quat.qx = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.0000335693; + UM6_quat.qy = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.0000335693; + UM6_quat.qz = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 6] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 7]))) * 0.0000335693; + QUAT_BFP_OF_REAL(ahrs_impl.ltp_to_imu_quat, UM6_quat); + break; + default: + break; + } } } /* UM6 Packet Collection */ -void UM6_packet_parse( uint8_t c ) { - switch (UM6_packet.status) { - case UM6PacketWaiting: - UM6_packet.msg_idx = 0; - if (c == 's') { - UM6_packet.status = UM6PacketReadingS; - UM6_packet.msg_buf[UM6_packet.msg_idx] = c; - UM6_packet.msg_idx++; - } else { - UM6_packet.hdr_error++; - UM6_packet.status = UM6PacketWaiting; - } - break; - case UM6PacketReadingS: - if (c == 'n') { - UM6_packet.status = UM6PacketReadingN; - UM6_packet.msg_buf[UM6_packet.msg_idx] = c; - UM6_packet.msg_idx++; - } else { - UM6_packet.hdr_error++; - UM6_packet.status = UM6PacketWaiting; - } - break; - case UM6PacketReadingN: - if (c == 'p') { - UM6_packet.status = UM6PacketReadingPT; - UM6_packet.msg_buf[UM6_packet.msg_idx] = c; - UM6_packet.msg_idx++; - } else { - UM6_packet.hdr_error++; - UM6_packet.status = UM6PacketWaiting; - } - break; - case UM6PacketReadingPT: - PacketType = c; - UM6_packet.msg_buf[UM6_packet.msg_idx] = c; - UM6_packet.msg_idx++; - UM6_packet.status = UM6PacketReadingAddr; - if ((PacketType & 0xC0) == 0xC0) { - PacketLength = 4*((PacketType >>2) & 0xF) + 7; // Batch, has 4*BatchLength bytes of data - } - else if ((PacketType & 0xC0) == 0x80) { - PacketLength = 11; // Not batch, has 4 bytes of data - } - else if ((PacketType & 0xC0) == 0x00) { - PacketLength = 7; // Not batch, no data - } - break; - case UM6PacketReadingAddr: - PacketAddr = c; - UM6_packet.msg_buf[UM6_packet.msg_idx] = c; - UM6_packet.msg_idx++; - UM6_packet.status = UM6PacketReadingData; - break; - case UM6PacketReadingData: - UM6_packet.msg_buf[UM6_packet.msg_idx] = c; - UM6_packet.msg_idx++; - if (UM6_packet.msg_idx == PacketLength) { - if (UM6_verify_chk(UM6_packet.msg_buf, PacketLength)) { - UM6_packet.msg_available = TRUE; +void UM6_packet_parse(uint8_t c) +{ + switch (UM6_packet.status) { + case UM6PacketWaiting: + UM6_packet.msg_idx = 0; + if (c == 's') { + UM6_packet.status = UM6PacketReadingS; + UM6_packet.msg_buf[UM6_packet.msg_idx] = c; + UM6_packet.msg_idx++; } else { - UM6_packet.msg_available = FALSE; - UM6_packet.chksm_error++; + UM6_packet.hdr_error++; + UM6_packet.status = UM6PacketWaiting; } - UM6_packet.status = UM6PacketWaiting; - } - break; - default: - UM6_packet.status = UM6PacketWaiting; - UM6_packet.msg_idx = 0; - break; + break; + case UM6PacketReadingS: + if (c == 'n') { + UM6_packet.status = UM6PacketReadingN; + UM6_packet.msg_buf[UM6_packet.msg_idx] = c; + UM6_packet.msg_idx++; + } else { + UM6_packet.hdr_error++; + UM6_packet.status = UM6PacketWaiting; + } + break; + case UM6PacketReadingN: + if (c == 'p') { + UM6_packet.status = UM6PacketReadingPT; + UM6_packet.msg_buf[UM6_packet.msg_idx] = c; + UM6_packet.msg_idx++; + } else { + UM6_packet.hdr_error++; + UM6_packet.status = UM6PacketWaiting; + } + break; + case UM6PacketReadingPT: + PacketType = c; + UM6_packet.msg_buf[UM6_packet.msg_idx] = c; + UM6_packet.msg_idx++; + UM6_packet.status = UM6PacketReadingAddr; + if ((PacketType & 0xC0) == 0xC0) { + PacketLength = 4 * ((PacketType >> 2) & 0xF) + 7; // Batch, has 4*BatchLength bytes of data + } else if ((PacketType & 0xC0) == 0x80) { + PacketLength = 11; // Not batch, has 4 bytes of data + } else if ((PacketType & 0xC0) == 0x00) { + PacketLength = 7; // Not batch, no data + } + break; + case UM6PacketReadingAddr: + PacketAddr = c; + UM6_packet.msg_buf[UM6_packet.msg_idx] = c; + UM6_packet.msg_idx++; + UM6_packet.status = UM6PacketReadingData; + break; + case UM6PacketReadingData: + UM6_packet.msg_buf[UM6_packet.msg_idx] = c; + UM6_packet.msg_idx++; + if (UM6_packet.msg_idx == PacketLength) { + if (UM6_verify_chk(UM6_packet.msg_buf, PacketLength)) { + UM6_packet.msg_available = TRUE; + } else { + UM6_packet.msg_available = FALSE; + UM6_packet.chksm_error++; + } + UM6_packet.status = UM6PacketWaiting; + } + break; + default: + UM6_packet.status = UM6PacketWaiting; + UM6_packet.msg_idx = 0; + break; } } /* no scaling */ -void imu_scale_gyro(struct Imu* _imu __attribute__((unused))) {} -void imu_scale_accel(struct Imu* _imu __attribute__((unused))) {} -void imu_scale_mag(struct Imu* _imu __attribute__((unused))) {} +void imu_scale_gyro(struct Imu *_imu __attribute__((unused))) {} +void imu_scale_accel(struct Imu *_imu __attribute__((unused))) {} +void imu_scale_mag(struct Imu *_imu __attribute__((unused))) {} diff --git a/sw/airborne/subsystems/imu/imu_um6.h b/sw/airborne/subsystems/imu/imu_um6.h index e8fdecb492..a3bf023525 100644 --- a/sw/airborne/subsystems/imu/imu_um6.h +++ b/sw/airborne/subsystems/imu/imu_um6.h @@ -19,17 +19,17 @@ * the Free Software Foundation, 59 Temple Place - Suite 330, * Boston, MA 02111-1307, USA. */ - /** - * @file imu_um6.h - * - * Driver for CH Robotics UM6 IMU/AHRS subsystem - * - * Takes care of configuration of the IMU, communication and parsing - * the received packets. See UM6 datasheet for configuration options. - * Should be used with ahrs_extern_euler AHRS subsystem. - * - * @author Michal Podhradsky - */ +/** +* @file imu_um6.h +* +* Driver for CH Robotics UM6 IMU/AHRS subsystem +* +* Takes care of configuration of the IMU, communication and parsing +* the received packets. See UM6 datasheet for configuration options. +* Should be used with ahrs_extern_euler AHRS subsystem. +* +* @author Michal Podhradsky +*/ #ifndef IMU_UM6_H #define IMU_UM6_H @@ -123,8 +123,8 @@ enum UM6Status { } #define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \ - imu_um6_event(_gyro_handler, _accel_handler, _mag_handler); \ -} + imu_um6_event(_gyro_handler, _accel_handler, _mag_handler); \ + } #endif /* IMU_UM6_H*/ diff --git a/sw/airborne/subsystems/ins.c b/sw/airborne/subsystems/ins.c index 6a52974f87..e1b6913dac 100644 --- a/sw/airborne/subsystems/ins.c +++ b/sw/airborne/subsystems/ins.c @@ -45,11 +45,12 @@ void WEAK ins_reset_local_origin(void) {} void WEAK ins_reset_altitude_ref(void) {} #if USE_GPS -void WEAK ins_reset_utm_zone(struct UtmCoor_f * utm) { +void WEAK ins_reset_utm_zone(struct UtmCoor_f *utm) +{ struct LlaCoor_f lla0; lla_of_utm_f(&lla0, utm); #ifdef GPS_USE_LATLONG - utm->zone = (gps.lla_pos.lon/1e7 + 180) / 6 + 1; + utm->zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1; #else utm->zone = gps.utm_pos.zone; #endif @@ -58,7 +59,7 @@ void WEAK ins_reset_utm_zone(struct UtmCoor_f * utm) { stateSetLocalUtmOrigin_f(utm); } #else -void WEAK ins_reset_utm_zone(struct UtmCoor_f * utm __attribute__((unused))) {} +void WEAK ins_reset_utm_zone(struct UtmCoor_f *utm __attribute__((unused))) {} #endif void WEAK ins_propagate(float dt __attribute__((unused))) {} diff --git a/sw/airborne/subsystems/ins.h b/sw/airborne/subsystems/ins.h index b50b94a2dd..771cfd4a91 100644 --- a/sw/airborne/subsystems/ins.h +++ b/sw/airborne/subsystems/ins.h @@ -33,8 +33,8 @@ #include "state.h" enum InsStatus { - INS_UNINIT=0, - INS_RUNNING=1 + INS_UNINIT = 0, + INS_RUNNING = 1 }; /* underlying includes (needed for parameters) */ @@ -78,7 +78,7 @@ extern void ins_reset_altitude_ref(void); * Can be overwritte by specifc INS implementation. * @param utm initial utm zone, returns the corrected utm position */ -extern void ins_reset_utm_zone(struct UtmCoor_f * utm); +extern void ins_reset_utm_zone(struct UtmCoor_f *utm); /** Propagation. Usually integrates the gyro rates to angles. * Reads the global #imu data struct. diff --git a/sw/airborne/subsystems/ins/hf_float.c b/sw/airborne/subsystems/ins/hf_float.c index 0d185eb92e..7caf3ece17 100644 --- a/sw/airborne/subsystems/ins/hf_float.c +++ b/sw/airborne/subsystems/ins/hf_float.c @@ -38,9 +38,9 @@ #ifdef SITL #include #define DBG_LEVEL 1 -#define PRINT_DBG(_l, _p) { \ - if (DBG_LEVEL >= _l) \ - printf _p; \ +#define PRINT_DBG(_l, _p) { \ + if (DBG_LEVEL >= _l) \ + printf _p; \ } #else #define PRINT_DBG(_l, _p) {} @@ -162,7 +162,7 @@ static int b2_hff_ps_counter; /* variables for mean accel buffer */ #define ACC_BUF_MAXN (GPS_LAG_N+10) -#define INC_ACC_IDX(idx) { idx = (idx + 1) < ACC_BUF_MAXN ? (idx + 1) : 0; } +#define INC_ACC_IDX(idx) { idx = (idx + 1) < ACC_BUF_MAXN ? (idx + 1) : 0; } /** buffer with past mean accel values for redoing the propagation */ struct FloatVect2 past_accel[ACC_BUF_MAXN]; @@ -176,11 +176,11 @@ static unsigned int acc_buf_n; ///< number of elements in buffer * stuff for ringbuffer to store past filter states */ #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++; \ +#define INC_RB_POINTER(ptr) { \ + if (ptr == &b2_hff_rb[HFF_RB_MAXN-1]) \ + ptr = b2_hff_rb; \ + else \ + ptr++; \ } /** ringbuffer with state and covariance when GPS was valid */ @@ -208,28 +208,29 @@ static uint16_t b2_hff_lost_counter; #ifdef GPS_LAG static void b2_hff_get_past_accel(unsigned int back_n); -static void b2_hff_rb_put_state(struct HfilterFloat* source); +static void b2_hff_rb_put_state(struct HfilterFloat *source); static void b2_hff_rb_drop_last(void); -static void b2_hff_set_state(struct HfilterFloat* dest, struct HfilterFloat* source); +static void b2_hff_set_state(struct HfilterFloat *dest, struct HfilterFloat *source); #endif static void b2_hff_init_x(float init_x, float init_xdot); static void b2_hff_init_y(float init_y, float init_ydot); -static void b2_hff_propagate_x(struct HfilterFloat* hff_work, float dt); -static void b2_hff_propagate_y(struct HfilterFloat* hff_work, float dt); +static void b2_hff_propagate_x(struct HfilterFloat *hff_work, float dt); +static void b2_hff_propagate_y(struct HfilterFloat *hff_work, float dt); -static void b2_hff_update_x(struct HfilterFloat* hff_work, float x_meas, float Rpos); -static void b2_hff_update_y(struct HfilterFloat* hff_work, float y_meas, float Rpos); +static void b2_hff_update_x(struct HfilterFloat *hff_work, float x_meas, float Rpos); +static void b2_hff_update_y(struct HfilterFloat *hff_work, float y_meas, float Rpos); -static void b2_hff_update_xdot(struct HfilterFloat* hff_work, float vel, float Rvel); -static void b2_hff_update_ydot(struct HfilterFloat* hff_work, float vel, float Rvel); +static void b2_hff_update_xdot(struct HfilterFloat *hff_work, float vel, float Rvel); +static void b2_hff_update_ydot(struct HfilterFloat *hff_work, float vel, float Rvel); #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_hff(struct transport_tx *trans, struct link_device *dev) { +static void send_hff(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_HFF(trans, dev, AC_ID, &b2_hff_state.x, &b2_hff_state.y, @@ -239,7 +240,8 @@ static void send_hff(struct transport_tx *trans, struct link_device *dev) { &b2_hff_state.ydotdot); } -static void send_hff_debug(struct transport_tx *trans, struct link_device *dev) { +static void send_hff_debug(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_HFF_DBG(trans, dev, AC_ID, &b2_hff_x_meas, &b2_hff_y_meas, @@ -252,7 +254,8 @@ static void send_hff_debug(struct transport_tx *trans, struct link_device *dev) } #ifdef GPS_LAG -static void send_hff_gps(struct transport_tx *trans, struct link_device *dev) { +static void send_hff_gps(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_HFF_GPS(trans, dev, AC_ID, &(b2_hff_rb_last->lag_counter), &lag_counter_err, @@ -262,7 +265,8 @@ static void send_hff_gps(struct transport_tx *trans, struct link_device *dev) { #endif -void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot) { +void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot) +{ Rgps_pos = HFF_R_POS; Rgps_vel = HFF_R_SPEED; b2_hff_init_x(init_x, init_xdot); @@ -305,35 +309,40 @@ void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot) { #endif #endif - init_butterworth_2_low_pass_int(&filter_x, HFF_LOWPASS_CUTOFF_FREQUENCY, (1. /AHRS_PROPAGATE_FREQUENCY), 0); - init_butterworth_2_low_pass_int(&filter_y, HFF_LOWPASS_CUTOFF_FREQUENCY, (1. /AHRS_PROPAGATE_FREQUENCY), 0); - init_butterworth_2_low_pass_int(&filter_z, HFF_LOWPASS_CUTOFF_FREQUENCY, (1. /AHRS_PROPAGATE_FREQUENCY), 0); + init_butterworth_2_low_pass_int(&filter_x, HFF_LOWPASS_CUTOFF_FREQUENCY, (1. / AHRS_PROPAGATE_FREQUENCY), 0); + init_butterworth_2_low_pass_int(&filter_y, HFF_LOWPASS_CUTOFF_FREQUENCY, (1. / AHRS_PROPAGATE_FREQUENCY), 0); + init_butterworth_2_low_pass_int(&filter_z, HFF_LOWPASS_CUTOFF_FREQUENCY, (1. / AHRS_PROPAGATE_FREQUENCY), 0); } -static void b2_hff_init_x(float init_x, float init_xdot) { +static void b2_hff_init_x(float init_x, float init_xdot) +{ b2_hff_state.x = init_x; b2_hff_state.xdot = init_xdot; int i, j; - 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)); @@ -355,16 +365,19 @@ static void b2_hff_get_past_accel(unsigned int back_n) { PRINT_DBG(1, ("Cannot go back zero steps!\n")); return; } - if ((int)(acc_buf_w - back_n) < 0) + if ((int)(acc_buf_w - back_n) < 0) { i = acc_buf_w - back_n + ACC_BUF_MAXN; - else + } else { 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)); + 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)); } -static void b2_hff_rb_put_state(struct HfilterFloat* source) { +static void b2_hff_rb_put_state(struct HfilterFloat *source) +{ /* copy state from source into buffer */ b2_hff_set_state(b2_hff_rb_put, source); b2_hff_rb_put->lag_counter = 0; @@ -382,7 +395,8 @@ static void b2_hff_rb_put_state(struct HfilterFloat* source) { PRINT_DBG(2, ("put state. fill count now: %d\n", b2_hff_rb_n)); } -static void b2_hff_rb_drop_last(void) { +static void b2_hff_rb_drop_last(void) +{ if (b2_hff_rb_n > 0) { INC_RB_POINTER(b2_hff_rb_last); b2_hff_rb_n--; @@ -395,25 +409,27 @@ static void b2_hff_rb_drop_last(void) { } /* copy source state to dest state */ -static void b2_hff_set_state(struct HfilterFloat* dest, struct HfilterFloat* source) { +static void b2_hff_set_state(struct HfilterFloat *dest, struct HfilterFloat *source) +{ dest->x = source->x; dest->xdot = source->xdot; dest->xdotdot = source->xdotdot; dest->y = source->y; 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++) { + 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]; } } } -static void b2_hff_propagate_past(struct HfilterFloat* hff_past) { +static 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++) { + 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)); @@ -431,10 +447,11 @@ static void b2_hff_propagate_past(struct HfilterFloat* 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]) + if (hff_past == &b2_hff_rb[HFF_RB_MAXN - 1]) { b2_hff_rb[0].lag_counter++; - else - (hff_past+1)->lag_counter++; + } else { + (hff_past + 1)->lag_counter++; + } } } @@ -450,9 +467,11 @@ static void b2_hff_propagate_past(struct HfilterFloat* hff_past) { #endif /* GPS_LAG */ -void b2_hff_propagate(void) { - if (b2_hff_lost_counter < b2_hff_lost_limit) +void b2_hff_propagate(void) +{ + if (b2_hff_lost_counter < b2_hff_lost_limit) { b2_hff_lost_counter++; + } #ifdef GPS_LAG /* continue re-propagating to catch up with the present */ @@ -476,7 +495,7 @@ void b2_hff_propagate(void) { b2_hff_ps_counter = 1; if (b2_hff_lost_counter < b2_hff_lost_limit) { struct Int32Vect3 filtered_accel_ltp; - struct Int32RMat* ltp_to_body_rmat = stateGetNedToBodyRMat_i(); + struct Int32RMat *ltp_to_body_rmat = stateGetNedToBodyRMat_i(); int32_rmat_transp_vmult(&filtered_accel_ltp, ltp_to_body_rmat, &acc_body_filtered); b2_hff_xdd_meas = ACCEL_FLOAT_OF_BFP(filtered_accel_ltp.x); b2_hff_ydd_meas = ACCEL_FLOAT_OF_BFP(filtered_accel_ltp.y); @@ -491,8 +510,9 @@ void b2_hff_propagate(void) { #ifdef GPS_LAG /* increase lag counter on last saved state */ - if (b2_hff_rb_n > 0) + if (b2_hff_rb_n > 0) { b2_hff_rb_last->lag_counter++; + } /* save filter state if needed */ if (save_counter == 0) { @@ -509,17 +529,20 @@ void b2_hff_propagate(void) { } } -void b2_hff_update_gps(struct FloatVect2* pos_ned, struct FloatVect2* speed_ned) { +void b2_hff_update_gps(struct FloatVect2 *pos_ned, struct FloatVect2 *speed_ned) +{ b2_hff_lost_counter = 0; #if USE_GPS_ACC4R Rgps_pos = (float) gps.pacc / 100.; - if (Rgps_pos < HFF_R_POS_MIN) + if (Rgps_pos < HFF_R_POS_MIN) { Rgps_pos = HFF_R_POS_MIN; + } Rgps_vel = (float) gps.sacc / 100.; - if (Rgps_vel < HFF_R_SPEED_MIN) + if (Rgps_vel < HFF_R_SPEED_MIN) { Rgps_vel = HFF_R_SPEED_MIN; + } #endif #ifdef GPS_LAG @@ -539,7 +562,8 @@ void b2_hff_update_gps(struct FloatVect2* pos_ned, struct FloatVect2* speed_ned) } else if (b2_hff_rb_n > 0) { /* roll back if state was saved approx when GPS was valid */ lag_counter_err = b2_hff_rb_last->lag_counter - GPS_LAG_N; - PRINT_DBG(2, ("update. rb_n: %d lag_counter: %d lag_cnt_err: %d\n", b2_hff_rb_n, b2_hff_rb_last->lag_counter, lag_counter_err)); + PRINT_DBG(2, ("update. rb_n: %d lag_counter: %d lag_cnt_err: %d\n", b2_hff_rb_n, b2_hff_rb_last->lag_counter, + lag_counter_err)); if (abs(lag_counter_err) <= GPS_LAG_TOL_N) { b2_hff_rb_last->rollback = TRUE; b2_hff_update_x(b2_hff_rb_last, pos_ned->x, Rgps_pos); @@ -548,10 +572,10 @@ void b2_hff_update_gps(struct FloatVect2* pos_ned, struct FloatVect2* speed_ned) b2_hff_update_xdot(b2_hff_rb_last, speed_ned->x, Rgps_vel); b2_hff_update_ydot(b2_hff_rb_last, speed_ned->y, Rgps_vel); #endif - past_save_counter = GPS_DT_N-1;// + lag_counter_err; + past_save_counter = GPS_DT_N - 1; // + lag_counter_err; PRINT_DBG(2, ("gps updated. past_save_counter: %d\n", past_save_counter)); b2_hff_propagate_past(b2_hff_rb_last); - } else if (lag_counter_err >= GPS_DT_N - (GPS_LAG_TOL_N+1)) { + } else if (lag_counter_err >= GPS_DT_N - (GPS_LAG_TOL_N + 1)) { /* apparently missed a GPS update, try next saved state */ PRINT_DBG(2, ("try next saved state\n")); b2_hff_rb_drop_last(); @@ -559,7 +583,7 @@ void b2_hff_update_gps(struct FloatVect2* pos_ned, struct FloatVect2* speed_ned) } } else if (save_counter < 0) { /* ringbuffer empty -> save output filter state at next GPS validity point in time */ - save_counter = GPS_DT_N-1 - (GPS_LAG_N % GPS_DT_N); + save_counter = GPS_DT_N - 1 - (GPS_LAG_N % GPS_DT_N); PRINT_DBG(2, ("rb empty, save counter set: %d\n", save_counter)); } @@ -567,7 +591,8 @@ void b2_hff_update_gps(struct FloatVect2* pos_ned, struct FloatVect2* speed_ned) } -void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 vel) { +void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 vel) +{ b2_hff_state.x = pos.x; b2_hff_state.y = pos.y; b2_hff_state.xdot = vel.x; @@ -601,13 +626,14 @@ void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 vel) { Pk1 = F * Pk0 * F' + Q; */ -static void b2_hff_propagate_x(struct HfilterFloat* hff_work, float dt) { +static void b2_hff_propagate_x(struct HfilterFloat *hff_work, float dt) +{ /* update state */ hff_work->xdotdot = b2_hff_xdd_meas; - hff_work->x = hff_work->x + dt * hff_work->xdot + dt*dt/2 * hff_work->xdotdot; + hff_work->x = hff_work->x + dt * hff_work->xdot + dt * dt / 2 * hff_work->xdotdot; hff_work->xdot = hff_work->xdot + dt * hff_work->xdotdot; /* update covariance */ - const float FPF00 = hff_work->xP[0][0] + dt * ( hff_work->xP[1][0] + hff_work->xP[0][1] + dt * hff_work->xP[1][1] ); + const float FPF00 = hff_work->xP[0][0] + dt * (hff_work->xP[1][0] + hff_work->xP[0][1] + dt * hff_work->xP[1][1]); const float FPF01 = hff_work->xP[0][1] + dt * hff_work->xP[1][1]; const float FPF10 = hff_work->xP[1][0] + dt * hff_work->xP[1][1]; const float FPF11 = hff_work->xP[1][1]; @@ -618,13 +644,14 @@ static void b2_hff_propagate_x(struct HfilterFloat* hff_work, float dt) { hff_work->xP[1][1] = FPF11 + Qdotdot; } -static void b2_hff_propagate_y(struct HfilterFloat* hff_work, float dt) { +static void b2_hff_propagate_y(struct HfilterFloat *hff_work, float dt) +{ /* update state */ hff_work->ydotdot = b2_hff_ydd_meas; - hff_work->y = hff_work->y + dt * hff_work->ydot + dt*dt/2 * hff_work->ydotdot; + hff_work->y = hff_work->y + dt * hff_work->ydot + dt * dt / 2 * hff_work->ydotdot; hff_work->ydot = hff_work->ydot + dt * hff_work->ydotdot; /* update covariance */ - const float FPF00 = hff_work->yP[0][0] + dt * ( hff_work->yP[1][0] + hff_work->yP[0][1] + dt * hff_work->yP[1][1] ); + const float FPF00 = hff_work->yP[0][0] + dt * (hff_work->yP[1][0] + hff_work->yP[0][1] + dt * hff_work->yP[1][1]); const float FPF01 = hff_work->yP[0][1] + dt * hff_work->yP[1][1]; const float FPF10 = hff_work->yP[1][0] + dt * hff_work->yP[1][1]; const float FPF11 = hff_work->yP[1][1]; @@ -655,18 +682,20 @@ static void b2_hff_propagate_y(struct HfilterFloat* hff_work, float dt) { // update covariance Pp = Pm - K*H*Pm; */ -void b2_hff_update_pos (struct FloatVect2 pos, struct FloatVect2 Rpos) { +void b2_hff_update_pos(struct FloatVect2 pos, struct FloatVect2 Rpos) +{ b2_hff_update_x(&b2_hff_state, pos.x, Rpos.x); b2_hff_update_y(&b2_hff_state, pos.y, Rpos.y); } -static void b2_hff_update_x(struct HfilterFloat* hff_work, float x_meas, float Rpos) { +static void b2_hff_update_x(struct HfilterFloat *hff_work, float x_meas, float Rpos) +{ b2_hff_x_meas = x_meas; const float y = x_meas - hff_work->x; const float S = hff_work->xP[0][0] + Rpos; - const float K1 = hff_work->xP[0][0] * 1/S; - const float K2 = hff_work->xP[1][0] * 1/S; + const float K1 = hff_work->xP[0][0] * 1 / S; + const float K2 = hff_work->xP[1][0] * 1 / S; hff_work->x = hff_work->x + K1 * y; hff_work->xdot = hff_work->xdot + K2 * y; @@ -682,13 +711,14 @@ static void b2_hff_update_x(struct HfilterFloat* hff_work, float x_meas, float R hff_work->xP[1][1] = P22; } -static void b2_hff_update_y(struct HfilterFloat* hff_work, float y_meas, float Rpos) { +static void b2_hff_update_y(struct HfilterFloat *hff_work, float y_meas, float Rpos) +{ b2_hff_y_meas = y_meas; const float y = y_meas - hff_work->y; const float S = hff_work->yP[0][0] + Rpos; - const float K1 = hff_work->yP[0][0] * 1/S; - const float K2 = hff_work->yP[1][0] * 1/S; + const float K1 = hff_work->yP[0][0] * 1 / S; + const float K2 = hff_work->yP[1][0] * 1 / S; hff_work->y = hff_work->y + K1 * y; hff_work->ydot = hff_work->ydot + K2 * y; @@ -724,18 +754,20 @@ static void b2_hff_update_y(struct HfilterFloat* hff_work, float y_meas, float R // update covariance Pp = Pm - K*H*Pm; */ -void b2_hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel) { +void b2_hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel) +{ b2_hff_update_xdot(&b2_hff_state, vel.x, Rvel.x); b2_hff_update_ydot(&b2_hff_state, vel.y, Rvel.y); } -static void b2_hff_update_xdot(struct HfilterFloat* hff_work, float vel, float Rvel) { +static void b2_hff_update_xdot(struct HfilterFloat *hff_work, float vel, float Rvel) +{ b2_hff_xd_meas = vel; const float yd = vel - hff_work->xdot; const float S = hff_work->xP[1][1] + Rvel; - const float K1 = hff_work->xP[0][1] * 1/S; - const float K2 = hff_work->xP[1][1] * 1/S; + const float K1 = hff_work->xP[0][1] * 1 / S; + const float K2 = hff_work->xP[1][1] * 1 / S; hff_work->x = hff_work->x + K1 * yd; hff_work->xdot = hff_work->xdot + K2 * yd; @@ -751,13 +783,14 @@ static void b2_hff_update_xdot(struct HfilterFloat* hff_work, float vel, float R hff_work->xP[1][1] = P22; } -static void b2_hff_update_ydot(struct HfilterFloat* hff_work, float vel, float Rvel) { +static void b2_hff_update_ydot(struct HfilterFloat *hff_work, float vel, float Rvel) +{ b2_hff_yd_meas = vel; const float yd = vel - hff_work->ydot; const float S = hff_work->yP[1][1] + Rvel; - const float K1 = hff_work->yP[0][1] * 1/S; - const float K2 = hff_work->yP[1][1] * 1/S; + const float K1 = hff_work->yP[0][1] * 1 / S; + const float K2 = hff_work->yP[1][1] * 1 / S; hff_work->y = hff_work->y + K1 * yd; hff_work->ydot = hff_work->ydot + K2 * yd; diff --git a/sw/airborne/subsystems/ins/hf_float.h b/sw/airborne/subsystems/ins/hf_float.h index 05eb642bc5..3d2d01091b 100644 --- a/sw/airborne/subsystems/ins/hf_float.h +++ b/sw/airborne/subsystems/ins/hf_float.h @@ -54,7 +54,7 @@ extern struct HfilterFloat b2_hff_state; extern void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot); extern void b2_hff_propagate(void); -extern void b2_hff_update_gps(struct FloatVect2* pos_ned, struct FloatVect2* speed_ned); +extern void b2_hff_update_gps(struct FloatVect2 *pos_ned, struct FloatVect2 *speed_ned); extern void b2_hff_update_pos(struct FloatVect2 pos, struct FloatVect2 Rpos); extern void b2_hff_update_vel(struct FloatVect2 vel, struct FloatVect2 Rvel); extern void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 vel); diff --git a/sw/airborne/subsystems/ins/ins_alt_float.c b/sw/airborne/subsystems/ins/ins_alt_float.c index 594e95604f..8166b5e0e8 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.c +++ b/sw/airborne/subsystems/ins/ins_alt_float.c @@ -74,7 +74,8 @@ static void alt_kalman_reset(void); static void alt_kalman_init(void); static void alt_kalman(float z_meas, float dt); -void ins_init(void) { +void ins_init(void) +{ struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, ground_alt, nav_utm_zone0 }; stateSetLocalUtmOrigin_f(&utm0); @@ -100,13 +101,14 @@ void ins_init(void) { /** Reset the geographic reference to the current GPS fix */ -void ins_reset_local_origin(void) { +void ins_reset_local_origin(void) +{ struct UtmCoor_f utm; #ifdef GPS_USE_LATLONG /* Recompute UTM coordinates in this zone */ struct LlaCoor_f lla; LLA_FLOAT_OF_BFP(lla, gps.lla_pos); - utm.zone = (gps.lla_pos.lon/1e7 + 180) / 6 + 1; + utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1; utm_of_lla_f(&utm, &lla); #else utm.zone = gps.utm_pos.zone; @@ -114,7 +116,7 @@ void ins_reset_local_origin(void) { utm.north = gps.utm_pos.north / 100.0f; #endif // ground_alt - utm.alt = gps.hmsl /1000.0f; + utm.alt = gps.hmsl / 1000.0f; // reset state UTM ref stateSetLocalUtmOrigin_f(&utm); @@ -123,7 +125,8 @@ void ins_reset_local_origin(void) { ins_impl.reset_alt_ref = TRUE; } -void ins_reset_altitude_ref(void) { +void ins_reset_altitude_ref(void) +{ struct UtmCoor_f utm = state.utm_origin_f; // ground_alt utm.alt = gps.hmsl / 1000.0f; @@ -135,7 +138,8 @@ void ins_reset_altitude_ref(void) { #if USE_BAROMETER -static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) { +static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) +{ // timestamp in usec when last callback was received static uint32_t last_ts = 0; // current timestamp @@ -157,8 +161,7 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pres ins_impl.alt_dot = 0.0f; ins_impl.qfe = *pressure; alt_kalman_reset(); - } - else { /* not realigning, so normal update with baro measurement */ + } else { /* not realigning, so normal update with baro measurement */ ins_impl.baro_alt = ground_alt + pprz_isa_height_of_pressure(*pressure, ins_impl.qfe); /* run the filter */ alt_kalman(ins_impl.baro_alt, dt); @@ -176,7 +179,8 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pres #endif -void ins_update_gps(void) { +void ins_update_gps(void) +{ #if USE_GPS struct UtmCoor_f utm; utm.east = gps.utm_pos.east / 100.0f; @@ -205,8 +209,7 @@ void ins_update_gps(void) { ins_impl.alt = falt; ins_impl.alt_dot = 0.0f; alt_kalman_reset(); - } - else { + } else { alt_kalman(falt, dt); ins_impl.alt_dot = -gps.ned_vel.z / 100.0f; } @@ -232,18 +235,21 @@ void ins_update_gps(void) { static float p[2][2]; -static void alt_kalman_reset(void) { +static void alt_kalman_reset(void) +{ p[0][0] = 1.0f; p[0][1] = 0.0f; p[1][0] = 0.0f; p[1][1] = 1.0f; } -static void alt_kalman_init(void) { +static void alt_kalman_init(void) +{ alt_kalman_reset(); } -static void alt_kalman(float z_meas, float dt) { +static void alt_kalman(float z_meas, float dt) +{ float R = GPS_R; float SIGMA2 = GPS_SIGMA2; @@ -280,18 +286,18 @@ static void alt_kalman(float z_meas, float dt) { #endif // USE_BAROMETER float q[2][2]; - q[0][0] = dt*dt*dt*dt/4.; - q[0][1] = dt*dt*dt/2.; - q[1][0] = dt*dt*dt/2.; - q[1][1] = dt*dt; + q[0][0] = dt * dt * dt * dt / 4.; + q[0][1] = dt * dt * dt / 2.; + q[1][0] = dt * dt * dt / 2.; + q[1][1] = dt * dt; /* predict */ ins_impl.alt += ins_impl.alt_dot * dt; - p[0][0] = p[0][0]+p[1][0]*dt+dt*(p[0][1]+p[1][1]*dt) + SIGMA2*q[0][0]; - p[0][1] = p[0][1]+p[1][1]*dt + SIGMA2*q[0][1]; - p[1][0] = p[1][0]+p[1][1]*dt + SIGMA2*q[1][0]; - p[1][1] = p[1][1] + SIGMA2*q[1][1]; + p[0][0] = p[0][0] + p[1][0] * dt + dt * (p[0][1] + p[1][1] * dt) + SIGMA2 * q[0][0]; + p[0][1] = p[0][1] + p[1][1] * dt + SIGMA2 * q[0][1]; + p[1][0] = p[1][0] + p[1][1] * dt + SIGMA2 * q[1][0]; + p[1][1] = p[1][1] + SIGMA2 * q[1][1]; /* error estimate */ float e = p[0][0] + R; @@ -305,14 +311,14 @@ static void alt_kalman(float z_meas, float dt) { ins_impl.alt += k_0 * e; ins_impl.alt_dot += k_1 * e; - p[1][0] = -p[0][0]*k_1+p[1][0]; - p[1][1] = -p[0][1]*k_1+p[1][1]; - p[0][0] = p[0][0] * (1-k_0); - p[0][1] = p[0][1] * (1-k_0); + p[1][0] = -p[0][0] * k_1 + p[1][0]; + p[1][1] = -p[0][1] * k_1 + p[1][1]; + p[0][0] = p[0][0] * (1 - k_0); + p[0][1] = p[0][1] * (1 - k_0); } #ifdef DEBUG_ALT_KALMAN - DOWNLINK_SEND_ALT_KALMAN(DefaultChannel,DefaultDevice,&(p[0][0]),&(p[0][1]),&(p[1][0]), &(p[1][1])); + DOWNLINK_SEND_ALT_KALMAN(DefaultChannel, DefaultDevice, &(p[0][0]), &(p[0][1]), &(p[1][0]), &(p[1][1])); #endif } diff --git a/sw/airborne/subsystems/ins/ins_ardrone2.c b/sw/airborne/subsystems/ins/ins_ardrone2.c index 893bdbff7f..052bb89c53 100644 --- a/sw/airborne/subsystems/ins/ins_ardrone2.c +++ b/sw/airborne/subsystems/ins/ins_ardrone2.c @@ -43,7 +43,8 @@ PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE") struct InsArdrone2 ins_impl; -void ins_init() { +void ins_init() +{ #if USE_INS_NAV_INIT struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */ llh_nav0.lat = NAV_LAT0; @@ -68,16 +69,20 @@ void ins_init() { INT32_VECT3_ZERO(ins_impl.ltp_accel); } -void ins_periodic( void ) { - if (ins_impl.ltp_initialized) +void ins_periodic(void) +{ + if (ins_impl.ltp_initialized) { ins.status = INS_RUNNING; + } } -void ins_reset_local_origin( void ) { +void ins_reset_local_origin(void) +{ ins_impl.ltp_initialized = FALSE; } -void ins_reset_altitude_ref( void ) { +void ins_reset_altitude_ref(void) +{ #if USE_GPS struct LlaCoor_i lla = { state.ned_origin_i.lla.lon, @@ -85,19 +90,20 @@ void ins_reset_altitude_ref( void ) { gps.lla_pos.alt }; ltp_def_from_lla_i(&ins_impl.ltp_def, &lla), - ins_impl.ltp_def.hmsl = gps.hmsl; + ins_impl.ltp_def.hmsl = gps.hmsl; stateSetLocalOrigin_i(&ins_impl.ltp_def); #endif } -void ins_propagate(float __attribute__((unused)) dt) { +void ins_propagate(float __attribute__((unused)) dt) +{ /* untilt accels and speeds */ - float_rmat_transp_vmult((struct FloatVect3*)&ins_impl.ltp_accel, + float_rmat_transp_vmult((struct FloatVect3 *)&ins_impl.ltp_accel, stateGetNedToBodyRMat_f(), - (struct FloatVect3*)&ahrs_impl.accel); - float_rmat_transp_vmult((struct FloatVect3*)&ins_impl.ltp_speed, + (struct FloatVect3 *)&ahrs_impl.accel); + float_rmat_transp_vmult((struct FloatVect3 *)&ins_impl.ltp_speed, stateGetNedToBodyRMat_f(), - (struct FloatVect3*)&ahrs_impl.speed); + (struct FloatVect3 *)&ahrs_impl.speed); //Add g to the accelerations ins_impl.ltp_accel.z += 9.81; @@ -115,12 +121,13 @@ void ins_propagate(float __attribute__((unused)) dt) { } -void ins_update_gps(void) { +void ins_update_gps(void) +{ #if USE_GPS //Check for GPS fix if (gps.fix == GPS_FIX_3D) { //Set the initial coordinates - if(!ins_impl.ltp_initialized) { + if (!ins_impl.ltp_initialized) { ltp_def_from_ecef_i(&ins_impl.ltp_def, &gps.ecef_pos); ins_impl.ltp_def.lla.alt = gps.lla_pos.alt; ins_impl.ltp_def.hmsl = gps.hmsl; diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.c b/sw/airborne/subsystems/ins/ins_float_invariant.c index 07e06165c8..c0dd45d78f 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant.c +++ b/sw/airborne/subsystems/ins/ins_float_invariant.c @@ -62,13 +62,14 @@ #if !INS_UPDATE_FW_ESTIMATOR && PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_ins_ref(struct transport_tx *trans, struct link_device *dev) { +static void send_ins_ref(struct transport_tx *trans, struct link_device *dev) +{ float foo = 0.; if (state.ned_initialized_i) { pprz_msg_send_INS_REF(trans, dev, AC_ID, - &state.ned_origin_i.ecef.x, &state.ned_origin_i.ecef.y, &state.ned_origin_i.ecef.z, - &state.ned_origin_i.lla.lat, &state.ned_origin_i.lla.lon, &state.ned_origin_i.lla.alt, - &state.ned_origin_i.hmsl, &foo); + &state.ned_origin_i.ecef.x, &state.ned_origin_i.ecef.y, &state.ned_origin_i.ecef.z, + &state.ned_origin_i.lla.lat, &state.ned_origin_i.lla.lon, &state.ned_origin_i.lla.alt, + &state.ned_origin_i.hmsl, &foo); } } #endif @@ -185,21 +186,22 @@ static void baro_cb(uint8_t sender_id, const float *pressure); bool_t ins_gps_fix_once; /* error computation */ -static inline void error_output(struct InsFloatInv * _ins); +static inline void error_output(struct InsFloatInv *_ins); /* propagation model (called by runge-kutta library) */ -static inline void invariant_model(float * o, const float * x, const int n, const float * u, const int m); +static inline void invariant_model(float *o, const float *x, const int n, const float *u, const int m); /** Right multiplication by a quaternion. * vi * q */ -void float_quat_vmul_right(struct FloatQuat* mright, const struct FloatQuat* q, - struct FloatVect3* vi); +void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q, + struct FloatVect3 *vi); /* init state and measurements */ -static inline void init_invariant_state(void) { +static inline void init_invariant_state(void) +{ // init state float_quat_identity(&ins_impl.state.quat); FLOAT_RATES_ZERO(ins_impl.state.bias); @@ -218,7 +220,8 @@ static inline void init_invariant_state(void) { ins_gps_fix_once = FALSE; } -void ins_init() { +void ins_init() +{ // init position #if INS_UPDATE_FW_ESTIMATOR @@ -277,14 +280,15 @@ void ins_init() { } -void ins_reset_local_origin( void ) { +void ins_reset_local_origin(void) +{ #if INS_UPDATE_FW_ESTIMATOR struct UtmCoor_f utm; #ifdef GPS_USE_LATLONG /* Recompute UTM coordinates in this zone */ struct LlaCoor_f lla; LLA_FLOAT_OF_BFP(lla, gps.lla_pos); - utm.zone = (gps.lla_pos.lon/1e7 + 180) / 6 + 1; + utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1; utm_of_lla_f(&utm, &lla); #else utm.zone = gps.utm_pos.zone; @@ -303,7 +307,8 @@ void ins_reset_local_origin( void ) { #endif } -void ins_reset_altitude_ref( void ) { +void ins_reset_altitude_ref(void) +{ #if INS_UPDATE_FW_ESTIMATOR struct UtmCoor_f utm = state.utm_origin_f; utm.alt = gps.hmsl / 1000.0f; @@ -316,12 +321,13 @@ void ins_reset_altitude_ref( void ) { }; struct LtpDef_i ltp_def; ltp_def_from_lla_i(<p_def, &lla), - ltp_def.hmsl = gps.hmsl; + ltp_def.hmsl = gps.hmsl; stateSetLocalOrigin_i(<p_def); #endif } -void ahrs_init(void) { +void ahrs_init(void) +{ ahrs.status = AHRS_UNINIT; } @@ -340,7 +346,8 @@ void ahrs_align(void) ins.status = INS_RUNNING; } -void ahrs_propagate(float dt) { +void ahrs_propagate(float dt) +{ struct FloatVect3 accel; struct FloatRates body_rates; @@ -367,10 +374,10 @@ void ahrs_propagate(float dt) { // propagate model struct inv_state new_state; - runge_kutta_4_float((float*)&new_state, - (float*)&ins_impl.state, INV_STATE_DIM, - (float*)&ins_impl.cmd, INV_COMMAND_DIM, - invariant_model, dt); + runge_kutta_4_float((float *)&new_state, + (float *)&ins_impl.state, INV_STATE_DIM, + (float *)&ins_impl.cmd, INV_COMMAND_DIM, + invariant_model, dt); ins_impl.state = new_state; // normalize quaternion @@ -388,81 +395,83 @@ void ahrs_propagate(float dt) { float_quat_vmult(&accel, &q_b2n, &ins_impl.cmd.accel); VECT3_SMUL(accel, accel, 1. / (ins_impl.state.as)); VECT3_ADD(accel, A); - stateSetAccelNed_f((struct NedCoor_f*)&accel); + stateSetAccelNed_f((struct NedCoor_f *)&accel); //------------------------------------------------------------// #if SEND_INVARIANT_FILTER struct FloatEulers eulers; FLOAT_EULERS_OF_QUAT(eulers, ins_impl.state.quat); - RunOnceEvery(3,{ - pprz_msg_send_INV_FILTER(trans, dev, AC_ID, - &ins_impl.state.quat.qi, - &eulers.phi, - &eulers.theta, - &eulers.psi, - &ins_impl.state.speed.x, - &ins_impl.state.speed.y, - &ins_impl.state.speed.z, - &ins_impl.state.pos.x, - &ins_impl.state.pos.y, - &ins_impl.state.pos.z, - &ins_impl.state.bias.p, - &ins_impl.state.bias.q, - &ins_impl.state.bias.r, - &ins_impl.state.as, - &ins_impl.state.hb, - &ins_impl.meas.baro_alt, - &ins_impl.meas.pos_gps.z) - }); + RunOnceEvery(3, { + pprz_msg_send_INV_FILTER(trans, dev, AC_ID, + &ins_impl.state.quat.qi, + &eulers.phi, + &eulers.theta, + &eulers.psi, + &ins_impl.state.speed.x, + &ins_impl.state.speed.y, + &ins_impl.state.speed.z, + &ins_impl.state.pos.x, + &ins_impl.state.pos.y, + &ins_impl.state.pos.z, + &ins_impl.state.bias.p, + &ins_impl.state.bias.q, + &ins_impl.state.bias.r, + &ins_impl.state.as, + &ins_impl.state.hb, + &ins_impl.meas.baro_alt, + &ins_impl.meas.pos_gps.z) + }); #endif #if LOG_INVARIANT_FILTER if (pprzLogFile.fs != NULL) { if (!log_started) { // log file header - sdLogWriteLog(&pprzLogFile, "p q r ax ay az gx gy gz gvx gvy gvz mx my mz b qi qx qy qz bp bq br vx vy vz px py pz hb as\n"); + sdLogWriteLog(&pprzLogFile, + "p q r ax ay az gx gy gz gvx gvy gvz mx my mz b qi qx qy qz bp bq br vx vy vz px py pz hb as\n"); log_started = TRUE; - } - else { - sdLogWriteLog(&pprzLogFile, "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n", - ins_impl.cmd.rates.p, - ins_impl.cmd.rates.q, - ins_impl.cmd.rates.r, - ins_impl.cmd.accel.x, - ins_impl.cmd.accel.y, - ins_impl.cmd.accel.z, - ins_impl.meas.pos_gps.x, - ins_impl.meas.pos_gps.y, - ins_impl.meas.pos_gps.z, - ins_impl.meas.speed_gps.x, - ins_impl.meas.speed_gps.y, - ins_impl.meas.speed_gps.z, - ins_impl.meas.mag.x, - ins_impl.meas.mag.y, - ins_impl.meas.mag.z, - ins_impl.meas.baro_alt, - ins_impl.state.quat.qi, - ins_impl.state.quat.qx, - ins_impl.state.quat.qy, - ins_impl.state.quat.qz, - ins_impl.state.bias.p, - ins_impl.state.bias.q, - ins_impl.state.bias.r, - ins_impl.state.speed.x, - ins_impl.state.speed.y, - ins_impl.state.speed.z, - ins_impl.state.pos.x, - ins_impl.state.pos.y, - ins_impl.state.pos.z, - ins_impl.state.hb, - ins_impl.state.as); + } else { + sdLogWriteLog(&pprzLogFile, + "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n", + ins_impl.cmd.rates.p, + ins_impl.cmd.rates.q, + ins_impl.cmd.rates.r, + ins_impl.cmd.accel.x, + ins_impl.cmd.accel.y, + ins_impl.cmd.accel.z, + ins_impl.meas.pos_gps.x, + ins_impl.meas.pos_gps.y, + ins_impl.meas.pos_gps.z, + ins_impl.meas.speed_gps.x, + ins_impl.meas.speed_gps.y, + ins_impl.meas.speed_gps.z, + ins_impl.meas.mag.x, + ins_impl.meas.mag.y, + ins_impl.meas.mag.z, + ins_impl.meas.baro_alt, + ins_impl.state.quat.qi, + ins_impl.state.quat.qx, + ins_impl.state.quat.qy, + ins_impl.state.quat.qz, + ins_impl.state.bias.p, + ins_impl.state.bias.q, + ins_impl.state.bias.r, + ins_impl.state.speed.x, + ins_impl.state.speed.y, + ins_impl.state.speed.z, + ins_impl.state.pos.x, + ins_impl.state.pos.y, + ins_impl.state.pos.z, + ins_impl.state.hb, + ins_impl.state.as); } } #endif } -void ahrs_update_gps(void) { +void ahrs_update_gps(void) +{ if (gps.fix == GPS_FIX_3D && ins.status == INS_RUNNING) { ins_gps_fix_once = TRUE; @@ -492,7 +501,8 @@ void ahrs_update_gps(void) { } -static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) { +static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) +{ static float ins_qfe = 101325.0f; static float alpha = 10.0f; static int32_t i = 1; @@ -506,8 +516,8 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pres baro_moy = *pressure; baro_prev = *pressure; } - baro_moy = (baro_moy*(i-1) + *pressure) / i; - alpha = (10.*alpha + (baro_moy-baro_prev)) / (11.0f); + baro_moy = (baro_moy * (i - 1) + *pressure) / i; + alpha = (10.*alpha + (baro_moy - baro_prev)) / (11.0f); baro_prev = baro_moy; // test stop condition if (fabs(alpha) < 0.005f) { @@ -519,19 +529,20 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pres ins_baro_initialized = TRUE; } i++; - } - else { /* normal update with baro measurement */ + } else { /* normal update with baro measurement */ ins_impl.meas.baro_alt = -pprz_isa_height_of_pressure(*pressure, ins_qfe); // Z down } } -void ahrs_update_accel(float dt __attribute__((unused))) { +void ahrs_update_accel(float dt __attribute__((unused))) +{ } // assume mag is dead when values are not moving anymore #define MAG_FROZEN_COUNT 30 -void ahrs_update_mag(float dt __attribute__((unused))) { +void ahrs_update_mag(float dt __attribute__((unused))) +{ static uint32_t mag_frozen_count = MAG_FROZEN_COUNT; static int32_t last_mx = 0; @@ -542,8 +553,7 @@ void ahrs_update_mag(float dt __attribute__((unused))) { FLOAT_VECT3_ZERO(ins_impl.meas.mag); mag_frozen_count = MAG_FROZEN_COUNT; } - } - else { + } else { // values are moving struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); struct Int32Vect3 mag_meas_body; @@ -561,12 +571,14 @@ void ahrs_update_mag(float dt __attribute__((unused))) { * * x_dot = evolution_model + (gain_matrix * error) */ -static inline void invariant_model(float* o, const float* x, const int n, const float* u, const int m __attribute__((unused))) { +static inline void invariant_model(float *o, const float *x, const int n, const float *u, + const int m __attribute__((unused))) +{ #pragma GCC diagnostic push // require GCC 4.6 #pragma GCC diagnostic ignored "-Wcast-qual" - struct inv_state* s = (struct inv_state*)x; - struct inv_command* c = (struct inv_command*)u; + struct inv_state *s = (struct inv_state *)x; + struct inv_command *c = (struct inv_command *)u; #pragma GCC diagnostic pop // require GCC 4.6 struct inv_state s_dot; struct FloatRates rates_unbiased; @@ -596,7 +608,7 @@ static inline void invariant_model(float* o, const float* x, const int n, const /* dot_V = A + (1/as) * (q * am * q-1) + ME */ struct FloatQuat q_b2n; float_quat_invert(&q_b2n, &(s->quat)); - float_quat_vmult((struct FloatVect3*)&s_dot.speed, &q_b2n, &(c->accel)); + float_quat_vmult((struct FloatVect3 *)&s_dot.speed, &q_b2n, &(c->accel)); VECT3_SMUL(s_dot.speed, s_dot.speed, 1. / (s->as)); VECT3_ADD(s_dot.speed, A); VECT3_ADD(s_dot.speed, ins_impl.corr.ME); @@ -615,14 +627,15 @@ static inline void invariant_model(float* o, const float* x, const int n, const s_dot.hb = ins_impl.corr.SE; // set output - memcpy(o, &s_dot, n*sizeof(float)); + memcpy(o, &s_dot, n * sizeof(float)); } /** Compute correction vectors * E = ( Å· - y ) * LE, ME, NE, OE : ( gain matrix * error ) */ -static inline void error_output(struct InsFloatInv * _ins) { +static inline void error_output(struct InsFloatInv *_ins) +{ struct FloatVect3 YBt, I, Ev, Eb, Ex, Itemp, Ebtemp, Evtemp; float Eh; @@ -650,17 +663,16 @@ static inline void error_output(struct InsFloatInv * _ins) { // or while waiting first GPS data to prevent diverging if ((gps.fix == GPS_FIX_3D && ins.status == INS_RUNNING #if INS_UPDATE_FW_ESTIMATOR - && state.utm_initialized_f + && state.utm_initialized_f #else - && state.ned_initialized_f + && state.ned_initialized_f #endif - ) || !ins_gps_fix_once) { + ) || !ins_gps_fix_once) { /* Ev = (V - YV) */ VECT3_DIFF(Ev, _ins->state.speed, _ins->meas.speed_gps); /* Ex = (X - YX) */ VECT3_DIFF(Ex, _ins->state.pos, _ins->meas.pos_gps); - } - else { + } else { FLOAT_VECT3_ZERO(Ev); FLOAT_VECT3_ZERO(Ex); } @@ -670,12 +682,12 @@ static inline void error_output(struct InsFloatInv * _ins) { /*--------------Gains--------------*/ /**** LvEv + LbEb = -lvIa x Ev + lb < B x Eb, Ia > Ia *****/ - VECT3_SMUL(Itemp, I, -_ins->gains.lv/100.); + VECT3_SMUL(Itemp, I, -_ins->gains.lv / 100.); VECT3_CROSS_PRODUCT(Evtemp, Itemp, Ev); VECT3_CROSS_PRODUCT(Ebtemp, B, Eb); temp = VECT3_DOT_PRODUCT(Ebtemp, I); - temp = (_ins->gains.lb/100.) * temp; + temp = (_ins->gains.lb / 100.) * temp; VECT3_SMUL(Ebtemp, I, temp); VECT3_ADD(Evtemp, Ebtemp); @@ -692,12 +704,12 @@ static inline void error_output(struct InsFloatInv * _ins) { _ins->corr.NE.z = ((-_ins->gains.nxz) * Ex.z) + ((-_ins->gains.nh) * Eh); /****** OvEv + ObEb = ovIa x Ev - ob < B x Eb, Ia > Ia ********/ - VECT3_SMUL(Itemp, I, _ins->gains.ov/1000.); + VECT3_SMUL(Itemp, I, _ins->gains.ov / 1000.); VECT3_CROSS_PRODUCT(Evtemp, Itemp, Ev); VECT3_CROSS_PRODUCT(Ebtemp, B, Eb); temp = VECT3_DOT_PRODUCT(Ebtemp, I); - temp = (-_ins->gains.ob/1000.) * temp; + temp = (-_ins->gains.ob / 1000.) * temp; VECT3_SMUL(Ebtemp, I, temp); VECT3_ADD(Evtemp, Ebtemp); @@ -705,7 +717,7 @@ static inline void error_output(struct InsFloatInv * _ins) { /* a scalar */ /****** RvEv + RhEh = rv < Ia, Ev > + (-rhEh) **************/ - _ins->corr.RE = ((_ins->gains.rv/100.) * VECT3_DOT_PRODUCT(Ev, I)) + ((-_ins->gains.rh/10000.) * Eh); + _ins->corr.RE = ((_ins->gains.rv / 100.) * VECT3_DOT_PRODUCT(Ev, I)) + ((-_ins->gains.rh / 10000.) * Eh); /****** ShEh ******/ _ins->corr.SE = (_ins->gains.sh) * Eh; @@ -713,8 +725,8 @@ static inline void error_output(struct InsFloatInv * _ins) { } -void float_quat_vmul_right(struct FloatQuat* mright, const struct FloatQuat* q, - struct FloatVect3* vi) +void float_quat_vmul_right(struct FloatQuat *mright, const struct FloatQuat *q, + struct FloatVect3 *vi) { struct FloatVect3 qvec, v1, v2; float qi; diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.h b/sw/airborne/subsystems/ins/ins_float_invariant.h index 7bd8fa0713..1411a1486d 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant.h +++ b/sw/airborne/subsystems/ins/ins_float_invariant.h @@ -41,7 +41,7 @@ struct inv_state { struct FloatRates bias; ///< Estimated gyro biases struct NedCoor_f speed; ///< Estimates speed struct NedCoor_f pos; ///< Estimates position - float hb; ///< Estimates barometers bias + float hb; ///< Estimates barometers bias float as; ///< Estimated accelerometer sensitivity //float cs; ///< Estimated magnetic sensitivity }; diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough.c b/sw/airborne/subsystems/ins/ins_gps_passthrough.c index 5dcf7b7f34..6a42450570 100644 --- a/sw/airborne/subsystems/ins/ins_gps_passthrough.c +++ b/sw/airborne/subsystems/ins/ins_gps_passthrough.c @@ -58,32 +58,36 @@ struct InsGpsPassthrough ins_impl; #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_ins(struct transport_tx *trans, struct link_device *dev) { +static void send_ins(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_INS(trans, dev, AC_ID, - &ins_impl.ltp_pos.x, &ins_impl.ltp_pos.y, &ins_impl.ltp_pos.z, - &ins_impl.ltp_speed.x, &ins_impl.ltp_speed.y, &ins_impl.ltp_speed.z, - &ins_impl.ltp_accel.x, &ins_impl.ltp_accel.y, &ins_impl.ltp_accel.z); + &ins_impl.ltp_pos.x, &ins_impl.ltp_pos.y, &ins_impl.ltp_pos.z, + &ins_impl.ltp_speed.x, &ins_impl.ltp_speed.y, &ins_impl.ltp_speed.z, + &ins_impl.ltp_accel.x, &ins_impl.ltp_accel.y, &ins_impl.ltp_accel.z); } -static void send_ins_z(struct transport_tx *trans, struct link_device *dev) { +static void send_ins_z(struct transport_tx *trans, struct link_device *dev) +{ static const float fake_baro_z = 0.0; pprz_msg_send_INS_Z(trans, dev, AC_ID, - (float*)&fake_baro_z, &ins_impl.ltp_pos.z, + (float *)&fake_baro_z, &ins_impl.ltp_pos.z, &ins_impl.ltp_speed.z, &ins_impl.ltp_accel.z); } -static void send_ins_ref(struct transport_tx *trans, struct link_device *dev) { +static void send_ins_ref(struct transport_tx *trans, struct link_device *dev) +{ static const float fake_qfe = 0.0; if (ins_impl.ltp_initialized) { pprz_msg_send_INS_REF(trans, dev, AC_ID, - &ins_impl.ltp_def.ecef.x, &ins_impl.ltp_def.ecef.y, &ins_impl.ltp_def.ecef.z, - &ins_impl.ltp_def.lla.lat, &ins_impl.ltp_def.lla.lon, &ins_impl.ltp_def.lla.alt, - &ins_impl.ltp_def.hmsl, (float*)&fake_qfe); + &ins_impl.ltp_def.ecef.x, &ins_impl.ltp_def.ecef.y, &ins_impl.ltp_def.ecef.z, + &ins_impl.ltp_def.lla.lat, &ins_impl.ltp_def.lla.lon, &ins_impl.ltp_def.lla.alt, + &ins_impl.ltp_def.hmsl, (float *)&fake_qfe); } } #endif -void ins_init(void) { +void ins_init(void) +{ #if USE_INS_NAV_INIT struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */ @@ -115,13 +119,16 @@ void ins_init(void) { #endif } -void ins_periodic(void) { - if (ins_impl.ltp_initialized) +void ins_periodic(void) +{ + if (ins_impl.ltp_initialized) { ins.status = INS_RUNNING; + } } -void ins_reset_local_origin(void) { +void ins_reset_local_origin(void) +{ ltp_def_from_ecef_i(&ins_impl.ltp_def, &gps.ecef_pos); ins_impl.ltp_def.lla.alt = gps.lla_pos.alt; ins_impl.ltp_def.hmsl = gps.hmsl; @@ -129,18 +136,20 @@ void ins_reset_local_origin(void) { ins_impl.ltp_initialized = TRUE; } -void ins_reset_altitude_ref(void) { +void ins_reset_altitude_ref(void) +{ struct LlaCoor_i lla = { state.ned_origin_i.lla.lon, state.ned_origin_i.lla.lat, gps.lla_pos.alt }; ltp_def_from_lla_i(&ins_impl.ltp_def, &lla), - ins_impl.ltp_def.hmsl = gps.hmsl; + ins_impl.ltp_def.hmsl = gps.hmsl; stateSetLocalOrigin_i(&ins_impl.ltp_def); } -void ins_update_gps(void) { +void ins_update_gps(void) +{ if (gps.fix == GPS_FIX_3D) { if (!ins_impl.ltp_initialized) { ins_reset_local_origin(); diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c b/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c index fdeae62121..f655f47f99 100644 --- a/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c +++ b/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c @@ -35,7 +35,8 @@ #include "subsystems/gps.h" #include "firmwares/fixedwing/nav.h" -void ins_init(void) { +void ins_init(void) +{ struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 }; stateSetLocalUtmOrigin_f(&utm0); stateSetPositionUtm_f(&utm0); @@ -43,13 +44,14 @@ void ins_init(void) { ins.status = INS_RUNNING; } -void ins_reset_local_origin(void) { +void ins_reset_local_origin(void) +{ struct UtmCoor_f utm; #ifdef GPS_USE_LATLONG /* Recompute UTM coordinates in this zone */ struct LlaCoor_f lla; LLA_FLOAT_OF_BFP(lla, gps.lla_pos); - utm.zone = (gps.lla_pos.lon/1e7 + 180) / 6 + 1; + utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1; utm_of_lla_f(&utm, &lla); #else utm.zone = gps.utm_pos.zone; @@ -62,13 +64,15 @@ void ins_reset_local_origin(void) { stateSetLocalUtmOrigin_f(&utm); } -void ins_reset_altitude_ref(void) { +void ins_reset_altitude_ref(void) +{ struct UtmCoor_f utm = state.utm_origin_f; utm.alt = gps.hmsl / 1000.0f; stateSetLocalUtmOrigin_f(&utm); } -void ins_update_gps(void) { +void ins_update_gps(void) +{ struct UtmCoor_f utm; utm.east = gps.utm_pos.east / 100.0f; utm.north = gps.utm_pos.north / 100.0f; diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index abf1560556..650ea29abc 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -121,24 +121,27 @@ struct InsInt ins_impl; #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_ins(struct transport_tx *trans, struct link_device *dev) { +static void send_ins(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_INS(trans, dev, AC_ID, - &ins_impl.ltp_pos.x, &ins_impl.ltp_pos.y, &ins_impl.ltp_pos.z, - &ins_impl.ltp_speed.x, &ins_impl.ltp_speed.y, &ins_impl.ltp_speed.z, - &ins_impl.ltp_accel.x, &ins_impl.ltp_accel.y, &ins_impl.ltp_accel.z); + &ins_impl.ltp_pos.x, &ins_impl.ltp_pos.y, &ins_impl.ltp_pos.z, + &ins_impl.ltp_speed.x, &ins_impl.ltp_speed.y, &ins_impl.ltp_speed.z, + &ins_impl.ltp_accel.x, &ins_impl.ltp_accel.y, &ins_impl.ltp_accel.z); } -static void send_ins_z(struct transport_tx *trans, struct link_device *dev) { +static void send_ins_z(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_INS_Z(trans, dev, AC_ID, - &ins_impl.baro_z, &ins_impl.ltp_pos.z, &ins_impl.ltp_speed.z, &ins_impl.ltp_accel.z); + &ins_impl.baro_z, &ins_impl.ltp_pos.z, &ins_impl.ltp_speed.z, &ins_impl.ltp_accel.z); } -static void send_ins_ref(struct transport_tx *trans, struct link_device *dev) { +static void send_ins_ref(struct transport_tx *trans, struct link_device *dev) +{ if (ins_impl.ltp_initialized) { pprz_msg_send_INS_REF(trans, dev, AC_ID, - &ins_impl.ltp_def.ecef.x, &ins_impl.ltp_def.ecef.y, &ins_impl.ltp_def.ecef.z, - &ins_impl.ltp_def.lla.lat, &ins_impl.ltp_def.lla.lon, &ins_impl.ltp_def.lla.alt, - &ins_impl.ltp_def.hmsl, &ins_impl.qfe); + &ins_impl.ltp_def.ecef.x, &ins_impl.ltp_def.ecef.y, &ins_impl.ltp_def.ecef.z, + &ins_impl.ltp_def.lla.lat, &ins_impl.ltp_def.lla.lon, &ins_impl.ltp_def.lla.alt, + &ins_impl.ltp_def.hmsl, &ins_impl.qfe); } } #endif @@ -151,7 +154,8 @@ static void ins_update_from_hff(void); #endif -void ins_init(void) { +void ins_init(void) +{ #if USE_INS_NAV_INIT ins_init_origin_from_flightplan(); @@ -190,12 +194,15 @@ void ins_init(void) { #endif } -void ins_periodic(void) { - if (ins_impl.ltp_initialized) +void ins_periodic(void) +{ + if (ins_impl.ltp_initialized) { ins.status = INS_RUNNING; + } } -void ins_reset_local_origin(void) { +void ins_reset_local_origin(void) +{ ins_impl.ltp_initialized = FALSE; #if USE_HFF ins_impl.hf_realign = TRUE; @@ -203,7 +210,8 @@ void ins_reset_local_origin(void) { ins_impl.vf_reset = TRUE; } -void ins_reset_altitude_ref(void) { +void ins_reset_altitude_ref(void) +{ #if USE_GPS struct LlaCoor_i lla = { state.ned_origin_i.lla.lon, @@ -211,13 +219,14 @@ void ins_reset_altitude_ref(void) { gps.lla_pos.alt }; ltp_def_from_lla_i(&ins_impl.ltp_def, &lla), - ins_impl.ltp_def.hmsl = gps.hmsl; + ins_impl.ltp_def.hmsl = gps.hmsl; stateSetLocalOrigin_i(&ins_impl.ltp_def); #endif ins_impl.vf_reset = TRUE; } -void ins_propagate(float dt) { +void ins_propagate(float dt) +{ /* untilt accels */ struct Int32Vect3 accel_meas_body; struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); @@ -229,8 +238,7 @@ void ins_propagate(float dt) { if (ins_impl.baro_initialized) { vff_propagate(z_accel_meas_float, dt); ins_update_from_vff(); - } - else { // feed accel from the sensors + } else { // feed accel from the sensors // subtract -9.81m/s2 (acceleration measured due to gravity, // but vehicle not accelerating in ltp) ins_impl.ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81); @@ -249,7 +257,8 @@ void ins_propagate(float dt) { ins_ned_to_state(); } -static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) { +static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) +{ if (!ins_impl.baro_initialized && *pressure > 1e-7) { // wait for a first positive value ins_impl.qfe = *pressure; @@ -262,8 +271,7 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pres ins_impl.qfe = *pressure; vff_realign(0.); ins_update_from_vff(); - } - else { + } else { ins_impl.baro_z = -pprz_isa_height_of_pressure(*pressure, ins_impl.qfe); #if USE_VFF_EXTENDED vff_update_baro(ins_impl.baro_z); @@ -276,7 +284,8 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pres } #if USE_GPS -void ins_update_gps(void) { +void ins_update_gps(void) +{ if (gps.fix == GPS_FIX_3D) { if (!ins_impl.ltp_initialized) { ltp_def_from_ecef_i(&ins_impl.ltp_def, &gps.ecef_pos); @@ -331,7 +340,8 @@ void ins_update_gps(void) { #if USE_SONAR -static void sonar_cb(uint8_t __attribute__((unused)) sender_id, const float *distance) { +static void sonar_cb(uint8_t __attribute__((unused)) sender_id, const float *distance) +{ static float last_offset = 0.; /* update filter assuming a flat ground */ @@ -346,8 +356,7 @@ static void sonar_cb(uint8_t __attribute__((unused)) sender_id, const float *dis && ins_impl.baro_initialized) { vff_update_z_conf(-(*distance), VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabsf(*distance)); last_offset = vff.offset; - } - else { + } else { /* update offset with last value to avoid divergence */ vff_update_offset(last_offset); } @@ -356,7 +365,8 @@ static void sonar_cb(uint8_t __attribute__((unused)) sender_id, const float *dis /** initialize the local origin (ltp_def) from flight plan position */ -static void ins_init_origin_from_flightplan(void) { +static void ins_init_origin_from_flightplan(void) +{ struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */ llh_nav0.lat = NAV_LAT0; @@ -374,19 +384,22 @@ static void ins_init_origin_from_flightplan(void) { } /** copy position and speed to state interface */ -static void ins_ned_to_state(void) { +static void ins_ned_to_state(void) +{ stateSetPositionNed_i(&ins_impl.ltp_pos); stateSetSpeedNed_i(&ins_impl.ltp_speed); stateSetAccelNed_i(&ins_impl.ltp_accel); #if defined SITL && USE_NPS - if (nps_bypass_ins) + if (nps_bypass_ins) { sim_overwrite_ins(); + } #endif } /** update ins state from vertical filter */ -static void ins_update_from_vff(void) { +static void ins_update_from_vff(void) +{ ins_impl.ltp_accel.z = ACCEL_BFP_OF_REAL(vff.zdotdot); ins_impl.ltp_speed.z = SPEED_BFP_OF_REAL(vff.zdot); ins_impl.ltp_pos.z = POS_BFP_OF_REAL(vff.z); @@ -394,7 +407,8 @@ static void ins_update_from_vff(void) { #if USE_HFF /** update ins state from horizontal filter */ -static void ins_update_from_hff(void) { +static void ins_update_from_hff(void) +{ ins_impl.ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot); ins_impl.ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot); ins_impl.ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot); diff --git a/sw/airborne/subsystems/ins/vf_extended_float.c b/sw/airborne/subsystems/ins/vf_extended_float.c index 2397ed0d22..9d13f62f79 100644 --- a/sw/airborne/subsystems/ins/vf_extended_float.c +++ b/sw/airborne/subsystems/ins/vf_extended_float.c @@ -69,27 +69,31 @@ struct VffExtended vff; #if PERIODIC_TELEMETRY #include "subsystems/datalink/telemetry.h" -static void send_vffe(struct transport_tx *trans, struct link_device *dev) { +static void send_vffe(struct transport_tx *trans, struct link_device *dev) +{ pprz_msg_send_VFF_EXTENDED(trans, dev, AC_ID, - &vff.z_meas, &vff.z_meas_baro, - &vff.z, &vff.zdot, &vff.zdotdot, - &vff.bias, &vff.offset); + &vff.z_meas, &vff.z_meas_baro, + &vff.z, &vff.zdot, &vff.zdotdot, + &vff.bias, &vff.offset); } #endif -void vff_init_zero(void) { +void vff_init_zero(void) +{ vff_init(0., 0., 0., 0.); } -void vff_init(float init_z, float init_zdot, float init_accel_bias, float init_baro_offset) { +void vff_init(float init_z, float init_zdot, float init_accel_bias, float init_baro_offset) +{ vff.z = init_z; vff.zdot = init_zdot; vff.bias = init_accel_bias; vff.offset = init_baro_offset; int i, j; - for (i=0; i= NB_BLOCK) - nav_block=NB_BLOCK-1; +void nav_init_block(void) +{ + if (nav_block >= NB_BLOCK) { + nav_block = NB_BLOCK - 1; + } nav_stage = 0; block_time = 0; InitStage(); } -void nav_goto_block(uint8_t b) { +void nav_goto_block(uint8_t b) +{ if (b != nav_block) { /* To avoid a loop in a the current block */ last_block = nav_block; last_stage = nav_stage; diff --git a/sw/airborne/subsystems/navigation/common_nav.c b/sw/airborne/subsystems/navigation/common_nav.c index fdf3c96dcb..6ef3b2110b 100644 --- a/sw/airborne/subsystems/navigation/common_nav.c +++ b/sw/airborne/subsystems/navigation/common_nav.c @@ -47,12 +47,13 @@ float max_dist_from_home = MAX_DIST_FROM_HOME; /** Computes squared distance to the HOME waypoint. * Updates #dist2_to_home and potentially sets #too_far_from_home */ -void compute_dist2_to_home(void) { - struct EnuCoor_f* pos = stateGetPositionEnu_f(); +void compute_dist2_to_home(void) +{ + struct EnuCoor_f *pos = stateGetPositionEnu_f(); float ph_x = waypoints[WP_HOME].x - pos->x; float ph_y = waypoints[WP_HOME].y - pos->y; - dist2_to_home = ph_x*ph_x + ph_y *ph_y; - too_far_from_home = dist2_to_home > (MAX_DIST_FROM_HOME*MAX_DIST_FROM_HOME); + dist2_to_home = ph_x * ph_x + ph_y * ph_y; + too_far_from_home = dist2_to_home > (MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME); #if defined InAirspace too_far_from_home = too_far_from_home || !(InAirspace(pos_x, pos_y)); #endif @@ -62,7 +63,8 @@ void compute_dist2_to_home(void) { static float previous_ground_alt; /** Reset the UTM zone to current GPS fix */ -unit_t nav_reset_utm_zone(void) { +unit_t nav_reset_utm_zone(void) +{ struct UtmCoor_f utm0; utm0.zone = nav_utm_zone0; @@ -80,7 +82,8 @@ unit_t nav_reset_utm_zone(void) { } /** Reset the geographic reference to the current GPS fix */ -unit_t nav_reset_reference(void) { +unit_t nav_reset_reference(void) +{ /* realign INS */ ins_reset_local_origin(); @@ -97,7 +100,8 @@ unit_t nav_reset_reference(void) { } /** Reset the altitude reference to the current GPS alt */ -unit_t nav_reset_alt(void) { +unit_t nav_reset_alt(void) +{ ins_reset_altitude_ref(); /* Ground alt */ @@ -108,15 +112,17 @@ unit_t nav_reset_alt(void) { } /** Shift altitude of the waypoint according to a new ground altitude */ -unit_t nav_update_waypoints_alt(void) { +unit_t nav_update_waypoints_alt(void) +{ uint8_t i; - for(i = 0; i < NB_WAYPOINT; i++) { + for (i = 0; i < NB_WAYPOINT; i++) { waypoints[i].a += ground_alt - previous_ground_alt; } return 0; } -void common_nav_periodic_task_4Hz() { +void common_nav_periodic_task_4Hz() +{ RunOnceEvery(4, { stage_time++; block_time++; }); } @@ -126,7 +132,8 @@ void common_nav_periodic_task_4Hz() { * @param[in] uy UTM y (north) coordinate * @param[in] alt Altitude above MSL. */ -void nav_move_waypoint(uint8_t wp_id, float ux, float uy, float alt) { +void nav_move_waypoint(uint8_t wp_id, float ux, float uy, float alt) +{ if (wp_id < nb_waypoint) { float dx, dy; dx = ux - nav_utm_east0 - waypoints[WP_HOME].x; diff --git a/sw/airborne/subsystems/navigation/common_nav.h b/sw/airborne/subsystems/navigation/common_nav.h index c3657846c4..b5afffd8ac 100644 --- a/sw/airborne/subsystems/navigation/common_nav.h +++ b/sw/airborne/subsystems/navigation/common_nav.h @@ -62,9 +62,9 @@ extern uint8_t nav_utm_zone0; void compute_dist2_to_home(void); unit_t nav_reset_utm_zone(void); -unit_t nav_reset_reference( void ) __attribute__ ((unused)); -unit_t nav_reset_alt(void) __attribute__ ((unused)); -unit_t nav_update_waypoints_alt( void ) __attribute__ ((unused)); +unit_t nav_reset_reference(void) __attribute__((unused)); +unit_t nav_reset_alt(void) __attribute__((unused)); +unit_t nav_update_waypoints_alt(void) __attribute__((unused)); void common_nav_periodic_task_4Hz(void); @@ -73,10 +73,10 @@ void common_nav_periodic_task_4Hz(void); #define NavSetAltitudeReferenceHere() ({ nav_reset_alt(); nav_update_waypoints_alt(); FALSE; }) #define NavSetWaypointHere(_wp) ({ \ - waypoints[_wp].x = stateGetPositionEnu_f()->x; \ - waypoints[_wp].y = stateGetPositionEnu_f()->y; \ - FALSE; \ -}) + waypoints[_wp].x = stateGetPositionEnu_f()->x; \ + waypoints[_wp].y = stateGetPositionEnu_f()->y; \ + FALSE; \ + }) #define NavSetWaypointPosAndAltHere(_wp) ({ \ waypoints[_wp].x = stateGetPositionEnu_f()->x; \ diff --git a/sw/airborne/subsystems/navigation/nav_survey_rectangle.c b/sw/airborne/subsystems/navigation/nav_survey_rectangle.c index 4a39f00ace..30ea75e5f2 100644 --- a/sw/airborne/subsystems/navigation/nav_survey_rectangle.c +++ b/sw/airborne/subsystems/navigation/nav_survey_rectangle.c @@ -32,7 +32,7 @@ static struct point survey_from; static struct point survey_to; -static bool_t survey_uturn __attribute__ ((unused)) = FALSE; +static bool_t survey_uturn __attribute__((unused)) = FALSE; static survey_orientation_t survey_orientation = NS; #define SurveyGoingNorth() ((survey_orientation == NS) && (survey_to.y > survey_from.y)) @@ -50,7 +50,8 @@ static survey_orientation_t survey_orientation = NS; #endif -void nav_survey_rectangle_init(uint8_t wp1, uint8_t wp2, float grid, survey_orientation_t so) { +void nav_survey_rectangle_init(uint8_t wp1, uint8_t wp2, float grid, survey_orientation_t so) +{ nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2)); nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2)); nav_survey_south = Min(WaypointY(wp1), WaypointY(wp2)); @@ -58,8 +59,10 @@ void nav_survey_rectangle_init(uint8_t wp1, uint8_t wp2, float grid, survey_orie survey_orientation = so; if (survey_orientation == NS) { - survey_from.x = survey_to.x = Min(Max(stateGetPositionEnu_f()->x, nav_survey_west+grid/2.), nav_survey_east-grid/2.); - if (stateGetPositionEnu_f()->y > nav_survey_north || (stateGetPositionEnu_f()->y > nav_survey_south && (*stateGetHorizontalSpeedDir_f()) > M_PI/2. && (*stateGetHorizontalSpeedDir_f()) < 3*M_PI/2)) { + survey_from.x = survey_to.x = Min(Max(stateGetPositionEnu_f()->x, nav_survey_west + grid / 2.), + nav_survey_east - grid / 2.); + if (stateGetPositionEnu_f()->y > nav_survey_north || (stateGetPositionEnu_f()->y > nav_survey_south + && (*stateGetHorizontalSpeedDir_f()) > M_PI / 2. && (*stateGetHorizontalSpeedDir_f()) < 3 * M_PI / 2)) { survey_to.y = nav_survey_south; survey_from.y = nav_survey_north; } else { @@ -67,8 +70,10 @@ void nav_survey_rectangle_init(uint8_t wp1, uint8_t wp2, float grid, survey_orie survey_to.y = nav_survey_north; } } else { /* survey_orientation == WE */ - survey_from.y = survey_to.y = Min(Max(stateGetPositionEnu_f()->y, nav_survey_south+grid/2.), nav_survey_north-grid/2.); - if (stateGetPositionEnu_f()->x > nav_survey_east || (stateGetPositionEnu_f()->x > nav_survey_west && (*stateGetHorizontalSpeedDir_f()) > M_PI)) { + survey_from.y = survey_to.y = Min(Max(stateGetPositionEnu_f()->y, nav_survey_south + grid / 2.), + nav_survey_north - grid / 2.); + if (stateGetPositionEnu_f()->x > nav_survey_east || (stateGetPositionEnu_f()->x > nav_survey_west + && (*stateGetHorizontalSpeedDir_f()) > M_PI)) { survey_to.x = nav_survey_west; survey_from.x = nav_survey_east; } else { @@ -82,7 +87,8 @@ void nav_survey_rectangle_init(uint8_t wp1, uint8_t wp2, float grid, survey_orie } -void nav_survey_rectangle(uint8_t wp1, uint8_t wp2) { +void nav_survey_rectangle(uint8_t wp1, uint8_t wp2) +{ static float survey_radius; nav_survey_active = TRUE; @@ -118,7 +124,7 @@ void nav_survey_rectangle(uint8_t wp1, uint8_t wp2) { if (survey_orientation == NS) { /* North or South limit reached, prepare U-turn and next leg */ float x0 = survey_from.x; /* Current longitude */ - if (x0+nav_survey_shift < nav_survey_west || x0+nav_survey_shift > nav_survey_east) { + if (x0 + nav_survey_shift < nav_survey_west || x0 + nav_survey_shift > nav_survey_east) { x0 += nav_survey_shift / 2; nav_survey_shift = -nav_survey_shift; } @@ -132,7 +138,7 @@ void nav_survey_rectangle(uint8_t wp1, uint8_t wp2) { survey_to.y = tmp; /** Do half a circle around WP 0 */ - waypoints[0].x = x0 - nav_survey_shift/2.; + waypoints[0].x = x0 - nav_survey_shift / 2.; waypoints[0].y = survey_from.y; /* Computes the right direction for the circle */ @@ -144,7 +150,7 @@ void nav_survey_rectangle(uint8_t wp1, uint8_t wp2) { /* East or West limit reached, prepare U-turn and next leg */ /* There is a y0 declared in math.h (for ARM) !!! */ float my_y0 = survey_from.y; /* Current latitude */ - if (my_y0+nav_survey_shift < nav_survey_south || my_y0+nav_survey_shift > nav_survey_north) { + if (my_y0 + nav_survey_shift < nav_survey_south || my_y0 + nav_survey_shift > nav_survey_north) { my_y0 += nav_survey_shift / 2; nav_survey_shift = -nav_survey_shift; } @@ -159,7 +165,7 @@ void nav_survey_rectangle(uint8_t wp1, uint8_t wp2) { /** Do half a circle around WP 0 */ waypoints[0].x = survey_from.x; - waypoints[0].y = my_y0 - nav_survey_shift/2.; + waypoints[0].y = my_y0 - nav_survey_shift / 2.; /* Computes the right direction for the circle */ survey_radius = nav_survey_shift / 2.; diff --git a/sw/airborne/subsystems/navigation/traffic_info.c b/sw/airborne/subsystems/navigation/traffic_info.c index faf3811eca..eeff6ebf14 100644 --- a/sw/airborne/subsystems/navigation/traffic_info.c +++ b/sw/airborne/subsystems/navigation/traffic_info.c @@ -34,13 +34,15 @@ uint8_t acs_idx; uint8_t the_acs_id[NB_ACS_ID]; struct ac_info_ the_acs[NB_ACS]; -void traffic_info_init( void ) { +void traffic_info_init(void) +{ the_acs_id[0] = 0; // ground station the_acs_id[AC_ID] = 1; the_acs[the_acs_id[AC_ID]].ac_id = AC_ID; acs_idx = 2; } -struct ac_info_ * get_ac_info(uint8_t id) { +struct ac_info_ *get_ac_info(uint8_t id) +{ return &the_acs[the_acs_id[id]]; } diff --git a/sw/airborne/subsystems/navigation/traffic_info.h b/sw/airborne/subsystems/navigation/traffic_info.h index 9d09d6e07a..eef8b7519b 100644 --- a/sw/airborne/subsystems/navigation/traffic_info.h +++ b/sw/airborne/subsystems/navigation/traffic_info.h @@ -63,10 +63,10 @@ extern struct ac_info_ the_acs[NB_ACS]; the_acs[the_acs_id[_id]].climb = _climb; \ the_acs[the_acs_id[_id]].itow = (uint32_t)_itow; \ } \ - } + } -extern void traffic_info_init( void ); +extern void traffic_info_init(void); -struct ac_info_ * get_ac_info(uint8_t id); +struct ac_info_ *get_ac_info(uint8_t id); #endif diff --git a/sw/airborne/subsystems/radio_control/joby.c b/sw/airborne/subsystems/radio_control/joby.c index beee5ebb11..dacb0d9a7f 100644 --- a/sw/airborne/subsystems/radio_control/joby.c +++ b/sw/airborne/subsystems/radio_control/joby.c @@ -28,7 +28,7 @@ static const int16_t rc_joby_signs[RADIO_CONTROL_NB_CHANNEL] = RC_JOBY_SIGNS; static void handle_channel(void (* callback)(void)) { - if (parser.parser_normal_buf == RC_JOBY_MAGIC_START) { + if (parser.parser_normal_buf == RC_JOBY_MAGIC_START) { // got start channel, look for channel 0 next parser.current_channel = 0; } else if (parser.current_channel == -1) { @@ -45,8 +45,9 @@ static void handle_channel(void (* callback)(void)) radio_control.frame_cpt++; radio_control.status = RC_OK; radio_control.time_since_last_frame = 0; - if (callback != NULL) - callback(); + if (callback != NULL) { + callback(); + } } } } @@ -75,8 +76,9 @@ void rc_joby_parse(int8_t c, void (* callback)(void)) { if (parser.current_byte == READING_HIGH_BYTE) { parser.high_byte_buf = c; - if (parser.current_channel >= 0 || parser.high_byte_buf == (RC_JOBY_MAGIC_START >> 8) || parser.current_inverted == READING_INVERTED) { - // only advance parser state to low byte if we're not looking for a sync byte which we didn't find + if (parser.current_channel >= 0 || parser.high_byte_buf == (RC_JOBY_MAGIC_START >> 8) + || parser.current_inverted == READING_INVERTED) { + // only advance parser state to low byte if we're not looking for a sync byte which we didn't find parser.current_byte = READING_LOW_BYTE; } } else { // READING_LOW_BYTE @@ -86,7 +88,8 @@ void rc_joby_parse(int8_t c, void (* callback)(void)) } } -void radio_control_impl_init(void) { +void radio_control_impl_init(void) +{ parser.current_byte = READING_HIGH_BYTE; parser.current_inverted = READING_NORMAL; parser.current_channel = -1; diff --git a/sw/airborne/subsystems/radio_control/joby.h b/sw/airborne/subsystems/radio_control/joby.h index aa8cec20d5..7d39f09d22 100644 --- a/sw/airborne/subsystems/radio_control/joby.h +++ b/sw/airborne/subsystems/radio_control/joby.h @@ -40,8 +40,7 @@ typedef enum { READING_INVERTED } parser_inverted_t; -struct rc_joby_parser_state -{ +struct rc_joby_parser_state { parser_byte_t current_byte; parser_inverted_t current_inverted; int current_channel; @@ -65,7 +64,7 @@ void rc_joby_parse(int8_t c, void (* callback)(void)); #define RadioControlEvent(_received_frame_handler) { \ while (RcLinkChAvailable()) { \ - rc_joby_parse(RcLinkGetCh(), _received_frame_handler); \ + rc_joby_parse(RcLinkGetCh(), _received_frame_handler); \ } \ } diff --git a/sw/airborne/subsystems/radio_control/joby_9ch.h b/sw/airborne/subsystems/radio_control/joby_9ch.h index 46f38cc894..c9bdef26a9 100644 --- a/sw/airborne/subsystems/radio_control/joby_9ch.h +++ b/sw/airborne/subsystems/radio_control/joby_9ch.h @@ -37,13 +37,13 @@ #define RC_JOBY_SYNC_2 0x12 #define RC_JOBY_SIGNS { 1, \ - 1, \ - 1, \ - 1, \ - 1, \ - 1, \ - 1, \ - 1, \ - 1 } + 1, \ + 1, \ + 1, \ + 1, \ + 1, \ + 1, \ + 1, \ + 1 } #endif /* RADIO_CONTROL_JOBY_9CH_H */ diff --git a/sw/airborne/subsystems/radio_control/rc_datalink.c b/sw/airborne/subsystems/radio_control/rc_datalink.c index c762d10ace..b0214e6211 100644 --- a/sw/airborne/subsystems/radio_control/rc_datalink.c +++ b/sw/airborne/subsystems/radio_control/rc_datalink.c @@ -31,16 +31,17 @@ int8_t rc_dl_values[ RC_DL_NB_CHANNEL ]; volatile bool_t rc_dl_frame_available; -void radio_control_impl_init(void) { +void radio_control_impl_init(void) +{ rc_dl_frame_available = FALSE; } -void parse_rc_3ch_datalink( uint8_t throttle_mode, - int8_t roll, - int8_t pitch) +void parse_rc_3ch_datalink(uint8_t throttle_mode, + int8_t roll, + int8_t pitch) { - uint8_t throttle = ((throttle_mode & 0xFC)>>2)*(128/64); + uint8_t throttle = ((throttle_mode & 0xFC) >> 2) * (128 / 64); uint8_t mode = throttle_mode & 0x03; rc_dl_values[RADIO_ROLL] = roll; @@ -53,11 +54,11 @@ void parse_rc_3ch_datalink( uint8_t throttle_mode, } void parse_rc_4ch_datalink( - uint8_t mode, - uint8_t throttle, - int8_t roll, - int8_t pitch, - int8_t yaw) + uint8_t mode, + uint8_t throttle, + int8_t roll, + int8_t pitch, + int8_t yaw) { rc_dl_values[RADIO_MODE] = (int8_t)mode; rc_dl_values[RADIO_THROTTLE] = (int8_t)throttle; @@ -73,13 +74,13 @@ void parse_rc_4ch_datalink( */ static void rc_datalink_normalize(int8_t *in, int16_t *out) { - out[RADIO_ROLL] = (MAX_PPRZ/128) * in[RADIO_ROLL]; + out[RADIO_ROLL] = (MAX_PPRZ / 128) * in[RADIO_ROLL]; Bound(out[RADIO_ROLL], MIN_PPRZ, MAX_PPRZ); - out[RADIO_PITCH] = (MAX_PPRZ/128) * in[RADIO_PITCH]; + out[RADIO_PITCH] = (MAX_PPRZ / 128) * in[RADIO_PITCH]; Bound(out[RADIO_PITCH], MIN_PPRZ, MAX_PPRZ); - out[RADIO_YAW] = (MAX_PPRZ/128) * in[RADIO_YAW]; + out[RADIO_YAW] = (MAX_PPRZ / 128) * in[RADIO_YAW]; Bound(out[RADIO_YAW], MIN_PPRZ, MAX_PPRZ); - out[RADIO_THROTTLE] = ((MAX_PPRZ/128) * in[RADIO_THROTTLE]); + out[RADIO_THROTTLE] = ((MAX_PPRZ / 128) * in[RADIO_THROTTLE]); Bound(out[RADIO_THROTTLE], 0, MAX_PPRZ); out[RADIO_MODE] = MAX_PPRZ * (in[RADIO_MODE] - 1); Bound(out[RADIO_MODE], MIN_PPRZ, MAX_PPRZ); diff --git a/sw/airborne/subsystems/radio_control/rc_datalink.h b/sw/airborne/subsystems/radio_control/rc_datalink.h index d8d7894ecc..432f5fcddb 100644 --- a/sw/airborne/subsystems/radio_control/rc_datalink.h +++ b/sw/airborne/subsystems/radio_control/rc_datalink.h @@ -50,19 +50,19 @@ extern volatile bool_t rc_dl_frame_available; * Mode and throttle are merge in the same byte */ extern void parse_rc_3ch_datalink( - uint8_t throttle_mode, - int8_t roll, - int8_t pitch); + uint8_t throttle_mode, + int8_t roll, + int8_t pitch); /** * Decode datalink message to get rc values with RC_4CH message */ extern void parse_rc_4ch_datalink( - uint8_t mode, - uint8_t throttle, - int8_t roll, - int8_t pitch, - int8_t yaw); + uint8_t mode, + uint8_t throttle, + int8_t roll, + int8_t pitch, + int8_t yaw); /** * RC event function with handler callback. diff --git a/sw/airborne/subsystems/radio_control/sbus_common.c b/sw/airborne/subsystems/radio_control/sbus_common.c index abacf2ce70..774eaae607 100644 --- a/sw/airborne/subsystems/radio_control/sbus_common.c +++ b/sw/airborne/subsystems/radio_control/sbus_common.c @@ -54,7 +54,7 @@ #endif -void sbus_common_init(struct Sbus* sbus_p, struct uart_periph* dev) +void sbus_common_init(struct Sbus *sbus_p, struct uart_periph *dev) { sbus_p->frame_available = FALSE; sbus_p->status = SBUS_STATUS_UNINIT; @@ -73,8 +73,8 @@ void sbus_common_init(struct Sbus* sbus_p, struct uart_periph* dev) /** Decode the raw buffer */ -static void decode_sbus_buffer(const uint8_t* src, uint16_t* dst, bool_t* available, - uint16_t* dstppm) +static void decode_sbus_buffer(const uint8_t *src, uint16_t *dst, bool_t *available, + uint16_t *dstppm) { // reset counters uint8_t byteInRawBuf = 0; @@ -112,7 +112,7 @@ static void decode_sbus_buffer(const uint8_t* src, uint16_t* dst, bool_t* availa // Decoding event function // Reading from UART -void sbus_common_decode_event(struct Sbus* sbus_p, struct uart_periph* dev) +void sbus_common_decode_event(struct Sbus *sbus_p, struct uart_periph *dev) { uint8_t rbyte; if (uart_char_available(dev)) { diff --git a/sw/airborne/subsystems/radio_control/sbus_common.h b/sw/airborne/subsystems/radio_control/sbus_common.h index 48b4476662..66fc1b0c46 100644 --- a/sw/airborne/subsystems/radio_control/sbus_common.h +++ b/sw/airborne/subsystems/radio_control/sbus_common.h @@ -87,12 +87,12 @@ struct Sbus { /** * Init function */ -void sbus_common_init(struct Sbus* sbus, struct uart_periph* dev); +void sbus_common_init(struct Sbus *sbus, struct uart_periph *dev); /** * Decoding event function */ -void sbus_common_decode_event(struct Sbus* sbus, struct uart_periph* dev); +void sbus_common_decode_event(struct Sbus *sbus, struct uart_periph *dev); /** diff --git a/sw/airborne/subsystems/radio_control/sbus_dual.c b/sw/airborne/subsystems/radio_control/sbus_dual.c index 9be43cfc6b..4f2954176f 100644 --- a/sw/airborne/subsystems/radio_control/sbus_dual.c +++ b/sw/airborne/subsystems/radio_control/sbus_dual.c @@ -81,7 +81,7 @@ void radio_control_impl_event(void (* _received_frame_handler)(void)) radio_control.radio_ok_cpt--; } else { radio_control.status = RC_OK; - NormalizePpmIIR(sbus2.pulses,radio_control); + NormalizePpmIIR(sbus2.pulses, radio_control); _received_frame_handler(); } sbus2.frame_available = FALSE; @@ -93,7 +93,7 @@ void radio_control_impl_event(void (* _received_frame_handler)(void)) radio_control.radio_ok_cpt--; } else { radio_control.status = RC_OK; - NormalizePpmIIR(sbus1.pulses,radio_control); + NormalizePpmIIR(sbus1.pulses, radio_control); _received_frame_handler(); } sbus1.frame_available = FALSE; diff --git a/sw/airborne/subsystems/radio_control/spektrum_dx7se.h b/sw/airborne/subsystems/radio_control/spektrum_dx7se.h index e79fe54029..42463e5c3d 100644 --- a/sw/airborne/subsystems/radio_control/spektrum_dx7se.h +++ b/sw/airborne/subsystems/radio_control/spektrum_dx7se.h @@ -35,12 +35,12 @@ #define RC_SPK_SYNC_2 0x12 #define RC_SPK_THROWS { MAX_PPRZ/MAX_SPK, \ - MAX_PPRZ/MAX_SPK, \ - -MAX_PPRZ/MAX_SPK, \ - MAX_PPRZ/MAX_SPK, \ - MAX_PPRZ/MAX_SPK, \ - -MAX_PPRZ/MAX_SPK, \ - MAX_PPRZ/MAX_SPK } + MAX_PPRZ/MAX_SPK, \ + -MAX_PPRZ/MAX_SPK, \ + MAX_PPRZ/MAX_SPK, \ + MAX_PPRZ/MAX_SPK, \ + -MAX_PPRZ/MAX_SPK, \ + MAX_PPRZ/MAX_SPK } /* aileron 1 diff --git a/sw/airborne/subsystems/radio_control/spektrum_dx7se_joby.h b/sw/airborne/subsystems/radio_control/spektrum_dx7se_joby.h index 44896f4d18..7a468a471b 100644 --- a/sw/airborne/subsystems/radio_control/spektrum_dx7se_joby.h +++ b/sw/airborne/subsystems/radio_control/spektrum_dx7se_joby.h @@ -34,12 +34,12 @@ #define RC_SPK_SYNC_2 0x01 #define RC_SPK_THROWS { MAX_PPRZ/MAX_SPK, \ - MAX_PPRZ/MAX_SPK, \ - -MAX_PPRZ/MAX_SPK, \ - -MAX_PPRZ/MAX_SPK, \ - -MAX_PPRZ/MAX_SPK, \ - MAX_PPRZ/MAX_SPK, \ - MAX_PPRZ/MAX_SPK } + MAX_PPRZ/MAX_SPK, \ + -MAX_PPRZ/MAX_SPK, \ + -MAX_PPRZ/MAX_SPK, \ + -MAX_PPRZ/MAX_SPK, \ + MAX_PPRZ/MAX_SPK, \ + MAX_PPRZ/MAX_SPK } /* aileron 1 diff --git a/sw/airborne/subsystems/radio_control/superbitrf_rc.c b/sw/airborne/subsystems/radio_control/superbitrf_rc.c index 1243019b69..052d820427 100644 --- a/sw/airborne/subsystems/radio_control/superbitrf_rc.c +++ b/sw/airborne/subsystems/radio_control/superbitrf_rc.c @@ -32,7 +32,8 @@ //#if DATALINK == SUPERBITRF //void radio_control_impl_init(void) {} //#else -void radio_control_impl_init(void) { +void radio_control_impl_init(void) +{ superbitrf_init(); } //#endif @@ -41,8 +42,8 @@ void radio_control_impl_init(void) { static void superbitrf_rc_normalize(int16_t *in, int16_t *out, uint8_t count) { uint8_t i; - for(i = 0; i < count; i++) { - if(i == RADIO_THROTTLE) { + for (i = 0; i < count; i++) { + if (i == RADIO_THROTTLE) { out[i] = (in[i] + MAX_PPRZ) / 2; Bound(out[i], 0, MAX_PPRZ); } else { @@ -56,7 +57,7 @@ void radio_control_impl_event(void (* _received_frame_handler)(void)) { cyrf6936_event(&superbitrf.cyrf6936); superbitrf_event(); - if(superbitrf.rc_frame_available) { + if (superbitrf.rc_frame_available) { radio_control.frame_cpt++; radio_control.time_since_last_frame = 0; radio_control.radio_ok_cpt = 0; diff --git a/sw/airborne/subsystems/sensors/infrared.c b/sw/airborne/subsystems/sensors/infrared.c index 2f124af071..ca3cafb41e 100644 --- a/sw/airborne/subsystems/sensors/infrared.c +++ b/sw/airborne/subsystems/sensors/infrared.c @@ -35,7 +35,8 @@ struct Infrared infrared; /** \brief Initialisation of \a ir structure */ -void infrared_struct_init(void) { +void infrared_struct_init(void) +{ infrared.roll_neutral = IR_ROLL_NEUTRAL_DEFAULT; infrared.pitch_neutral = IR_PITCH_NEUTRAL_DEFAULT; diff --git a/sw/airborne/subsystems/sensors/infrared.h b/sw/airborne/subsystems/sensors/infrared.h index 7c29d9bcda..b728c1fb6a 100644 --- a/sw/airborne/subsystems/sensors/infrared.h +++ b/sw/airborne/subsystems/sensors/infrared.h @@ -147,13 +147,13 @@ struct Infrared { extern struct Infrared infrared; #define UpdateIRValue(_v) { \ - infrared.value.ir1 = (IR_IR1_SIGN)*_v.ir1; \ - infrared.value.ir2 = (IR_IR2_SIGN)*_v.ir2; \ - infrared.value.ir3 = (IR_TOP_SIGN)*_v.ir3; \ - infrared.roll = infrared.lateral_correction * IR_RollOfIrs(infrared.value.ir1, infrared.value.ir2); \ - infrared.pitch = infrared.longitudinal_correction * IR_PitchOfIrs(infrared.value.ir1, infrared.value.ir2); \ - infrared.top = infrared.vertical_correction * IR_TopOfIr(infrared.value.ir3); \ -} + infrared.value.ir1 = (IR_IR1_SIGN)*_v.ir1; \ + infrared.value.ir2 = (IR_IR2_SIGN)*_v.ir2; \ + infrared.value.ir3 = (IR_TOP_SIGN)*_v.ir3; \ + infrared.roll = infrared.lateral_correction * IR_RollOfIrs(infrared.value.ir1, infrared.value.ir2); \ + infrared.pitch = infrared.longitudinal_correction * IR_PitchOfIrs(infrared.value.ir1, infrared.value.ir2); \ + infrared.top = infrared.vertical_correction * IR_TopOfIr(infrared.value.ir3); \ + } // initialization of the infrared structure void infrared_struct_init(void); diff --git a/sw/airborne/subsystems/sensors/infrared_adc.c b/sw/airborne/subsystems/sensors/infrared_adc.c index cf8b7db70e..153a2ad982 100644 --- a/sw/airborne/subsystems/sensors/infrared_adc.c +++ b/sw/airborne/subsystems/sensors/infrared_adc.c @@ -52,11 +52,13 @@ static struct adc_buf buf_ir3; struct Infrared_raw ir_adc; // Standard infrared implementation -void infrared_init(void) { +void infrared_init(void) +{ infrared_adc_init(); } -void infrared_update(void) { +void infrared_update(void) +{ infrared_adc_update(); } @@ -66,7 +68,8 @@ void infrared_event(void) {} /** \brief Initialisation of \a ir */ /** Initialize \a adc_buf_channel */ -void infrared_adc_init(void) { +void infrared_adc_init(void) +{ #if ! (defined SITL || defined HITL) adc_buf_channel(ADC_CHANNEL_IR1, &buf_ir1, ADC_CHANNEL_IR_NB_SAMPLES); adc_buf_channel(ADC_CHANNEL_IR2, &buf_ir2, ADC_CHANNEL_IR_NB_SAMPLES); @@ -86,12 +89,13 @@ void infrared_adc_init(void) { /** \brief Update \a ir_roll and ir_pitch from ADCs or from simulator * message in HITL and SITL modes */ -void infrared_adc_update(void) { +void infrared_adc_update(void) +{ #if ! (defined SITL || defined HITL) - ir_adc.ir1 = (int32_t)(buf_ir1.sum/buf_ir1.av_nb_sample) - IR_ADC_IR1_NEUTRAL; - ir_adc.ir2 = (int32_t)(buf_ir2.sum/buf_ir2.av_nb_sample) - IR_ADC_IR2_NEUTRAL; + ir_adc.ir1 = (int32_t)(buf_ir1.sum / buf_ir1.av_nb_sample) - IR_ADC_IR1_NEUTRAL; + ir_adc.ir2 = (int32_t)(buf_ir2.sum / buf_ir2.av_nb_sample) - IR_ADC_IR2_NEUTRAL; #ifdef ADC_CHANNEL_IR_TOP - ir_adc.ir3 = (int32_t)(buf_ir3.sum/buf_ir3.av_nb_sample) - IR_ADC_TOP_NEUTRAL; + ir_adc.ir3 = (int32_t)(buf_ir3.sum / buf_ir3.av_nb_sample) - IR_ADC_TOP_NEUTRAL; #endif // IR_TOP UpdateIRValue(ir_adc); #endif /* !SITL && !HITL */ diff --git a/sw/airborne/subsystems/sensors/infrared_i2c.c b/sw/airborne/subsystems/sensors/infrared_i2c.c index 95aae006a5..82912dff7b 100644 --- a/sw/airborne/subsystems/sensors/infrared_i2c.c +++ b/sw/airborne/subsystems/sensors/infrared_i2c.c @@ -67,21 +67,25 @@ static uint8_t ir_i2c_hor_status; struct i2c_transaction irh_trans, irv_trans; // Standard infrared implementation -void infrared_init(void) { +void infrared_init(void) +{ infrared_i2c_init(); } -void infrared_update(void) { +void infrared_update(void) +{ infrared_i2c_update(); } -void infrared_event(void) { +void infrared_event(void) +{ infrared_i2cEvent(); } /** Initialisation */ -void infrared_i2c_init( void ) { +void infrared_i2c_init(void) +{ ir_i2c_data_hor_available = FALSE; ir_i2c_data_ver_available = FALSE; ir_i2c_hor_status = IR_I2C_IDLE; @@ -94,7 +98,8 @@ void infrared_i2c_init( void ) { infrared_struct_init(); } -void infrared_i2c_update( void ) { +void infrared_i2c_update(void) +{ #if ! (defined SITL || defined HITL) // IR horizontal if (irh_trans.status == I2CTransDone && ir_i2c_hor_status == IR_I2C_IDLE) { @@ -125,19 +130,20 @@ void infrared_i2c_update( void ) { #define FilterIR(_ir_prev, _ir_next) (((1<ts = 0; aos.traj->ts = 1.; // default to one second @@ -162,7 +182,8 @@ void aos_init(int traj_nb) { }; -void aos_compute_sensors(void) { +void aos_compute_sensors(void) +{ struct FloatRates gyro; RATES_SUM(gyro, aos.imu_rates, aos.gyro_bias); @@ -214,7 +235,8 @@ void aos_compute_sensors(void) { } -void aos_compute_state(void) { +void aos_compute_state(void) +{ aos.time += aos.dt; aos.traj->update_fun(); @@ -222,17 +244,18 @@ void aos_compute_state(void) { } -void aos_run(void) { +void aos_run(void) +{ aos_compute_state(); aos_compute_sensors(); #ifndef DISABLE_ALIGNEMENT if (ahrs.status == AHRS_UNINIT) { ahrs_aligner_run(); - if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) + if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) { ahrs_align(); - } - else { + } + } else { #endif /* DISABLE_ALIGNEMENT */ ahrs_propagate(aos.dt); ahrs_update_accel(); @@ -259,7 +282,7 @@ void aos_run(void) { if (!ahrs_impl.heading_aligned) { ahrs_realign_heading(heading); } else { - RunOnceEvery(100,ahrs_update_heading(heading)); + RunOnceEvery(100, ahrs_update_heading(heading)); } } #endif @@ -275,18 +298,20 @@ void aos_run(void) { -static void traj_static_static_init(void) { +static void traj_static_static_init(void) +{ aos.traj->te = 120.; } -static void traj_static_static_update(void) { +static void traj_static_static_update(void) +{ // if (aos.time > 3) { // EULERS_ASSIGN(aos.ltp_to_imu_euler, RadOfDeg(5), 0, 0); // FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler); - // } + // } // aos.imu_rates.p = 0.; // aos.imu_rates.q = 0.; // aos.imu_rates.r = 0.; @@ -297,18 +322,20 @@ static void traj_static_static_update(void) { // // // -static void traj_static_sine_init(void) { +static void traj_static_sine_init(void) +{ aos.traj->te = 10.; } -static void traj_static_sine_update(void) { +static void traj_static_sine_update(void) +{ - aos.imu_rates.p = RadOfDeg(200)*cos(aos.time); - aos.imu_rates.q = RadOfDeg(200)*cos(0.7*aos.time+2); - aos.imu_rates.r = RadOfDeg(200)*cos(0.8*aos.time+1); + aos.imu_rates.p = RadOfDeg(200) * cos(aos.time); + aos.imu_rates.q = RadOfDeg(200) * cos(0.7 * aos.time + 2); + aos.imu_rates.r = RadOfDeg(200) * cos(0.8 * aos.time + 1); FLOAT_QUAT_INTEGRATE(aos.ltp_to_imu_quat, aos.imu_rates, aos.dt); FLOAT_EULERS_OF_QUAT(aos.ltp_to_imu_euler, aos.ltp_to_imu_quat); @@ -321,26 +348,28 @@ static void traj_static_sine_update(void) { // to achieve it // static void traj_sineX_quad_init(void) { aos.traj->te = 60.; } -static void traj_sineX_quad_update(void) { +static void traj_sineX_quad_update(void) +{ const float om = RadOfDeg(10); - if ( aos.time > (M_PI/om) ) { + if (aos.time > (M_PI / om)) { const float a = 20; struct FloatVect3 jerk; - VECT3_ASSIGN(jerk , -a*om*om*om*cos(om*aos.time), 0, 0); - VECT3_ASSIGN(aos.ltp_accel , -a*om*om *sin(om*aos.time), 0, 0); - VECT3_ASSIGN(aos.ltp_vel , a*om *cos(om*aos.time), 0, 0); - VECT3_ASSIGN(aos.ltp_pos , a *sin(om*aos.time), 0, 0); + VECT3_ASSIGN(jerk , -a * om * om * om * cos(om * aos.time), 0, 0); + VECT3_ASSIGN(aos.ltp_accel , -a * om * om * sin(om * aos.time), 0, 0); + VECT3_ASSIGN(aos.ltp_vel , a * om * cos(om * aos.time), 0, 0); + VECT3_ASSIGN(aos.ltp_pos , a * sin(om * aos.time), 0, 0); // this is based on differential flatness of the quad EULERS_ASSIGN(aos.ltp_to_imu_euler, 0., atan2(aos.ltp_accel.x, 9.81), 0.); FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler); const struct FloatEulers e_dot = { 0., - 9.81*jerk.x / ( (9.81*9.81) + (aos.ltp_accel.x*aos.ltp_accel.x)), - 0. }; + 9.81 * jerk.x / ((9.81 * 9.81) + (aos.ltp_accel.x * aos.ltp_accel.x)), + 0. + }; FLOAT_RATES_OF_EULER_DOT(aos.imu_rates, aos.ltp_to_imu_euler, e_dot); } } @@ -350,7 +379,8 @@ static void traj_sineX_quad_update(void) { // // static void traj_step_phi_init(void) { aos.traj->te = 40.;} -static void traj_step_phi_update(void) { +static void traj_step_phi_update(void) +{ if (aos.time > 5) { EULERS_ASSIGN(aos.ltp_to_imu_euler, RadOfDeg(5), 0, 0); FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler); @@ -360,18 +390,21 @@ static void traj_step_phi_update(void) { // // // -static void traj_step_phi_2nd_order_init(void) { +static void traj_step_phi_2nd_order_init(void) +{ aos.traj->te = 0.; aos.traj->te = 40.; } -static void traj_step_phi_2nd_order_update(void) { +static void traj_step_phi_2nd_order_update(void) +{ if (aos.time > 15) { const float omega = RadOfDeg(100); const float xi = 0.9; struct FloatRates raccel; - RATES_ASSIGN(raccel, -2.*xi*omega*aos.imu_rates.p - omega*omega*(aos.ltp_to_imu_euler.phi - RadOfDeg(5)), 0., 0.); + RATES_ASSIGN(raccel, -2.*xi * omega * aos.imu_rates.p - omega * omega * (aos.ltp_to_imu_euler.phi - RadOfDeg(5)), 0., + 0.); FLOAT_RATES_INTEGRATE_FI(aos.imu_rates, raccel, aos.dt); FLOAT_QUAT_INTEGRATE(aos.ltp_to_imu_quat, aos.imu_rates, aos.dt); FLOAT_EULERS_OF_QUAT(aos.ltp_to_imu_euler, aos.ltp_to_imu_quat); @@ -381,35 +414,38 @@ static void traj_step_phi_2nd_order_update(void) { static void traj_step_biasp_init(void) { aos.traj->te = 120.; } -static void traj_step_biasp_update(void) { if (aos.time > 5) aos.gyro_bias.p = RadOfDeg(3);} +static void traj_step_biasp_update(void) { if (aos.time > 5) { aos.gyro_bias.p = RadOfDeg(3); }} -static void traj_coordinated_circle_init(void) { +static void traj_coordinated_circle_init(void) +{ aos.traj->te = 120.; const float speed = 15; // m/s const float R = 100; // radius in m // tan phi = v^2/Rg - float phi = atan2(speed*speed, R*9.81); + float phi = atan2(speed * speed, R * 9.81); EULERS_ASSIGN(aos.ltp_to_imu_euler, phi, 0, M_PI_2); FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler); } -static void traj_coordinated_circle_update(void) { +static void traj_coordinated_circle_update(void) +{ const float speed = 15; // m/s const float R = 100; // radius in m float omega = speed / R; // tan phi = v^2/Rg - float phi = atan2(speed*speed, R*9.81); - if ( aos.time > 2.*M_PI/omega) { - VECT3_ASSIGN(aos.ltp_pos, R*cos(omega*aos.time), R*sin(omega*aos.time), 0.); - VECT3_ASSIGN(aos.ltp_vel, -omega*R*sin(omega*aos.time), omega*R*cos(omega*aos.time), 0.); - VECT3_ASSIGN(aos.ltp_accel, -omega*omega*R*cos(omega*aos.time), -omega*omega*R*sin(omega*aos.time), 0.); + float phi = atan2(speed * speed, R * 9.81); + if (aos.time > 2.*M_PI / omega) { + VECT3_ASSIGN(aos.ltp_pos, R * cos(omega * aos.time), R * sin(omega * aos.time), 0.); + VECT3_ASSIGN(aos.ltp_vel, -omega * R * sin(omega * aos.time), omega * R * cos(omega * aos.time), 0.); + VECT3_ASSIGN(aos.ltp_accel, -omega * omega * R * cos(omega * aos.time), -omega * omega * R * sin(omega * aos.time), 0.); // float psi = atan2(aos.ltp_pos.y, aos.ltp_pos.x); - float psi = M_PI_2 + omega*aos.time; while (psi>M_PI) psi -= 2.*M_PI; + float psi = M_PI_2 + omega * aos.time; + while (psi > M_PI) { psi -= 2.*M_PI; } EULERS_ASSIGN(aos.ltp_to_imu_euler, phi, 0, psi); FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler); @@ -423,13 +459,14 @@ static void traj_coordinated_circle_update(void) { //static char** traj_stop_stop_x_desc(void) { - // static const char** desc = - // {"stop top", NULL}; +// static const char** desc = +// {"stop top", NULL}; // return desc; //} static void traj_stop_stop_x_init(void) { aos.traj->te = 30.;} -static void traj_stop_stop_x_update(void){ +static void traj_stop_stop_x_update(void) +{ const float t0 = 5.; const float dt_jerk = 0.75; @@ -440,19 +477,20 @@ static void traj_stop_stop_x_update(void){ FLOAT_VECT3_INTEGRATE_FI(aos.ltp_vel, aos.ltp_accel, aos.dt); FLOAT_VECT3_INTEGRATE_FI(aos.ltp_accel, aos.ltp_jerk, aos.dt); - if (aos.time < t0) return; - else if (aos.time < t0+dt_jerk) { - VECT3_ASSIGN(aos.ltp_jerk , val_jerk, 0., 0.); } - else if (aos.time < t0 + 2.*dt_jerk) { - VECT3_ASSIGN(aos.ltp_jerk , -val_jerk, 0., 0.); } - else if (aos.time < t0 + 2.*dt_jerk + dt_nojerk) { - VECT3_ASSIGN(aos.ltp_jerk , 0. , 0., 0.); } - else if (aos.time < t0 + 3.*dt_jerk + dt_nojerk) { - VECT3_ASSIGN(aos.ltp_jerk , -val_jerk, 0., 0.); } - else if (aos.time < t0 + 4.*dt_jerk + dt_nojerk) { - VECT3_ASSIGN(aos.ltp_jerk , val_jerk, 0., 0.); } - else { - VECT3_ASSIGN(aos.ltp_jerk , 0. , 0., 0.); } + if (aos.time < t0) { return; } + else if (aos.time < t0 + dt_jerk) { + VECT3_ASSIGN(aos.ltp_jerk , val_jerk, 0., 0.); + } else if (aos.time < t0 + 2.*dt_jerk) { + VECT3_ASSIGN(aos.ltp_jerk , -val_jerk, 0., 0.); + } else if (aos.time < t0 + 2.*dt_jerk + dt_nojerk) { + VECT3_ASSIGN(aos.ltp_jerk , 0. , 0., 0.); + } else if (aos.time < t0 + 3.*dt_jerk + dt_nojerk) { + VECT3_ASSIGN(aos.ltp_jerk , -val_jerk, 0., 0.); + } else if (aos.time < t0 + 4.*dt_jerk + dt_nojerk) { + VECT3_ASSIGN(aos.ltp_jerk , val_jerk, 0., 0.); + } else { + VECT3_ASSIGN(aos.ltp_jerk , 0. , 0., 0.); + } // this is based on differential flatness of the quad @@ -460,13 +498,15 @@ static void traj_stop_stop_x_update(void){ FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler); const struct FloatEulers e_dot = { 0., - 9.81*aos.ltp_jerk.x / ( (9.81*9.81) + (aos.ltp_accel.x*aos.ltp_accel.x)), - 0. }; + 9.81 * aos.ltp_jerk.x / ((9.81 * 9.81) + (aos.ltp_accel.x * aos.ltp_accel.x)), + 0. + }; FLOAT_RATES_OF_EULER_DOT(aos.imu_rates, aos.ltp_to_imu_euler, e_dot); } -static void traj_bungee_takeoff_init(void) { +static void traj_bungee_takeoff_init(void) +{ aos.traj->te = 40.; EULERS_ASSIGN(aos.ltp_to_imu_euler, 0, RadOfDeg(10), 0); @@ -474,17 +514,17 @@ static void traj_bungee_takeoff_init(void) { } -static void traj_bungee_takeoff_update(void) { +static void traj_bungee_takeoff_update(void) +{ const float initial_bungee_accel = 20.0; // in m/s^2 const float start = 5; const float duration = 2; struct FloatVect3 accel = {0, 0, 0}; //acceleration in imu x-direction in m/s^2 - if (aos.time > start && aos.time < start+duration) { + if (aos.time > start && aos.time < start + duration) { accel.x = initial_bungee_accel * (1 - (aos.time - start) / duration); - } - else { + } else { accel.x = 0; } diff --git a/sw/airborne/test/ahrs/ahrs_on_synth.h b/sw/airborne/test/ahrs/ahrs_on_synth.h index aa94cce520..30b123a89a 100644 --- a/sw/airborne/test/ahrs/ahrs_on_synth.h +++ b/sw/airborne/test/ahrs/ahrs_on_synth.h @@ -4,8 +4,8 @@ #include "math/pprz_algebra_float.h" struct traj { - char* name; - char* desc; + char *name; + char *desc; void (*init_fun)(void); void (*update_fun)(void); @@ -15,7 +15,7 @@ struct traj { struct AhrsOnSynth { - struct traj* traj; + struct traj *traj; double time; double dt; @@ -58,6 +58,6 @@ extern void aos_compute_state(void); #define AHRS_TYPE_FCQ 5 #define AHRS_TYPE_NB 6 -extern char* ahrs_type_str[AHRS_TYPE_NB]; +extern char *ahrs_type_str[AHRS_TYPE_NB]; #endif /* AHRS_ON_SYNTH_H */ 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 c107a2c3dd..c33ac19c74 100644 --- a/sw/airborne/test/ahrs/run_ahrs_on_flight_log.c +++ b/sw/airborne/test/ahrs/run_ahrs_on_flight_log.c @@ -65,10 +65,10 @@ static struct test_output output[MAX_SAMPLE]; #define IN_FILE "/home/poine/work/savannah/paparazzi3.broken_stm32_deletion/trunk/sw/simulator/scilab/q6d/data/stop_stop_sensors.txt" -static void read_ascii_flight_log(const char* filename); +static void read_ascii_flight_log(const char *filename); static void feed_imu(int i); static void store_filter_output(int i); -static void dump_output(const char* filename); +static void dump_output(const char *filename); /* from fms_autopilot_msg.h */ #define VI_IMU_DATA_VALID 0 @@ -76,29 +76,32 @@ static void dump_output(const char* filename); #define IMU_AVAILABLE(_flag) (_flag & (1<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); + } while (ret == 18 && nb_samples < MAX_SAMPLE); nb_samples--; fclose(fd); printf("read %d points in file %s\n", nb_samples, filename); @@ -137,11 +140,11 @@ static void read_ascii_flight_log(const char* filename) { /* * */ -static void feed_imu(int i) { - if (i>0) { +static void feed_imu(int i) +{ + if (i > 0) { RATES_COPY(imu.gyro_prev, imu.gyro); - } - else { + } else { RATES_BFP_OF_REAL(imu.gyro_prev, samples[0].gyro); } RATES_BFP_OF_REAL(imu.gyro, samples[i].gyro); @@ -153,7 +156,8 @@ static void feed_imu(int i) { * */ #if defined AHRS_TYPE && AHRS_TYPE == AHRS_TYPE_FLQ -static void store_filter_output(int i) { +static void store_filter_output(int i) +{ #ifdef OUTPUT_IN_BODY_FRAME QUAT_COPY(output[i].quat_est, ahrs_impl.ltp_to_body_quat); RATES_COPY(output[i].rate_est, ahrs_impl.body_rate); @@ -165,7 +169,8 @@ static void store_filter_output(int i) { memcpy(output[i].P, ahrs_impl.P, sizeof(ahrs_impl.P)); } #elif defined AHRS_TYPE && AHRS_TYPE == AHRS_TYPE_FCR -static void store_filter_output(int i) { +static void store_filter_output(int i) +{ #ifdef OUTPUT_IN_BODY_FRAME QUAT_COPY(output[i].quat_est, ahrs_impl.ltp_to_body_quat); RATES_COPY(output[i].rate_est, ahrs_impl.body_rate); @@ -177,7 +182,8 @@ static void store_filter_output(int i) { // memcpy(output[i].P, ahrs_impl.P, sizeof(ahrs_impl.P)); } #elif defined AHRS_TYPE && AHRS_TYPE == AHRS_TYPE_ICE -static void store_filter_output(int i) { +static void store_filter_output(int i) +{ #ifdef OUTPUT_IN_BODY_FRAME QUAT_FLOAT_OF_BFP(output[i].quat_est, ahrs_impl.ltp_to_body_quat); RATES_FLOAT_OF_BFP(output[i].rate_est, ahrs_impl.body_rate); @@ -195,17 +201,19 @@ static void store_filter_output(int i) { /* * */ -static void dump_output(const char* filename) { - FILE* fd = fopen(filename, "w"); +static void dump_output(const char *filename) +{ + FILE *fd = fopen(filename, "w"); int i; - for (i=0; i 1) + if (argc > 1) { traj = atoi(argv[1]); + } aos_init(traj); report(); @@ -32,7 +34,8 @@ int main(int argc, char** argv) { } -static void report(void) { +static void report(void) +{ int output_sensors = FALSE; int output_pos = FALSE; @@ -40,16 +43,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 +72,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 +86,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.y, + 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 d042c0f673..252e5c8b4a 100644 --- a/sw/airborne/test/ahrs/run_ahrs_on_synth_ivy.c +++ b/sw/airborne/test/ahrs/run_ahrs_on_synth_ivy.c @@ -19,9 +19,10 @@ -gboolean timeout_callback(gpointer data) { +gboolean timeout_callback(gpointer data) +{ - for (int i=0; i<20; i++) { + for (int i = 0; i < 20; i++) { aos_compute_state(); aos_compute_sensors(); #ifndef DISABLE_PROPAGATE @@ -31,46 +32,46 @@ gboolean timeout_callback(gpointer data) { ahrs_update_accel(); #endif #ifndef DISABLE_MAG_UPDATE - if (!(i%5)) ahrs_update_mag(); + if (!(i % 5)) { ahrs_update_mag(); } #endif } #if AHRS_TYPE == AHRS_TYPE_ICE || AHRS_TYPE == AHRS_TYPE_ICQ - EULERS_FLOAT_OF_BFP(ahrs_float.ltp_to_imu_euler, ahrs.ltp_to_imu_euler); + EULERS_FLOAT_OF_BFP(ahrs_float.ltp_to_imu_euler, ahrs.ltp_to_imu_euler); #endif #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); + IvySendMsg("183 AHRS_GYRO_BIAS_INT %d %d %d", + 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); + 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); #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; } @@ -79,15 +80,16 @@ gboolean timeout_callback(gpointer data) { -int main ( int argc, char** argv) { +int main(int argc, char **argv) +{ printf("hello\n"); - g_timeout_add(1000/25, timeout_callback, NULL); + g_timeout_add(1000 / 25, timeout_callback, NULL); GMainLoop *ml = g_main_loop_new(NULL, FALSE); - IvyInit ("test_ahrs", "test_ahrs READY", NULL, NULL, NULL, NULL); + IvyInit("test_ahrs", "test_ahrs READY", NULL, NULL, NULL, NULL); IvyStart("127.255.255.255"); imu_init(); diff --git a/sw/airborne/test/generic_uart_tunnel.c b/sw/airborne/test/generic_uart_tunnel.c index 09b588d29e..ff94a261ff 100644 --- a/sw/airborne/test/generic_uart_tunnel.c +++ b/sw/airborne/test/generic_uart_tunnel.c @@ -27,26 +27,30 @@ #include "mcu_periph/sys_time.h" #include "led.h" -static inline void main_init( void ); -static inline void main_periodic_task( void ); -static inline void main_event_task( void ); +static inline void main_init(void); +static inline void main_periodic_task(void); +static inline void main_event_task(void); -int main( void ) { +int main(void) +{ main_init(); - while(1) { - if (sys_time_check_and_ack_timer(0)) + while (1) { + if (sys_time_check_and_ack_timer(0)) { main_periodic_task(); + } main_event_task(); } return 0; } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); } -static inline void main_periodic_task( void ) { +static inline void main_periodic_task(void) +{ RunOnceEvery(100, { LED_TOGGLE(4); LED_PERIODIC(); @@ -54,12 +58,15 @@ static inline void main_periodic_task( void ) { } -static inline void main_event_task( void ) { +static inline void main_event_task(void) +{ - if (uart_char_available(&uart2)) + if (uart_char_available(&uart2)) { uart_transmit(&uart1, uart_getch(&uart2)); + } - if (uart_char_available(&uart1)) + if (uart_char_available(&uart1)) { uart_transmit(&uart2, uart_getch(&uart1)); + } } diff --git a/sw/airborne/test/mcu_periph/test_adc.c b/sw/airborne/test/mcu_periph/test_adc.c index f446d48acf..b75c264b44 100644 --- a/sw/airborne/test/mcu_periph/test_adc.c +++ b/sw/airborne/test/mcu_periph/test_adc.c @@ -29,52 +29,54 @@ #include "subsystems/datalink/downlink.h" int main_periodic(void); -static inline void main_init( void ); -static inline void main_periodic_task( void ); -static inline void main_event_task( void ); +static inline void main_init(void); +static inline void main_periodic_task(void); +static inline void main_event_task(void); #define NB_ADC 8 #define ADC_NB_SAMPLES 16 static struct adc_buf buf_adc[NB_ADC]; -static inline void main_init( void ) { - mcu_init(); - sys_time_register_timer((1./100), NULL); - adc_init(); +static inline void main_init(void) +{ + mcu_init(); + sys_time_register_timer((1. / 100), NULL); + adc_init(); #ifdef ADC_0 - adc_buf_channel(ADC_0, &buf_adc[0], ADC_NB_SAMPLES); + adc_buf_channel(ADC_0, &buf_adc[0], ADC_NB_SAMPLES); #endif #ifdef ADC_1 - adc_buf_channel(ADC_1, &buf_adc[1], ADC_NB_SAMPLES); + adc_buf_channel(ADC_1, &buf_adc[1], ADC_NB_SAMPLES); #endif #ifdef ADC_2 - adc_buf_channel(ADC_2, &buf_adc[2], ADC_NB_SAMPLES); + adc_buf_channel(ADC_2, &buf_adc[2], ADC_NB_SAMPLES); #endif #ifdef ADC_3 - adc_buf_channel(ADC_3, &buf_adc[3], ADC_NB_SAMPLES); + adc_buf_channel(ADC_3, &buf_adc[3], ADC_NB_SAMPLES); #endif #ifdef ADC_4 - adc_buf_channel(ADC_4, &buf_adc[4], ADC_NB_SAMPLES); + adc_buf_channel(ADC_4, &buf_adc[4], ADC_NB_SAMPLES); #endif #ifdef ADC_5 - adc_buf_channel(ADC_5, &buf_adc[5], ADC_NB_SAMPLES); + adc_buf_channel(ADC_5, &buf_adc[5], ADC_NB_SAMPLES); #endif #ifdef ADC_6 - adc_buf_channel(ADC_6, &buf_adc[6], ADC_NB_SAMPLES); + adc_buf_channel(ADC_6, &buf_adc[6], ADC_NB_SAMPLES); #endif #ifdef ADC_7 - adc_buf_channel(ADC_7, &buf_adc[7], ADC_NB_SAMPLES); + adc_buf_channel(ADC_7, &buf_adc[7], ADC_NB_SAMPLES); #endif - mcu_int_enable(); + mcu_int_enable(); } -int main( void ) { +int main(void) +{ main_init(); - while(1) { + while (1) { if (sys_time_check_and_ack_timer(0)) { main_periodic_task(); } @@ -83,20 +85,23 @@ int main( void ) { return 0; } -static inline void main_periodic_task( void ) { +static inline void main_periodic_task(void) +{ RunOnceEvery(100, {DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);}); RunOnceEvery(100, {uint32_t sec = sys_time.nb_sec; DOWNLINK_SEND_TIME(DefaultChannel, DefaultDevice, &sec);}); LED_PERIODIC(); uint16_t values[NB_ADC]; uint8_t i; - for(i = 0; i < NB_ADC; i++) + for (i = 0; i < NB_ADC; i++) { values[i] = buf_adc[i].sum / ADC_NB_SAMPLES; + } uint8_t id = 42; DOWNLINK_SEND_ADC(DefaultChannel, DefaultDevice, &id, NB_ADC, values); } -static inline void main_event_task( void ) { +static inline void main_event_task(void) +{ } diff --git a/sw/airborne/test/mcu_periph/test_gpio.c b/sw/airborne/test/mcu_periph/test_gpio.c index d259125026..21d0f753ce 100644 --- a/sw/airborne/test/mcu_periph/test_gpio.c +++ b/sw/airborne/test/mcu_periph/test_gpio.c @@ -39,17 +39,20 @@ PRINT_CONFIG_VAR(TEST_GPIO2) /* * Called from the systime interrupt handler */ -static inline void main_periodic(uint8_t id __attribute__((unused))) { +static inline void main_periodic(uint8_t id __attribute__((unused))) +{ gpio_toggle(TEST_GPIO1); } -static inline void main_periodic_2(void) { +static inline void main_periodic_2(void) +{ gpio_toggle(TEST_GPIO2); } -int main(void) { +int main(void) +{ // not calling mcu_init with PERIPHERALS_AUTO_INIT // rather explicitly init only sys_time @@ -64,7 +67,7 @@ int main(void) { mcu_int_enable(); - while(1) { + while (1) { if (sys_time_check_and_ack_timer(tmr_2)) { main_periodic_2(); } diff --git a/sw/airborne/test/mcu_periph/test_sys_time_timer.c b/sw/airborne/test/mcu_periph/test_sys_time_timer.c index 65cf01a76f..ac4de4f641 100644 --- a/sw/airborne/test/mcu_periph/test_sys_time_timer.c +++ b/sw/airborne/test/mcu_periph/test_sys_time_timer.c @@ -25,12 +25,13 @@ #include "mcu_periph/sys_time.h" #include "interrupt_hw.h" -static inline void main_periodic_02( void ); -static inline void main_periodic_03( void ); -static inline void main_periodic_05( uint8_t id ); -static inline void main_event( void ); +static inline void main_periodic_02(void); +static inline void main_periodic_03(void); +static inline void main_periodic_05(uint8_t id); +static inline void main_event(void); -int main(void) { +int main(void) +{ mcu_init(); unsigned int tmr_02 = sys_time_register_timer(0.2, NULL); @@ -39,11 +40,13 @@ int main(void) { mcu_int_enable(); - while(1) { - if (sys_time_check_and_ack_timer(tmr_02)) + while (1) { + if (sys_time_check_and_ack_timer(tmr_02)) { main_periodic_02(); - if (sys_time_check_and_ack_timer(tmr_03)) + } + if (sys_time_check_and_ack_timer(tmr_03)) { main_periodic_03(); + } main_event(); } @@ -53,27 +56,31 @@ int main(void) { /* Called from main loop polling */ -static inline void main_periodic_02( void ) { +static inline void main_periodic_02(void) +{ #ifdef LED_GREEN - LED_TOGGLE(LED_GREEN); + LED_TOGGLE(LED_GREEN); #endif } -static inline void main_periodic_03( void ) { +static inline void main_periodic_03(void) +{ #ifdef LED_BLUE - LED_TOGGLE(LED_BLUE); + LED_TOGGLE(LED_BLUE); #endif } /* Called from the systime interrupt handler */ -static inline void main_periodic_05( __attribute__((unused)) uint8_t id ) { +static inline void main_periodic_05(__attribute__((unused)) uint8_t id) +{ #ifdef LED_RED - LED_TOGGLE(LED_RED); + LED_TOGGLE(LED_RED); #endif } -static inline void main_event( void ) { +static inline void main_event(void) +{ } diff --git a/sw/airborne/test/mcu_periph/test_sys_time_usleep.c b/sw/airborne/test/mcu_periph/test_sys_time_usleep.c index 887758ed0a..89c728a1d5 100644 --- a/sw/airborne/test/mcu_periph/test_sys_time_usleep.c +++ b/sw/airborne/test/mcu_periph/test_sys_time_usleep.c @@ -29,14 +29,15 @@ static inline void main_periodic_1(void); static inline void main_periodic_15(void); static inline void main_periodic_05(uint8_t id); -int main(void) { +int main(void) +{ mcu_init(); sys_time_register_timer(0.5, main_periodic_05); mcu_int_enable(); - while(1) { + while (1) { /* sleep for 1s */ sys_time_usleep(1000000); main_periodic_1(); @@ -52,23 +53,26 @@ int main(void) { /* * Called from main loop polling */ -static inline void main_periodic_1(void) { +static inline void main_periodic_1(void) +{ #ifdef LED_GREEN - LED_TOGGLE(LED_GREEN); + LED_TOGGLE(LED_GREEN); #endif } -static inline void main_periodic_15(void) { +static inline void main_periodic_15(void) +{ #ifdef LED_BLUE - LED_TOGGLE(LED_BLUE); + LED_TOGGLE(LED_BLUE); #endif } /* * Called from the systime interrupt handler */ -static inline void main_periodic_05(uint8_t id __attribute__((unused))) { +static inline void main_periodic_05(uint8_t id __attribute__((unused))) +{ #ifdef LED_RED - LED_TOGGLE(LED_RED); + LED_TOGGLE(LED_RED); #endif } diff --git a/sw/airborne/test/mcu_periph/test_uart.c b/sw/airborne/test/mcu_periph/test_uart.c index 0a2bb53675..be0d49a882 100644 --- a/sw/airborne/test/mcu_periph/test_uart.c +++ b/sw/airborne/test/mcu_periph/test_uart.c @@ -25,27 +25,31 @@ #include "mcu_periph/sys_time.h" #include "led.h" -static inline void main_init( void ); -static inline void main_periodic( void ); +static inline void main_init(void); +static inline void main_periodic(void); -int main(void) { +int main(void) +{ main_init(); while (1) { - if (sys_time_check_and_ack_timer(0)) + if (sys_time_check_and_ack_timer(0)) { main_periodic(); + } } return 0; } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); } -static inline void main_periodic( void ) { +static inline void main_periodic(void) +{ char ch; #if USE_UART1 diff --git a/sw/airborne/test/peripherals/test_ami601.c b/sw/airborne/test/peripherals/test_ami601.c index ad250aea17..72b5ea4a13 100644 --- a/sw/airborne/test/peripherals/test_ami601.c +++ b/sw/airborne/test/peripherals/test_ami601.c @@ -35,25 +35,28 @@ #include "std.h" -static inline void main_init( void ); -static inline void main_periodic_task( void ); -static inline void main_event_task( void ); +static inline void main_init(void); +static inline void main_periodic_task(void); +static inline void main_event_task(void); static inline void on_mag(void); -int main( void ) { +int main(void) +{ main_init(); - while(1) { - if (sys_time_check_and_ack_timer(0)) + while (1) { + if (sys_time_check_and_ack_timer(0)) { main_periodic_task(); + } main_event_task(); } return 0; } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); LED_ON(4); ami601_init(); @@ -61,19 +64,22 @@ static inline void main_init( void ) { mcu_int_enable(); } -static inline void main_periodic_task( void ) { +static inline void main_periodic_task(void) +{ // RunOnceEvery(100, {DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);}); RunOnceEvery(10, { ami601_read();}); } -static inline void main_event_task( void ) { +static inline void main_event_task(void) +{ AMI601Event(on_mag); } -static inline void on_mag(void) { +static inline void on_mag(void) +{ LED_TOGGLE(4); ami601_status = AMI601_IDLE; struct Int32Vect3 bla = {ami601_values[0], ami601_values[1], ami601_values[2]}; diff --git a/sw/airborne/test/peripherals/test_lis302dl_spi.c b/sw/airborne/test/peripherals/test_lis302dl_spi.c index e08913cfa3..8504ae520b 100644 --- a/sw/airborne/test/peripherals/test_lis302dl_spi.c +++ b/sw/airborne/test/peripherals/test_lis302dl_spi.c @@ -46,16 +46,18 @@ PRINT_CONFIG_VAR(LIS302DL_SPI_SLAVE_IDX) struct Lis302dl_Spi lis302; -static inline void main_init( void ); -static inline void main_periodic_task( void ); -static inline void main_event_task( void ); +static inline void main_init(void); +static inline void main_periodic_task(void); +static inline void main_event_task(void); -int main(void) { +int main(void) +{ main_init(); - while(1) { - if (sys_time_check_and_ack_timer(0)) + while (1) { + if (sys_time_check_and_ack_timer(0)) { main_periodic_task(); + } main_event_task(); } @@ -63,16 +65,18 @@ int main(void) { } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); mcu_int_enable(); - sys_time_register_timer((1./50), NULL); + sys_time_register_timer((1. / 50), NULL); lis302dl_spi_init(&lis302, &(LIS302DL_SPI_DEV), LIS302DL_SPI_SLAVE_IDX); } -static inline void main_periodic_task( void ) { +static inline void main_periodic_task(void) +{ RunOnceEvery(100, DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM)); if (sys_time.nb_sec > 1) { @@ -83,9 +87,11 @@ static inline void main_periodic_task( void ) { } } -static inline void main_event_task( void ) { - if (sys_time.nb_sec > 1) +static inline void main_event_task(void) +{ + if (sys_time.nb_sec > 1) { lis302dl_spi_event(&lis302); + } if (lis302.data_available) { struct Int32Vect3 accel; @@ -93,11 +99,11 @@ static inline void main_event_task( void ) { lis302.data_available = FALSE; RunOnceEvery(10, { - DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, - &accel.x, &accel.y, &accel.z); + DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, + &accel.x, &accel.y, &accel.z); #if USE_LED_6 - LED_TOGGLE(6); + LED_TOGGLE(6); #endif - }); + }); } } diff --git a/sw/airborne/test/peripherals/test_ms2100.c b/sw/airborne/test/peripherals/test_ms2100.c index 51059fa4a4..6bc6ef4ac4 100644 --- a/sw/airborne/test/peripherals/test_ms2100.c +++ b/sw/airborne/test/peripherals/test_ms2100.c @@ -28,16 +28,18 @@ #include "peripherals/ms2100.h" #include "led.h" -static inline void main_init( void ); -static inline void main_periodic_task( void ); -static inline void main_event_task( void ); +static inline void main_init(void); +static inline void main_periodic_task(void); +static inline void main_event_task(void); -int main(void) { +int main(void) +{ main_init(); - while(1) { - if (sys_time_check_and_ack_timer(0)) + while (1) { + if (sys_time_check_and_ack_timer(0)) { main_periodic_task(); + } main_event_task(); } @@ -45,38 +47,40 @@ int main(void) { } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); - sys_time_register_timer((1./50), NULL); + sys_time_register_timer((1. / 50), NULL); ms2100_init(&ms2100, &(MS2100_SPI_DEV), MS2100_SLAVE_IDX); mcu_int_enable(); } -static inline void main_periodic_task( void ) { - RunOnceEvery(10, - { - uint16_t foo = sys_time.nb_sec; - DOWNLINK_SEND_BOOT(DefaultChannel, DefaultDevice, &foo); - LED_TOGGLE(2); - LED_PERIODIC(); - }); +static inline void main_periodic_task(void) +{ + RunOnceEvery(10, { + uint16_t foo = sys_time.nb_sec; + DOWNLINK_SEND_BOOT(DefaultChannel, DefaultDevice, &foo); + LED_TOGGLE(2); + LED_PERIODIC(); + }); ms2100_periodic(&ms2100); } -static inline void main_event_task( void ) { +static inline void main_event_task(void) +{ ms2100_event(&ms2100); if (ms2100.status == MS2100_DATA_AVAILABLE) { RunOnceEvery(10, { - int32_t mag_x = ms2100.data.vect.x; - int32_t mag_y = ms2100.data.vect.y; - int32_t mag_z = ms2100.data.vect.z; - DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, - &mag_x, &mag_y, &mag_z); - }); + int32_t mag_x = ms2100.data.vect.x; + int32_t mag_y = ms2100.data.vect.y; + int32_t mag_z = ms2100.data.vect.z; + DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, + &mag_x, &mag_y, &mag_z); + }); ms2100.status = MS2100_IDLE; } } diff --git a/sw/airborne/test/pprz_algebra_print.h b/sw/airborne/test/pprz_algebra_print.h index 744a431e83..5fb95a832b 100644 --- a/sw/airborne/test/pprz_algebra_print.h +++ b/sw/airborne/test/pprz_algebra_print.h @@ -4,59 +4,59 @@ #include -#define DISPLAY_FLOAT_VECT3(text, _v) { \ - printf("%s %f %f %f\n",text, (_v).x, (_v).y, (_v).z); \ +#define DISPLAY_FLOAT_VECT3(text, _v) { \ + printf("%s %f %f %f\n",text, (_v).x, (_v).y, (_v).z); \ } -#define DISPLAY_FLOAT_RATES(text, _v) { \ - printf("%s %f %f %f\n",text, (_v).p, (_v).q, (_v).r); \ +#define DISPLAY_FLOAT_RATES(text, _v) { \ + printf("%s %f %f %f\n",text, (_v).p, (_v).q, (_v).r); \ } -#define DISPLAY_FLOAT_RATES_DEG(text, _v) { \ +#define DISPLAY_FLOAT_RATES_DEG(text, _v) { \ printf("%s %f %f %f\n",text, DegOfRad((_v).p), DegOfRad((_v).q), DegOfRad((_v).r)); \ } -#define DISPLAY_FLOAT_RMAT(text, mat) { \ +#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) { \ - printf("%s %f %f %f\n",text, (_e).phi, (_e).theta, (_e).psi); \ +#define DISPLAY_FLOAT_EULERS(text, _e) { \ + printf("%s %f %f %f\n",text, (_e).phi, (_e).theta, (_e).psi); \ } -#define DISPLAY_FLOAT_EULERS_DEG(text, _e) { \ - printf("%s %f %f %f\n",text, DegOfRad((_e).phi), \ - DegOfRad((_e).theta), DegOfRad((_e).psi)); \ +#define DISPLAY_FLOAT_EULERS_DEG(text, _e) { \ + printf("%s %f %f %f\n",text, DegOfRad((_e).phi), \ + DegOfRad((_e).theta), DegOfRad((_e).psi)); \ } -#define DISPLAY_FLOAT_QUAT(text, quat) { \ - float quat_norm = FLOAT_QUAT_NORM(quat); \ +#define DISPLAY_FLOAT_QUAT(text, quat) { \ + float quat_norm = FLOAT_QUAT_NORM(quat); \ printf("%s %f %f %f %f (%f)\n",text, quat.qi, quat.qx, quat.qy, quat.qz, quat_norm); \ } -#define DISPLAY_FLOAT_QUAT_AS_INT(text, quat) { \ - float quat_norm = FLOAT_QUAT_NORM(quat); \ - struct Int32Quat qi; \ - QUAT_BFP_OF_REAL(qi, quat); \ +#define DISPLAY_FLOAT_QUAT_AS_INT(text, quat) { \ + float quat_norm = FLOAT_QUAT_NORM(quat); \ + struct Int32Quat qi; \ + QUAT_BFP_OF_REAL(qi, quat); \ printf("%s %d %d %d %d (%f)\n",text, qi.qi, qi.qx, qi.qy, qi.qz, quat_norm); \ } -#define DISPLAY_FLOAT_QUAT_AS_EULERS_DEG(text, quat) { \ - struct FloatEulers _fe; \ - float_eulers_of_quat(&_fe, &quat); \ - DISPLAY_FLOAT_EULERS_DEG(text, _fe); \ +#define DISPLAY_FLOAT_QUAT_AS_EULERS_DEG(text, quat) { \ + struct FloatEulers _fe; \ + float_eulers_of_quat(&_fe, &quat); \ + DISPLAY_FLOAT_EULERS_DEG(text, _fe); \ } -#define DISPLAY_FLOAT_RMAT_AS_EULERS_DEG(text, _mat) { \ - struct FloatEulers _fe; \ - float_eulers_of_rmat(&_fe, &(_mat)); \ - DISPLAY_FLOAT_EULERS_DEG(text, _fe); \ +#define DISPLAY_FLOAT_RMAT_AS_EULERS_DEG(text, _mat) { \ + struct FloatEulers _fe; \ + float_eulers_of_rmat(&_fe, &(_mat)); \ + DISPLAY_FLOAT_EULERS_DEG(text, _fe); \ } @@ -64,98 +64,98 @@ -#define DISPLAY_INT32_VECT3(text, _v) { \ - int32_t norm = INT32_VECT3_NORM(_v); \ - printf("%s %d %d %d (%d)\n",text, (_v).x, (_v).y, (_v).z, norm); \ +#define DISPLAY_INT32_VECT3(text, _v) { \ + int32_t norm = INT32_VECT3_NORM(_v); \ + printf("%s %d %d %d (%d)\n",text, (_v).x, (_v).y, (_v).z, norm); \ } -#define DISPLAY_INT32_RATES(text, _v) { \ - printf("%s %d %d %d\n",text, (_v).p, (_v).q, (_v).r); \ +#define DISPLAY_INT32_RATES(text, _v) { \ + printf("%s %d %d %d\n",text, (_v).p, (_v).q, (_v).r); \ } -#define DISPLAY_INT32_RATES_AS_FLOAT(text, _r) { \ - struct FloatRates _fr; \ - RATES_FLOAT_OF_BFP(_fr, (_r)); \ - DISPLAY_FLOAT_RATES(text, _fr); \ +#define DISPLAY_INT32_RATES_AS_FLOAT(text, _r) { \ + struct FloatRates _fr; \ + RATES_FLOAT_OF_BFP(_fr, (_r)); \ + DISPLAY_FLOAT_RATES(text, _fr); \ } -#define DISPLAY_INT32_RATES_AS_FLOAT_DEG(text, _r) { \ - struct FloatRates _fr; \ - RATES_FLOAT_OF_BFP(_fr, (_r)); \ - DISPLAY_FLOAT_RATES_DEG(text, _fr); \ +#define DISPLAY_INT32_RATES_AS_FLOAT_DEG(text, _r) { \ + struct FloatRates _fr; \ + RATES_FLOAT_OF_BFP(_fr, (_r)); \ + DISPLAY_FLOAT_RATES_DEG(text, _fr); \ } -#define DISPLAY_INT32_EULERS(text, _e) { \ - printf("%s %d %d %d\n",text, (_e).phi, (_e).theta, (_e).psi); \ +#define DISPLAY_INT32_EULERS(text, _e) { \ + printf("%s %d %d %d\n",text, (_e).phi, (_e).theta, (_e).psi); \ } -#define DISPLAY_INT32_EULERS_AS_FLOAT(text, _ie) { \ - struct FloatEulers _fe; \ - EULERS_FLOAT_OF_BFP(_fe, (_ie)); \ - DISPLAY_FLOAT_EULERS(text, _fe); \ +#define DISPLAY_INT32_EULERS_AS_FLOAT(text, _ie) { \ + struct FloatEulers _fe; \ + EULERS_FLOAT_OF_BFP(_fe, (_ie)); \ + DISPLAY_FLOAT_EULERS(text, _fe); \ } -#define DISPLAY_INT32_EULERS_AS_FLOAT_DEG(text, _ie) { \ - struct FloatEulers _fe; \ - EULERS_FLOAT_OF_BFP(_fe, (_ie)); \ - DISPLAY_FLOAT_EULERS_DEG(text, _fe); \ +#define DISPLAY_INT32_EULERS_AS_FLOAT_DEG(text, _ie) { \ + struct FloatEulers _fe; \ + EULERS_FLOAT_OF_BFP(_fe, (_ie)); \ + DISPLAY_FLOAT_EULERS_DEG(text, _fe); \ } -#define DISPLAY_INT32_QUAT(text, quat) { \ - int32_t quat_norm; \ - INT32_QUAT_NORM(quat_norm, quat); \ +#define DISPLAY_INT32_QUAT(text, quat) { \ + int32_t quat_norm; \ + INT32_QUAT_NORM(quat_norm, quat); \ printf("%s %d %d %d %d (%d)\n",text, quat.qi, quat.qx, quat.qy, quat.qz, quat_norm); \ } -#define DISPLAY_INT32_QUAT_2(text, quat) { \ - 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< -#define MAT_PRINT(_i, _j, A) { \ - for (int i=0; i<_i; i++) { \ - for (int j=0; j<_j; j++) \ - printf("%f ", A[i][j]); \ - printf("\n"); \ - } \ +#define MAT_PRINT(_i, _j, A) { \ + for (int i=0; i<_i; i++) { \ + for (int j=0; j<_j; j++) \ + printf("%f ", A[i][j]); \ + printf("\n"); \ + } \ } diff --git a/sw/airborne/test/subsystems/test_ahrs.c b/sw/airborne/test/subsystems/test_ahrs.c index 8781242dd9..f776bf227d 100644 --- a/sw/airborne/test/subsystems/test_ahrs.c +++ b/sw/airborne/test/subsystems/test_ahrs.c @@ -141,11 +141,11 @@ static inline void on_mag_event(void) static inline void main_report(void) { - PeriodicPrescaleBy10( { + PeriodicPrescaleBy10({ DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, - &imu.accel_unscaled.x, - &imu.accel_unscaled.y, - &imu.accel_unscaled.z); + &imu.accel_unscaled.x, + &imu.accel_unscaled.y, + &imu.accel_unscaled.z); }, { DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, &imu.gyro_unscaled.p, @@ -153,9 +153,9 @@ static inline void main_report(void) &imu.gyro_unscaled.r); }, { DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, - &imu.mag_unscaled.x, - &imu.mag_unscaled.y, - &imu.mag_unscaled.z); + &imu.mag_unscaled.x, + &imu.mag_unscaled.y, + &imu.mag_unscaled.z); }, { DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, DefaultDevice, &imu.accel.x, @@ -163,16 +163,16 @@ static inline void main_report(void) &imu.accel.z); }, { DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, DefaultDevice, - &imu.gyro.p, - &imu.gyro.q, - &imu.gyro.r); + &imu.gyro.p, + &imu.gyro.q, + &imu.gyro.r); }, { DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, DefaultDevice, - &imu.mag.x, - &imu.mag.y, - &imu.mag.z); + &imu.mag.x, + &imu.mag.y, + &imu.mag.z); }, { @@ -209,25 +209,25 @@ static inline void main_report(void) float_eulers_of_quat(<p_to_imu_euler, &ahrs_impl.ltp_to_imu_quat); struct Int32Eulers euler_i; EULERS_BFP_OF_REAL(euler_i, ltp_to_imu_euler); - struct Int32Eulers* eulers_body = stateGetNedToBodyEulers_i(); + struct Int32Eulers *eulers_body = stateGetNedToBodyEulers_i(); DOWNLINK_SEND_AHRS_EULER_INT(DefaultChannel, DefaultDevice, - &euler_i.phi, - &euler_i.theta, - &euler_i.psi, - &(eulers_body->phi), - &(eulers_body->theta), - &(eulers_body->psi)); + &euler_i.phi, + &euler_i.theta, + &euler_i.psi, + &(eulers_body->phi), + &(eulers_body->theta), + &(eulers_body->psi)); #else struct Int32Eulers ltp_to_imu_euler; int32_eulers_of_quat(<p_to_imu_euler, &ahrs_impl.ltp_to_imu_quat); - struct Int32Eulers* eulers = stateGetNedToBodyEulers_i(); + struct Int32Eulers *eulers = stateGetNedToBodyEulers_i(); DOWNLINK_SEND_AHRS_EULER_INT(DefaultChannel, DefaultDevice, - <p_to_imu_euler.phi, - <p_to_imu_euler.theta, - <p_to_imu_euler.psi, - &(eulers->phi), - &(eulers->theta), - &(eulers->psi)); + <p_to_imu_euler.phi, + <p_to_imu_euler.theta, + <p_to_imu_euler.psi, + &(eulers->phi), + &(eulers->theta), + &(eulers->psi)); #endif }, { #ifndef AHRS_FLOAT diff --git a/sw/airborne/test/subsystems/test_imu.c b/sw/airborne/test/subsystems/test_imu.c index 07326f0889..dbc1f69695 100644 --- a/sw/airborne/test/subsystems/test_imu.c +++ b/sw/airborne/test/subsystems/test_imu.c @@ -37,144 +37,150 @@ #include "interrupt_hw.h" -static inline void main_init( void ); -static inline void main_periodic_task( void ); -static inline void main_event_task( void ); +static inline void main_init(void); +static inline void main_periodic_task(void); +static inline void main_event_task(void); static inline void on_gyro_event(void); static inline void on_accel_event(void); static inline void on_mag_event(void); -int main( void ) { +int main(void) +{ main_init(); - while(1) { - if (sys_time_check_and_ack_timer(0)) + while (1) { + if (sys_time_check_and_ack_timer(0)) { main_periodic_task(); + } main_event_task(); } return 0; } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); imu_init(); mcu_int_enable(); } -static inline void led_toggle ( void ) { +static inline void led_toggle(void) +{ #ifdef BOARD_LISA_L LED_TOGGLE(7); #endif } -static inline void main_periodic_task( void ) { +static inline void main_periodic_task(void) +{ RunOnceEvery(100, { - led_toggle(); - DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM); - }); + led_toggle(); + DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM); + }); #if USE_I2C2 RunOnceEvery(111, { - uint16_t i2c2_queue_full_cnt = i2c2.errors->queue_full_cnt; - uint16_t i2c2_ack_fail_cnt = i2c2.errors->ack_fail_cnt; - uint16_t i2c2_miss_start_stop_cnt = i2c2.errors->miss_start_stop_cnt; - uint16_t i2c2_arb_lost_cnt = i2c2.errors->arb_lost_cnt; - uint16_t i2c2_over_under_cnt = i2c2.errors->over_under_cnt; - uint16_t i2c2_pec_recep_cnt = i2c2.errors->pec_recep_cnt; - uint16_t i2c2_timeout_tlow_cnt = i2c2.errors->timeout_tlow_cnt; - uint16_t i2c2_smbus_alert_cnt = i2c2.errors->smbus_alert_cnt; - uint16_t i2c2_unexpected_event_cnt = i2c2.errors->unexpected_event_cnt; - uint32_t i2c2_last_unexpected_event = i2c2.errors->last_unexpected_event; - const uint8_t _bus2 = 2; - DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice, - &i2c2_queue_full_cnt, - &i2c2_ack_fail_cnt, - &i2c2_miss_start_stop_cnt, - &i2c2_arb_lost_cnt, - &i2c2_over_under_cnt, - &i2c2_pec_recep_cnt, - &i2c2_timeout_tlow_cnt, - &i2c2_smbus_alert_cnt, - &i2c2_unexpected_event_cnt, - &i2c2_last_unexpected_event, - &_bus2); - }); + uint16_t i2c2_queue_full_cnt = i2c2.errors->queue_full_cnt; + uint16_t i2c2_ack_fail_cnt = i2c2.errors->ack_fail_cnt; + uint16_t i2c2_miss_start_stop_cnt = i2c2.errors->miss_start_stop_cnt; + uint16_t i2c2_arb_lost_cnt = i2c2.errors->arb_lost_cnt; + uint16_t i2c2_over_under_cnt = i2c2.errors->over_under_cnt; + uint16_t i2c2_pec_recep_cnt = i2c2.errors->pec_recep_cnt; + uint16_t i2c2_timeout_tlow_cnt = i2c2.errors->timeout_tlow_cnt; + uint16_t i2c2_smbus_alert_cnt = i2c2.errors->smbus_alert_cnt; + uint16_t i2c2_unexpected_event_cnt = i2c2.errors->unexpected_event_cnt; + uint32_t i2c2_last_unexpected_event = i2c2.errors->last_unexpected_event; + const uint8_t _bus2 = 2; + DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, DefaultDevice, + &i2c2_queue_full_cnt, + &i2c2_ack_fail_cnt, + &i2c2_miss_start_stop_cnt, + &i2c2_arb_lost_cnt, + &i2c2_over_under_cnt, + &i2c2_pec_recep_cnt, + &i2c2_timeout_tlow_cnt, + &i2c2_smbus_alert_cnt, + &i2c2_unexpected_event_cnt, + &i2c2_last_unexpected_event, + &_bus2); + }); #endif - if (sys_time.nb_sec > 1) imu_periodic(); + if (sys_time.nb_sec > 1) { imu_periodic(); } RunOnceEvery(10, { LED_PERIODIC();}); } -static inline void main_event_task( void ) { +static inline void main_event_task(void) +{ ImuEvent(on_gyro_event, on_accel_event, on_mag_event); } -static inline void on_accel_event(void) { +static inline void on_accel_event(void) +{ imu_scale_accel(&imu); RunOnceEvery(50, LED_TOGGLE(3)); static uint8_t cnt; cnt++; - if (cnt > 15) cnt = 0; + if (cnt > 15) { cnt = 0; } if (cnt == 0) { DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel, DefaultDevice, - &imu.accel_unscaled.x, - &imu.accel_unscaled.y, - &imu.accel_unscaled.z); - } - else if (cnt == 7) { + &imu.accel_unscaled.x, + &imu.accel_unscaled.y, + &imu.accel_unscaled.z); + } else if (cnt == 7) { DOWNLINK_SEND_IMU_ACCEL_SCALED(DefaultChannel, DefaultDevice, - &imu.accel.x, - &imu.accel.y, - &imu.accel.z); + &imu.accel.x, + &imu.accel.y, + &imu.accel.z); } } -static inline void on_gyro_event(void) { +static inline void on_gyro_event(void) +{ imu_scale_gyro(&imu); RunOnceEvery(50, LED_TOGGLE(2)); static uint8_t cnt; cnt++; - if (cnt > 15) cnt = 0; + if (cnt > 15) { cnt = 0; } if (cnt == 0) { DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel, DefaultDevice, - &imu.gyro_unscaled.p, - &imu.gyro_unscaled.q, - &imu.gyro_unscaled.r); - } - else if (cnt == 7) { + &imu.gyro_unscaled.p, + &imu.gyro_unscaled.q, + &imu.gyro_unscaled.r); + } else if (cnt == 7) { DOWNLINK_SEND_IMU_GYRO_SCALED(DefaultChannel, DefaultDevice, - &imu.gyro.p, - &imu.gyro.q, - &imu.gyro.r); + &imu.gyro.p, + &imu.gyro.q, + &imu.gyro.r); } } -static inline void on_mag_event(void) { +static inline void on_mag_event(void) +{ imu_scale_mag(&imu); static uint8_t cnt; cnt++; - if (cnt > 10) cnt = 0; + if (cnt > 10) { cnt = 0; } if (cnt == 0) { DOWNLINK_SEND_IMU_MAG_SCALED(DefaultChannel, DefaultDevice, - &imu.mag.x, - &imu.mag.y, - &imu.mag.z); - } - else if (cnt == 5) { + &imu.mag.x, + &imu.mag.y, + &imu.mag.z); + } else if (cnt == 5) { DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel, DefaultDevice, - &imu.mag_unscaled.x, - &imu.mag_unscaled.y, - &imu.mag_unscaled.z); + &imu.mag_unscaled.x, + &imu.mag_unscaled.y, + &imu.mag_unscaled.z); } } diff --git a/sw/airborne/test/subsystems/test_radio_control.c b/sw/airborne/test/subsystems/test_radio_control.c index 9963615f15..e6c4d77aa0 100644 --- a/sw/airborne/test/subsystems/test_radio_control.c +++ b/sw/airborne/test/subsystems/test_radio_control.c @@ -30,65 +30,72 @@ #include "subsystems/radio_control.h" -static inline void main_init( void ); -static inline void main_periodic_task( void ); -static inline void main_event_task( void ); -static void main_on_radio_control_frame( void ); +static inline void main_init(void); +static inline void main_periodic_task(void); +static inline void main_event_task(void); +static void main_on_radio_control_frame(void); -int main( void ) { +int main(void) +{ main_init(); - while(1) { - if (sys_time_check_and_ack_timer(0)) + while (1) { + if (sys_time_check_and_ack_timer(0)) { main_periodic_task(); + } main_event_task(); } return 0; } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); radio_control_init(); mcu_int_enable(); } extern uint32_t debug_len; -static inline void main_periodic_task( void ) { +static inline void main_periodic_task(void) +{ RunOnceEvery(51, { - /*LED_TOGGLE(2);*/ - uint32_t sec = sys_time.nb_sec; - DOWNLINK_SEND_TIME(DefaultChannel, DefaultDevice, &sec); + /*LED_TOGGLE(2);*/ + uint32_t sec = sys_time.nb_sec; + DOWNLINK_SEND_TIME(DefaultChannel, DefaultDevice, &sec); }); RunOnceEvery(10, {radio_control_periodic_task();}); 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);}); + 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); + }); #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(); } -static inline void main_event_task( void ) { +static inline void main_event_task(void) +{ RadioControlEvent(main_on_radio_control_frame); } -static void main_on_radio_control_frame( void ) { +static void main_on_radio_control_frame(void) +{ // RunOnceEvery(10, {DOWNLINK_SEND_RC(RADIO_CONTROL_NB_CHANNEL, radio_control.values);}); diff --git a/sw/airborne/test/subsystems/test_settings.c b/sw/airborne/test/subsystems/test_settings.c index 25e2474265..63f2973a1f 100644 --- a/sw/airborne/test/subsystems/test_settings.c +++ b/sw/airborne/test/subsystems/test_settings.c @@ -33,9 +33,9 @@ #include "mcu_periph/uart.h" #include "messages.h" -static inline void main_init( void ); -static inline void main_periodic( void ); -static inline void main_event( void ); +static inline void main_init(void); +static inline void main_periodic(void); +static inline void main_event(void); float setting_a; @@ -43,60 +43,66 @@ float setting_b; float setting_c; float setting_d; -int main( void ) { +int main(void) +{ main_init(); - while(1) { - if (sys_time_check_and_ack_timer(0)) + while (1) { + if (sys_time_check_and_ack_timer(0)) { main_periodic(); + } main_event(); } return 0; } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); settings_init(); mcu_int_enable(); } -static inline void main_periodic( void ) { +static inline void main_periodic(void) +{ RunOnceEvery(100, { - DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM); - PeriodicSendDlValue(&(DefaultChannel).trans_tx, &(DefaultDevice).device); - }); + DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM); + PeriodicSendDlValue(&(DefaultChannel).trans_tx, &(DefaultDevice).device); + }); } -static inline void main_event( void ) { +static inline void main_event(void) +{ DatalinkEvent(); } -void dl_parse_msg(void) { +void dl_parse_msg(void) +{ datalink_time = 0; uint8_t msg_id = dl_buffer[1]; switch (msg_id) { - case DL_PING: { - DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice); - } - break; - case DL_SETTING: - if (DL_SETTING_ac_id(dl_buffer) == AC_ID) { - uint8_t i = DL_SETTING_index(dl_buffer); - float val = DL_SETTING_value(dl_buffer); - DlSetting(i, val); - DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val); + case DL_PING: { + DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice); } break; - default: - break; + case DL_SETTING: + if (DL_SETTING_ac_id(dl_buffer) == AC_ID) { + uint8_t i = DL_SETTING_index(dl_buffer); + float val = DL_SETTING_value(dl_buffer); + DlSetting(i, val); + DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val); + } + break; + default: + break; } } diff --git a/sw/airborne/test/test_actuators_pwm.c b/sw/airborne/test/test_actuators_pwm.c index 920c0d234c..b656e5a9e3 100644 --- a/sw/airborne/test/test_actuators_pwm.c +++ b/sw/airborne/test/test_actuators_pwm.c @@ -40,35 +40,40 @@ #include "subsystems/actuators/actuators_pwm.h" -static inline void main_init( void ); -static inline void main_periodic( void ); +static inline void main_init(void); +static inline void main_periodic(void); static inline void main_event(void); -int main(void) { +int main(void) +{ main_init(); while (1) { - if (sys_time_check_and_ack_timer(0)) + if (sys_time_check_and_ack_timer(0)) { main_periodic(); + } main_event(); }; return 0; } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); ActuatorsPwmInit(); } -static inline void main_periodic( void ) { +static inline void main_periodic(void) +{ ActuatorsPwmCommit(); LED_PERIODIC(); RunOnceEvery(100, {DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);}); } -static inline void main_event(void) { +static inline void main_event(void) +{ DatalinkEvent(); } @@ -76,44 +81,41 @@ static inline void main_event(void) { #define IdOfMsg(x) (x[1]) -void dl_parse_msg( void ) { +void dl_parse_msg(void) +{ uint8_t msg_id = IdOfMsg(dl_buffer); switch (msg_id) { - case DL_PING: - { - DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice); - } - break; + case DL_PING: { + DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice); + } + break; - case DL_SET_ACTUATOR: - { - uint8_t servo_no = DL_SET_ACTUATOR_no(dl_buffer); - uint16_t servo_value = DL_SET_ACTUATOR_value(dl_buffer); - LED_TOGGLE(2); - if (servo_no < ACTUATORS_PWM_NB) { - ActuatorPwmSet(servo_no, servo_value); - } + case DL_SET_ACTUATOR: { + uint8_t servo_no = DL_SET_ACTUATOR_no(dl_buffer); + uint16_t servo_value = DL_SET_ACTUATOR_value(dl_buffer); + LED_TOGGLE(2); + if (servo_no < ACTUATORS_PWM_NB) { + ActuatorPwmSet(servo_no, servo_value); } - break; + } + break; - case DL_SETTING: - { - if (DL_SETTING_ac_id(dl_buffer) != AC_ID) break; - uint8_t i = DL_SETTING_index(dl_buffer); - float var = DL_SETTING_value(dl_buffer); - DlSetting(i, var); - DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &var); - } - break; + case DL_SETTING: { + if (DL_SETTING_ac_id(dl_buffer) != AC_ID) { break; } + uint8_t i = DL_SETTING_index(dl_buffer); + float var = DL_SETTING_value(dl_buffer); + DlSetting(i, var); + DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &var); + } + break; - case DL_GET_SETTING : - { - if (DL_GET_SETTING_ac_id(dl_buffer) != AC_ID) break; - uint8_t i = DL_GET_SETTING_index(dl_buffer); - float val = settings_get_value(i); - DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val); - } - break; + case DL_GET_SETTING : { + if (DL_GET_SETTING_ac_id(dl_buffer) != AC_ID) { break; } + uint8_t i = DL_GET_SETTING_index(dl_buffer); + float val = settings_get_value(i); + DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val); + } + break; default: break; diff --git a/sw/airborne/test/test_actuators_pwm_sin.c b/sw/airborne/test/test_actuators_pwm_sin.c index fd6f46f16c..f3638a3120 100644 --- a/sw/airborne/test/test_actuators_pwm_sin.c +++ b/sw/airborne/test/test_actuators_pwm_sin.c @@ -32,26 +32,30 @@ #include "subsystems/actuators/actuators_pwm.h" #include "led.h" -static inline void main_init( void ); -static inline void main_periodic( void ); +static inline void main_init(void); +static inline void main_periodic(void); -int main(void) { +int main(void) +{ main_init(); while (1) { - if (sys_time_check_and_ack_timer(0)) + if (sys_time_check_and_ack_timer(0)) { main_periodic(); + } }; return 0; } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); ActuatorsPwmInit(); } -static inline void main_periodic( void ) { +static inline void main_periodic(void) +{ static float foo = 0.; foo += 0.0025; int32_t bar = 1500 + 500. * sin(foo); diff --git a/sw/airborne/test/test_algebra.c b/sw/airborne/test/test_algebra.c index c982564955..a5ff10c96a 100644 --- a/sw/airborne/test/test_algebra.c +++ b/sw/airborne/test/test_algebra.c @@ -38,11 +38,12 @@ float test_rmat_of_eulers_312(void); float test_quat(void); float test_quat2(void); -float test_INT32_QUAT_OF_RMAT(struct FloatEulers* eul, bool_t display); +float test_INT32_QUAT_OF_RMAT(struct FloatEulers *eul, bool_t display); void test_of_axis_angle(void); -int main(int argc, char** argv) { +int main(int argc, char **argv) +{ // test_1(); // test_2(); @@ -69,7 +70,8 @@ int main(int argc, char** argv) { } -static void test_1(void) { +static void test_1(void) +{ struct FloatEulers euler_f = { RadOfDeg(45.), RadOfDeg(0.), RadOfDeg(0.)}; DISPLAY_FLOAT_EULERS("euler_f", euler_f); @@ -93,7 +95,8 @@ static void test_1(void) { } -static void test_2(void) { +static void test_2(void) +{ struct Int32Vect3 v1 = { 5000, 5000, 5000 }; DISPLAY_INT32_VECT3("v1", v1); @@ -144,11 +147,13 @@ static void test_2(void) { } -static void test_3(void) { +static void test_3(void) +{ /* Compute BODY to IMU eulers */ struct Int32Eulers b2i_e; - EULERS_ASSIGN(b2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(10.66)), ANGLE_BFP_OF_REAL(RadOfDeg(-0.7)), ANGLE_BFP_OF_REAL(RadOfDeg(0.))); + EULERS_ASSIGN(b2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(10.66)), ANGLE_BFP_OF_REAL(RadOfDeg(-0.7)), + ANGLE_BFP_OF_REAL(RadOfDeg(0.))); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("b2i_e", b2i_e); /* Compute BODY to IMU quaternion */ @@ -166,7 +171,8 @@ static void test_3(void) { /* Compute LTP to IMU eulers */ struct Int32Eulers l2i_e; - EULERS_ASSIGN(l2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(0.)), ANGLE_BFP_OF_REAL(RadOfDeg(20.)), ANGLE_BFP_OF_REAL(RadOfDeg(0.))); + EULERS_ASSIGN(l2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(0.)), ANGLE_BFP_OF_REAL(RadOfDeg(20.)), + ANGLE_BFP_OF_REAL(RadOfDeg(0.))); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2i_e", l2i_e); /* Compute LTP to IMU quaternion */ @@ -198,7 +204,7 @@ static void test_3(void) { // DISPLAY_INT32_RMAT("l2b_r", l2b_r); DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r); - /* again but from quaternion */ + /* again but from quaternion */ struct Int32RMat l2b_r2; int32_rmat_of_quat(&l2b_r2, &l2b_q); // DISPLAY_INT32_RMAT("l2b_r2", l2b_r2); @@ -220,12 +226,14 @@ static void test_3(void) { -static void test_4_int(void) { +static void test_4_int(void) +{ printf("euler to quat to euler - int\n"); /* initial euler angles */ struct Int32Eulers _e; - EULERS_ASSIGN(_e, ANGLE_BFP_OF_REAL(RadOfDeg(-10.66)), ANGLE_BFP_OF_REAL(RadOfDeg(-0.7)), ANGLE_BFP_OF_REAL(RadOfDeg(0.))); + EULERS_ASSIGN(_e, ANGLE_BFP_OF_REAL(RadOfDeg(-10.66)), ANGLE_BFP_OF_REAL(RadOfDeg(-0.7)), + ANGLE_BFP_OF_REAL(RadOfDeg(0.))); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("euler orig ", _e); /* transform to quaternion */ @@ -245,7 +253,8 @@ static void test_4_int(void) { -static void test_4_float(void) { +static void test_4_float(void) +{ printf("euler to quat to euler - float\n"); /* initial euler angles */ @@ -268,7 +277,8 @@ static void test_4_float(void) { } -static void test_5(void) { +static void test_5(void) +{ struct FloatEulers fe; EULERS_ASSIGN(fe, RadOfDeg(-10.66), RadOfDeg(-0.7), RadOfDeg(0.)); @@ -287,7 +297,8 @@ static void test_5(void) { } -float test_eulers_of_quat(struct FloatQuat fq, int display) { +float test_eulers_of_quat(struct FloatQuat fq, int display) +{ struct FloatEulers fe; float_eulers_of_quat(&fe, &fq); @@ -312,7 +323,8 @@ float test_eulers_of_quat(struct FloatQuat fq, int display) { } -float test_eulers_of_rmat(struct FloatRMat frm, int display) { +float test_eulers_of_rmat(struct FloatRMat frm, int display) +{ struct FloatEulers fe; float_eulers_of_rmat(&fe, &frm); @@ -340,7 +352,8 @@ float test_eulers_of_rmat(struct FloatRMat frm, int display) { -static void test_6(void) { +static void test_6(void) +{ printf("\n"); struct FloatEulers ea2b; @@ -370,7 +383,8 @@ static void test_6(void) { -float test_rmat_comp(struct FloatRMat ma2b_f, struct FloatRMat mb2c_f, int display) { +float test_rmat_comp(struct FloatRMat ma2b_f, struct FloatRMat mb2c_f, int display) +{ struct FloatRMat ma2c_f; float_rmat_comp(&ma2c_f, &ma2b_f, &mb2c_f); @@ -396,7 +410,8 @@ float test_rmat_comp(struct FloatRMat ma2b_f, struct FloatRMat mb2c_f, int displ } -float test_quat_comp(struct FloatQuat qa2b_f, struct FloatQuat qb2c_f, int display) { +float test_quat_comp(struct FloatQuat qa2b_f, struct FloatQuat qb2c_f, int display) +{ struct FloatQuat qa2c_f; float_quat_comp(&qa2c_f, &qa2b_f, &qb2c_f); @@ -423,7 +438,8 @@ float test_quat_comp(struct FloatQuat qa2b_f, struct FloatQuat qb2c_f, int displ -static void test_7(void) { +static void test_7(void) +{ printf("\n"); struct FloatEulers ea2c; @@ -454,7 +470,8 @@ static void test_7(void) { #define NB_ITER 100000 #define SEED_RANDOM_FROM_TIME 1 -static void test_8(void) { +static void test_8(void) +{ printf("Running %d iterations of test_INT32_QUAT_OF_RMAT\n", NB_ITER); #if SEED_RANDOM_FROM_TIME #pragma message "Seeding random from current time" @@ -468,14 +485,14 @@ static void test_8(void) { float sum_err = 0.; int nb_err = 0; int i; - for (i=0; i max_err) max_err = err; + if (err > max_err) { max_err = err; } if (err > .01) { nb_err++; printf("\nIteration %d with large error: %f\n", i, err); @@ -484,11 +501,12 @@ static void test_8(void) { printf("\n"); } } - printf("Number of errors %d, average error %f, max error %f\n", nb_err, sum_err/NB_ITER, max_err); + printf("Number of errors %d, average error %f, max error %f\n", nb_err, sum_err / NB_ITER, max_err); } -static void test_9(void) { +static void test_9(void) +{ struct FloatEulers eul; eul.phi = RadOfDeg(80.821376); eul.theta = RadOfDeg(44.227319); @@ -496,7 +514,8 @@ static void test_9(void) { float err = test_INT32_QUAT_OF_RMAT(&eul, TRUE); } -static void test_10(void) { +static void test_10(void) +{ struct FloatEulers euler; EULERS_ASSIGN(euler , RadOfDeg(0.), RadOfDeg(10.), RadOfDeg(0.)); @@ -525,7 +544,8 @@ static void test_10(void) { } -float test_rmat_comp_inv(struct FloatRMat ma2c_f, struct FloatRMat mb2c_f, int display) { +float test_rmat_comp_inv(struct FloatRMat ma2c_f, struct FloatRMat mb2c_f, int display) +{ struct FloatRMat ma2b_f; float_rmat_comp_inv(&ma2b_f, &ma2c_f, &mb2c_f); @@ -550,7 +570,8 @@ float test_rmat_comp_inv(struct FloatRMat ma2c_f, struct FloatRMat mb2c_f, int d } -float test_quat_comp_inv(struct FloatQuat qa2c_f, struct FloatQuat qb2c_f, int display) { +float test_quat_comp_inv(struct FloatQuat qa2c_f, struct FloatQuat qb2c_f, int display) +{ struct FloatQuat qa2b_f; float_quat_comp_inv(&qa2b_f, &qa2c_f, &qb2c_f); @@ -577,7 +598,8 @@ float test_quat_comp_inv(struct FloatQuat qa2c_f, struct FloatQuat qb2c_f, int d -void test_of_axis_angle(void) { +void test_of_axis_angle(void) +{ struct FloatVect3 axis = { 0., 1., 0.}; FLOAT_VECT3_NORMALIZE(axis); @@ -632,7 +654,8 @@ void test_of_axis_angle(void) { } -float test_quat_of_rmat(void) { +float test_quat_of_rmat(void) +{ // struct FloatEulers eul = {-0.280849, 0.613423, -1.850440}; struct FloatEulers eul = {RadOfDeg(0.131579), RadOfDeg(-62.397659), RadOfDeg(-110.470299)}; @@ -674,7 +697,8 @@ float test_quat_of_rmat(void) { } -float test_rmat_of_eulers_312(void) { +float test_rmat_of_eulers_312(void) +{ struct FloatEulers eul312_f; EULERS_ASSIGN(eul312_f, RadOfDeg(45.), RadOfDeg(22.), RadOfDeg(0.)); @@ -696,7 +720,8 @@ float test_rmat_of_eulers_312(void) { } -void test1234(void) { +void test1234(void) +{ struct FloatEulers eul = {RadOfDeg(33.), RadOfDeg(25.), RadOfDeg(26.)}; struct FloatVect3 uz = { 0., 0., 1.}; @@ -724,7 +749,8 @@ void test1234(void) { } -float test_quat(void) { +float test_quat(void) +{ struct FloatVect3 u = { 1., 2., 3.}; FLOAT_VECT3_NORMALIZE(u); float angle = RadOfDeg(30.); @@ -763,7 +789,8 @@ float test_quat(void) { } -float test_quat2(void) { +float test_quat2(void) +{ struct FloatEulers eula2b; EULERS_ASSIGN(eula2b, RadOfDeg(70.), RadOfDeg(0.), RadOfDeg(0.)); @@ -779,8 +806,8 @@ float test_quat2(void) { double_quat_of_eulers(&qa2b_d, &eula2b_d); DISPLAY_FLOAT_QUAT("qa2b_d", qa2b_d); - struct FloatVect3 u = { 1., 0., 0.}; - float angle = RadOfDeg(70.); + struct FloatVect3 u = { 1., 0., 0.}; + float angle = RadOfDeg(70.); struct FloatQuat q; FLOAT_QUAT_OF_AXIS_ANGLE(q, u, angle); @@ -814,45 +841,46 @@ float test_quat2(void) { } -float test_INT32_QUAT_OF_RMAT(struct FloatEulers* eul_f, bool_t display) { +float test_INT32_QUAT_OF_RMAT(struct FloatEulers *eul_f, bool_t display) +{ struct Int32Eulers eul321_i; EULERS_BFP_OF_REAL(eul321_i, (*eul_f)); struct Int32Eulers eul312_i; EULERS_BFP_OF_REAL(eul312_i, (*eul_f)); - if (display) DISPLAY_INT32_EULERS("eul312_i", eul312_i); + if (display) { DISPLAY_INT32_EULERS("eul312_i", eul312_i); } struct FloatRMat rmat_f; FLOAT_RMAT_OF_EULERS_321(rmat_f, (*eul_f)); - if (display) DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat float", rmat_f); - if (display) DISPLAY_FLOAT_RMAT("rmat float", rmat_f); + if (display) { DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat float", rmat_f); } + if (display) { DISPLAY_FLOAT_RMAT("rmat float", rmat_f); } struct Int32RMat rmat_i; int32_rmat_of_eulers_321(&rmat_i, &eul321_i); - if (display) DISPLAY_INT32_RMAT_AS_EULERS_DEG("rmat int", rmat_i); - if (display) DISPLAY_INT32_RMAT("rmat int", rmat_i); - if (display) DISPLAY_INT32_RMAT_AS_FLOAT("rmat int", rmat_i); + if (display) { DISPLAY_INT32_RMAT_AS_EULERS_DEG("rmat int", rmat_i); } + if (display) { DISPLAY_INT32_RMAT("rmat int", rmat_i); } + if (display) { DISPLAY_INT32_RMAT_AS_FLOAT("rmat int", rmat_i); } struct FloatQuat qf; float_quat_of_rmat(&qf, &rmat_f); //FLOAT_QUAT_WRAP_SHORTEST(qf); - if (display) DISPLAY_FLOAT_QUAT("qf", qf); + if (display) { DISPLAY_FLOAT_QUAT("qf", qf); } struct Int32Quat qi; int32_quat_of_rmat(&qi, &rmat_i); //int32_quat_wrap_shortest(&qi); - if (display) DISPLAY_INT32_QUAT("qi", qi); - if (display) DISPLAY_INT32_QUAT_2("qi", qi); + if (display) { DISPLAY_INT32_QUAT("qi", qi); } + if (display) { DISPLAY_INT32_QUAT_2("qi", qi); } struct FloatQuat qif; QUAT_FLOAT_OF_BFP(qif, qi); // dot product of two quaternions is 1 if they represent same rotation - float qi_dot_qf = qif.qi*qf.qi + qif.qx*qf.qx + qif.qy*qf.qy + qif.qz*qf.qz; + float qi_dot_qf = qif.qi * qf.qi + qif.qx * qf.qx + qif.qy * qf.qy + qif.qz * qf.qz; float err_norm = fabs(fabs(qi_dot_qf) - 1.); - if (display) printf("err %f\n", err_norm); - if (display) printf("\n"); + if (display) { printf("err %f\n", err_norm); } + if (display) { printf("\n"); } return err_norm; diff --git a/sw/airborne/test/test_baro_board.c b/sw/airborne/test/test_baro_board.c index ac244154be..abed668fb2 100644 --- a/sw/airborne/test/test_baro_board.c +++ b/sw/airborne/test/test_baro_board.c @@ -39,9 +39,9 @@ #define ABI_C #include "subsystems/abi.h" -static inline void main_init( void ); -static inline void main_periodic_task( void ); -static inline void main_event_task( void ); +static inline void main_init(void); +static inline void main_periodic_task(void); +static inline void main_event_task(void); #ifndef BARO_PERIODIC_FREQUENCY #define BARO_PERIODIC_FREQUENCY 50 @@ -61,41 +61,48 @@ static abi_event pressure_abs_ev; tid_t baro_tid; -int main(void) { +int main(void) +{ main_init(); - while(1) { - if (sys_time_check_and_ack_timer(0)) + while (1) { + if (sys_time_check_and_ack_timer(0)) { main_periodic_task(); - if (sys_time_check_and_ack_timer(baro_tid)) + } + if (sys_time_check_and_ack_timer(baro_tid)) { baro_periodic(); + } main_event_task(); } return 0; } -static void pressure_abs_cb(uint8_t __attribute__((unused)) sender_id, const float * pressure) { +static void pressure_abs_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) +{ float p = *pressure; float foo = 42.; DOWNLINK_SEND_BARO_RAW(DefaultChannel, DefaultDevice, &p, &foo); } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); baro_init(); - baro_tid = sys_time_register_timer(1./BARO_PERIODIC_FREQUENCY, NULL); + baro_tid = sys_time_register_timer(1. / BARO_PERIODIC_FREQUENCY, NULL); AbiBindMsgBARO_ABS(BARO_ABS_ID, &pressure_abs_ev, pressure_abs_cb); } -static inline void main_periodic_task( void ) { +static inline void main_periodic_task(void) +{ LED_PERIODIC(); RunOnceEvery(256, {DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);}); } -static inline void main_event_task( void ) { +static inline void main_event_task(void) +{ BaroEvent(); } diff --git a/sw/airborne/test/test_bla.c b/sw/airborne/test/test_bla.c index e1869b068a..377c5309e4 100644 --- a/sw/airborne/test/test_bla.c +++ b/sw/airborne/test/test_bla.c @@ -13,7 +13,8 @@ #include "pprz_algebra_print.h" -int main(int argc, char** argv) { +int main(int argc, char **argv) +{ struct FloatEulers ea = { RadOfDeg(45.), RadOfDeg(30.), RadOfDeg(0.)}; DISPLAY_FLOAT_EULERS("ea", ea); diff --git a/sw/airborne/test/test_can.c b/sw/airborne/test/test_can.c index b711e4b91b..9e28a1f2a9 100644 --- a/sw/airborne/test/test_can.c +++ b/sw/airborne/test/test_can.c @@ -31,16 +31,17 @@ #include "mcu_periph/uart.h" #include "mcu_periph/can.h" -static inline void main_init( void ); -static inline void main_periodic_task( void ); -static inline void main_event_task( void ); +static inline void main_init(void); +static inline void main_periodic_task(void); +static inline void main_event_task(void); void main_on_can_msg(uint32_t id, uint8_t *data, int len); uint8_t tx_data[8]; uint8_t rx_data[8]; bool new_can_data = false; -int main(void) { +int main(void) +{ main_init(); tx_data[0] = 0; @@ -54,24 +55,27 @@ int main(void) { new_can_data = false; - while(1) { - if (sys_time_check_and_ack_timer(0)) + while (1) { + if (sys_time_check_and_ack_timer(0)) { main_periodic_task(); + } main_event_task(); } return 0; } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); - sys_time_register_timer((0.5/PERIODIC_FREQUENCY), NULL); + sys_time_register_timer((0.5 / PERIODIC_FREQUENCY), NULL); ppz_can_init(main_on_can_msg); } -static inline void main_periodic_task( void ) { +static inline void main_periodic_task(void) +{ - tx_data[0]+=1; + tx_data[0] += 1; ppz_can_transmit(0, tx_data, 8); LED_PERIODIC(); @@ -80,7 +84,8 @@ static inline void main_periodic_task( void ) { -static inline void main_event_task( void ) { +static inline void main_event_task(void) +{ if (new_can_data) { if (rx_data[0] & 0x10) { @@ -118,7 +123,7 @@ static inline void main_event_task( void ) { void main_on_can_msg(uint32_t id __attribute__((unused)), uint8_t *data, int len __attribute__((unused))) { - for (int i = 0; i<8; i++) { + for (int i = 0; i < 8; i++) { rx_data[i] = data[i]; } diff --git a/sw/airborne/test/test_datalink.c b/sw/airborne/test/test_datalink.c index 85e468352d..6db79cb101 100644 --- a/sw/airborne/test/test_datalink.c +++ b/sw/airborne/test/test_datalink.c @@ -28,43 +28,48 @@ #include "subsystems/datalink/datalink.h" -static inline void main_init( void ); -static inline void main_periodic( void ); -static inline void main_event( void ); +static inline void main_init(void); +static inline void main_periodic(void); +static inline void main_event(void); -int main(void) { +int main(void) +{ main_init(); while (1) { - if (sys_time_check_and_ack_timer(0)) + if (sys_time_check_and_ack_timer(0)) { main_periodic(); + } main_event(); } return 0; } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); } -static inline void main_periodic( void ) { +static inline void main_periodic(void) +{ RunOnceEvery(100, {DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);}); } -static inline void main_event( void ) { +static inline void main_event(void) +{ DatalinkEvent(); } -void dl_parse_msg(void) { +void dl_parse_msg(void) +{ // FIXME : when i remove the datalink=0 line it stops working !!!! datalink_time = 0; uint8_t msg_id = dl_buffer[1]; switch (msg_id) { - case DL_PING: - { + case DL_PING: { DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice); } break; diff --git a/sw/airborne/test/test_esc_asctecv1_simple.c b/sw/airborne/test/test_esc_asctecv1_simple.c index 37c3b63ede..a6a665d71c 100644 --- a/sw/airborne/test/test_esc_asctecv1_simple.c +++ b/sw/airborne/test/test_esc_asctecv1_simple.c @@ -28,32 +28,36 @@ #include "mcu_periph/i2c.h" -static inline void main_init( void ); -static inline void main_periodic_task( void ); -static inline void main_event_task( void ); +static inline void main_init(void); +static inline void main_periodic_task(void); +static inline void main_event_task(void); static struct i2c_transaction trans; -int main(void) { +int main(void) +{ main_init(); - while(1) { - if (sys_time_check_and_ack_timer(0)) + while (1) { + if (sys_time_check_and_ack_timer(0)) { main_periodic_task(); + } main_event_task(); } return 0; } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); } -static inline void main_periodic_task( void ) { +static inline void main_periodic_task(void) +{ trans.type = I2CTransTx; trans.slave_addr = 0x02; @@ -62,7 +66,7 @@ static inline void main_periodic_task( void ) { trans.buf[1] = 100; trans.buf[2] = 100; trans.buf[3] = 1; - i2c_submit(&i2c1,&trans); + i2c_submit(&i2c1, &trans); LED_PERIODIC(); @@ -70,6 +74,7 @@ static inline void main_periodic_task( void ) { -static inline void main_event_task( void ) { +static inline void main_event_task(void) +{ } diff --git a/sw/airborne/test/test_esc_mkk_simple.c b/sw/airborne/test/test_esc_mkk_simple.c index 9a793a088e..08d277902c 100644 --- a/sw/airborne/test/test_esc_mkk_simple.c +++ b/sw/airborne/test/test_esc_mkk_simple.c @@ -26,38 +26,42 @@ #include "mcu_periph/i2c.h" -static inline void main_init( void ); -static inline void main_periodic_task( void ); -static inline void main_event_task( void ); +static inline void main_init(void); +static inline void main_periodic_task(void); +static inline void main_event_task(void); static struct i2c_transaction trans; -int main(void) { +int main(void) +{ main_init(); - while(1) { - if (sys_time_check_and_ack_timer(0)) + while (1) { + if (sys_time_check_and_ack_timer(0)) { main_periodic_task(); + } main_event_task(); } return 0; } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); } -static inline void main_periodic_task( void ) { +static inline void main_periodic_task(void) +{ trans.type = I2CTransTx; trans.buf[0] = 0x04; trans.len_w = 1; trans.slave_addr = 0x58; - i2c_submit(&ACTUATORS_MKK_DEV,&trans); + i2c_submit(&ACTUATORS_MKK_DEV, &trans); LED_PERIODIC(); @@ -65,6 +69,7 @@ static inline void main_periodic_task( void ) { -static inline void main_event_task( void ) { +static inline void main_event_task(void) +{ } diff --git a/sw/airborne/test/test_manual.c b/sw/airborne/test/test_manual.c index eaadae8ec8..e166b458d4 100644 --- a/sw/airborne/test/test_manual.c +++ b/sw/airborne/test/test_manual.c @@ -45,8 +45,8 @@ #include "subsystems/radio_control.h" -static inline void main_init( void ); -static inline void main_periodic( void ); +static inline void main_init(void); +static inline void main_periodic(void); static inline void main_event(void); static void on_rc_frame(void); @@ -56,20 +56,24 @@ tid_t radio_control_tid; ///< id for radio_control_periodic_task() timer bool_t autopilot_motors_on; -int main(void) { +int main(void) +{ main_init(); while (1) { - if (sys_time_check_and_ack_timer(main_periodic_tid)) + if (sys_time_check_and_ack_timer(main_periodic_tid)) { main_periodic(); - if (sys_time_check_and_ack_timer(radio_control_tid)) + } + if (sys_time_check_and_ack_timer(radio_control_tid)) { radio_control_periodic_task(); + } main_event(); }; return 0; } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); actuators_init(); @@ -80,8 +84,8 @@ static inline void main_init( void ) { radio_control_init(); mcu_int_enable(); - main_periodic_tid = sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); - radio_control_tid = sys_time_register_timer((1./60.), NULL); + main_periodic_tid = sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); + radio_control_tid = sys_time_register_timer((1. / 60.), NULL); // just to make it usable in a standard rotorcraft airframe file // with @@ -89,7 +93,8 @@ static inline void main_init( void ) { autopilot_motors_on = TRUE; } -static inline void main_periodic( void ) { +static inline void main_periodic(void) +{ SetActuatorsFromCommands(commands, 0); @@ -100,60 +105,59 @@ static inline void main_periodic( void ) { RunOnceEvery(102, {DOWNLINK_SEND_ACTUATORS(DefaultChannel, DefaultDevice, ACTUATORS_NB, actuators);}); } -static inline void main_event(void) { +static inline void main_event(void) +{ DatalinkEvent(); RadioControlEvent(on_rc_frame); } #define IdOfMsg(x) (x[1]) -void dl_parse_msg( void ) { +void dl_parse_msg(void) +{ uint8_t msg_id = IdOfMsg(dl_buffer); switch (msg_id) { - case DL_PING: - { - DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice); - } - break; + case DL_PING: { + DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice); + } + break; - case DL_SETTING: - { - if (DL_SETTING_ac_id(dl_buffer) != AC_ID) break; - uint8_t i = DL_SETTING_index(dl_buffer); - float var = DL_SETTING_value(dl_buffer); - DlSetting(i, var); - DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &var); - } - break; + case DL_SETTING: { + if (DL_SETTING_ac_id(dl_buffer) != AC_ID) { break; } + uint8_t i = DL_SETTING_index(dl_buffer); + float var = DL_SETTING_value(dl_buffer); + DlSetting(i, var); + DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &var); + } + break; - case DL_GET_SETTING : - { - if (DL_GET_SETTING_ac_id(dl_buffer) != AC_ID) break; - uint8_t i = DL_GET_SETTING_index(dl_buffer); - float val = settings_get_value(i); - DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val); - } - break; + case DL_GET_SETTING : { + if (DL_GET_SETTING_ac_id(dl_buffer) != AC_ID) { break; } + uint8_t i = DL_GET_SETTING_index(dl_buffer); + float val = settings_get_value(i); + DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val); + } + break; #ifdef RADIO_CONTROL_TYPE_DATALINK case DL_RC_3CH : #ifdef RADIO_CONTROL_DATALINK_LED LED_TOGGLE(RADIO_CONTROL_DATALINK_LED); #endif parse_rc_3ch_datalink( - DL_RC_3CH_throttle_mode(dl_buffer), - DL_RC_3CH_roll(dl_buffer), - DL_RC_3CH_pitch(dl_buffer)); + DL_RC_3CH_throttle_mode(dl_buffer), + DL_RC_3CH_roll(dl_buffer), + DL_RC_3CH_pitch(dl_buffer)); break; case DL_RC_4CH : #ifdef RADIO_CONTROL_DATALINK_LED LED_TOGGLE(RADIO_CONTROL_DATALINK_LED); #endif parse_rc_4ch_datalink( - DL_RC_4CH_mode(dl_buffer), - DL_RC_4CH_throttle(dl_buffer), - DL_RC_4CH_roll(dl_buffer), - DL_RC_4CH_pitch(dl_buffer), - DL_RC_4CH_yaw(dl_buffer)); + DL_RC_4CH_mode(dl_buffer), + DL_RC_4CH_throttle(dl_buffer), + DL_RC_4CH_roll(dl_buffer), + DL_RC_4CH_pitch(dl_buffer), + DL_RC_4CH_yaw(dl_buffer)); break; #endif // RADIO_CONTROL_TYPE_DATALINK @@ -163,7 +167,8 @@ void dl_parse_msg( void ) { } -static void on_rc_frame(void) { +static void on_rc_frame(void) +{ /* if there are some commands that should always be set from RC, do it */ #ifdef SetAutoCommandsFromRC diff --git a/sw/airborne/test/test_matrix.c b/sw/airborne/test/test_matrix.c index 13f503f0b9..9f97c16f4b 100644 --- a/sw/airborne/test/test_matrix.c +++ b/sw/airborne/test/test_matrix.c @@ -5,38 +5,43 @@ -int main(int argc, char** argv) { +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]; printf("\n"); - MAT_PRINT(2,3,A); + MAT_PRINT(2, 3, A); printf("\n"); - MAT_PRINT(3,2,B); + MAT_PRINT(3, 2, B); printf("\n"); - MAT_MUL_T(2,3,2, D, A, C); - MAT_PRINT(2,2,D); + MAT_MUL_T(2, 3, 2, D, A, C); + MAT_PRINT(2, 2, D); printf("\n"); MAT_INV33(F, E); - MAT_PRINT(3,3,F); + MAT_PRINT(3, 3, F); printf("\n"); diff --git a/sw/airborne/test/test_nav.c b/sw/airborne/test/test_nav.c index 81255fbf89..8b72408a19 100644 --- a/sw/airborne/test/test_nav.c +++ b/sw/airborne/test/test_nav.c @@ -3,7 +3,8 @@ #define FloatEqual_3(f1, f2) (fabs(f1 - f2) < 1e-3) -int main(void) { +int main(void) +{ float angle = 370.; NormCourse(angle); assert(FloatEqual_3(angle, 10.)); @@ -13,10 +14,10 @@ int main(void) { NormCourse(angle); assert(angle > 360); - assert(CloseDegAngles(180, 175+10*360)); - assert(CloseDegAngles(175+10*360, 180)); - assert(CloseDegAngles(355, 4+5*360)); - assert(CloseDegAngles(4+5*360, 355)); + assert(CloseDegAngles(180, 175 + 10 * 360)); + assert(CloseDegAngles(175 + 10 * 360, 180)); + assert(CloseDegAngles(355, 4 + 5 * 360)); + assert(CloseDegAngles(4 + 5 * 360, 355)); return 0; } diff --git a/sw/airborne/test/test_telemetry.c b/sw/airborne/test/test_telemetry.c index aaca8ddcd1..da7d4f4e20 100644 --- a/sw/airborne/test/test_telemetry.c +++ b/sw/airborne/test/test_telemetry.c @@ -33,27 +33,31 @@ #include "subsystems/datalink/downlink.h" #include "led.h" -static inline void main_init( void ); -static inline void main_periodic( void ); +static inline void main_init(void); +static inline void main_periodic(void); -int main(void) { +int main(void) +{ main_init(); while (1) { - if (sys_time_check_and_ack_timer(0)) + if (sys_time_check_and_ack_timer(0)) { main_periodic(); + } } return 0; } -static inline void main_init( void ) { +static inline void main_init(void) +{ mcu_init(); - sys_time_register_timer((1./PERIODIC_FREQUENCY), NULL); + sys_time_register_timer((1. / PERIODIC_FREQUENCY), NULL); mcu_int_enable(); } -static inline void main_periodic( void ) { +static inline void main_periodic(void) +{ RunOnceEvery(10, {DOWNLINK_SEND_ALIVE(DefaultChannel, DefaultDevice, 16, MD5SUM);}); LED_PERIODIC(); }