mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-10-02 12:23:17 +08:00
Merge pull request #2027 from martinmm/master_ins_temp_ctrl
- add temperature control module for 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.
This commit is contained in:
Felix Ruess
GitHub
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,98 @@
|
||||
<!-- Logitech F310 controller profile
|
||||
|
||||
Joystick has six axes:
|
||||
axis 0: X on left stick
|
||||
axis 1: Y on left stick
|
||||
axis 2: linear button on left side (LT)
|
||||
axis 3: X on right stick
|
||||
axis 4: Y on right stick
|
||||
axis 5: linear button on right side (RT)
|
||||
axis 6: X on D pad
|
||||
axis 7: Y on D pad
|
||||
|
||||
and 11 buttons:
|
||||
button 0: A, green
|
||||
button 1: B, red
|
||||
button 2: X, blue
|
||||
button 3: Y, orange
|
||||
button 4: LB, left front up
|
||||
button 5: RB, left front up
|
||||
button 6: BACK, left top
|
||||
button 7: START, right top
|
||||
button 8: logitech, top "home" button
|
||||
button 9: left stick press
|
||||
button 10: right stick press
|
||||
|
||||
The "mode" button swaps the axes on the left stick and the d pad.
|
||||
-->
|
||||
|
||||
<joystick>
|
||||
<input>
|
||||
<axis index="0" name="lx" limit="1.00" trim="0"/>
|
||||
<axis index="1" name="ly" trim="0"/>
|
||||
<axis index="2" name="lt" limit="1.00" trim="0"/>
|
||||
<axis index="3" name="rx" limit="1.00" trim="0"/>
|
||||
<axis index="4" name="ry" limit="1.00" trim="0"/>
|
||||
<axis index="5" name="rt" limit="1.00" trim="0"/>
|
||||
<axis index="6" name="dpadx"/>
|
||||
<axis index="7" name="dpady"/>
|
||||
<button index="0" name="b0"/>
|
||||
<button index="1" name="b1"/>
|
||||
<button index="2" name="b2"/>
|
||||
<button index="3" name="b3"/>
|
||||
<button index="4" name="b4"/>
|
||||
<button index="5" name="b5"/>
|
||||
<button index="6" name="b6"/>
|
||||
<button index="7" name="b7"/>
|
||||
<button index="8" name="b8"/>
|
||||
<button index="9" name="b9"/>
|
||||
<button index="10" name="b10"/>
|
||||
<button index="11" name="b11"/>
|
||||
</input>
|
||||
|
||||
<variables>
|
||||
<!-- manual by default and when pressing A, AUTO1 on B, AUTO2 on Y -->
|
||||
<var name="mode" default="2"/>
|
||||
<set var="mode" value="0" on_event="b0"/>
|
||||
<set var="mode" value="1" on_event="b1"/>
|
||||
<set var="mode" value="2" on_event="b3"/>
|
||||
</variables>
|
||||
|
||||
<messages period="0.017">
|
||||
|
||||
<message class="datalink" name="RC_4CH" send_always="true">
|
||||
<field name="mode" value="mode"/>
|
||||
<field name="throttle" value="Fit(0-ry,-127,127,0,127)"/>
|
||||
<field name="roll" value="rx"/>
|
||||
<field name="yaw" value="Fit(lx,-127,127,-64,64)"/>
|
||||
<field name="pitch" value="ly"/>
|
||||
</message>
|
||||
|
||||
<!-- trim commands -->
|
||||
<!-- not needed
|
||||
<message class="trim_plus" name="lx" on_event="b9"/>
|
||||
<message class="trim_minus" name="lx" on_event="b8"/>
|
||||
<message class="trim_plus" name="ly" on_event="b10"/>
|
||||
<message class="trim_minus" name="ly" on_event="b11"/>
|
||||
<message class="trim_plus" name="rx" on_event="b2"/>
|
||||
<message class="trim_minus" name="rx" on_event="b0"/>
|
||||
<message class="trim_minus" name="ry" on_event="b3"/>
|
||||
<message class="trim_plus" name="ry" on_event="b1"/>
|
||||
<message class="trim_save" name="" on_event="b5"/>
|
||||
-->
|
||||
|
||||
<!-- Button "6" / "BACK" for kill -->
|
||||
<message class="ground" name="DL_SETTING" on_event="b6">
|
||||
<field name="index" value="IndexOfSetting(kill_throttle)"/>
|
||||
<field name="value" value="1"/>
|
||||
</message>
|
||||
|
||||
<!-- Button "7" / "START" for resurrect -->
|
||||
<message class="ground" name="DL_SETTING" on_event="b7">
|
||||
<field name="index" value="IndexOfSetting(kill_throttle)"/>
|
||||
<field name="value" value="0"/>
|
||||
</message>
|
||||
|
||||
</messages>
|
||||
|
||||
</joystick>
|
||||
@@ -0,0 +1,98 @@
|
||||
<!-- Logitech F310 controller profile
|
||||
|
||||
Joystick has six axes:
|
||||
axis 0: X on left stick
|
||||
axis 1: Y on left stick
|
||||
axis 2: linear button on left side (LT)
|
||||
axis 3: X on right stick
|
||||
axis 4: Y on right stick
|
||||
axis 5: linear button on right side (RT)
|
||||
axis 6: X on D pad
|
||||
axis 7: Y on D pad
|
||||
|
||||
and 11 buttons:
|
||||
button 0: A, green
|
||||
button 1: B, red
|
||||
button 2: X, blue
|
||||
button 3: Y, orange
|
||||
button 4: LB, left front up
|
||||
button 5: RB, left front up
|
||||
button 6: BACK, left top
|
||||
button 7: START, right top
|
||||
button 8: logitech, top "home" button
|
||||
button 9: left stick press
|
||||
button 10: right stick press
|
||||
|
||||
The "mode" button swaps the axes on the left stick and the d pad.
|
||||
-->
|
||||
|
||||
<joystick>
|
||||
<input>
|
||||
<axis index="0" name="lx" limit="1.00" trim="0"/>
|
||||
<axis index="1" name="ly" trim="0"/>
|
||||
<axis index="2" name="lt" limit="1.00" trim="0"/>
|
||||
<axis index="3" name="rx" limit="1.00" trim="0"/>
|
||||
<axis index="4" name="ry" limit="1.00" trim="0"/>
|
||||
<axis index="5" name="rt" limit="1.00" trim="0"/>
|
||||
<axis index="6" name="dpadx"/>
|
||||
<axis index="7" name="dpady"/>
|
||||
<button index="0" name="b0"/>
|
||||
<button index="1" name="b1"/>
|
||||
<button index="2" name="b2"/>
|
||||
<button index="3" name="b3"/>
|
||||
<button index="4" name="b4"/>
|
||||
<button index="5" name="b5"/>
|
||||
<button index="6" name="b6"/>
|
||||
<button index="7" name="b7"/>
|
||||
<button index="8" name="b8"/>
|
||||
<button index="9" name="b9"/>
|
||||
<button index="10" name="b10"/>
|
||||
<button index="11" name="b11"/>
|
||||
</input>
|
||||
|
||||
<variables>
|
||||
<!-- manual by default and when pressing A, AUTO1 on B, AUTO2 on Y -->
|
||||
<var name="mode" default="2"/>
|
||||
<set var="mode" value="0" on_event="b0"/>
|
||||
<set var="mode" value="1" on_event="b1"/>
|
||||
<set var="mode" value="2" on_event="b3"/>
|
||||
</variables>
|
||||
|
||||
<messages period="0.017">
|
||||
|
||||
<message class="datalink" name="RC_4CH" send_always="true">
|
||||
<field name="mode" value="mode"/>
|
||||
<field name="throttle" value="Fit(0-ly,-127,127,0,127)"/>
|
||||
<field name="roll" value="rx"/>
|
||||
<field name="yaw" value="Fit(lx,-127,127,-64,64)"/>
|
||||
<field name="pitch" value="ry"/>
|
||||
</message>
|
||||
|
||||
<!-- trim commands -->
|
||||
<!-- not needed
|
||||
<message class="trim_plus" name="lx" on_event="b9"/>
|
||||
<message class="trim_minus" name="lx" on_event="b8"/>
|
||||
<message class="trim_plus" name="ly" on_event="b10"/>
|
||||
<message class="trim_minus" name="ly" on_event="b11"/>
|
||||
<message class="trim_plus" name="rx" on_event="b2"/>
|
||||
<message class="trim_minus" name="rx" on_event="b0"/>
|
||||
<message class="trim_minus" name="ry" on_event="b3"/>
|
||||
<message class="trim_plus" name="ry" on_event="b1"/>
|
||||
<message class="trim_save" name="" on_event="b5"/>
|
||||
-->
|
||||
|
||||
<!-- Button "6" / "BACK" for kill -->
|
||||
<message class="ground" name="DL_SETTING" on_event="b6">
|
||||
<field name="index" value="IndexOfSetting(kill_throttle)"/>
|
||||
<field name="value" value="1"/>
|
||||
</message>
|
||||
|
||||
<!-- Button "7" / "START" for resurrect -->
|
||||
<message class="ground" name="DL_SETTING" on_event="b7">
|
||||
<field name="index" value="IndexOfSetting(kill_throttle)"/>
|
||||
<field name="value" value="0"/>
|
||||
</message>
|
||||
|
||||
</messages>
|
||||
|
||||
</joystick>
|
||||
@@ -0,0 +1,26 @@
|
||||
<!--
|
||||
|
||||
thomaspfeifer.net PPM2USB joystick adapter with Futaba transmitter
|
||||
|
||||
-->
|
||||
|
||||
<joystick>
|
||||
<input>
|
||||
<axis index="0" name="mode"/>
|
||||
<axis index="1" name="pitch"/>
|
||||
<axis index="2" name="throttle"/>
|
||||
<axis index="3" name="yaw"/>
|
||||
<axis index="4" name="roll"/>
|
||||
<axis index="5" name="kill"/>
|
||||
</input>
|
||||
|
||||
<messages period="0.05">
|
||||
<message class="datalink" name="RC_4CH" send_always="true">
|
||||
<field name="mode" value="PprzMode(mode)"/>
|
||||
<field name="throttle" value="Fit(throttle,-100,100,0,127)" />
|
||||
<field name="roll" value="Fit(-roll,-100,100,-127,127)" />
|
||||
<field name="pitch" value="Fit(pitch,-100,100,-127,127)" />
|
||||
<field name="yaw" value="Fit(yaw,-100,100,-127,127)"/>
|
||||
</message>
|
||||
</messages>
|
||||
</joystick>
|
||||
@@ -0,0 +1,25 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="humid_sht_uart" dir="meteo">
|
||||
<doc>
|
||||
<description>
|
||||
SHTxx humidity sensor.
|
||||
This reads the values for humidity and temperature from the SHTxx sensor through an uart.
|
||||
</description>
|
||||
<configure name="SHT_UART" value="UARTX" description="UART on which computer is connected"/>
|
||||
</doc>
|
||||
<header>
|
||||
<file name="humid_sht_uart.h"/>
|
||||
</header>
|
||||
<init fun="humid_sht_uart_init()"/>
|
||||
<periodic fun="humid_sht_uart_periodic()" freq="1."/>
|
||||
<event fun="humid_sht_uart_event()"/>
|
||||
<makefile target="ap">
|
||||
<configure name="SHT_UART" case="upper|lower"/>
|
||||
<file name="humid_sht_uart.c"/>
|
||||
<define name="USE_$(SHT_UART_UPPER)"/>
|
||||
<define name="MET_LINK" value="$(SHT_UART_LOWER)"/>
|
||||
<define name="$(SHT_UART_UPPER)_BAUD" value="B115200"/>
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
@@ -0,0 +1,18 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="imu_temp_ctrl" dir="ins">
|
||||
<doc>
|
||||
<description>
|
||||
Bebop2/Disco INS (MPU6x) sensor temperature control.
|
||||
</description>
|
||||
<define name="IMU_TEMP_CTRL_DEBUG" description="output heater setting in percent and temperature in TMP_STATUS message"/>
|
||||
</doc>
|
||||
<header>
|
||||
<file name="imu_temp_ctrl.h"/>
|
||||
</header>
|
||||
<init fun="imu_temp_ctrl_init()"/>
|
||||
<periodic fun="imu_temp_ctrl_periodic()" freq="20."/>
|
||||
<makefile target="ap">
|
||||
<file name="imu_temp_ctrl.c"/>
|
||||
</makefile>
|
||||
</module>
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,146 @@
|
||||
|
||||
#ifndef DUALCDCUSB_H
|
||||
#define DUALCDCUSB_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <Arduino.h>
|
||||
|
||||
#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 */
|
||||
@@ -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
|
||||
@@ -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 <DUALCDCUSB.h>
|
||||
|
||||
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();
|
||||
}
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @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 <stdio.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
#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);
|
||||
}
|
||||
@@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @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 */
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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 */
|
||||
Reference in New Issue
Block a user