From 784813824a282e9fd28edef0b48ce8fdfdf8bbed Mon Sep 17 00:00:00 2001 From: Martin Mueller Date: Sat, 25 Feb 2017 22:46:38 +0100 Subject: [PATCH 1/4] add temperature control module for Parrot Bebop2/Disco It is possible to control the temperature of the MPU6xxx in the Bebop2 and Disco through resistors powered by PWM_6. Code mostly taken from Ardupilot Seems this does not make a big difference even if flying in cold conditions but does rather improve things than do harm. MPU temperature will go to about +15 deg C in a Bebop2 at 0 deg C ambient temperature. Having the gyro/accel calibrated at just below +50 deg C after running the original Parrot software creates a 30+ deg C change. --- conf/modules/imu_temp_ctrl.xml | 18 +++ sw/airborne/modules/ins/imu_temp_ctrl.c | 149 ++++++++++++++++++++++++ sw/airborne/modules/ins/imu_temp_ctrl.h | 51 ++++++++ 3 files changed, 218 insertions(+) create mode 100644 conf/modules/imu_temp_ctrl.xml create mode 100644 sw/airborne/modules/ins/imu_temp_ctrl.c create mode 100644 sw/airborne/modules/ins/imu_temp_ctrl.h diff --git a/conf/modules/imu_temp_ctrl.xml b/conf/modules/imu_temp_ctrl.xml new file mode 100644 index 0000000000..f283fb138f --- /dev/null +++ b/conf/modules/imu_temp_ctrl.xml @@ -0,0 +1,18 @@ + + + + + + Bebop2/Disco INS (MPU6x) sensor temperature control. + + + +
+ +
+ + + + + + diff --git a/sw/airborne/modules/ins/imu_temp_ctrl.c b/sw/airborne/modules/ins/imu_temp_ctrl.c new file mode 100644 index 0000000000..e7fc041240 --- /dev/null +++ b/sw/airborne/modules/ins/imu_temp_ctrl.c @@ -0,0 +1,149 @@ +/* + * Copyright (C) 2017 The Paparazzi team + * based on code from the Ardupilot project + * + * This file is part of paparazzi. + * + * This program 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 3 of the License, or + * (at your option) any later version. + * + * This program 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 this program. If not, see . + */ + +/** + * @file modules/ins/imu_temp_ctrl.c + * + * INS temperature control on pwm 6 for Bebop2, pwm 10 for DISCO + * + * Controls the heating resistors in the Bebop2 to keep the MPU6050 + * gyro/accel INS sensors at a constant temperature + * + */ + +#include +#include +#include + +#include "std.h" +#include "mcu_periph/uart.h" +#include "pprzlink/messages.h" +#include "subsystems/datalink/downlink.h" +#include "imu_temp_ctrl.h" + +uint8_t imu_temp_ctrl_ok = 0; +int pwm_heat_duty_fd = 0; + +#ifdef BEBOP_VERSION2 +#define PWM_HEAT_CHAN PWM_HEAT_CHAN_BEBOP2 +#elif BOARD==disco +#define PWM_HEAT_CHAN PWM_HEAT_CHAN_DISCO +#else +#error temp control only implemented in Parrot Bebop2 and Disco +#endif + +void imu_temp_ctrl_periodic(void) +{ + float temp_current, error; + static float sum_error = 0; + uint32_t output = 0; + + temp_current = imu_bebop.mpu.temp; + + if (imu_temp_ctrl_ok == 1) { + /* minimal PI algo without dt from Ardupilot */ + error = (float) ((IMU_TEMP_TARGET) - temp_current); + + /* Don't accumulate errors if the integrated error is superior + * to the max duty cycle(pwm_period) + */ + if ((fabsf(sum_error) * IMU_TEMP_CTRL_KI < IMU_TEMP_CTRL_DUTY_MAX)) { + sum_error = sum_error + error; + } + + output = IMU_TEMP_CTRL_KP * error + IMU_TEMP_CTRL_KI * sum_error; + + if (output > IMU_TEMP_CTRL_DUTY_MAX) { + output = IMU_TEMP_CTRL_DUTY_MAX; + } else if (output < 0) { + output = 0; + } + + if (dprintf(pwm_heat_duty_fd, "%u", output) < 0) { + printf("[temp-ctrl] could not set duty cycle\n"); + } + } + +#ifdef IMU_TEMP_CTRL_DEBUG + uint16_t duty_cycle; + duty_cycle = (uint16_t) ((uint32_t) output / (IMU_TEMP_CTRL_DUTY_MAX/100)); + + RunOnceEvery(IMU_TEMP_CTRL_PERIODIC_FREQ, DOWNLINK_SEND_TMP_STATUS(DefaultChannel, DefaultDevice, &duty_cycle, &temp_current)); +#endif +} + +void imu_temp_ctrl_init(void) +{ + int pwm_heat_run_fd, ret; + char* path; + + ret = asprintf(&path, "/sys/class/pwm/pwm_%u/run", PWM_HEAT_CHAN); + if (ret < 0) { + printf("[temp-ctrl] could not create pwm path\n"); + return; + } + + pwm_heat_run_fd = open(path, O_WRONLY | O_CREAT | O_TRUNC, 0666); + free(path); + if (pwm_heat_run_fd < 0) { + printf("[temp-ctrl] could not open pwm run device\n"); + return; + } + + ret = asprintf(&path, "/sys/class/pwm/pwm_%u/duty_ns", PWM_HEAT_CHAN); + if (ret < 0) { + printf("[temp-ctrl] could not create pwm duty path\n"); + close(pwm_heat_run_fd); + return; + } + + pwm_heat_duty_fd = open(path, O_WRONLY | O_CREAT | O_TRUNC, 0666); + free(path); + if (pwm_heat_duty_fd < 0) { + printf("[temp-ctrl] could not open pwm duty path\n"); + close(pwm_heat_run_fd); + return; + } + + ret = write(pwm_heat_duty_fd, "0", 1); + if (ret != 1) { + printf("[temp-ctrl] could not set duty cycle\n"); + goto error; + } + + ret = write(pwm_heat_run_fd, "0", 1); + if (ret != 1) { + printf("[temp-ctrl] could not disable pwm\n"); + goto error; + } + + ret = write(pwm_heat_run_fd, "1", 1); + if (ret != 1) { + printf("[temp-ctrl] could not enable pwm\n"); + goto error; + } + + imu_temp_ctrl_ok = 1; + return; + +error: + close(pwm_heat_run_fd); + close(pwm_heat_duty_fd); +} diff --git a/sw/airborne/modules/ins/imu_temp_ctrl.h b/sw/airborne/modules/ins/imu_temp_ctrl.h new file mode 100644 index 0000000000..d4fb08f3bb --- /dev/null +++ b/sw/airborne/modules/ins/imu_temp_ctrl.h @@ -0,0 +1,51 @@ +/* + * Copyright (C) 2017 The Paparazzi team + * based on code from the Ardupilot project + * + * This file is part of paparazzi. + * + * This program 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 3 of the License, or + * (at your option) any later version. + * + * This program 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 this program. If not, see . + */ + +/** + * @file modules/ins/imu_temp_ctrl.c + * + * INS temperature control on pwm 6 for Bebop2, pwm 10 for DISCO + * + * Controls the heating resistors in the Bebop2 to keep the MPU6050 + * gyro/accel INS sensors at a constant temperature + * + */ + +#ifndef IMU_TEMP_CTRL_H +#define IMU_TEMP_CTRL_H + +#include "std.h" + +void imu_temp_ctrl_init(void); +void imu_temp_ctrl_periodic(void); + +#define IMU_TEMP_CTRL_DUTY_MAX 125000 + +#ifndef IMU_TEMP_TARGET +#define IMU_TEMP_TARGET 50 +#endif + +#define IMU_TEMP_CTRL_KP 20000 +#define IMU_TEMP_CTRL_KI 60 + +#define PWM_HEAT_CHAN_BEBOP2 6 +#define PWM_HEAT_CHAN_DISCO 10 + +#endif /* IMU_TEMP_CTRL_H */ From c8f657b39ef06d53e39e7041b298d96355ba8bf2 Mon Sep 17 00:00:00 2001 From: Martin Mueller Date: Wed, 1 Mar 2017 23:06:37 +0100 Subject: [PATCH 2/4] [bebop2] add usb dual cdc board to Bebop2 for serial modem and payload Sparkfun FIO v3 board adds simultaneous XBee serial modem and SHT75 humidity payload sensor. --- conf/modules/humid_sht_uart.xml | 25 ++ .../libraries/DUALCDCUSB/DUALCDCUSB.cpp | 265 ++++++++++++++ .../libraries/DUALCDCUSB/DUALCDCUSB.h | 146 ++++++++ .../non_ap/modemhumid/libraries/readme.txt | 5 + .../non_ap/modemhumid/modemhumid.ino | 86 +++++ .../firmwares/non_ap/modemhumid/sht.ino | 346 ++++++++++++++++++ sw/airborne/modules/meteo/humid_sht_uart.c | 115 ++++++ sw/airborne/modules/meteo/humid_sht_uart.h | 60 +++ 8 files changed, 1048 insertions(+) create mode 100644 conf/modules/humid_sht_uart.xml create mode 100644 sw/airborne/firmwares/non_ap/modemhumid/libraries/DUALCDCUSB/DUALCDCUSB.cpp create mode 100644 sw/airborne/firmwares/non_ap/modemhumid/libraries/DUALCDCUSB/DUALCDCUSB.h create mode 100644 sw/airborne/firmwares/non_ap/modemhumid/libraries/readme.txt create mode 100644 sw/airborne/firmwares/non_ap/modemhumid/modemhumid.ino create mode 100644 sw/airborne/firmwares/non_ap/modemhumid/sht.ino create mode 100644 sw/airborne/modules/meteo/humid_sht_uart.c create mode 100644 sw/airborne/modules/meteo/humid_sht_uart.h diff --git a/conf/modules/humid_sht_uart.xml b/conf/modules/humid_sht_uart.xml new file mode 100644 index 0000000000..07ed12e44c --- /dev/null +++ b/conf/modules/humid_sht_uart.xml @@ -0,0 +1,25 @@ + + + + + + SHTxx humidity sensor. + This reads the values for humidity and temperature from the SHTxx sensor through an uart. + + + +
+ +
+ + + + + + + + + + + + diff --git a/sw/airborne/firmwares/non_ap/modemhumid/libraries/DUALCDCUSB/DUALCDCUSB.cpp b/sw/airborne/firmwares/non_ap/modemhumid/libraries/DUALCDCUSB/DUALCDCUSB.cpp new file mode 100644 index 0000000000..1747ddbde5 --- /dev/null +++ b/sw/airborne/firmwares/non_ap/modemhumid/libraries/DUALCDCUSB/DUALCDCUSB.cpp @@ -0,0 +1,265 @@ + + +/* Copyright (c) 2011, Peter Barrett +** +** Permission to use, copy, modify, and/or distribute this software for +** any purpose with or without fee is hereby granted, provided that the +** above copyright notice and this permission notice appear in all copies. +** +** THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL +** WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED +** WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR +** BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES +** OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, +** WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, +** ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS +** SOFTWARE. +*/ + +/* adapted to support 2nd CDC USB port by Martin Mueller */ + +#include +#include "USBAPI.h" +#include "DUALCDCUSB.h" + +#if defined(USBCON) + +typedef struct +{ + u32 dwDTERate; + u8 bCharFormat; + u8 bParityType; + u8 bDataBits; + u8 lineState; +} LineInfo; + +static volatile LineInfo _usbLineInfo = { 57600, 0x00, 0x00, 0x00, 0x00 }; +static volatile int32_t breakValue = -1; + +DUALCDCUSB_ DualCDCUSB; + +bool DUALCDCUSB_::setup(USBSetup& setup) +{ + u8 r = setup.bRequest; + u8 requestType = setup.bmRequestType; + + if (REQUEST_DEVICETOHOST_CLASS_INTERFACE == requestType) { + if (CDC_GET_LINE_CODING == r) { + USB_SendControl(0,(void*)&_usbLineInfo,7); + return true; + } + } + + if (REQUEST_HOSTTODEVICE_CLASS_INTERFACE == requestType) { + if (CDC_SEND_BREAK == r) { + breakValue = ((uint16_t)setup.wValueH << 8) | setup.wValueL; + } + + if (CDC_SET_LINE_CODING == r) { + USB_RecvControl((void*)&_usbLineInfo,7); + } + + if (CDC_SET_CONTROL_LINE_STATE == r) { + _usbLineInfo.lineState = setup.wValueL; + } + + if (CDC_SET_LINE_CODING == r || CDC_SET_CONTROL_LINE_STATE == r) { + /* nothing to do here */ + } + return true; + } + return false; +} + +void DUALCDCUSB_::get_status(unsigned char* buf) +{ +} + +int DUALCDCUSB_::getInterface(uint8_t* interfaceCount) +{ + *interfaceCount += 2; + + CDCDescriptor _cdcInterface2 = { + /* Interface Association */ + D_IAD(2,2,CDC_COMMUNICATION_INTERFACE_CLASS,CDC_ABSTRACT_CONTROL_MODEL,1), + + /* CDC communication interface */ + D_INTERFACE(CDC_ACM_INTERFACE2,1,CDC_COMMUNICATION_INTERFACE_CLASS,CDC_ABSTRACT_CONTROL_MODEL,0), + /* Header (1.10 bcd) */ + D_CDCCS(CDC_HEADER,0x10,0x01), + /* Device handles call management (not) */ + D_CDCCS(CDC_CALL_MANAGEMENT,1,1), + /* SET_LINE_CODING, GET_LINE_CODING, SET_CONTROL_LINE_STATE supported */ + D_CDCCS4(CDC_ABSTRACT_CONTROL_MANAGEMENT,6), + /* Communication interface is master, data interface is slave 0 */ + D_CDCCS(CDC_UNION,CDC_ACM_INTERFACE2,CDC_DATA_INTERFACE2), + D_ENDPOINT(USB_ENDPOINT_IN (CDC_ENDPOINT_ACM2),USB_ENDPOINT_TYPE_INTERRUPT,0x10,0x40), + + /* CDC data interface */ + D_INTERFACE(CDC_DATA_INTERFACE2,2,CDC_DATA_INTERFACE_CLASS,0,0), + D_ENDPOINT(USB_ENDPOINT_OUT(CDC_ENDPOINT_OUT2),USB_ENDPOINT_TYPE_BULK,USB_EP_SIZE,0), + D_ENDPOINT(USB_ENDPOINT_IN(CDC_ENDPOINT_IN2),USB_ENDPOINT_TYPE_BULK,USB_EP_SIZE,0) + }; + return USB_SendControl(0, &_cdcInterface2, sizeof(_cdcInterface2)); +} + +int DUALCDCUSB_::getDescriptor(USBSetup& setup __attribute__((unused))) +{ + return 0; +} + +uint8_t DUALCDCUSB_::getShortName(char* name) +{ + memcpy(name, "Paparazzi", 9); + return 9; +} + +bool DUALCDCUSB_::begin(void) +{ +} + +DUALCDCUSB_::DUALCDCUSB_(void) : PluggableUSBModule(3, 2, epType) +{ + epType[0] = EP_TYPE_INTERRUPT_IN; + epType[1] = EP_TYPE_BULK_OUT; + epType[2] = EP_TYPE_BULK_IN; + PluggableUSB().plug(this); +} + +void Serial__::begin(unsigned long /* baud_count */) +{ + peek_buffer = -1; +} + +void Serial__::begin(unsigned long /* baud_count */, byte /* config */) +{ + peek_buffer = -1; +} + +void Serial__::end(void) +{ +} + +int Serial__::available(void) +{ + if (peek_buffer >= 0) { + return 1 + USB_Available(CDC_RX2); + } + return USB_Available(CDC_RX2); +} + +int Serial__::peek(void) +{ + if (peek_buffer < 0) + peek_buffer = USB_Recv(CDC_RX2); + return peek_buffer; +} + +int Serial__::read(void) +{ + if (peek_buffer >= 0) { + int c = peek_buffer; + peek_buffer = -1; + return c; + } + return USB_Recv(CDC_RX2); +} + +int Serial__::availableForWrite(void) +{ + return USB_SendSpace(CDC_TX2); +} + +void Serial__::flush(void) +{ + USB_Flush(CDC_TX2); +} + +size_t Serial__::write(uint8_t c) +{ + return write(&c, 1); +} + +size_t Serial__::write(const uint8_t *buffer, size_t size) +{ + /* only try to send bytes if the high-level CDC connection itself + is open (not just the pipe) - the OS should set lineState when the port + is opened and clear lineState when the port is closed. + bytes sent before the user opens the connection or after + the connection is closed are lost - just like with a UART. */ + + // TODO - ZE - check behavior on different OSes and test what happens if an + // open connection isn't broken cleanly (cable is yanked out, host dies + // or locks up, or host virtual serial port hangs) + if (_usbLineInfo.lineState > 0) { + int r = USB_Send(CDC_TX2|TRANSFER_RELEASE,buffer,size); + if (r > 0) { + return r; + } else { + setWriteError(); + return 0; + } + } + setWriteError(); + return 0; +} + +// This operator is a convenient way for a sketch to check whether the +// port has actually been configured and opened by the host (as opposed +// to just being connected to the host). It can be used, for example, in +// setup() before printing to ensure that an application on the host is +// actually ready to receive and display the data. +// We add a short delay before returning to fix a bug observed by Federico +// where the port is configured (lineState != 0) but not quite opened. +Serial__::operator bool() { + bool result = false; + if (_usbLineInfo.lineState > 0) + result = true; + delay(10); + return result; +} + +unsigned long Serial__::baud() { + // Disable interrupts while reading a multi-byte value + uint32_t baudrate; + ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { + baudrate = _usbLineInfo.dwDTERate; + } + return baudrate; +} + +uint8_t Serial__::stopbits() { + return _usbLineInfo.bCharFormat; +} + +uint8_t Serial__::paritytype() { + return _usbLineInfo.bParityType; +} + +uint8_t Serial__::numbits() { + return _usbLineInfo.bDataBits; +} + +bool Serial__::dtr() { + return _usbLineInfo.lineState & 0x1; +} + +bool Serial__::rts() { + return _usbLineInfo.lineState & 0x2; +} + +int32_t Serial__::readBreak() { + int32_t ret; + // Disable IRQs while reading and clearing breakValue to make + // sure we don't overwrite a value just set by the ISR. + ATOMIC_BLOCK(ATOMIC_RESTORESTATE) { + ret = breakValue; + breakValue = -1; + } + return ret; +} + +Serial__ Serial2; + + +#endif /* USBCON */ diff --git a/sw/airborne/firmwares/non_ap/modemhumid/libraries/DUALCDCUSB/DUALCDCUSB.h b/sw/airborne/firmwares/non_ap/modemhumid/libraries/DUALCDCUSB/DUALCDCUSB.h new file mode 100644 index 0000000000..4a069e721b --- /dev/null +++ b/sw/airborne/firmwares/non_ap/modemhumid/libraries/DUALCDCUSB/DUALCDCUSB.h @@ -0,0 +1,146 @@ + +#ifndef DUALCDCUSB_H +#define DUALCDCUSB_H + +#include +#include + +#if ARDUINO < 10606 +#error DUALCDCUSB needs Arduino IDE 1.6.6 or higher +#endif + +#if !defined(USBCON) +#error DUALCDCUSB needs an USB MCU +#endif + +#if defined(ARDUINO_ARCH_AVR) +#include "PluggableUSB.h" +#else +#include "USB/PluggableUSB.h" +#endif + +#if defined(__SAMD21G18A__) +#define USB_SendControl USBDevice.sendControl +#define USB_Available USBDevice.available +#define USB_Recv USBDevice.recv +#define USB_Send USBDevice.send +#define USB_Flush USBDevice.flush +#define is_write_enabled(x) (1) +#endif +#define CDC_ENDPOINT_ACM2 (4) +#define CDC_ENDPOINT_OUT2 (5) +#define CDC_ENDPOINT_IN2 (6) + +#define CDC_ACM_INTERFACE2 2 // CDC ACM +#define CDC_DATA_INTERFACE2 3 // CDC Data + +#define CDC_RX2 CDC_ENDPOINT_OUT2 +#define CDC_TX2 CDC_ENDPOINT_IN2 + +#ifndef SERIAL_BUFFER_SIZE +#if ((RAMEND - RAMSTART) < 1023) +#define SERIAL_BUFFER_SIZE 16 +#else +#define SERIAL_BUFFER_SIZE 64 +#endif +#endif +#if (SERIAL_BUFFER_SIZE>256) +#error Please lower the CDC Buffer size +#endif + +_Pragma("pack(1)") + + + +class Serial__ : public Stream +{ +private: + int peek_buffer; +public: + Serial__() { peek_buffer = -1; }; + void begin(unsigned long); + void begin(unsigned long, uint8_t); + void end(void); + + virtual int available(void); + virtual int peek(void); + virtual int read(void); + int availableForWrite(void); + virtual void flush(void); + virtual size_t write(uint8_t); + virtual size_t write(const uint8_t*, size_t); + using Print::write; // pull in write(str) and write(buf, size) from Print + operator bool(); + + volatile uint8_t _rx_buffer_head; + volatile uint8_t _rx_buffer_tail; + unsigned char _rx_buffer[SERIAL_BUFFER_SIZE]; + + // This method allows processing "SEND_BREAK" requests sent by + // the USB host. Those requests indicate that the host wants to + // send a BREAK signal and are accompanied by a single uint16_t + // value, specifying the duration of the break. The value 0 + // means to end any current break, while the value 0xffff means + // to start an indefinite break. + // readBreak() will return the value of the most recent break + // request, but will return it at most once, returning -1 when + // readBreak() is called again (until another break request is + // received, which is again returned once). + // This also mean that if two break requests are received + // without readBreak() being called in between, the value of the + // first request is lost. + // Note that the value returned is a long, so it can return + // 0-0xffff as well as -1. + int32_t readBreak(); + + // These return the settings specified by the USB host for the + // serial port. These aren't really used, but are offered here + // in case a sketch wants to act on these settings. + uint32_t baud(); + uint8_t stopbits(); + uint8_t paritytype(); + uint8_t numbits(); + bool dtr(); + bool rts(); + enum { + ONE_STOP_BIT = 0, + ONE_AND_HALF_STOP_BIT = 1, + TWO_STOP_BITS = 2, + }; + enum { + NO_PARITY = 0, + ODD_PARITY = 1, + EVEN_PARITY = 2, + MARK_PARITY = 3, + SPACE_PARITY = 4, + }; + +}; +extern Serial__ Serial2; + +typedef struct { + InterfaceDescriptor DualCDCInterface; +} dual_cdc_usb_Descriptor; + +_Pragma("pack()") + +class DUALCDCUSB_ : public PluggableUSBModule { + +protected: + int getInterface(uint8_t* interfaceNum); + int getDescriptor(USBSetup& setup); + bool setup(USBSetup& setup); + uint8_t getShortName(char* name); + +public: + bool begin(void); + void get_status(unsigned char* buf); + DUALCDCUSB_(void); + +private: + uint8_t epType[3]; +}; + +extern DUALCDCUSB_ DualCDCUSB; + +#endif /* DUALCDCUSB_H */ diff --git a/sw/airborne/firmwares/non_ap/modemhumid/libraries/readme.txt b/sw/airborne/firmwares/non_ap/modemhumid/libraries/readme.txt new file mode 100644 index 0000000000..c62e90e279 --- /dev/null +++ b/sw/airborne/firmwares/non_ap/modemhumid/libraries/readme.txt @@ -0,0 +1,5 @@ +This is a library to create two serial USB ports with ATMEGA32U4 based +Arduino boards as the Sparkfun FIO v3. Copy it to your local Ardiono library +folder, usually ~/Arduino/libraries + +Tested with Arduino 1.8.1 diff --git a/sw/airborne/firmwares/non_ap/modemhumid/modemhumid.ino b/sw/airborne/firmwares/non_ap/modemhumid/modemhumid.ino new file mode 100644 index 0000000000..a22482c238 --- /dev/null +++ b/sw/airborne/firmwares/non_ap/modemhumid/modemhumid.ino @@ -0,0 +1,86 @@ +/* + * Copyright (C) 2017 The Paparazzi Team + * + * 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 modemhumid.ino + * + * modem and humidity sensor for Parrot Bebop2 using two serial ports + * through a Sparkfun FIO v3 with XBee and SHT75 sensor + * + * requires DUALCDCUSB library, tested with Arduino v1.8.1 + * + */ + +#include + +int RXLED = 17; + +void setup() +{ + pinMode(RXLED, OUTPUT); // Set RX LED as an output + + Serial.begin(57600); // USB CDC 0 port + Serial1.begin(57600); // serial port + Serial2.begin(57600); // USB CDC 1 port + + TXLED1; // set the LED off + digitalWrite(RXLED, HIGH); // set the LED off + + humid_sht_init(); +} + +void loop() +{ + int i, av, ret; + size_t re; + unsigned char buff[256], device_addr = 0xF0, len = 2; + unsigned short tmd_temperature; + float ftmd_temperature; + static unsigned long ticks = 0, last_ticks = 0; + + while (1) { + + /* USB CDC 0 -> UART */ + av = Serial.available(); + if (av > 0) { + if (av > 256) + av = 256; + ret = Serial.readBytes(buff, av); + Serial1.write(buff, ret); + } + + /* UART -> USB CDC 0*/ + av = Serial1.available(); + if (av > 0) { + if (av > 256) + av = 256; + ret = Serial1.readBytes(buff, av); + Serial.write(buff, ret); + } + + ticks = millis(); + if (last_ticks == 0) last_ticks = ticks; + if (last_ticks + 2 < ticks) { + last_ticks = ticks; + humid_sht_periodic(); + } + } +} diff --git a/sw/airborne/firmwares/non_ap/modemhumid/sht.ino b/sw/airborne/firmwares/non_ap/modemhumid/sht.ino new file mode 100644 index 0000000000..eb51e1082b --- /dev/null +++ b/sw/airborne/firmwares/non_ap/modemhumid/sht.ino @@ -0,0 +1,346 @@ + +//#define DEBUG + +#define TRUE true +#define FALSE false + +#define noACK 0 +#define ACK 1 +#define TEMP 0 +#define HUMI 1 + +//adr command r/w +//000 0011 0 +#define STATUS_REG_W 0x06 +//000 0011 1 +#define STATUS_REG_R 0x07 +//000 0001 1 +#define MEASURE_TEMP 0x03 +//000 0010 1 +#define MEASURE_HUMI 0x05 +//000 1111 0 +#define RESET 0x1e + + +#define SHT_IDLE 0 +#define SHT_MEASURING_HUMID 1 +#define SHT_MEASURING_TEMP 2 + +/* D4, PD4 */ +#define SHT_DAT_GPIO 4 +/* D5, PC6 */ +#define SHT_SCK_GPIO 5 + +#if !defined SHT_DAT_GPIO || !defined SHT_SCK_GPIO +#error You need to define SHT_DAT_GPIO and SHT_SCK_GPIO +#endif + +/// set data pin to input +#define DATA_SET pinMode(SHT_DAT_GPIO, INPUT); +/// set data pin to output +#define DATA_CLR pinMode(SHT_DAT_GPIO, OUTPUT) +/// get data pin +#define DATA_IN digitalRead(SHT_DAT_GPIO) + +/// set clock pin to high +#define SCK_SET digitalWrite(SHT_SCK_GPIO, HIGH) +/// set clock pin to low +#define SCK_CLR digitalWrite(SHT_SCK_GPIO, LOW) +/// set clock pin to output +#define SCK_OUT pinMode(SHT_SCK_GPIO, OUTPUT) + +uint16_t humidsht, tempsht; +float fhumidsht, ftempsht; +bool humid_sht_available; +uint8_t humid_sht_status; + +uint8_t s_write_byte(uint8_t value); +uint8_t s_read_byte(uint8_t ack); +void s_transstart(void); +void s_connectionreset(void); +uint8_t s_read_statusreg(uint8_t *p_value, uint8_t *p_checksum); +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); + + +uint8_t s_write_byte(uint8_t value) +{ + 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; } + SCK_SET; //clk for SENSI-BUS + 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) + SCK_CLR; + + return error; //error=1 in case of no acknowledge +} + +uint8_t s_read_byte(uint8_t ack) +{ + uint8_t i, val = 0; + + DATA_SET; //release DATA-line + 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 + SCK_CLR; + } + + 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 + // _nop_();_nop_();_nop_(); //pulswith approx. 5 us + SCK_CLR; + DATA_SET; //release DATA-line + return val; +} + +void s_transstart(void) +{ + // generates a transmission start + // _____ ________ + // DATA: |_______| + // ___ ___ + // SCK : ___| |___| |______ + + DATA_SET; SCK_CLR; //Initial state + SCK_CLR;// _nop_(); + SCK_SET; + SCK_SET;// _nop_(); + DATA_CLR; + DATA_CLR;// _nop_(); + SCK_CLR; + SCK_CLR; SCK_CLR; SCK_CLR; // _nop_();_nop_();_nop_(); + SCK_SET; + SCK_SET;// _nop_(); + DATA_SET; + DATA_SET;// _nop_(); + SCK_CLR; +} + +void s_connectionreset(void) +{ + // communication reset: DATA-line=1 and at least 9 SCK cycles followed by transstart + // _____________________________________________________ ________ + // DATA: |_______| + // _ _ _ _ _ _ _ _ _ ___ ___ + // SCK : __| |__| |__| |__| |__| |__| |__| |__| |__| |______| |___| |______ + + uint8_t i; + + DATA_SET; SCK_CLR; //Initial state + for (i = 0; i < 9; i++) { //9 SCK cycles + SCK_SET; + SCK_CLR; + } + s_transstart(); //transmission start +} + +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; + + 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) + 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; + + 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 + 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; + 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; + 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 + *(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 + + return error; +} + +uint8_t s_start_measure(uint8_t mode) +{ + // makes a measurement (humidity/temperature) with checksum + 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; + default : break; + } + + return error; +} + +uint8_t s_read_measure(uint16_t *p_value, uint8_t *p_checksum) +{ + // reads a measurement (humidity/temperature) with checksum + uint8_t error = 0; + + 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 + + return error; +} + +void calc_sht(uint16_t hum, uint16_t tem, float *fhum , float *ftem) +{ + // calculates temperature [ C] and humidity [%RH] + // input : humi [Ticks] (12 bit) + // temp [Ticks] (14 bit) + // output: humi [%RH] + // temp [ C] + + 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 T1 = 0.01; // for 14 Bit @ 5V + const float T2 = 0.00008; // for 14 Bit @ 5V + float rh; // rh: Humidity [Ticks] 12 Bit + float t; // t: Temperature [Ticks] 14 Bit + float rh_lin; // rh_lin: Humidity linear + float rh_true; // rh_true: Temperature compensated humidity + float t_C; // t_C : Temperature [ C] + + 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 + *ftem = t_C; //return temperature [ C] + *fhum = rh_true; //return humidity[%RH] +} + +uint8_t humid_sht_reset(void) +{ + // resets the sensor by a softreset + uint8_t error = 0; + + s_connectionreset(); //reset communication + 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) +{ + /* Configure DAT/SCL pin as GPIO */ + DATA_SET; + SCK_OUT; + SCK_CLR; + + humid_sht_available = FALSE; + humid_sht_status = SHT_IDLE; +} + +void humid_sht_periodic(void) +{ + uint8_t error = 0, checksum, i; + uint8_t data[9] = { 0 }; + uint16_t chk = 0; + + 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) { + /* busy? */ + if (DATA_IN) return; + /* get data */ + error += s_read_measure(&humidsht, &checksum); + + if (error != 0) { + s_connectionreset(); + s_start_measure(HUMI); //restart + } else { + error += s_start_measure(TEMP); + humid_sht_status = SHT_MEASURING_TEMP; + } + } else if (humid_sht_status == SHT_MEASURING_TEMP) { + /* busy? */ + if (DATA_IN) return; + /* get data */ + error += s_read_measure(&tempsht, &checksum); + + if (error != 0) { + s_connectionreset(); + s_start_measure(TEMP); //restart + } else { + + /* + [0] 0xFF + [1] sht_temp_0 + [2] sht_temp_1 + [3] sht_humid_0 + [4] sht_humid_1 + [5] checksum + */ + + data[0] = 0xFF; + data[1] = tempsht & 0xFF; + data[2] = (tempsht >> 8) & 0xFF; + data[3] = humidsht & 0xFF; + data[4] = (humidsht >> 8) & 0xFF; + for (i = 1; i < 5; i++) + chk += data[i]; + data[5] = chk & 0xFF; + + humid_sht_available = TRUE; + s_connectionreset(); + s_start_measure(HUMI); + humid_sht_status = SHT_MEASURING_HUMID; + +#ifdef DEBUG + calc_sht(humidsht, tempsht, &fhumidsht, &ftempsht); + Serial2.println(fhumidsht); + Serial2.println(ftempsht); +#else + Serial2.write(data, 6); +#endif + + humid_sht_available = FALSE; + } + } +} + diff --git a/sw/airborne/modules/meteo/humid_sht_uart.c b/sw/airborne/modules/meteo/humid_sht_uart.c new file mode 100644 index 0000000000..ed6390ee26 --- /dev/null +++ b/sw/airborne/modules/meteo/humid_sht_uart.c @@ -0,0 +1,115 @@ +/* + * Copyright (C) 2008-2017 The Paparazzi team + * + * 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 modules/meteo/humid_sht_uart.c + * + * SHTxx sensor interface + * + * This reads the values for humidity and temperature from the SHTxx sensor through an uart. + * + */ + +#include "std.h" +#include "mcu_periph/gpio.h" +#include "mcu_periph/uart.h" +#include "pprzlink/messages.h" +#include "subsystems/datalink/downlink.h" +#include "humid_sht_uart.h" + +uint16_t humidsht, tempsht; +float fhumidsht, ftempsht; +bool humid_sht_available; + +void calc_sht(uint16_t hum, uint16_t tem, float *fhum , float *ftem); +void humid_sht_uart_parse(uint8_t c); + +void calc_sht(uint16_t hum, uint16_t tem, float *fhum , float *ftem) +{ + // calculates temperature [ C] and humidity [%RH] + // input : humi [Ticks] (12 bit) + // temp [Ticks] (14 bit) + // output: humi [%RH] + // temp [ C] + + 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 T1 = 0.01; // for 14 Bit @ 5V + const float T2 = 0.00008; // for 14 Bit @ 5V + float rh; // rh: Humidity [Ticks] 12 Bit + float t; // t: Temperature [Ticks] 14 Bit + float rh_lin; // rh_lin: Humidity linear + float rh_true; // rh_true: Temperature compensated humidity + float t_C; // t_C : Temperature [ C] + + 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 + *ftem = t_C; //return temperature [ C] + *fhum = rh_true; //return humidity[%RH] +} + +void humid_sht_uart_periodic(void) +{ +} + +/* airspeed_otf_parse */ +void humid_sht_uart_parse(uint8_t c) +{ + static uint8_t msg_cnt = 0; + static uint8_t data[6]; + uint16_t i, chk = 0; + + if (msg_cnt > 0) { + data[msg_cnt++] = c; + if (msg_cnt == 6) { + tempsht = data[1] | (data[2] << 8); + humidsht = data[3] | (data[4] << 8); + for (i = 1; i < 5; i++) + chk += data[i]; + if (data[5] == (chk & 0xFF)) { + calc_sht(humidsht, tempsht, &fhumidsht, &ftempsht); + DOWNLINK_SEND_SHT_STATUS(DefaultChannel, DefaultDevice, &humidsht, &tempsht, &fhumidsht, &ftempsht); + } + msg_cnt = 0; + } + } + else if (c == 0xFF) + msg_cnt = 1; +} + +void humid_sht_uart_init(void) +{ +} + +void humid_sht_uart_event(void) +{ + while (MetBuffer()) { + uint8_t ch = MetGetch(); + humid_sht_uart_parse(ch); + } +} diff --git a/sw/airborne/modules/meteo/humid_sht_uart.h b/sw/airborne/modules/meteo/humid_sht_uart.h new file mode 100644 index 0000000000..63cbf7f619 --- /dev/null +++ b/sw/airborne/modules/meteo/humid_sht_uart.h @@ -0,0 +1,60 @@ +/* + * Copyright (C) 2008-2017 The Paparazzi team + * + * 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 modules/meteo/humid_sht_uart.h + * + * SHTxx sensor interface. + * + * This reads the values for humidity and temperature from the SHTxx sensor through an uart. + */ + +#ifndef HUMID_SHT_H +#define HUMID_SHT_H + +#include "std.h" + +#ifndef SITL +#include "mcu_periph/uart.h" + +#define MetLinkDevice (&(MET_LINK).device) + +#define MetBuffer() MetLinkDevice->char_available(MetLinkDevice->periph) +#define MetGetch() MetLinkDevice->get_byte(MetLinkDevice->periph) +#define ReadMetBuffer() { while (MetBuffer()&&!met_msg_received) parse_met_buffer(MetGetch()); } +#define MetSend1(c) MetLinkDevice->put_byte(MetLinkDevice->periph, 0, c) +#define MetUartSend1(c) MetSend1(c) +#define MetSend(_dat,_len) { for (uint8_t i = 0; i< (_len); i++) MetSend1(_dat[i]); }; +#define MetUartSetBaudrate(_b) uart_periph_set_baudrate(&(MET_LINK), _b) +#define MetUartRunning (MET_LINK).tx_running + +#endif /** !SITL */ + +extern uint16_t humidsht, tempsht; +extern float fhumidsht, ftempsht; +extern bool humid_sht_available; +extern uint8_t humid_sht_status; + +void humid_sht_uart_init(void); +void humid_sht_uart_periodic(void); +void humid_sht_uart_event(void); + +#endif /* HUMID_SHT_H */ From c062daec544f9c13f1bfb3f940b7187de877badc Mon Sep 17 00:00:00 2001 From: Martin Mueller Date: Wed, 1 Mar 2017 23:12:15 +0100 Subject: [PATCH 3/4] [bebop2] add example ac for usb cdc --- conf/airframes/{mm => MM}/bebop.xml | 0 conf/airframes/MM/bebop2_lum1_xbee.xml | 262 +++++++++++++++++++++++++ 2 files changed, 262 insertions(+) rename conf/airframes/{mm => MM}/bebop.xml (100%) create mode 100644 conf/airframes/MM/bebop2_lum1_xbee.xml diff --git a/conf/airframes/mm/bebop.xml b/conf/airframes/MM/bebop.xml similarity index 100% rename from conf/airframes/mm/bebop.xml rename to conf/airframes/MM/bebop.xml diff --git a/conf/airframes/MM/bebop2_lum1_xbee.xml b/conf/airframes/MM/bebop2_lum1_xbee.xml new file mode 100644 index 0000000000..fd135389b3 --- /dev/null +++ b/conf/airframes/MM/bebop2_lum1_xbee.xml @@ -0,0 +1,262 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + +
+ + + + + + + + + +
+ + + +
+ +
+ + + + + + + + + + + + + + +
+ + + +
+ + + + + + + + + + + + + + + + +
+ +
+ + +
+ + +
+ + + + + + + +
+ +
+ + + + + + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + + +
+ +
+ + + + + +
+ +
+ + +
+ +
+ + + +
+ +
+ + + + + + +
+ +
+ + + + + +
+ +
+ +
+ From 0abc28a5010f7552b67bb5ac3d103a0de0c12848 Mon Sep 17 00:00:00 2001 From: Martin Mueller Date: Wed, 1 Mar 2017 23:13:04 +0100 Subject: [PATCH 4/4] [joystick] add logitech 310 and ppm2usb adapter --- conf/joystick/logitech_f310_mode1.xml | 98 +++++++++++++++++++++++++++ conf/joystick/logitech_f310_mode2.xml | 98 +++++++++++++++++++++++++++ conf/joystick/ppm2usb.xml | 26 +++++++ 3 files changed, 222 insertions(+) create mode 100644 conf/joystick/logitech_f310_mode1.xml create mode 100644 conf/joystick/logitech_f310_mode2.xml create mode 100644 conf/joystick/ppm2usb.xml diff --git a/conf/joystick/logitech_f310_mode1.xml b/conf/joystick/logitech_f310_mode1.xml new file mode 100644 index 0000000000..32c3b9852e --- /dev/null +++ b/conf/joystick/logitech_f310_mode1.xml @@ -0,0 +1,98 @@ + + + + + + + + + + + + +