diff --git a/conf/Makefile.disco b/conf/Makefile.disco new file mode 100644 index 0000000000..1fef38cd55 --- /dev/null +++ b/conf/Makefile.disco @@ -0,0 +1,36 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# Copyright (C) 2014 Freek van Tienen +# +# 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, see +# . +# + +# +# This Makefile uses the generic Makefile.arm-linux and adds upload rules for the Disco +# + +include $(PAPARAZZI_SRC)/conf/Makefile.arm-linux + +# reuse bebop flashing script +DRONE = $(PAPARAZZI_SRC)/sw/tools/parrot/bebop.py + +# Program the device and start it. +upload program: $(OBJDIR)/$(TARGET).elf + $(Q)$(DRONE) --host=$(HOST) --min_version='1.0.0' --max_version='1.2.1' upload_file_and_run $(OBJDIR)/$(TARGET).elf $(SUB_DIR) + +# Listing of phony targets. +.PHONY : upload program diff --git a/conf/airframes/ENAC/fixed-wing/disco.xml b/conf/airframes/ENAC/fixed-wing/disco.xml new file mode 100644 index 0000000000..e9bfac32a4 --- /dev/null +++ b/conf/airframes/ENAC/fixed-wing/disco.xml @@ -0,0 +1,231 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + +
+ + + + + + + + + +
+ + +
+ +
+ + + + + + + + + + + + + + + + + +
+ + + +
+ + + + + +
+ +
+ + +
+ +
+ + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + +
+ +
+ +
+ +
+ + + + + +
+ +
+ + + + + + + + +
+ +
+ + + + + + +
+ + diff --git a/conf/boards/disco.makefile b/conf/boards/disco.makefile new file mode 100644 index 0000000000..718af7e673 --- /dev/null +++ b/conf/boards/disco.makefile @@ -0,0 +1,59 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# disco.makefile +# +# + +BOARD=disco +BOARD_CFG=\"boards/$(BOARD).h\" + +ARCH=linux +$(TARGET).ARCHDIR = $(ARCH) +# reuse conf/Makefile.bebop (with specific upload rules) instead of only Makefile.linux: +ap.MAKEFILE = disco + +# ----------------------------------------------------------------------- +USER=foobar +HOST?=192.168.42.1 +SUB_DIR=internal_000/paparazzi +FTP_DIR=/data/ftp +TARGET_DIR=$(FTP_DIR)/$(SUB_DIR) +# ----------------------------------------------------------------------- + +# The datalink default uses UDP +MODEM_HOST ?= 192.168.42.255 + +# The GPS sensor is connected internally +GPS_PORT ?= UART1 +GPS_BAUD ?= B230400 + +# SBUS port, mapped to internal /dev/uart-sbus +SBUS_PORT ?= UART3 +$(TARGET).CFLAGS += -DUSE_ARBITRARY_BAUDRATE + +# handle linux signals by hand +$(TARGET).CFLAGS += -DUSE_LINUX_SIGNAL -D_GNU_SOURCE + +# board specific init function +$(TARGET).srcs += $(SRC_BOARD)/board.c + +# Compile the video specific parts +VIDEO_SRC = boards/bebop +#$(TARGET).CFLAGS += -DI2C_BUF_LEN=56 -DUSE_I2C0 +#$(TARGET).srcs += $(VIDEO_SRC)/mt9v117.c $(VIDEO_SRC)/mt9f002.c modules/computer_vision/lib/isp/libisp.c modules/computer_vision/lib/isp/libisp_config.c + +# Link static (Done for GLIBC) +$(TARGET).CFLAGS += -DLINUX_LINK_STATIC +$(TARGET).LDFLAGS += -static + +# limit main loop to 1kHz so ap doesn't need 100% cpu +#$(TARGET).CFLAGS += -DLIMIT_EVENT_POLLING + +# ----------------------------------------------------------------------- + +# default LED configuration +RADIO_CONTROL_LED ?= none +BARO_LED ?= none +AHRS_ALIGNER_LED ?= none +GPS_LED ?= none +SYS_TIME_LED ?= none diff --git a/conf/firmwares/subsystems/fixedwing/autopilot.makefile b/conf/firmwares/subsystems/fixedwing/autopilot.makefile index 128ce64fec..ae42d74500 100644 --- a/conf/firmwares/subsystems/fixedwing/autopilot.makefile +++ b/conf/firmwares/subsystems/fixedwing/autopilot.makefile @@ -143,6 +143,9 @@ endif # ns_srcs += mcu_periph/uart.c ns_srcs += $(SRC_ARCH)/mcu_periph/uart_arch.c +ifeq ($(ARCH), linux) +ns_srcs += $(SRC_ARCH)/serial_port.c +endif # diff --git a/conf/firmwares/subsystems/shared/baro_board.makefile b/conf/firmwares/subsystems/shared/baro_board.makefile index ac458e2ace..cccf0b4a3b 100644 --- a/conf/firmwares/subsystems/shared/baro_board.makefile +++ b/conf/firmwares/subsystems/shared/baro_board.makefile @@ -38,6 +38,15 @@ else ifeq ($(BOARD), bebop) BARO_BOARD_SRCS += peripherals/ms5611_i2c.c BARO_BOARD_SRCS += boards/baro_board_ms5611_i2c.c +# Disco baro +else ifeq ($(BOARD), disco) + BARO_BOARD_CFLAGS += -DBARO_BOARD=BARO_MS5611_I2C + BARO_BOARD_CFLAGS += -DUSE_I2C1 + BARO_BOARD_CFLAGS += -DBB_MS5611_I2C_DEV=i2c1 + BARO_BOARD_SRCS += peripherals/ms5611.c + BARO_BOARD_SRCS += peripherals/ms5611_i2c.c + BARO_BOARD_SRCS += boards/baro_board_ms5611_i2c.c + # Lisa/M baro else ifeq ($(BOARD), lisa_m) ifeq ($(BOARD_VERSION), 1.0) diff --git a/conf/modules/actuators_disco.xml b/conf/modules/actuators_disco.xml new file mode 100644 index 0000000000..8019b5ed86 --- /dev/null +++ b/conf/modules/actuators_disco.xml @@ -0,0 +1,19 @@ + + + + + + Actuators Driver for Disco plane + + +
+ +
+ + + + + + + + diff --git a/conf/modules/imu_disco.xml b/conf/modules/imu_disco.xml new file mode 100644 index 0000000000..15892bfb20 --- /dev/null +++ b/conf/modules/imu_disco.xml @@ -0,0 +1,39 @@ + + + + + + Driver for IMU on the Parrot Disco drone. + - Accelerometer/Gyroscope: MPU6000 via I2C2 + - Magnetometer: AK8963 via I2C1 + +
+ + + + + + + +
+ + + + +
+ +
+ + + + + + + + + + + + + + diff --git a/sw/airborne/arch/linux/mcu_periph/pwm_sysfs.c b/sw/airborne/arch/linux/mcu_periph/pwm_sysfs.c new file mode 100644 index 0000000000..ac69b2a0e8 --- /dev/null +++ b/sw/airborne/arch/linux/mcu_periph/pwm_sysfs.c @@ -0,0 +1,177 @@ +/* + * Copyright (C) 2017 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, see + * . + */ + +/** @file arch/linux/mcu_periph/pwm_sysfs.c + * PWM servos handling using Linux sysfs + * + * Based on PWM_Sysfs driver from ardupilot released under GPL v3 + */ + +#include "mcu_periph/pwm_sysfs.h" + +#include +#include +#include +#include +#include +#include +#include + +// default period in nano seconds (100Hz) +#define PWM_SYSFS_DEFAULT_PERIOD 10000000 + +// print debug +#if PWM_SYSFS_DEBUG +#define PS_DEBUG_PRINT printf +#else +#define PS_DEBUG_PRINT(...) {} +#endif + +// Write in file +static int write_file(const char *path, const char *fmt, ...); +// Read file content +static int read_file(const char *path, const char *fmt, ...) __attribute__((unused)); + +int pwm_sysfs_init(struct PWM_Sysfs *pwm, char *base_path, + char *_export, char *_enable, + char *_duty, char *_period, + uint8_t channel) +{ + // path to export file (create new PWM) + char export_path[PWM_SYSFS_PATH_LEN]; + + if (base_path == NULL || _export == NULL || _enable == NULL || + _period == NULL || _duty == NULL) { + // invalid paths + PS_DEBUG_PRINT("invalid paths\n"); + return -1; + } + // store paths + snprintf(export_path, PWM_SYSFS_PATH_LEN, "%s/%s", base_path, _export); + snprintf(pwm->enable_path, PWM_SYSFS_PATH_LEN, "%s/pwm_%u/%s", base_path, channel, _enable); + snprintf(pwm->duty_path, PWM_SYSFS_PATH_LEN, "%s/pwm_%u/%s", base_path, channel, _duty); + snprintf(pwm->period_path, PWM_SYSFS_PATH_LEN, "%s/pwm_%u/%s", base_path, channel, _period); + + PS_DEBUG_PRINT("%s\n%s\n%s\n%s\n", + export_path, pwm->enable_path, pwm->duty_path, pwm->period_path); + + // export new PWM entry + write_file(export_path, "%u", channel); + // open duty cycle file + pwm->duty_cycle_fd = open(pwm->duty_path, O_RDWR | O_CLOEXEC); + if (pwm->duty_cycle_fd < 0) { + // fail to open duty file + PS_DEBUG_PRINT("failed to open FD cycle: %d\n", pwm->duty_cycle_fd); + return -2; + } + // set default period + pwm_sysfs_set_period(pwm, PWM_SYSFS_DEFAULT_PERIOD); + + // init with sucess + return 0; +} + +void pwm_sysfs_set_period(struct PWM_Sysfs *pwm, uint32_t period) +{ + if (write_file(pwm->period_path, "%u", period) < 0) { + PS_DEBUG_PRINT("failed to set period\n"); + return; + } + PS_DEBUG_PRINT("setting period %d\n", period); + pwm->period_nsec = period; +} + +void pwm_sysfs_set_duty(struct PWM_Sysfs *pwm, uint32_t duty) +{ + if (dprintf(pwm->duty_cycle_fd, "%u", duty) < 0) { + PS_DEBUG_PRINT("failed to set duty %u; %d; %s\n", duty, pwm->duty_cycle_fd, pwm->duty_path); + return; + } + pwm->duty_cycle_nsec = duty; + // enable pwm if not done + if (!pwm->enabled) { + pwm_sysfs_enable(pwm, true); + } +} + +void pwm_sysfs_enable(struct PWM_Sysfs *pwm, bool enable) +{ + int en = enable ? 1 : 0; + int ret = write_file(pwm->enable_path, "%u", en); + if (ret < 0) { + PS_DEBUG_PRINT("failed to enable: %d; %s\n", ret, pwm->enable_path); + return; + } + PS_DEBUG_PRINT("enable channel: %d; %s\n", en, pwm->enable_path); + pwm->enabled = enable; +} + + +static int write_file(const char *path, const char *fmt, ...) +{ + errno = 0; + + int fd = open(path, O_WRONLY | O_CLOEXEC); + if (fd == -1) { + return -errno; + } + + va_list args; + va_start(args, fmt); + + int ret = vdprintf(fd, fmt, args); + int errno_bkp = errno; + close(fd); + + va_end(args); + + if (ret < 1) { + return -errno_bkp; + } + + return ret; +} + +static int read_file(const char *path, const char *fmt, ...) +{ + errno = 0; + + FILE *file = fopen(path, "re"); + if (!file) { + return -errno; + } + + va_list args; + va_start(args, fmt); + + int ret = vfscanf(file, fmt, args); + int errno_bkp = errno; + fclose(file); + + va_end(args); + + if (ret < 1) { + return -errno_bkp; + } + + return ret; +} + + diff --git a/sw/airborne/arch/linux/mcu_periph/pwm_sysfs.h b/sw/airborne/arch/linux/mcu_periph/pwm_sysfs.h new file mode 100644 index 0000000000..20be9d38d6 --- /dev/null +++ b/sw/airborne/arch/linux/mcu_periph/pwm_sysfs.h @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2017 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, see + * . + */ + +/** @file arch/linux/mcu_periph/pwm_sysfs.h + * PWM servos handling using Linux sysfs + * + * Based on PWM_Sysfs driver from ardupilot released under GPL v3 + */ + +#ifndef PWM_SYSFS_H +#define PWM_SYSFS_H + +#include "std.h" + +#define PWM_SYSFS_PATH_LEN 64 + +/** PWM structure + */ +struct PWM_Sysfs { + uint32_t duty_cycle_nsec; ///< current duty cycle (in nsec) + uint32_t period_nsec; ///< current period (in nsec) + bool enabled; ///< true if pwm is enabled + int duty_cycle_fd; ///< file descriptor to write/update duty cycle + char enable_path[PWM_SYSFS_PATH_LEN]; ///< path to enable file + char duty_path[PWM_SYSFS_PATH_LEN]; ///< path to duty file + char period_path[PWM_SYSFS_PATH_LEN]; ///< path to period file +}; + +extern int pwm_sysfs_init(struct PWM_Sysfs *pwm, char *base_path, + char *_export, char *_enable, + char *_duty, char *_period, + uint8_t channel); +extern void pwm_sysfs_set_period(struct PWM_Sysfs *pwm, uint32_t period); +extern void pwm_sysfs_set_duty(struct PWM_Sysfs *pwm, uint32_t duty); +extern void pwm_sysfs_enable(struct PWM_Sysfs *pwm, bool enable); + +#endif + diff --git a/sw/airborne/arch/linux/mcu_periph/uart_arch.c b/sw/airborne/arch/linux/mcu_periph/uart_arch.c index d1fa9b2d49..01385a8900 100644 --- a/sw/airborne/arch/linux/mcu_periph/uart_arch.c +++ b/sw/airborne/arch/linux/mcu_periph/uart_arch.c @@ -212,8 +212,9 @@ static void *uart_thread(void *data __attribute__((unused))) return 0; } - -void uart_periph_set_baudrate(struct uart_periph *periph, uint32_t baud) +// open serial link +// close first if already openned +static void uart_periph_open(struct uart_periph *periph, uint32_t baud) { periph->baudrate = baud; @@ -240,6 +241,32 @@ void uart_periph_set_baudrate(struct uart_periph *periph, uint32_t baud) } } +void uart_periph_set_baudrate(struct uart_periph *periph, uint32_t baud) +{ + periph->baudrate = baud; + + // open serial port if not done + if (periph->reg_addr == NULL) { + uart_periph_open(periph, baud); + } + if (periph->reg_addr == NULL) { + // periph not started, do nothiing + return; + } + struct SerialPort *port = (struct SerialPort *)(periph->reg_addr); + serial_port_set_baudrate(port, baud); +} + +void uart_periph_set_bits_stop_parity(struct uart_periph *periph, uint8_t bits, uint8_t stop, uint8_t parity) +{ + if (periph->reg_addr == NULL) { + // periph not started, do nothiing + return; + } + struct SerialPort *port = (struct SerialPort *)(periph->reg_addr); + serial_port_set_bits_stop_parity(port, bits, stop, parity); +} + void uart_put_byte(struct uart_periph *periph, long fd __attribute__((unused)), uint8_t data) { if (periph->reg_addr == NULL) { return; } // device not initialized ? diff --git a/sw/airborne/arch/linux/mcu_periph/uart_arch.h b/sw/airborne/arch/linux/mcu_periph/uart_arch.h index b6b8b6ec85..2351fc179f 100644 --- a/sw/airborne/arch/linux/mcu_periph/uart_arch.h +++ b/sw/airborne/arch/linux/mcu_periph/uart_arch.h @@ -37,7 +37,16 @@ #include "mcu_periph/uart.h" // for definition of baud rates +#if !USE_ARBITRARY_BAUDRATE #include +#else +#include +#endif + +// strange speed for SBUS +#ifndef B100000 +#define B100000 100000 +#endif // for conversion between linux baud rate definition and actual speed static inline int uart_speed(int def) @@ -50,6 +59,7 @@ static inline int uart_speed(int def) case B19200: return 19200; case B38400: return 38400; case B57600: return 57600; + case B100000: return 100000; case B115200: return 115200; case B230400: return 230400; #ifdef B921600 diff --git a/sw/airborne/arch/linux/serial_port.c b/sw/airborne/arch/linux/serial_port.c index 08a77af89c..cf38217c9b 100644 --- a/sw/airborne/arch/linux/serial_port.c +++ b/sw/airborne/arch/linux/serial_port.c @@ -9,6 +9,58 @@ #include #include +/** + * some definitions from PPRZ uart driver as we can't include it directly + */ + +// for definition of baud rates +#if !USE_ARBITRARY_BAUDRATE +#include +#else +#include +#endif + +// strange speed for SBUS +#ifndef B100000 +#define B100000 100000 +#endif + +// for conversion between linux baud rate definition and actual speed +static inline int uart_speed(int def) +{ + switch (def) { + case B1200: return 1200; + case B2400: return 2400; + case B4800: return 4800; + case B9600: return 9600; + case B19200: return 19200; + case B38400: return 38400; + case B57600: return 57600; + case B100000: return 100000; + case B115200: return 115200; + case B230400: return 230400; +#ifdef B921600 + case B921600: return 921600; +#endif + default: return 9600; + } +} + +#define UBITS_7 7 +#define UBITS_8 8 + +#define USTOP_1 1 +#define USTOP_2 2 + +#define UPARITY_NO 0 +#define UPARITY_ODD 1 +#define UPARITY_EVEN 2 + +/////////////////////// + + + + // IUCLC flag translates upper case to lower case (actually needed here?) // but is not in POSIX and OSX #ifndef IUCLC @@ -31,6 +83,9 @@ void serial_port_free(struct SerialPort *me) } +#if !USE_ARBITRARY_BAUDRATE +// classic set baud function + void serial_port_flush(struct SerialPort *me) { /* @@ -148,3 +203,234 @@ void serial_port_close(struct SerialPort *me) } + +int serial_port_set_baudrate(struct SerialPort *me, speed_t speed) +{ + /* if null pointer or file descriptor indicates error just bail */ + if (!me || me->fd < 0) { + return -1; + } + if (cfsetispeed(&me->cur_termios, speed)) { + TRACE(TRACE_ERROR, "%s, set term speed failed: %s (%d)\n", device, strerror(errno), errno); + close(me->fd); + return -1; + } + return 0; +} + +int serial_port_set_bits_stop_parity(struct SerialPort *me, const int bits, const int stop, const int parity) +{ + /* if null pointer or file descriptor indicates error just bail */ + if (!me || me->fd < 0) { + return -1; + } + // clear data bits + me->cur_termios.c_cflag &= ~CSIZE; + if (bits == UBITS_7) { + me->cur_termios.c_cflag |= CS7; + } else { + me->cur_termios.c_cflag |= CS8; + } + // set stops + if (stop == USTOP_1) { + me->cur_termios.c_cflag &= ~CSTOPB; + } else { + me->cur_termios.c_cflag |= CSTOPB; + } + // set parity + if (parity == UPARITY_EVEN) { + me->cur_termios.c_cflag |= PARENB; + me->cur_termios.c_cflag &= ~PARODD; + } else if (parity == UPARITY_ODD) { + me->cur_termios.c_cflag |= PARENB; + me->cur_termios.c_cflag |= PARODD; + } else { + me->cur_termios.c_cflag &= ~PARENB; + } + // set new parameters + if (tcsetattr(me->fd, TCSADRAIN, &me->cur_termios)) { + TRACE(TRACE_ERROR, "setting term attributes (%s) (%d)\n", strerror(errno), errno); + return -2; + } + return 0; +} + +#else // USE_ARBITRARY_BAUDRATE +// use termios2 interface +// needed to set arbitrary speed +// it is the case for the SBUS RC input + +int ioctl(int d, int request, ...); // avoid warning +int tcflush (int __fd, int __queue_selector); // avoid warning + +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); + fprintf(stderr, "flush (%d) failed: %s (%d)\n", me->fd, strerror(errno), errno); + } +} + +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); + 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) +{ + 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); + return -1; + } + + if (ioctl(me->fd, TCGETS2, &me->orig_termios)) { + perror("TCGETS2"); + 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 |= IGNPAR; + /* control modes*/ + 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 |= NOFLSH; + /* output modes */ + me->cur_termios.c_oflag &=~(OPOST|ONLCR|OCRNL|ONOCR|ONLRET); + + me->cur_termios.c_cflag &= ~CBAUD; + me->cur_termios.c_cflag |= BOTHER; + me->cur_termios.c_ispeed = uart_speed(speed); // set real speed + me->cur_termios.c_ospeed = uart_speed(speed); // set real speed + + if (ioctl(me->fd, TCSETS2, &me->cur_termios)) { + perror("TCSETS2"); + close(me->fd); + return -1; + } + + serial_port_flush(me); + + if (ioctl(me->fd, TCGETS2, &me->cur_termios)) + { + perror("TCGETS2"); + return 5; + } + + return 0; +} + +int serial_port_open(struct SerialPort *me, const char *device, + void(*term_conf_callback)(struct termios *, speed_t *)) +{ + (void)me; + (void)device; + (void)term_conf_callback; + return 0; +} + +void serial_port_close(struct SerialPort *me) +{ + + /* if null pointer or file descriptor indicates error just bail */ + if (!me || me->fd < 0) { + return; + } + if (tcflush(me->fd, TCIOFLUSH)) { + TRACE(TRACE_ERROR, "flushing (%s) (%d)\n", strerror(errno), errno); + close(me->fd); + return; + } + if (ioctl(me->fd, TCSETS2, &me->orig_termios)) { // Restore modes. + TRACE(TRACE_ERROR, "restoring term attributes (%s) (%d)\n", strerror(errno), errno); + perror("TCSETS2"); + close(me->fd); + return; + } + if (close(me->fd)) { + TRACE(TRACE_ERROR, "closing %s (%d)\n", strerror(errno), errno); + return; + } + return; + +} + +int serial_port_set_baudrate(struct SerialPort *me, speed_t speed) +{ + /* if null pointer or file descriptor indicates error just bail */ + if (!me || me->fd < 0) { + return -1; + } + + me->cur_termios.c_cflag &= ~CBAUD; + me->cur_termios.c_cflag |= BOTHER; + me->cur_termios.c_ispeed = uart_speed(speed); // set real speed + me->cur_termios.c_ospeed = uart_speed(speed); // set real speed + + // set new parameters + if (ioctl(me->fd, TCSETS2, &me->cur_termios)) { + perror("TCSETS2"); + close(me->fd); + return -1; + } + return 0; +} + + +int serial_port_set_bits_stop_parity(struct SerialPort *me, const int bits, const int stop, const int parity) +{ + /* if null pointer or file descriptor indicates error just bail */ + if (!me || me->fd < 0) { + return -1; + } + // clear data bits + me->cur_termios.c_cflag &= ~CSIZE; + if (bits == UBITS_7) { + me->cur_termios.c_cflag |= CS7; + } else { + me->cur_termios.c_cflag |= CS8; + } + // set stops + if (stop == USTOP_1) { + me->cur_termios.c_cflag &= ~CSTOPB; + } else { + me->cur_termios.c_cflag |= CSTOPB; + } + // set parity + if (parity == UPARITY_EVEN) { + me->cur_termios.c_cflag |= PARENB; + me->cur_termios.c_cflag &= ~PARODD; + me->cur_termios.c_iflag |= INPCK; + } else if (parity == UPARITY_ODD) { + me->cur_termios.c_cflag |= PARENB; + me->cur_termios.c_cflag |= PARODD; + me->cur_termios.c_iflag |= INPCK; + } else { + me->cur_termios.c_cflag &= ~PARENB; + me->cur_termios.c_iflag &= ~INPCK; + } + // set new parameters + if (ioctl(me->fd, TCSETS2, &me->cur_termios)) { + perror("TCSETS2"); + close(me->fd); + return -1; + } + return 0; +} + +#endif + diff --git a/sw/airborne/arch/linux/serial_port.h b/sw/airborne/arch/linux/serial_port.h index c72c4a4ce0..19345f147a 100644 --- a/sw/airborne/arch/linux/serial_port.h +++ b/sw/airborne/arch/linux/serial_port.h @@ -23,6 +23,7 @@ #ifndef SERIAL_PORT_H #define SERIAL_PORT_H +#if !USE_ARBITRARY_BAUDRATE #include struct SerialPort { @@ -31,6 +32,17 @@ struct SerialPort { struct termios cur_termios; /* tty state structure */ }; +#else +#include + +struct SerialPort { + int fd; /* serial device fd */ + struct termios2 orig_termios; /* saved tty state structure */ + struct termios2 cur_termios; /* tty state structure */ +}; + +#endif + extern struct SerialPort *serial_port_new(void); extern void serial_port_free(struct SerialPort *me); extern void serial_port_flush(struct SerialPort *me); @@ -39,5 +51,7 @@ extern int serial_port_open_raw(struct SerialPort *me, const char *device, spee 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 int serial_port_set_baudrate(struct SerialPort *me, speed_t speed); +extern int serial_port_set_bits_stop_parity(struct SerialPort *me, const int bits, const int stop, const int parity); #endif /* SERIAL_PORT_H */ diff --git a/sw/airborne/boards/disco.h b/sw/airborne/boards/disco.h new file mode 100644 index 0000000000..e77509400d --- /dev/null +++ b/sw/airborne/boards/disco.h @@ -0,0 +1,73 @@ +/* + * Copyright (C) 2017 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, see + * . + * + */ + +#ifndef CONFIG_DISCO +#define CONFIG_DISCO + +#define BOARD_DISCO + +#include "std.h" +#include "peripherals/video_device.h" +// reuse bebop video driver +#include "boards/bebop/mt9f002.h" + +/** uart connected to GPS internally */ +#define UART1_DEV /dev/ttyPA1 +#define GPS_UBX_ENABLE_NMEA_DATA_MASK 0xff +/** FTDI cable for stereoboard or external GPS */ +#define UART2_DEV /dev/ttyUSB0 +/** uart connected to SBUS input */ +#define UART3_DEV /dev/uart-sbus + +/* Default actuators driver */ +#define DEFAULT_ACTUATORS "boards/disco/actuators.h" +#define ActuatorDefaultSet(_x,_y) ActuatorsDiscoSet(_x,_y) +#define ActuatorsDefaultInit() ActuatorsDiscoInit() +#define ActuatorsDefaultCommit() ActuatorsDiscoCommit() + +/* Cameras */ +extern struct video_config_t bottom_camera; +extern struct video_config_t front_camera; + +/* ISP */ +struct mt9f002_t mt9f002; + +/* by default activate onboard baro */ +#ifndef USE_BARO_BOARD +#define USE_BARO_BOARD 1 +#endif + +/* The ADC from the sonar */ +#if USE_ADC0 +#define ADC0_ID 0 +#define ADC0_CHANNELS 2 +#define ADC0_CHANNELS_CNT 1 +#define ADC0_BUF_LENGTH 8192 +#endif + +/* The SPI from the sonar */ +#if USE_SPI0 +#define SPI0_MODE 0 +#define SPI0_BITS_PER_WORD 8 +#define SPI0_MAX_SPEED_HZ 320000 +#endif + +#endif /* CONFIG_DISCO */ diff --git a/sw/airborne/boards/disco/actuators.c b/sw/airborne/boards/disco/actuators.c new file mode 100644 index 0000000000..5b5178f785 --- /dev/null +++ b/sw/airborne/boards/disco/actuators.c @@ -0,0 +1,183 @@ +/* + * + * Copyright (C) 2014-2015 Freek van Tienen + * Copyright (C) 2017 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, see + * . + */ + +/** + * @file boards/disco/actuators.c + * Actuator driver for the disco + * + * Disco plane is using the same ESC (I2C) than bebop for its motor + * and Pwm_sysfs linux driver for the PWM outputs + * + * Some part of this code is coming from the APM Disco and Bebop drivers + */ + +#include "subsystems/actuators.h" +#include "subsystems/electrical.h" +#include "actuators.h" +#include "autopilot.h" +#include "subsystems/abi.h" +#include +#include + +#include + +/** + * private observation structure + */ +struct __attribute__((__packed__)) disco_bldc_obs { + uint16_t rpm; + uint16_t batt_mv; + uint8_t status; + uint8_t error; + uint8_t motors_err; + uint8_t temp; + /* bit 0 indicates an overcurrent on the RC receiver port when high + * bits #1-#6 indicate an overcurrent on the #1-#6 PPM servos + */ + uint8_t overrcurrent; + uint8_t checksum; +} obs_data; + +/** + * Internal mapping of the PWM with output index + * servo rail 1 <-> linux pwm_4 + * servo rail 2 <-> linux pwm_5 + * servo rail 3 <-> linux pwm_6 + * servo rail 4 <-> linux pwm_1 + * servo rail 5 <-> linux pwm_2 + * servo rail 6 <-> linux pwm_3 + */ +static uint8_t disco_channels[] = { 4, 5, 6, 1, 2, 3 }; + +#define DISCO_BLDC_STATUS_STOPPED 1 +#define DISCO_BLDC_STATUS_RAMPUP 2 +#define DISCO_BLDC_STATUS_RUNNING 4 +#define DISCO_BLDC_STATUS_RAMPDOWN 5 + +// motor start threshold in RPM +// RPM range on disco is [1000, 12500] +// start and 1100 +#define DISCO_BLDC_START_MOTOR_THRESHOLD 1100 + +struct ActuatorsDisco actuators_disco; +static uint8_t actuators_disco_checksum(uint8_t *bytes, uint8_t size); + +void actuators_disco_init(void) +{ + /* Initialize the I2C connection */ + actuators_disco.i2c_trans.slave_addr = ACTUATORS_DISCO_ADDR; + actuators_disco.i2c_trans.status = I2CTransDone; + actuators_disco.motor_rpm = 0; + int i = 0; + for (i = 0; i < ACTUATORS_DISCO_PWM_NB; i++) { + pwm_sysfs_init(&actuators_disco.pwm[i], "/sys/class/pwm", "export", "run", "duty_ns", "period_ns", disco_channels[i]); + } +} + +void actuators_disco_set(uint8_t idx, uint16_t val) +{ + if (idx == ACTUATORS_DISCO_MOTOR_IDX) { + actuators_disco.motor_rpm = val; + } else if (idx > ACTUATORS_DISCO_PWM_NB) { + // wrong index, do nothing + } else { + // val is a PWM value in ms, convert to ns + pwm_sysfs_set_duty(&actuators_disco.pwm[idx-1], val * 1000); + } +} + +void actuators_disco_commit(void) +{ + // Handle PWM outputs + // already done in set function (FIXME ?) + + // Handle motor speed controller + + // Receive the status + actuators_disco.i2c_trans.buf[0] = ACTUATORS_DISCO_GET_OBS_DATA; + i2c_transceive(&i2c1, &actuators_disco.i2c_trans, actuators_disco.i2c_trans.slave_addr, 1, sizeof(obs_data)); + // copy data from buffer +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wcast-qual" + memcpy(&obs_data, (uint8_t*)actuators_disco.i2c_trans.buf, sizeof(obs_data)); +#pragma GCC diagnostic pop + + // Update status + electrical.vsupply = be16toh(obs_data.batt_mv) / 100; + // extract 'rpm saturation bit' + actuators_disco.rpm_saturated = (obs_data.rpm & (1 << 7)) ? true : false; + // clear 'rpm saturation bit' and fix endianess + obs_data.rpm &= (uint16_t)(~(1 << 7)); + obs_data.rpm = be16toh(obs_data.rpm); + if (obs_data.rpm == 0) { + actuators_disco.rpm_saturated = false; + } + + // When detected a suicide + uint8_t bldc_status = obs_data.status & 0x07; + if (obs_data.error == 2 && bldc_status != DISCO_BLDC_STATUS_STOPPED) { + kill_throttle = true; + } + + // Start the motors + if (bldc_status != DISCO_BLDC_STATUS_RUNNING && + bldc_status != DISCO_BLDC_STATUS_RAMPUP && + actuators_disco.motor_rpm > DISCO_BLDC_START_MOTOR_THRESHOLD) { + // Reset the error + actuators_disco.i2c_trans.buf[0] = ACTUATORS_DISCO_CLEAR_ERROR; + i2c_transmit(&i2c1, &actuators_disco.i2c_trans, actuators_disco.i2c_trans.slave_addr, 1); + + // Start the motors + actuators_disco.i2c_trans.buf[0] = ACTUATORS_DISCO_START_PROP; + i2c_transmit(&i2c1, &actuators_disco.i2c_trans, actuators_disco.i2c_trans.slave_addr, 1); + } + // Stop the motors + else if ((bldc_status == DISCO_BLDC_STATUS_RUNNING || bldc_status == DISCO_BLDC_STATUS_RAMPUP) && + actuators_disco.motor_rpm < DISCO_BLDC_START_MOTOR_THRESHOLD) { + actuators_disco.i2c_trans.buf[0] = ACTUATORS_DISCO_STOP_PROP; + i2c_transmit(&i2c1, &actuators_disco.i2c_trans, actuators_disco.i2c_trans.slave_addr, 1); + } else if (bldc_status == DISCO_BLDC_STATUS_RUNNING) { + // Send the commands + actuators_disco.i2c_trans.buf[0] = ACTUATORS_DISCO_SET_REF_SPEED; + actuators_disco.i2c_trans.buf[1] = actuators_disco.motor_rpm >> 8; + actuators_disco.i2c_trans.buf[2] = actuators_disco.motor_rpm & 0xFF; + actuators_disco.i2c_trans.buf[3] = 0x00; // enable security +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wcast-qual" + actuators_disco.i2c_trans.buf[4] = actuators_disco_checksum((uint8_t *)actuators_disco.i2c_trans.buf, 3); +#pragma GCC diagnostic pop + i2c_transmit(&i2c1, &actuators_disco.i2c_trans, actuators_disco.i2c_trans.slave_addr, 11); + } + + // Send ABI message + AbiSendMsgRPM(RPM_SENSOR_ID, &actuators_disco.rpm_obs, 1); +} + +static uint8_t actuators_disco_checksum(uint8_t *bytes, uint8_t size) +{ + uint8_t checksum = 0; + for (int i = 0; i < size; i++) { + checksum = checksum ^ bytes[i]; + } + return checksum; +} + diff --git a/sw/airborne/boards/disco/actuators.h b/sw/airborne/boards/disco/actuators.h new file mode 100644 index 0000000000..6c1ecb90cb --- /dev/null +++ b/sw/airborne/boards/disco/actuators.h @@ -0,0 +1,72 @@ +/* + * Copyright (C) 2014 Freek van Tienen + * Copyright (C) 2017 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, see + * . + */ + +/** + * @file boards/disco/actuators.h + * Actuator driver for the disco + * + * Disco plane is using the same ESC (I2C) than bebop for its motor + * and Pwm_sysfs linux driver for the PWM outputs + */ + +#ifndef ACTUATORS_DISCO_H_ +#define ACTUATORS_DISCO_H_ + +#include "std.h" +#include "mcu_periph/i2c.h" +#include "mcu_periph/pwm_sysfs.h" + +// full 8-bit address +#define ACTUATORS_DISCO_ADDR 0x10 + +// I2C Commands +#define ACTUATORS_DISCO_SET_REF_SPEED 0x02 ///< Set reference speed +#define ACTUATORS_DISCO_GET_OBS_DATA 0x20 ///< Get observation data +#define ACTUATORS_DISCO_START_PROP 0x40 ///< Start the propellers +#define ACTUATORS_DISCO_TOGGLE_GPIO 0x4D ///< Toggle GPIO (reset, red led, green led) +#define ACTUATORS_DISCO_STOP_PROP 0x60 ///< Stop the propellers +#define ACTUATORS_DISCO_CLEAR_ERROR 0x80 ///< Clear all current erros +#define ACTUATORS_DISCO_PLAY_SOUND 0x82 ///< Play a sound (0=stop, 1=short beep, 2=boot beep, 3=Be-Bop-Ah-Lula, negative=repeat) +#define ACTUATORS_DISCO_GET_INFO 0xA0 ///< Get version information + +// PWM setup +#define ACTUATORS_DISCO_PWM_NB 6 ///< Max number of PWM channels +#define ACTUATORS_DISCO_MOTOR_IDX 0 ///< Index for motor BLDC + +struct ActuatorsDisco { + struct i2c_transaction i2c_trans; ///< I2C transaction for communicating with the bebop BLDC driver + uint16_t motor_rpm; ///< Motor RPM setpoint + uint16_t rpm_obs; ///< Measured RPM + struct PWM_Sysfs pwm[ACTUATORS_DISCO_PWM_NB]; ///< Array of PWM outputs + uint8_t status; ///< Status flag + bool rpm_saturated; ///< RPM saturation flag (bit 15 in obs data) +}; + +#define ActuatorsDiscoSet(_i, _v) actuators_disco_set(_i, _v) +#define ActuatorsDiscoCommit() actuators_disco_commit() +#define ActuatorsDiscoInit() actuators_disco_init() + +extern struct ActuatorsDisco actuators_disco; +extern void actuators_disco_set(uint8_t idx, uint16_t val); +extern void actuators_disco_commit(void); +extern void actuators_disco_init(void); + +#endif /* ACTUATORS_DISCO_H_ */ diff --git a/sw/airborne/boards/disco/baro_board.h b/sw/airborne/boards/disco/baro_board.h new file mode 100644 index 0000000000..73eac26d66 --- /dev/null +++ b/sw/airborne/boards/disco/baro_board.h @@ -0,0 +1,42 @@ +/* + * Copyright (C) 2014 Freek van Tienen + * Copyright (C) 2017 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/disco/baro_board.h + * Paparazzi Disco Baro Sensor implementation for the MS5607. + * Actually uses the MS5611 driver, but sets BB_MS5611_TYPE_MS5607 to TRUE. + */ + +#ifndef BOARDS_DISCO_BARO_H +#define BOARDS_DISCO_BARO_H + +#define BB_MS5611_TYPE_MS5607 TRUE + +// only for printing the baro type during compilation +#ifndef BARO_BOARD +#define BARO_BOARD BARO_MS5611_I2C +#endif + +extern void baro_event(void); +#define BaroEvent baro_event + +#endif /* BOARDS_DISCO_BARO_H */ diff --git a/sw/airborne/boards/disco/board.c b/sw/airborne/boards/disco/board.c new file mode 100644 index 0000000000..766e8eb173 --- /dev/null +++ b/sw/airborne/boards/disco/board.c @@ -0,0 +1,139 @@ +/* + * Copyright (C) 2017 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, see + * . + */ + +/** + * @file boards/disco/board.c + * + * Disco specific board initialization function. + * + */ + +#include +#include +#include +#include +#include "mcu_periph/i2c.h" +#include "mcu.h" +#include "boards/disco.h" +// Reusing bebop video driver +#include "boards/bebop/mt9v117.h" +#include "boards/bebop/mt9f002.h" + +/* Initialize MT9F002 chipset (Front camera) */ +//struct mt9f002_t mt9f002 = { +// // Precomputed values to go from InputCLK of (26/2)MHz to 96MH +// .interface = MT9F002_PARALLEL, +// .input_clk_freq = (26 / 2), +// .vt_pix_clk_div = 7, +// .vt_sys_clk_div = 1, +// .pre_pll_clk_div = 1, +// .pll_multiplier = 59, +// .op_pix_clk_div = 8, +// .op_sys_clk_div = 1, +// .shift_vt_pix_clk_div = 1, +// .rowSpeed_2_0 = 1, +// .row_speed_10_8 = 1, +// +// // Initial values +// .target_fps = MT9F002_TARGET_FPS, +// .target_exposure = MT9F002_TARGET_EXPOSURE, +// .gain_green1 = MT9F002_GAIN_GREEN1, +// .gain_blue = MT9F002_GAIN_BLUE, +// .gain_red = MT9F002_GAIN_RED, +// .gain_green2 = MT9F002_GAIN_GREEN2, +// .output_width = MT9F002_OUTPUT_WIDTH, +// .output_height = MT9F002_OUTPUT_HEIGHT, +// .output_scaler = MT9F002_OUTPUT_SCALER, +// .offset_x = MT9F002_INITIAL_OFFSET_X, +// .offset_y = MT9F002_INITIAL_OFFSET_Y, +// +// .x_odd_inc = MT9F002_X_ODD_INC_VAL, +// .y_odd_inc = MT9F002_Y_ODD_INC_VAL, +// +// // I2C connection port +// .i2c_periph = &i2c0 +//}; + +static int kill_gracefull(char *process_name) +{ + /* "pidof" always in /bin on Bebop firmware tested 1.98, 2.0.57, no need for "which" */ + char pidof_commandline[200] = "/bin/pidof "; + strcat(pidof_commandline, process_name); + /* Bebop Busybox a + $ cat /proc/sys/kernel/pid_max + Gives max 32768, makes sure it never kills existing other process + */ + char pid[7] = ""; + int ret = 0; /* Return code of kill system call */ + FILE *fp; + + while (ret == 0) { + fp = popen(pidof_commandline, "r"); + if (fp != NULL) { /* Could open the pidof with line */ + if (fgets(pid, sizeof(pid) - 1, fp) != NULL) { + //printf("Process ID deducted: \"%s\"\n", pid); + if (atoi(pid) > 0) { /* To make sure we end 0 > There is a real process id found */ + char kill_command_and_process[200] = "kill -9 "; /* BTW there is no pkill on this Busybox */ + strcat(kill_command_and_process, pid); + ret = system(kill_command_and_process); + /* No need to wait, since if it is not closed the next pidof scan still will still find it and try to kill it */ + } + } else { + ret = 256; /* Could not get handle */ + pclose(fp); + } + } else { + ret = 256; /* fp NULL, so no process, just return */ + return 0; + } + } /* end while */ + return 0; +} + +void board_init(void) +{ + /* + * Process running by default for firmware >= v1.98 + * + * - /bin/sh - /usr/bin/DragonStarter.sh -out2null + * - //usr/bin/dragon-prog + * + * Thus two process to kill, the DragonStarter first + * This to make sure OEM program does not get re-started + * + */ + int ret __attribute__((unused)) = system("killall -q -15 DragonStarter.sh"); + usleep(50000); /* Give DragonStarter 50ms time to end on a busy system */ + kill_gracefull("dragon-prog"); +} + +void board_init2(void) +{ + /* Initialize MT9V117 chipset (Bottom camera) */ + //struct mt9v117_t mt9v117 = { + // // Initial values + + // // I2C connection port + // .i2c_periph = &i2c0 + //}; + //mt9v117_init(&mt9v117); + + //mt9f002_init(&mt9f002); +} diff --git a/sw/airborne/subsystems/imu/imu_disco.c b/sw/airborne/subsystems/imu/imu_disco.c new file mode 100644 index 0000000000..c5c56e03fe --- /dev/null +++ b/sw/airborne/subsystems/imu/imu_disco.c @@ -0,0 +1,136 @@ +/* + * Copyright (C) 2014 Freek van Tienen + * Copyright (C) 2017 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, see + * . + */ + +/** + * @file subsystems/imu/imu_disco.c + * Driver for the Disco magnetometer, accelerometer and gyroscope + */ + +#include "subsystems/imu.h" +#include "subsystems/abi.h" +#include "mcu_periph/i2c.h" + + +/* I2C is hardwired on Disco autopilot */ +#define DISCO_MAG_I2C_DEV i2c1 +PRINT_CONFIG_VAR(DISCO_MAG_I2C_DEV) +#define DISCO_MPU_I2C_DEV i2c2 +PRINT_CONFIG_VAR(DISCO_MPU_I2C_DEV) + + +#if !defined DISCO_LOWPASS_FILTER && !defined DISCO_SMPLRT_DIV +#if (PERIODIC_FREQUENCY == 60) || (PERIODIC_FREQUENCY == 120) +/* Accelerometer: Bandwidth 44Hz, Delay 4.9ms + * Gyroscope: Bandwidth 42Hz, Delay 4.8ms sampling 1kHz + */ +#define DISCO_LOWPASS_FILTER MPU60X0_DLPF_42HZ +#define DISCO_SMPLRT_DIV 9 +PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz at 1kHz internal sampling") +#endif +#endif +PRINT_CONFIG_VAR(DISCO_SMPLRT_DIV) +PRINT_CONFIG_VAR(DISCO_LOWPASS_FILTER) + +#ifndef DISCO_GYRO_RANGE +#define DISCO_GYRO_RANGE MPU60X0_GYRO_RANGE_1000 +#endif +PRINT_CONFIG_VAR(DISCO_GYRO_RANGE) + +#ifndef DISCO_ACCEL_RANGE +#define DISCO_ACCEL_RANGE MPU60X0_ACCEL_RANGE_8G +#endif +PRINT_CONFIG_VAR(DISCO_ACCEL_RANGE) + +/** Disco IMU data */ +struct ImuDisco imu_disco; + +/** + * Disco IMU initializtion of the MPU-60x0 and HMC58xx + */ +void imu_disco_init(void) +{ + /* MPU-60X0 */ + mpu60x0_i2c_init(&imu_disco.mpu, &(DISCO_MPU_I2C_DEV), MPU60X0_ADDR); + imu_disco.mpu.config.smplrt_div = DISCO_SMPLRT_DIV; + imu_disco.mpu.config.dlpf_cfg = DISCO_LOWPASS_FILTER; + imu_disco.mpu.config.gyro_range = DISCO_GYRO_RANGE; + imu_disco.mpu.config.accel_range = DISCO_ACCEL_RANGE; + + /* AKM8963 */ + ak8963_init(&imu_disco.ak, &(DISCO_MAG_I2C_DEV), AK8963_ADDR); +} + +/** + * Handle all the periodic tasks of the Disco IMU components. + * Read the MPU60x0 every periodic call and the HMC58XX every 10th call. + */ +void imu_disco_periodic(void) +{ + // Start reading the latest gyroscope data + mpu60x0_i2c_periodic(&imu_disco.mpu); + + // AKM8963 + ak8963_periodic(&imu_disco.ak); +} + +/** + * Handle all the events of the Disco IMU components. + * When there is data available convert it to the correct axis and save it in the imu structure. + */ +void imu_disco_event(void) +{ + uint32_t now_ts = get_sys_time_usec(); + + /* MPU-60x0 event taks */ + mpu60x0_i2c_event(&imu_disco.mpu); + + if (imu_disco.mpu.data_available) { + /* set correct orientation here */ + RATES_ASSIGN(imu.gyro_unscaled, + -imu_disco.mpu.data_rates.rates.p, + -imu_disco.mpu.data_rates.rates.q, + imu_disco.mpu.data_rates.rates.r); + VECT3_ASSIGN(imu.accel_unscaled, + -imu_disco.mpu.data_accel.vect.x, + -imu_disco.mpu.data_accel.vect.y, + imu_disco.mpu.data_accel.vect.z); + + imu_disco.mpu.data_available = false; + imu_scale_gyro(&imu); + imu_scale_accel(&imu); + AbiSendMsgIMU_GYRO_INT32(IMU_BOARD_ID, now_ts, &imu.gyro); + AbiSendMsgIMU_ACCEL_INT32(IMU_BOARD_ID, now_ts, &imu.accel); + } + + /* AKM8963 event task */ + ak8963_event(&imu_disco.ak); + + if (imu_disco.ak.data_available) { + VECT3_ASSIGN(imu.mag_unscaled, + imu_disco.ak.data.vect.x, + imu_disco.ak.data.vect.y, + imu_disco.ak.data.vect.z); + + imu_disco.ak.data_available = false; + imu_scale_mag(&imu); + AbiSendMsgIMU_MAG_INT32(IMU_BOARD_ID, now_ts, &imu.mag); + } +} diff --git a/sw/airborne/subsystems/imu/imu_disco.h b/sw/airborne/subsystems/imu/imu_disco.h new file mode 100644 index 0000000000..ed4bcf600e --- /dev/null +++ b/sw/airborne/subsystems/imu/imu_disco.h @@ -0,0 +1,85 @@ +/* + * Copyright (C) 2014 Freek van Tienen + * Copyright (C) 2017 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, see + * . + */ + +/** + * @file subsystems/imu/imu_disco.h + * Interface for the Disco magnetometer, accelerometer and gyroscope + */ + + +#ifndef IMU_DISCO_H +#define IMU_DISCO_H + +#include "generated/airframe.h" +#include "subsystems/imu.h" + +#include "peripherals/ak8963.h" +#include "peripherals/mpu60x0_i2c.h" + +/** default gyro sensitivy and neutral from the datasheet + * MPU with 1000 deg/s has 32.8 LSB/(deg/s) + * sens = 1/32.8 * pi/180 * 2^INT32_RATE_FRAC + * sens = 1/32.8 * pi/180 * 4096 = 2.17953 + */ +#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS +// FIXME +#define IMU_GYRO_P_SENS 2.17953 +#define IMU_GYRO_P_SENS_NUM 18271 +#define IMU_GYRO_P_SENS_DEN 8383 +#define IMU_GYRO_Q_SENS 2.17953 +#define IMU_GYRO_Q_SENS_NUM 18271 +#define IMU_GYRO_Q_SENS_DEN 8383 +#define IMU_GYRO_R_SENS 2.17953 +#define IMU_GYRO_R_SENS_NUM 18271 +#define IMU_GYRO_R_SENS_DEN 8383 +#endif + +/** default accel sensitivy from the datasheet + * MPU with 8g has 4096 LSB/g + * sens = 9.81 [m/s^2] / 4096 [LSB/g] * 2^INT32_ACCEL_FRAC = 2.4525 + */ +#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS +// FIXME +#define IMU_ACCEL_X_SENS 2.4525 +#define IMU_ACCEL_X_SENS_NUM 981 +#define IMU_ACCEL_X_SENS_DEN 400 +#define IMU_ACCEL_Y_SENS 2.4525 +#define IMU_ACCEL_Y_SENS_NUM 981 +#define IMU_ACCEL_Y_SENS_DEN 400 +#define IMU_ACCEL_Z_SENS 2.4525 +#define IMU_ACCEL_Z_SENS_NUM 981 +#define IMU_ACCEL_Z_SENS_DEN 400 +#endif + +/** Everything that is in the disco IMU */ +struct ImuDisco { + struct Mpu60x0_I2c mpu; ///< The MPU gyro/accel device + struct Ak8963 ak; ///< The AK8963 mag +}; + +extern struct ImuDisco imu_disco; + +extern void imu_disco_init(void); +extern void imu_disco_periodic(void); +extern void imu_disco_event(void); + +#endif /* IMU_DISCO_H */ + diff --git a/sw/airborne/subsystems/radio_control/sbus_common.c b/sw/airborne/subsystems/radio_control/sbus_common.c index 951766e3de..4f56701743 100644 --- a/sw/airborne/subsystems/radio_control/sbus_common.c +++ b/sw/airborne/subsystems/radio_control/sbus_common.c @@ -60,8 +60,8 @@ void sbus_common_init(struct Sbus *sbus_p, struct uart_periph *dev) sbus_p->status = SBUS_STATUS_UNINIT; // Set UART parameters (100K, 8 bits, 2 stops, even parity) - uart_periph_set_bits_stop_parity(dev, UBITS_8, USTOP_2, UPARITY_EVEN); uart_periph_set_baudrate(dev, B100000); + uart_periph_set_bits_stop_parity(dev, UBITS_8, USTOP_2, UPARITY_EVEN); // Set polarity #ifdef RC_POLARITY_GPIO_PORT diff --git a/sw/tools/parrot/bebop.py b/sw/tools/parrot/bebop.py index d1c2544845..d3405d06b1 100755 --- a/sw/tools/parrot/bebop.py +++ b/sw/tools/parrot/bebop.py @@ -60,6 +60,10 @@ def bebop_status(): parser = argparse.ArgumentParser(description='Bebop helper tool. Use bebop.py -h for help') parser.add_argument('--host', metavar='HOST', default='192.168.42.1', help='the ip address of bebop') +parser.add_argument('--min_version', metavar='MIN', default='3.2.0', + help='force minimum version allowed') +parser.add_argument('--max_version', metavar='MAX', default='3.9.0', + help='force maximum version allowed') subparsers = parser.add_subparsers(title='Command to execute', metavar='command', dest='command') # All the subcommands and arguments @@ -125,8 +129,8 @@ elif args.command == 'upload_file_and_run': #check firmware version v = parrot_utils.check_version(tn, '') print("Checking Bebop firmware version... " + str(v) ) - if ((v < parrot_utils.ParrotVersion('3.2.0')) or (v > parrot_utils.ParrotVersion('3.9.0'))): - print("Error: please upgrade your Bebop firmware to version between 3.2.0 and 3.9.0!") + if ((v < parrot_utils.ParrotVersion(args.min_version)) or (v > parrot_utils.ParrotVersion(args.max_version))): + print("Error: please upgrade your Bebop firmware to version between " + args.min_version + " and " + args.max_version + "!") else: print("Kill running " + f[1] + " and make folder " + args.folder) parrot_utils.execute_command(tn,"killall -9 " + f[1])