dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'master' of github.com:PX4/Firmware into airspeed_test_fix

This commit is contained in:
Lorenz Meier 2014-07-12 21:35:46 +02:00
parent 0d1ac42354 5f8baed876
commit 5a22ef1c28
50 changed files with 1842 additions and 209 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Makefile
View File

@ -210,11 +210,11 @@ menuconfig:
endif
$(NUTTX_SRC):
$(Q) (./Tools/check_submodules.sh)
$(Q) ($(PX4_BASE)/Tools/check_submodules.sh)
.PHONY: checksubmodules
checksubmodules:
$(Q) (./Tools/check_submodules.sh)
$(Q) ($(PX4_BASE)/Tools/check_submodules.sh)
.PHONY: updatesubmodules
updatesubmodules:

28
ROMFS/px4fmu_common/init.d/3033_wingwing
View File

@ -11,25 +11,35 @@ if [ $DO_AUTOCONFIG == yes ]
then
param set BAT_N_CELLS 2
param set FW_AIRSPD_MAX 15
param set FW_AIRSPD_MIN 7
param set FW_AIRSPD_TRIM 11
param set FW_AIRSPD_MIN 10
param set FW_AIRSPD_TRIM 13
param set FW_ATT_TC 0.3
param set FW_L1_DAMPING 0.74
param set FW_L1_PERIOD 12
param set FW_L1_PERIOD 16
param set FW_LND_ANG 15
param set FW_LND_FLALT 5
param set FW_LND_HHDIST 15
param set FW_LND_HVIRT 13
param set FW_LND_TLALT 5
param set FW_THR_LND_MAX 0
param set FW_P_ROLLFF 2
param set FW_PR_FF 0.6
param set FW_PR_IMAX 0.2
param set FW_PR_P 0.06
param set FW_PR_FF 0.35
param set FW_PR_I 0.005
param set FW_PR_IMAX 0.4
param set FW_PR_P 0.08
param set FW_RR_FF 0.6
param set FW_RR_I 0.005
param set FW_RR_IMAX 0.2
param set FW_RR_P 0.09
param set FW_THR_CRUISE 0.65
param set FW_RR_P 0.04
param set MT_TKF_PIT_MAX 30.0
param set MT_ACC_D 0.2
param set MT_ACC_P 0.6
param set MT_A_LP 0.5
param set MT_PIT_OFF 0.1
param set MT_PIT_I 0.1
param set MT_THR_OFF 0.65
param set MT_THR_I 0.35
param set MT_THR_P 0.2
param set MT_THR_FF 1.5
fi
set MIXER wingwing

1
ROMFS/px4fmu_common/init.d/rc.sensors
View File

@ -24,6 +24,7 @@ fi
if ver hwcmp PX4FMU_V2
then
# IMPORTANT: EXTERNAL BUSES SHOULD BE SCANNED FIRST
if lsm303d start
then
echo "[init] Using LSM303D"

12
Tools/check_submodules.sh
View File

@ -1,5 +1,11 @@
#!/bin/sh
[ -n "$GIT_SUBMODULES_ARE_EVIL" ] && {
# GIT_SUBMODULES_ARE_EVIL is set, meaning user doesn't want submodules
echo "Skipping submodules. NUTTX_SRC is set to $NUTTX_SRC"
exit 0
}
if [ -d NuttX/nuttx ];
then
STATUSRETVAL=$(git submodule summary | grep -A20 -i "NuttX" | grep "<")
@ -8,8 +14,10 @@ if [ -d NuttX/nuttx ];
else
echo ""
echo ""
echo "NuttX sub repo not at correct version. Try 'git submodule update'"
echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
echo " NuttX sub repo not at correct version. Try 'git submodule update'"
echo " or follow instructions on http://pixhawk.org/dev/git/submodules"
echo ""
echo " DO NOT FORGET TO RUN 'make distclean && make archives' AFTER EACH NUTTX UPDATE!"
echo ""
echo ""
echo "New commits required:"

7
Tools/px_uploader.py
View File

@ -63,6 +63,7 @@ import zlib
import base64
import time
import array
import os
from sys import platform as _platform
@ -449,6 +450,12 @@ parser.add_argument('--baud', action="store", type=int, default=115200, help="Ba
parser.add_argument('firmware', action="store", help="Firmware file to be uploaded")
args = parser.parse_args()
# warn people about ModemManager which interferes badly with Pixhawk
if os.path.exists("/usr/sbin/ModemManager"):
print("==========================================================================================================")
print("WARNING: You should uninstall ModemManager as it conflicts with any non-modem serial device (like Pixhawk)")
print("==========================================================================================================")
# Load the firmware file
fw = firmware(args.firmware)
print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision')))

1
makefiles/config_px4fmu-v1_default.mk
View File

@ -25,6 +25,7 @@ MODULES += drivers/mpu6000
MODULES += drivers/hmc5883
MODULES += drivers/ms5611
MODULES += drivers/mb12xx
MODULES += drivers/ll40ls
MODULES += drivers/gps
MODULES += drivers/hil
MODULES += drivers/hott/hott_telemetry

1
makefiles/config_px4fmu-v2_default.mk
View File

@ -28,6 +28,7 @@ MODULES += drivers/hmc5883
MODULES += drivers/ms5611
MODULES += drivers/mb12xx
MODULES += drivers/sf0x
MODULES += drivers/ll40ls
MODULES += drivers/gps
MODULES += drivers/hil
MODULES += drivers/hott/hott_telemetry

1
makefiles/config_px4fmu-v2_test.mk
View File

@ -42,6 +42,7 @@ MODULES += modules/uORB
LIBRARIES += lib/mathlib/CMSIS
MODULES += lib/mathlib
MODULES += lib/mathlib/math/filter
MODULES += lib/conversion
#
# Libraries

2
src/drivers/ardrone_interface/module.mk
View File

@ -39,3 +39,5 @@ MODULE_COMMAND = ardrone_interface
SRCS = ardrone_interface.c \
ardrone_motor_control.c
MODULE_STACKSIZE = 1200
MAXOPTIMIZATION = -Os

2
src/drivers/blinkm/module.mk
View File

@ -38,3 +38,5 @@
MODULE_COMMAND = blinkm
SRCS = blinkm.cpp
MAXOPTIMIZATION = -Os

1
src/drivers/boards/px4fmu-v1/board_config.h
View File

@ -86,7 +86,6 @@ __BEGIN_DECLS
#define GPIO_SPI_CS_SDCARD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4)
#define PX4_SPI_BUS_SENSORS 1
#define PX4_SPI_BUS_EXT 2
/*
* Use these in place of the spi_dev_e enumeration to

13
src/drivers/boards/px4fmu-v2/board_config.h
View File

@ -108,6 +108,8 @@ __BEGIN_DECLS
#define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2)
#define GPIO_SPI_CS_EXT0 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4)
#define GPIO_SPI_CS_EXT1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14)
#define GPIO_SPI_CS_EXT2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
#define GPIO_SPI_CS_EXT3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
#define PX4_SPI_BUS_SENSORS 1
#define PX4_SPI_BUS_EXT 4
@ -121,10 +123,19 @@ __BEGIN_DECLS
/* External bus */
#define PX4_SPIDEV_EXT0 1
#define PX4_SPIDEV_EXT1 2
#define PX4_SPIDEV_EXT2 3
#define PX4_SPIDEV_EXT3 4
/* FMUv3 SPI on external bus */
#define PX4_SPIDEV_EXT_MPU PX4_SPIDEV_EXT0
#define PX4_SPIDEV_EXT_BARO PX4_SPIDEV_EXT1
#define PX4_SPIDEV_EXT_ACCEL_MAG PX4_SPIDEV_EXT2
#define PX4_SPIDEV_EXT_GYRO PX4_SPIDEV_EXT3
/* I2C busses */
#define PX4_I2C_BUS_EXPANSION 1
#define PX4_I2C_BUS_LED 2
#define PX4_I2C_BUS_ONBOARD 2
#define PX4_I2C_BUS_LED PX4_I2C_BUS_ONBOARD
/* Devices on the onboard bus.
*

26
src/drivers/boards/px4fmu-v2/px4fmu_spi.c
View File

@ -98,8 +98,12 @@ __EXPORT void weak_function stm32_spiinitialize(void)
#ifdef CONFIG_STM32_SPI4
stm32_configgpio(GPIO_SPI_CS_EXT0);
stm32_configgpio(GPIO_SPI_CS_EXT1);
stm32_configgpio(GPIO_SPI_CS_EXT2);
stm32_configgpio(GPIO_SPI_CS_EXT3);
stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1);
stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1);
stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1);
stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1);
#endif
}
@ -174,12 +178,32 @@ __EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI_CS_EXT0, !selected);
stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1);
stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1);
stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1);
break;
case PX4_SPIDEV_EXT1:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI_CS_EXT1, !selected);
stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1);
stm32_gpiowrite(GPIO_SPI_CS_EXT1, !selected);
stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1);
stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1);
break;
case PX4_SPIDEV_EXT2:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1);
stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1);
stm32_gpiowrite(GPIO_SPI_CS_EXT2, !selected);
stm32_gpiowrite(GPIO_SPI_CS_EXT3, 1);
break;
case PX4_SPIDEV_EXT3:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1);
stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1);
stm32_gpiowrite(GPIO_SPI_CS_EXT2, 1);
stm32_gpiowrite(GPIO_SPI_CS_EXT3, !selected);
break;
default:

7
src/drivers/device/cdev.cpp
View File

@ -268,6 +268,13 @@ CDev::ioctl(struct file *filp, int cmd, unsigned long arg)
break;
}
/* try the superclass. The different ioctl() function form
* means we need to copy arg */
unsigned arg2 = arg;
int ret = Device::ioctl(cmd, arg2);
if (ret != -ENODEV)
return ret;
return -ENOTTY;
}

12
src/drivers/device/device.cpp
View File

@ -42,6 +42,7 @@
#include <nuttx/arch.h>
#include <stdio.h>
#include <unistd.h>
#include <drivers/drv_device.h>
namespace device
{
@ -93,6 +94,13 @@ Device::Device(const char *name,
_irq_attached(false)
{
sem_init(&_lock, 0, 1);
/* setup a default device ID. When bus_type is UNKNOWN the
other fields are invalid */
_device_id.devid_s.bus_type = DeviceBusType_UNKNOWN;
_device_id.devid_s.bus = 0;
_device_id.devid_s.address = 0;
_device_id.devid_s.devtype = 0;
}
Device::~Device()
@ -238,6 +246,10 @@ Device::write(unsigned offset, void *data, unsigned count)
int
Device::ioctl(unsigned operation, unsigned &arg)
{
switch (operation) {
case DEVIOCGDEVICEID:
return (int)_device_id.devid;
}
return -ENODEV;
}

28
src/drivers/device/device.h
View File

@ -124,9 +124,37 @@ public:
*/
virtual int ioctl(unsigned operation, unsigned &arg);
/*
device bus types for DEVID
*/
enum DeviceBusType {
DeviceBusType_UNKNOWN = 0,
DeviceBusType_I2C = 1,
DeviceBusType_SPI = 2
};
/*
broken out device elements. The bitfields are used to keep
the overall value small enough to fit in a float accurately,
which makes it possible to transport over the MAVLink
parameter protocol without loss of information.
*/
struct DeviceStructure {
enum DeviceBusType bus_type:3;
uint8_t bus:5; // which instance of the bus type
uint8_t address; // address on the bus (eg. I2C address)
uint8_t devtype; // device class specific device type
};
union DeviceId {
struct DeviceStructure devid_s;
uint32_t devid;
};
protected:
const char *_name; /**< driver name */
bool _debug_enabled; /**< if true, debug messages are printed */
union DeviceId _device_id; /**< device identifier information */
/**
* Constructor

8
src/drivers/device/i2c.cpp
View File

@ -62,6 +62,12 @@ I2C::I2C(const char *name,
_frequency(frequency),
_dev(nullptr)
{
// fill in _device_id fields for a I2C device
_device_id.devid_s.bus_type = DeviceBusType_I2C;
_device_id.devid_s.bus = bus;
_device_id.devid_s.address = address;
// devtype needs to be filled in by the driver
_device_id.devid_s.devtype = 0;
}
I2C::~I2C()
@ -201,4 +207,4 @@ I2C::transfer(i2c_msg_s *msgv, unsigned msgs)
return ret;
}
} // namespace device
} // namespace device

10
src/drivers/device/spi.cpp
View File

@ -69,12 +69,18 @@ SPI::SPI(const char *name,
// protected
locking_mode(LOCK_PREEMPTION),
// private
_bus(bus),
_device(device),
_mode(mode),
_frequency(frequency),
_dev(nullptr)
_dev(nullptr),
_bus(bus)
{
// fill in _device_id fields for a SPI device
_device_id.devid_s.bus_type = DeviceBusType_SPI;
_device_id.devid_s.bus = bus;
_device_id.devid_s.address = (uint8_t)device;
// devtype needs to be filled in by the driver
_device_id.devid_s.devtype = 0;
}
SPI::~SPI()

4
src/drivers/device/spi.h
View File

@ -124,12 +124,14 @@ protected:
LockMode locking_mode; /**< selected locking mode */
private:
int _bus;
enum spi_dev_e _device;
enum spi_mode_e _mode;
uint32_t _frequency;
struct spi_dev_s *_dev;
protected:
int _bus;
int _transfer(uint8_t *send, uint8_t *recv, unsigned len);
};

7
src/drivers/drv_device.h
View File

@ -59,4 +59,11 @@
/** check publication block status */
#define DEVIOCGPUBBLOCK _DEVICEIOC(1)
/**
* Return device ID, to enable matching of configuration parameters
* (such as compass offsets) to specific sensors
*/
#define DEVIOCGDEVICEID _DEVICEIOC(2)
#endif /* _DRV_DEVICE_H */

7
src/drivers/drv_mag.h
View File

@ -81,6 +81,13 @@ struct mag_scale {
*/
ORB_DECLARE(sensor_mag);
/*
* mag device types, for _device_id
*/
#define DRV_MAG_DEVTYPE_HMC5883 1
#define DRV_MAG_DEVTYPE_LSM303D 2
/*
* ioctl() definitions
*/

2
src/drivers/frsky_telemetry/module.mk
View File

@ -41,3 +41,5 @@ SRCS = frsky_data.c \
frsky_telemetry.c
MODULE_STACKSIZE = 1200
MAXOPTIMIZATION = -Os

2
src/drivers/gps/module.mk
View File

@ -43,3 +43,5 @@ SRCS = gps.cpp \
ubx.cpp
MODULE_STACKSIZE = 1200
MAXOPTIMIZATION = -Os

323
src/drivers/hmc5883/hmc5883.cpp
View File

@ -71,13 +71,16 @@
#include <uORB/topics/subsystem_info.h>
#include <float.h>
#include <getopt.h>
#include <lib/conversion/rotation.h>
/*
* HMC5883 internal constants and data structures.
*/
#define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883
#define HMC5883L_DEVICE_PATH "/dev/hmc5883"
#define HMC5883L_DEVICE_PATH_INT "/dev/hmc5883_int"
#define HMC5883L_DEVICE_PATH_EXT "/dev/hmc5883_ext"
/* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */
#define HMC5883_CONVERSION_INTERVAL (1000000 / 150) /* microseconds */
@ -130,7 +133,7 @@ static const int ERROR = -1;
class HMC5883 : public device::I2C
{
public:
HMC5883(int bus);
HMC5883(int bus, const char *path, enum Rotation rotation);
virtual ~HMC5883();
virtual int init();
@ -163,15 +166,21 @@ private:
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
perf_counter_t _range_errors;
perf_counter_t _conf_errors;
/* status reporting */
bool _sensor_ok; /**< sensor was found and reports ok */
bool _calibrated; /**< the calibration is valid */
int _bus; /**< the bus the device is connected to */
enum Rotation _rotation;
struct mag_report _last_report; /**< used for info() */
uint8_t _range_bits;
uint8_t _conf_reg;
/**
* Test whether the device supported by the driver is present at a
* specific address.
@ -229,6 +238,23 @@ private:
*/
int set_range(unsigned range);
/**
* check the sensor range.
*
* checks that the range of the sensor is correctly set, to
* cope with communication errors causing the range to change
*/
void check_range(void);
/**
* check the sensor configuration.
*
* checks that the config of the sensor is correctly set, to
* cope with communication errors causing the configuration to
* change
*/
void check_conf(void);
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
@ -319,8 +345,8 @@ private:
extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]);
HMC5883::HMC5883(int bus) :
I2C("HMC5883", HMC5883L_DEVICE_PATH, bus, HMC5883L_ADDRESS, 400000),
HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) :
I2C("HMC5883", path, bus, HMC5883L_ADDRESS, 400000),
_measure_ticks(0),
_reports(nullptr),
_range_scale(0), /* default range scale from counts to gauss */
@ -332,10 +358,18 @@ HMC5883::HMC5883(int bus) :
_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
_range_errors(perf_alloc(PC_COUNT, "hmc5883_range_errors")),
_conf_errors(perf_alloc(PC_COUNT, "hmc5883_conf_errors")),
_sensor_ok(false),
_calibrated(false),
_bus(bus)
_bus(bus),
_rotation(rotation),
_last_report{0},
_range_bits(0),
_conf_reg(0)
{
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
// enable debug() calls
_debug_enabled = false;
@ -366,6 +400,8 @@ HMC5883::~HMC5883()
perf_free(_sample_perf);
perf_free(_comms_errors);
perf_free(_buffer_overflows);
perf_free(_range_errors);
perf_free(_conf_errors);
}
int
@ -396,45 +432,43 @@ out:
int HMC5883::set_range(unsigned range)
{
uint8_t range_bits;
if (range < 1) {
range_bits = 0x00;
_range_bits = 0x00;
_range_scale = 1.0f / 1370.0f;
_range_ga = 0.88f;
} else if (range <= 1) {
range_bits = 0x01;
_range_bits = 0x01;
_range_scale = 1.0f / 1090.0f;
_range_ga = 1.3f;
} else if (range <= 2) {
range_bits = 0x02;
_range_bits = 0x02;
_range_scale = 1.0f / 820.0f;
_range_ga = 1.9f;
} else if (range <= 3) {
range_bits = 0x03;
_range_bits = 0x03;
_range_scale = 1.0f / 660.0f;
_range_ga = 2.5f;
} else if (range <= 4) {
range_bits = 0x04;
_range_bits = 0x04;
_range_scale = 1.0f / 440.0f;
_range_ga = 4.0f;
} else if (range <= 4.7f) {
range_bits = 0x05;
_range_bits = 0x05;
_range_scale = 1.0f / 390.0f;
_range_ga = 4.7f;
} else if (range <= 5.6f) {
range_bits = 0x06;
_range_bits = 0x06;
_range_scale = 1.0f / 330.0f;
_range_ga = 5.6f;
} else {
range_bits = 0x07;
_range_bits = 0x07;
_range_scale = 1.0f / 230.0f;
_range_ga = 8.1f;
}
@ -444,7 +478,7 @@ int HMC5883::set_range(unsigned range)
/*
* Send the command to set the range
*/
ret = write_reg(ADDR_CONF_B, (range_bits << 5));
ret = write_reg(ADDR_CONF_B, (_range_bits << 5));
if (OK != ret)
perf_count(_comms_errors);
@ -455,7 +489,53 @@ int HMC5883::set_range(unsigned range)
if (OK != ret)
perf_count(_comms_errors);
return !(range_bits_in == (range_bits << 5));
return !(range_bits_in == (_range_bits << 5));
}
/**
check that the range register has the right value. This is done
periodically to cope with I2C bus noise causing the range of the
compass changing.
*/
void HMC5883::check_range(void)
{
int ret;
uint8_t range_bits_in;
ret = read_reg(ADDR_CONF_B, range_bits_in);
if (OK != ret) {
perf_count(_comms_errors);
return;
}
if (range_bits_in != (_range_bits<<5)) {
perf_count(_range_errors);
ret = write_reg(ADDR_CONF_B, (_range_bits << 5));
if (OK != ret)
perf_count(_comms_errors);
}
}
/**
check that the configuration register has the right value. This is
done periodically to cope with I2C bus noise causing the
configuration of the compass to change.
*/
void HMC5883::check_conf(void)
{
int ret;
uint8_t conf_reg_in;
ret = read_reg(ADDR_CONF_A, conf_reg_in);
if (OK != ret) {
perf_count(_comms_errors);
return;
}
if (conf_reg_in != _conf_reg) {
perf_count(_conf_errors);
ret = write_reg(ADDR_CONF_A, _conf_reg);
if (OK != ret)
perf_count(_comms_errors);
}
}
int
@ -789,7 +869,7 @@ HMC5883::collect()
} report;
int ret = -EIO;
uint8_t cmd;
uint8_t check_counter;
perf_begin(_sample_perf);
struct mag_report new_report;
@ -862,6 +942,9 @@ HMC5883::collect()
/* z remains z */
new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
// apply user specified rotation
rotate_3f(_rotation, new_report.x, new_report.y, new_report.z);
if (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
if (_mag_topic != -1) {
@ -885,6 +968,21 @@ HMC5883::collect()
/* notify anyone waiting for data */
poll_notify(POLLIN);
/*
periodically check the range register and configuration
registers. With a bad I2C cable it is possible for the
registers to become corrupt, leading to bad readings. It
doesn't happen often, but given the poor cables some
vehicles have it is worth checking for.
*/
check_counter = perf_event_count(_sample_perf) % 256;
if (check_counter == 0) {
check_range();
}
if (check_counter == 128) {
check_conf();
}
ret = OK;
out:
@ -1158,25 +1256,24 @@ int HMC5883::set_excitement(unsigned enable)
{
int ret;
/* arm the excitement strap */
uint8_t conf_reg;
ret = read_reg(ADDR_CONF_A, conf_reg);
ret = read_reg(ADDR_CONF_A, _conf_reg);
if (OK != ret)
perf_count(_comms_errors);
if (((int)enable) < 0) {
conf_reg |= 0x01;
_conf_reg |= 0x01;
} else if (enable > 0) {
conf_reg |= 0x02;
_conf_reg |= 0x02;
} else {
conf_reg &= ~0x03;
_conf_reg &= ~0x03;
}
// ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)conf_reg);
// ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)_conf_reg);
ret = write_reg(ADDR_CONF_A, conf_reg);
ret = write_reg(ADDR_CONF_A, _conf_reg);
if (OK != ret)
perf_count(_comms_errors);
@ -1186,7 +1283,7 @@ int HMC5883::set_excitement(unsigned enable)
//print_info();
return !(conf_reg == conf_reg_ret);
return !(_conf_reg == conf_reg_ret);
}
int
@ -1244,63 +1341,87 @@ namespace hmc5883
#endif
const int ERROR = -1;
HMC5883 *g_dev;
HMC5883 *g_dev_int;
HMC5883 *g_dev_ext;
void start();
void test();
void reset();
void info();
int calibrate();
void start(int bus, enum Rotation rotation);
void test(int bus);
void reset(int bus);
void info(int bus);
int calibrate(int bus);
void usage();
/**
* Start the driver.
*/
void
start()
start(int bus, enum Rotation rotation)
{
int fd;
if (g_dev != nullptr)
/* if already started, the still command succeeded */
errx(0, "already started");
/* create the driver, attempt expansion bus first */
g_dev = new HMC5883(PX4_I2C_BUS_EXPANSION);
if (g_dev != nullptr && OK != g_dev->init()) {
delete g_dev;
g_dev = nullptr;
if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) {
if (g_dev_ext != nullptr)
errx(0, "already started external");
g_dev_ext = new HMC5883(PX4_I2C_BUS_EXPANSION, HMC5883L_DEVICE_PATH_EXT, rotation);
if (g_dev_ext != nullptr && OK != g_dev_ext->init()) {
delete g_dev_ext;
g_dev_ext = nullptr;
}
}
#ifdef PX4_I2C_BUS_ONBOARD
/* if this failed, attempt onboard sensor */
if (g_dev == nullptr) {
g_dev = new HMC5883(PX4_I2C_BUS_ONBOARD);
if (g_dev != nullptr && OK != g_dev->init()) {
if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) {
if (g_dev_int != nullptr)
errx(0, "already started internal");
g_dev_int = new HMC5883(PX4_I2C_BUS_ONBOARD, HMC5883L_DEVICE_PATH_INT, rotation);
if (g_dev_int != nullptr && OK != g_dev_int->init()) {
if (bus == PX4_I2C_BUS_ONBOARD) {
goto fail;
}
}
if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) {
goto fail;
}
}
#endif
if (g_dev == nullptr)
if (g_dev_int == nullptr && g_dev_ext == nullptr)
goto fail;
/* set the poll rate to default, starts automatic data collection */
fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
if (g_dev_int != nullptr) {
fd = open(HMC5883L_DEVICE_PATH_INT, O_RDONLY);
if (fd < 0)
goto fail;
if (fd < 0)
goto fail;
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
goto fail;
close(fd);
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
goto fail;
if (g_dev_ext != nullptr) {
fd = open(HMC5883L_DEVICE_PATH_EXT, O_RDONLY);
if (fd < 0)
goto fail;
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
goto fail;
close(fd);
}
exit(0);
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) {
delete g_dev_int;
g_dev_int = nullptr;
}
if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) {
delete g_dev_ext;
g_dev_ext = nullptr;
}
errx(1, "driver start failed");
@ -1312,16 +1433,17 @@ fail:
* and automatic modes.
*/
void
test()
test(int bus)
{
struct mag_report report;
ssize_t sz;
int ret;
const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT);
int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
int fd = open(path, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH);
err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", path);
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
@ -1414,14 +1536,15 @@ test()
* configuration register A back to 00 (Normal Measurement Mode), e.g. 0x10.
* Using the self test method described above, the user can scale sensor
*/
int calibrate()
int calibrate(int bus)
{
int ret;
const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT);
int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
int fd = open(path, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", HMC5883L_DEVICE_PATH);
err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", path);
if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) {
warnx("failed to enable sensor calibration mode");
@ -1441,9 +1564,11 @@ int calibrate()
* Reset the driver.
*/
void
reset()
reset(int bus)
{
int fd = open(HMC5883L_DEVICE_PATH, O_RDONLY);
const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT);
int fd = open(path, O_RDONLY);
if (fd < 0)
err(1, "failed ");
@ -1461,8 +1586,9 @@ reset()
* Print a little info about the driver.
*/
void
info()
info(int bus)
{
HMC5883 *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext);
if (g_dev == nullptr)
errx(1, "driver not running");
@ -1472,40 +1598,91 @@ info()
exit(0);
}
void
usage()
{
warnx("missing command: try 'start', 'info', 'test', 'reset', 'info', 'calibrate'");
warnx("options:");
warnx(" -R rotation");
warnx(" -C calibrate on start");
warnx(" -X only external bus");
#ifdef PX4_I2C_BUS_ONBOARD
warnx(" -I only internal bus");
#endif
}
} // namespace
int
hmc5883_main(int argc, char *argv[])
{
int ch;
int bus = -1;
enum Rotation rotation = ROTATION_NONE;
bool calibrate = false;
while ((ch = getopt(argc, argv, "XIR:C")) != EOF) {
switch (ch) {
case 'R':
rotation = (enum Rotation)atoi(optarg);
break;
#ifdef PX4_I2C_BUS_ONBOARD
case 'I':
bus = PX4_I2C_BUS_ONBOARD;
break;
#endif
case 'X':
bus = PX4_I2C_BUS_EXPANSION;
break;
case 'C':
calibrate = true;
break;
default:
hmc5883::usage();
exit(0);
}
}
const char *verb = argv[optind];
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start"))
hmc5883::start();
if (!strcmp(verb, "start")) {
hmc5883::start(bus, rotation);
if (calibrate) {
if (hmc5883::calibrate(bus) == 0) {
errx(0, "calibration successful");
} else {
errx(1, "calibration failed");
}
}
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test"))
hmc5883::test();
if (!strcmp(verb, "test"))
hmc5883::test(bus);
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset"))
hmc5883::reset();
if (!strcmp(verb, "reset"))
hmc5883::reset(bus);
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
hmc5883::info();
if (!strcmp(verb, "info") || !strcmp(verb, "status"))
hmc5883::info(bus);
/*
* Autocalibrate the scaling
*/
if (!strcmp(argv[1], "calibrate")) {
if (hmc5883::calibrate() == 0) {
if (!strcmp(verb, "calibrate")) {
if (hmc5883::calibrate(bus) == 0) {
errx(0, "calibration successful");
} else {

2
src/drivers/hott/hott_sensors/module.mk
View File

@ -40,3 +40,5 @@ MODULE_COMMAND = hott_sensors
SRCS = hott_sensors.cpp \
../messages.cpp \
../comms.cpp
MAXOPTIMIZATION = -Os

2
src/drivers/hott/hott_telemetry/module.mk
View File

@ -40,3 +40,5 @@ MODULE_COMMAND = hott_telemetry
SRCS = hott_telemetry.cpp \
../messages.cpp \
../comms.cpp
MAXOPTIMIZATION = -Os

70
src/drivers/l3gd20/l3gd20.cpp
View File

@ -54,6 +54,7 @@
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <getopt.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
@ -68,6 +69,7 @@
#include <board_config.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/conversion/rotation.h>
#define L3GD20_DEVICE_PATH "/dev/l3gd20"
@ -184,7 +186,7 @@ extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); }
class L3GD20 : public device::SPI
{
public:
L3GD20(int bus, const char* path, spi_dev_e device);
L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation);
virtual ~L3GD20();
virtual int init();
@ -229,6 +231,8 @@ private:
/* true if an L3G4200D is detected */
bool _is_l3g4200d;
enum Rotation _rotation;
/**
* Start automatic measurement.
*/
@ -328,7 +332,7 @@ private:
int self_test();
};
L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation) :
SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */),
_call_interval(0),
_reports(nullptr),
@ -345,7 +349,8 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
_gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
_gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
_gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
_is_l3g4200d(false)
_is_l3g4200d(false),
_rotation(rotation)
{
// enable debug() calls
_debug_enabled = true;
@ -821,7 +826,7 @@ L3GD20::measure()
// if the gyro doesn't have any data ready then re-schedule
// for 100 microseconds later. This ensures we don't double
// read a value and then miss the next value
if (stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) {
if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) {
perf_count(_reschedules);
hrt_call_delay(&_call, 100);
return;
@ -914,6 +919,9 @@ L3GD20::measure()
report.y = _gyro_filter_y.apply(report.y);
report.z = _gyro_filter_z.apply(report.z);
// apply user specified rotation
rotate_3f(_rotation, report.x, report.y, report.z);
report.scaling = _gyro_range_scale;
report.range_rad_s = _gyro_range_rad_s;
@ -974,7 +982,8 @@ namespace l3gd20
L3GD20 *g_dev;
void start();
void usage();
void start(bool external_bus, enum Rotation rotation);
void test();
void reset();
void info();
@ -983,7 +992,7 @@ void info();
* Start the driver.
*/
void
start()
start(bool external_bus, enum Rotation rotation)
{
int fd;
@ -991,7 +1000,15 @@ start()
errx(0, "already started");
/* create the driver */
g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
if (external_bus) {
#ifdef PX4_SPI_BUS_EXT
g_dev = new L3GD20(PX4_SPI_BUS_EXT, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_EXT_GYRO, rotation);
#else
errx(0, "External SPI not available");
#endif
} else {
g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO, rotation);
}
if (g_dev == nullptr)
goto fail;
@ -1103,35 +1120,64 @@ info()
exit(0);
}
void
usage()
{
warnx("missing command: try 'start', 'info', 'test', 'reset'");
warnx("options:");
warnx(" -X (external bus)");
warnx(" -R rotation");
}
} // namespace
int
l3gd20_main(int argc, char *argv[])
{
bool external_bus = false;
int ch;
enum Rotation rotation = ROTATION_NONE;
/* jump over start/off/etc and look at options first */
while ((ch = getopt(argc, argv, "XR:")) != EOF) {
switch (ch) {
case 'X':
external_bus = true;
break;
case 'R':
rotation = (enum Rotation)atoi(optarg);
break;
default:
l3gd20::usage();
exit(0);
}
}
const char *verb = argv[optind];
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start"))
l3gd20::start();
if (!strcmp(verb, "start"))
l3gd20::start(external_bus, rotation);
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test"))
if (!strcmp(verb, "test"))
l3gd20::test();
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset"))
if (!strcmp(verb, "reset"))
l3gd20::reset();
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info"))
if (!strcmp(verb, "info"))
l3gd20::info();
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");

882
src/drivers/ll40ls/ll40ls.cpp Normal file
View File

@ -0,0 +1,882 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ll40ls.cpp
* @author Allyson Kreft
*
* Driver for the PulsedLight Lidar-Lite range finders connected via I2C.
*/
#include <nuttx/config.h>
#include <drivers/device/i2c.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <board_config.h>
/* Configuration Constants */
#define LL40LS_BUS PX4_I2C_BUS_EXPANSION
#define LL40LS_BASEADDR 0x42 /* 7-bit address */
#define LL40LS_DEVICE_PATH "/dev/ll40ls"
/* LL40LS Registers addresses */
#define LL40LS_MEASURE_REG 0x00 /* Measure range register */
#define LL40LS_MSRREG_ACQUIRE 0x04 /* Value to initiate a measurement, varies based on sensor revision */
#define LL40LS_DISTHIGH_REG 0x8F /* High byte of distance register, auto increment */
/* Device limits */
#define LL40LS_MIN_DISTANCE (0.00f)
#define LL40LS_MAX_DISTANCE (14.00f)
#define LL40LS_CONVERSION_INTERVAL 100000 /* 100ms */
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
class LL40LS : public device::I2C
{
public:
LL40LS(int bus = LL40LS_BUS, int address = LL40LS_BASEADDR);
virtual ~LL40LS();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
protected:
virtual int probe();
private:
float _min_distance;
float _max_distance;
work_s _work;
RingBuffer *_reports;
bool _sensor_ok;
int _measure_ticks;
bool _collect_phase;
int _class_instance;
orb_advert_t _range_finder_topic;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
/**
* Test whether the device supported by the driver is present at a
* specific address.
*
* @param address The I2C bus address to probe.
* @return True if the device is present.
*/
int probe_address(uint8_t address);
/**
* Initialise the automatic measurement state machine and start it.
*
* @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors.
*/
void start();
/**
* Stop the automatic measurement state machine.
*/
void stop();
/**
* Set the min and max distance thresholds if you want the end points of the sensors
* range to be brought in at all, otherwise it will use the defaults LL40LS_MIN_DISTANCE
* and LL40LS_MAX_DISTANCE
*/
void set_minimum_distance(float min);
void set_maximum_distance(float max);
float get_minimum_distance();
float get_maximum_distance();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void cycle();
int measure();
int collect();
/**
* Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
};
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int ll40ls_main(int argc, char *argv[]);
LL40LS::LL40LS(int bus, int address) :
I2C("LL40LS", LL40LS_DEVICE_PATH, bus, address, 100000),
_min_distance(LL40LS_MIN_DISTANCE),
_max_distance(LL40LS_MAX_DISTANCE),
_reports(nullptr),
_sensor_ok(false),
_measure_ticks(0),
_collect_phase(false),
_class_instance(-1),
_range_finder_topic(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_read")),
_comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows"))
{
// up the retries since the device misses the first measure attempts
I2C::_retries = 3;
// enable debug() calls
_debug_enabled = false;
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
}
LL40LS::~LL40LS()
{
/* make sure we are truly inactive */
stop();
/* free any existing reports */
if (_reports != nullptr) {
delete _reports;
}
if (_class_instance != -1) {
unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance);
}
// free perf counters
perf_free(_sample_perf);
perf_free(_comms_errors);
perf_free(_buffer_overflows);
}
int
LL40LS::init()
{
int ret = ERROR;
/* do I2C init (and probe) first */
if (I2C::init() != OK) {
goto out;
}
/* allocate basic report buffers */
_reports = new RingBuffer(2, sizeof(range_finder_report));
if (_reports == nullptr) {
goto out;
}
_class_instance = register_class_devname(RANGE_FINDER_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the range finder topic */
struct range_finder_report rf_report;
measure();
_reports->get(&rf_report);
_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report);
if (_range_finder_topic < 0) {
debug("failed to create sensor_range_finder object. Did you start uOrb?");
}
}
ret = OK;
/* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true;
out:
return ret;
}
int
LL40LS::probe()
{
return measure();
}
void
LL40LS::set_minimum_distance(float min)
{
_min_distance = min;
}
void
LL40LS::set_maximum_distance(float max)
{
_max_distance = max;
}
float
LL40LS::get_minimum_distance()
{
return _min_distance;
}
float
LL40LS::get_maximum_distance()
{
return _max_distance;
}
int
LL40LS::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_measure_ticks = 0;
return OK;
/* external signalling (DRDY) not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(LL40LS_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* convert hz to tick interval via microseconds */
unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(LL40LS_CONVERSION_INTERVAL)) {
return -EINVAL;
}
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0) {
return SENSOR_POLLRATE_MANUAL;
}
return (1000 / _measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) {
return -EINVAL;
}
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _reports->size();
case SENSORIOCRESET:
/* XXX implement this */
return -EINVAL;
case RANGEFINDERIOCSETMINIUMDISTANCE: {
set_minimum_distance(*(float *)arg);
return 0;
}
break;
case RANGEFINDERIOCSETMAXIUMDISTANCE: {
set_maximum_distance(*(float *)arg);
return 0;
}
break;
default:
/* give it to the superclass */
return I2C::ioctl(filp, cmd, arg);
}
}
ssize_t
LL40LS::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct range_finder_report);
struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer);
int ret = 0;
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is enabled */
if (_measure_ticks > 0) {
/*
* While there is space in the caller's buffer, and reports, copy them.
* Note that we may be pre-empted by the workq thread while we are doing this;
* we are careful to avoid racing with them.
*/
while (count--) {
if (_reports->get(rbuf)) {
ret += sizeof(*rbuf);
rbuf++;
}
}
/* if there was no data, warn the caller */
return ret ? ret : -EAGAIN;
}
/* manual measurement - run one conversion */
do {
_reports->flush();
/* trigger a measurement */
if (OK != measure()) {
ret = -EIO;
break;
}
/* wait for it to complete */
usleep(LL40LS_CONVERSION_INTERVAL);
/* run the collection phase */
if (OK != collect()) {
ret = -EIO;
break;
}
/* state machine will have generated a report, copy it out */
if (_reports->get(rbuf)) {
ret = sizeof(*rbuf);
}
} while (0);
return ret;
}
int
LL40LS::measure()
{
int ret;
/*
* Send the command to begin a measurement.
*/
const uint8_t cmd[2] = { LL40LS_MEASURE_REG, LL40LS_MSRREG_ACQUIRE };
ret = transfer(cmd, sizeof(cmd), nullptr, 0);
if (OK != ret) {
perf_count(_comms_errors);
log("i2c::transfer returned %d", ret);
return ret;
}
ret = OK;
return ret;
}
int
LL40LS::collect()
{
int ret = -EIO;
/* read from the sensor */
uint8_t val[2] = {0, 0};
perf_begin(_sample_perf);
// read the high and low byte distance registers
uint8_t distance_reg = LL40LS_DISTHIGH_REG;
ret = transfer(&distance_reg, 1, &val[0], sizeof(val));
if (ret < 0) {
log("error reading from sensor: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
uint16_t distance = (val[0] << 8) | val[1];
float si_units = distance * 0.01f; /* cm to m */
struct range_finder_report report;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.distance = si_units;
if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) {
report.valid = 1;
}
else {
report.valid = 0;
}
/* publish it, if we are the primary */
if (_range_finder_topic >= 0) {
orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
}
if (_reports->force(&report)) {
perf_count(_buffer_overflows);
}
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
void
LL40LS::start()
{
/* reset the report ring and state machine */
_collect_phase = false;
_reports->flush();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&LL40LS::cycle_trampoline, this, 1);
/* notify about state change */
struct subsystem_info_s info = {
true,
true,
true,
SUBSYSTEM_TYPE_RANGEFINDER
};
static orb_advert_t pub = -1;
if (pub > 0) {
orb_publish(ORB_ID(subsystem_info), pub, &info);
} else {
pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
}
void
LL40LS::stop()
{
work_cancel(HPWORK, &_work);
}
void
LL40LS::cycle_trampoline(void *arg)
{
LL40LS *dev = (LL40LS *)arg;
dev->cycle();
}
void
LL40LS::cycle()
{
/* collection phase? */
if (_collect_phase) {
/* perform collection */
if (OK != collect()) {
log("collection error");
/* restart the measurement state machine */
start();
return;
}
/* next phase is measurement */
_collect_phase = false;
/*
* Is there a collect->measure gap?
*/
if (_measure_ticks > USEC2TICK(LL40LS_CONVERSION_INTERVAL)) {
/* schedule a fresh cycle call when we are ready to measure again */
work_queue(HPWORK,
&_work,
(worker_t)&LL40LS::cycle_trampoline,
this,
_measure_ticks - USEC2TICK(LL40LS_CONVERSION_INTERVAL));
return;
}
}
/* measurement phase */
if (OK != measure()) {
log("measure error");
}
/* next phase is collection */
_collect_phase = true;
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&LL40LS::cycle_trampoline,
this,
USEC2TICK(LL40LS_CONVERSION_INTERVAL));
}
void
LL40LS::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
perf_print_counter(_buffer_overflows);
printf("poll interval: %u ticks\n", _measure_ticks);
_reports->print_info("report queue");
}
/**
* Local functions in support of the shell command.
*/
namespace ll40ls
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
const int ERROR = -1;
LL40LS *g_dev;
void start();
void stop();
void test();
void reset();
void info();
/**
* Start the driver.
*/
void
start()
{
int fd;
if (g_dev != nullptr) {
errx(1, "already started");
}
/* create the driver */
g_dev = new LL40LS(LL40LS_BUS);
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init()) {
goto fail;
}
/* set the poll rate to default, starts automatic data collection */
fd = open(LL40LS_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
goto fail;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail;
}
exit(0);
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
errx(1, "driver start failed");
}
/**
* Stop the driver
*/
void stop()
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
} else {
errx(1, "driver not running");
}
exit(0);
}
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
void
test()
{
struct range_finder_report report;
ssize_t sz;
int ret;
int fd = open(LL40LS_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
err(1, "%s open failed (try 'll40ls start' if the driver is not running", LL40LS_DEVICE_PATH);
}
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
err(1, "immediate read failed");
}
warnx("single read");
warnx("measurement: %0.2f m", (double)report.distance);
warnx("time: %lld", report.timestamp);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
errx(1, "failed to set 2Hz poll rate");
}
/* read the sensor 5x and report each value */
for (unsigned i = 0; i < 5; i++) {
struct pollfd fds;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = poll(&fds, 1, 2000);
if (ret != 1) {
errx(1, "timed out waiting for sensor data");
}
/* now go get it */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
err(1, "periodic read failed");
}
warnx("periodic read %u", i);
warnx("measurement: %0.3f", (double)report.distance);
warnx("time: %lld", report.timestamp);
}
/* reset the sensor polling to default rate */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
errx(1, "failed to set default poll rate");
}
errx(0, "PASS");
}
/**
* Reset the driver.
*/
void
reset()
{
int fd = open(LL40LS_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
err(1, "failed ");
}
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
err(1, "driver reset failed");
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "driver poll restart failed");
}
exit(0);
}
/**
* Print a little info about the driver.
*/
void
info()
{
if (g_dev == nullptr) {
errx(1, "driver not running");
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
exit(0);
}
} // namespace
int
ll40ls_main(int argc, char *argv[])
{
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start")) {
ll40ls::start();
}
/*
* Stop the driver
*/
if (!strcmp(argv[1], "stop")) {
ll40ls::stop();
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test")) {
ll40ls::test();
}
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset")) {
ll40ls::reset();
}
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
ll40ls::info();
}
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
}

42
src/drivers/ll40ls/module.mk Normal file
View File

@ -0,0 +1,42 @@
############################################################################
#
# Copyright (c) 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Makefile to build the PulsedLight Lidar-Lite driver.
#
MODULE_COMMAND = ll40ls
SRCS = ll40ls.cpp
MAXOPTIMIZATION = -Os

86
src/drivers/lsm303d/lsm303d.cpp
View File

@ -52,6 +52,7 @@
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <getopt.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
@ -68,6 +69,7 @@
#include <board_config.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/conversion/rotation.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
@ -75,12 +77,17 @@
#endif
static const int ERROR = -1;
// enable this to debug the buggy lsm303d sensor in very early
// prototype pixhawk boards
#define CHECK_EXTREMES 0
/* SPI protocol address bits */
#define DIR_READ (1<<7)
#define DIR_WRITE (0<<7)
#define ADDR_INCREMENT (1<<6)
#define LSM303D_DEVICE_PATH_ACCEL "/dev/lsm303d_accel"
#define LSM303D_DEVICE_PATH_ACCEL_EXT "/dev/lsm303d_accel_ext"
#define LSM303D_DEVICE_PATH_MAG "/dev/lsm303d_mag"
/* register addresses: A: accel, M: mag, T: temp */
@ -216,7 +223,7 @@ class LSM303D_mag;
class LSM303D : public device::SPI
{
public:
LSM303D(int bus, const char* path, spi_dev_e device);
LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rotation);
virtual ~LSM303D();
virtual int init();
@ -305,7 +312,8 @@ private:
uint64_t _last_log_us;
uint64_t _last_log_sync_us;
uint64_t _last_log_reg_us;
uint64_t _last_log_alarm_us;
uint64_t _last_log_alarm_us;
enum Rotation _rotation;
/**
* Start automatic measurement.
@ -485,7 +493,7 @@ private:
};
LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rotation) :
SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within safety margins for LSM303D */),
_mag(new LSM303D_mag(this)),
_call_accel_interval(0),
@ -519,8 +527,11 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
_last_log_us(0),
_last_log_sync_us(0),
_last_log_reg_us(0),
_last_log_alarm_us(0)
_last_log_alarm_us(0),
_rotation(rotation)
{
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D;
// enable debug() calls
_debug_enabled = true;
@ -830,7 +841,9 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen)
*/
while (count--) {
if (_accel_reports->get(arb)) {
#if CHECK_EXTREMES
check_extremes(arb);
#endif
ret += sizeof(*arb);
arb++;
}
@ -1533,6 +1546,9 @@ LSM303D::measure()
accel_report.y = _accel_filter_y.apply(y_in_new);
accel_report.z = _accel_filter_z.apply(z_in_new);
// apply user specified rotation
rotate_3f(_rotation, accel_report.x, accel_report.y, accel_report.z);
accel_report.scaling = _accel_range_scale;
accel_report.range_m_s2 = _accel_range_m_s2;
@ -1609,6 +1625,9 @@ LSM303D::mag_measure()
mag_report.scaling = _mag_range_scale;
mag_report.range_ga = (float)_mag_range_ga;
// apply user specified rotation
rotate_3f(_rotation, mag_report.x, mag_report.y, mag_report.z);
_mag_reports->force(&mag_report);
/* XXX please check this poll_notify, is it the right one? */
@ -1774,26 +1793,34 @@ namespace lsm303d
LSM303D *g_dev;
void start();
void start(bool external_bus, enum Rotation rotation);
void test();
void reset();
void info();
void regdump();
void logging();
void usage();
/**
* Start the driver.
*/
void
start()
start(bool external_bus, enum Rotation rotation)
{
int fd, fd_mag;
if (g_dev != nullptr)
errx(0, "already started");
/* create the driver */
g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
if (external_bus) {
#ifdef PX4_SPI_BUS_EXT
g_dev = new LSM303D(PX4_SPI_BUS_EXT, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_EXT_ACCEL_MAG, rotation);
#else
errx(0, "External SPI not available");
#endif
} else {
g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG, rotation);
}
if (g_dev == nullptr) {
warnx("failed instantiating LSM303D obj");
@ -1989,47 +2016,76 @@ logging()
exit(0);
}
void
usage()
{
warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump', 'logging'");
warnx("options:");
warnx(" -X (external bus)");
warnx(" -R rotation");
}
} // namespace
int
lsm303d_main(int argc, char *argv[])
{
bool external_bus = false;
int ch;
enum Rotation rotation = ROTATION_NONE;
/* jump over start/off/etc and look at options first */
while ((ch = getopt(argc, argv, "XR:")) != EOF) {
switch (ch) {
case 'X':
external_bus = true;
break;
case 'R':
rotation = (enum Rotation)atoi(optarg);
break;
default:
lsm303d::usage();
exit(0);
}
}
const char *verb = argv[optind];
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start"))
lsm303d::start();
if (!strcmp(verb, "start"))
lsm303d::start(external_bus, rotation);
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test"))
if (!strcmp(verb, "test"))
lsm303d::test();
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset"))
if (!strcmp(verb, "reset"))
lsm303d::reset();
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info"))
if (!strcmp(verb, "info"))
lsm303d::info();
/*
* dump device registers
*/
if (!strcmp(argv[1], "regdump"))
if (!strcmp(verb, "regdump"))
lsm303d::regdump();
/*
* dump device registers
*/
if (!strcmp(argv[1], "logging"))
if (!strcmp(verb, "logging"))
lsm303d::logging();
errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'logging' or 'regdump'");

45
src/drivers/mb12xx/mb12xx.cpp
View File

@ -74,6 +74,7 @@
/* Configuration Constants */
#define MB12XX_BUS PX4_I2C_BUS_EXPANSION
#define MB12XX_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */
#define MB12XX_DEVICE_PATH "/dev/mb12xx"
/* MB12xx Registers addresses */
@ -124,6 +125,7 @@ private:
bool _sensor_ok;
int _measure_ticks;
bool _collect_phase;
int _class_instance;
orb_advert_t _range_finder_topic;
@ -187,13 +189,14 @@ private:
extern "C" __EXPORT int mb12xx_main(int argc, char *argv[]);
MB12XX::MB12XX(int bus, int address) :
I2C("MB12xx", RANGE_FINDER_DEVICE_PATH, bus, address, 100000),
I2C("MB12xx", MB12XX_DEVICE_PATH, bus, address, 100000),
_min_distance(MB12XX_MIN_DISTANCE),
_max_distance(MB12XX_MAX_DISTANCE),
_reports(nullptr),
_sensor_ok(false),
_measure_ticks(0),
_collect_phase(false),
_class_instance(-1),
_range_finder_topic(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")),
_comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")),
@ -215,6 +218,15 @@ MB12XX::~MB12XX()
if (_reports != nullptr) {
delete _reports;
}
if (_class_instance != -1) {
unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance);
}
// free perf counters
perf_free(_sample_perf);
perf_free(_comms_errors);
perf_free(_buffer_overflows);
}
int
@ -234,13 +246,18 @@ MB12XX::init()
goto out;
}
/* get a publish handle on the range finder topic */
struct range_finder_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report);
_class_instance = register_class_devname(RANGE_FINDER_DEVICE_PATH);
if (_range_finder_topic < 0) {
debug("failed to create sensor_range_finder object. Did you start uOrb?");
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the range finder topic */
struct range_finder_report rf_report;
measure();
_reports->get(&rf_report);
_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report);
if (_range_finder_topic < 0) {
debug("failed to create sensor_range_finder object. Did you start uOrb?");
}
}
ret = OK;
@ -505,8 +522,10 @@ MB12XX::collect()
report.distance = si_units;
report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
/* publish it */
orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
/* publish it, if we are the primary */
if (_range_finder_topic >= 0) {
orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
}
if (_reports->force(&report)) {
perf_count(_buffer_overflows);
@ -665,7 +684,7 @@ start()
}
/* set the poll rate to default, starts automatic data collection */
fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
fd = open(MB12XX_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
goto fail;
@ -715,10 +734,10 @@ test()
ssize_t sz;
int ret;
int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
int fd = open(MB12XX_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
err(1, "%s open failed (try 'mb12xx start' if the driver is not running", RANGE_FINDER_DEVICE_PATH);
err(1, "%s open failed (try 'mb12xx start' if the driver is not running", MB12XX_DEVICE_PATH);
}
/* do a simple demand read */
@ -776,7 +795,7 @@ test()
void
reset()
{
int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
int fd = open(MB12XX_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
err(1, "failed ");

2
src/drivers/mb12xx/module.mk
View File

@ -38,3 +38,5 @@
MODULE_COMMAND = mb12xx
SRCS = mb12xx.cpp
MAXOPTIMIZATION = -Os

4
src/drivers/mkblctrl/module.mk
View File

@ -37,6 +37,8 @@
MODULE_COMMAND = mkblctrl
SRCS = mkblctrl.cpp
SRCS = mkblctrl.cpp
INCLUDE_DIRS += $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
MAXOPTIMIZATION = -Os

163
src/drivers/mpu6000/mpu6000.cpp
View File

@ -55,6 +55,7 @@
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <getopt.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
@ -71,12 +72,15 @@
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/conversion/rotation.h>
#define DIR_READ 0x80
#define DIR_WRITE 0x00
#define MPU_DEVICE_PATH_ACCEL "/dev/mpu6000_accel"
#define MPU_DEVICE_PATH_GYRO "/dev/mpu6000_gyro"
#define MPU_DEVICE_PATH_ACCEL_EXT "/dev/mpu6000_accel_ext"
#define MPU_DEVICE_PATH_GYRO_EXT "/dev/mpu6000_gyro_ext"
// MPU 6000 registers
#define MPUREG_WHOAMI 0x75
@ -177,7 +181,7 @@ class MPU6000_gyro;
class MPU6000 : public device::SPI
{
public:
MPU6000(int bus, spi_dev_e device);
MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation);
virtual ~MPU6000();
virtual int init();
@ -232,6 +236,8 @@ private:
math::LowPassFilter2p _gyro_filter_y;
math::LowPassFilter2p _gyro_filter_z;
enum Rotation _rotation;
/**
* Start automatic measurement.
*/
@ -345,7 +351,7 @@ private:
class MPU6000_gyro : public device::CDev
{
public:
MPU6000_gyro(MPU6000 *parent);
MPU6000_gyro(MPU6000 *parent, const char *path);
~MPU6000_gyro();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
@ -368,9 +374,9 @@ private:
/** driver 'main' command */
extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); }
MPU6000::MPU6000(int bus, spi_dev_e device) :
SPI("MPU6000", MPU_DEVICE_PATH_ACCEL, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED),
_gyro(new MPU6000_gyro(this)),
MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev_e device, enum Rotation rotation) :
SPI("MPU6000", path_accel, bus, device, SPIDEV_MODE3, MPU6000_LOW_BUS_SPEED),
_gyro(new MPU6000_gyro(this, path_gyro)),
_product(0),
_call_interval(0),
_accel_reports(nullptr),
@ -391,7 +397,8 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
_accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ)
_gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_rotation(rotation)
{
// disable debug() calls
_debug_enabled = false;
@ -666,7 +673,9 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz)
/*
choose next highest filter frequency available
*/
if (frequency_hz <= 5) {
if (frequency_hz == 0) {
filter = BITS_DLPF_CFG_2100HZ_NOLPF;
} else if (frequency_hz <= 5) {
filter = BITS_DLPF_CFG_5HZ;
} else if (frequency_hz <= 10) {
filter = BITS_DLPF_CFG_10HZ;
@ -922,10 +931,11 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
return _accel_filter_x.get_cutoff_freq();
case ACCELIOCSLOWPASS:
// XXX decide on relationship of both filters
// i.e. disable the on-chip filter
//_set_dlpf_filter((uint16_t)arg);
if (arg == 0) {
// allow disabling of on-chip filter using
// zero as desired filter frequency
_set_dlpf_filter(0);
}
_accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
_accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
_accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
@ -1009,8 +1019,11 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
_gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
_gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
_gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
// XXX check relation to the internal lowpass
//_set_dlpf_filter((uint16_t)arg);
if (arg == 0) {
// allow disabling of on-chip filter using 0
// as desired frequency
_set_dlpf_filter(0);
}
return OK;
case GYROIOCSSCALE:
@ -1295,6 +1308,9 @@ MPU6000::measure()
arb.y = _accel_filter_y.apply(y_in_new);
arb.z = _accel_filter_z.apply(z_in_new);
// apply user specified rotation
rotate_3f(_rotation, arb.x, arb.y, arb.z);
arb.scaling = _accel_range_scale;
arb.range_m_s2 = _accel_range_m_s2;
@ -1313,6 +1329,9 @@ MPU6000::measure()
grb.y = _gyro_filter_y.apply(y_gyro_in_new);
grb.z = _gyro_filter_z.apply(z_gyro_in_new);
// apply user specified rotation
rotate_3f(_rotation, grb.x, grb.y, grb.z);
grb.scaling = _gyro_range_scale;
grb.range_rad_s = _gyro_range_rad_s;
@ -1350,8 +1369,8 @@ MPU6000::print_info()
_gyro_reports->print_info("gyro queue");
}
MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) :
CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO),
MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) :
CDev("MPU6000_gyro", path),
_parent(parent),
_gyro_topic(-1),
_gyro_class_instance(-1)
@ -1407,36 +1426,49 @@ MPU6000_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
namespace mpu6000
{
MPU6000 *g_dev;
MPU6000 *g_dev_int; // on internal bus
MPU6000 *g_dev_ext; // on external bus
void start();
void test();
void reset();
void info();
void start(bool, enum Rotation);
void test(bool);
void reset(bool);
void info(bool);
void usage();
/**
* Start the driver.
*/
void
start()
start(bool external_bus, enum Rotation rotation)
{
int fd;
MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int;
const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL;
const char *path_gyro = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO;
if (g_dev != nullptr)
if (*g_dev_ptr != nullptr)
/* if already started, the still command succeeded */
errx(0, "already started");
/* create the driver */
g_dev = new MPU6000(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_MPU);
if (external_bus) {
#ifdef PX4_SPI_BUS_EXT
*g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_MPU, rotation);
#else
errx(0, "External SPI not available");
#endif
} else {
*g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU, rotation);
}
if (g_dev == nullptr)
if (*g_dev_ptr == nullptr)
goto fail;
if (OK != g_dev->init())
if (OK != (*g_dev_ptr)->init())
goto fail;
/* set the poll rate to default, starts automatic data collection */
fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
fd = open(path_accel, O_RDONLY);
if (fd < 0)
goto fail;
@ -1449,9 +1481,9 @@ start()
exit(0);
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
if (*g_dev_ptr != nullptr) {
delete (*g_dev_ptr);
*g_dev_ptr = nullptr;
}
errx(1, "driver start failed");
@ -1463,24 +1495,26 @@ fail:
* and automatic modes.
*/
void
test()
test(bool external_bus)
{
const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL;
const char *path_gyro = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO;
accel_report a_report;
gyro_report g_report;
ssize_t sz;
/* get the driver */
int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
int fd = open(path_accel, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)",
MPU_DEVICE_PATH_ACCEL);
path_accel);
/* get the driver */
int fd_gyro = open(MPU_DEVICE_PATH_GYRO, O_RDONLY);
int fd_gyro = open(path_gyro, O_RDONLY);
if (fd_gyro < 0)
err(1, "%s open failed", MPU_DEVICE_PATH_GYRO);
err(1, "%s open failed", path_gyro);
/* reset to manual polling */
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0)
@ -1528,7 +1562,7 @@ test()
/* XXX add poll-rate tests here too */
reset();
reset(external_bus);
errx(0, "PASS");
}
@ -1536,9 +1570,10 @@ test()
* Reset the driver.
*/
void
reset()
reset(bool external_bus)
{
int fd = open(MPU_DEVICE_PATH_ACCEL, O_RDONLY);
const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL;
int fd = open(path_accel, O_RDONLY);
if (fd < 0)
err(1, "failed ");
@ -1558,47 +1593,77 @@ reset()
* Print a little info about the driver.
*/
void
info()
info(bool external_bus)
{
if (g_dev == nullptr)
MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int;
if (*g_dev_ptr == nullptr)
errx(1, "driver not running");
printf("state @ %p\n", g_dev);
g_dev->print_info();
printf("state @ %p\n", *g_dev_ptr);
(*g_dev_ptr)->print_info();
exit(0);
}
void
usage()
{
warnx("missing command: try 'start', 'info', 'test', 'reset'");
warnx("options:");
warnx(" -X (external bus)");
warnx(" -R rotation");
}
} // namespace
int
mpu6000_main(int argc, char *argv[])
{
bool external_bus = false;
int ch;
enum Rotation rotation = ROTATION_NONE;
/* jump over start/off/etc and look at options first */
while ((ch = getopt(argc, argv, "XR:")) != EOF) {
switch (ch) {
case 'X':
external_bus = true;
break;
case 'R':
rotation = (enum Rotation)atoi(optarg);
break;
default:
mpu6000::usage();
exit(0);
}
}
const char *verb = argv[optind];
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start"))
mpu6000::start();
if (!strcmp(verb, "start"))
mpu6000::start(external_bus, rotation);
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test"))
mpu6000::test();
if (!strcmp(verb, "test"))
mpu6000::test(external_bus);
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset"))
mpu6000::reset();
if (!strcmp(verb, "reset"))
mpu6000::reset(external_bus);
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info"))
mpu6000::info();
if (!strcmp(verb, "info"))
mpu6000::info(external_bus);
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
}

47
src/drivers/ms5611/ms5611.cpp
View File

@ -50,6 +50,7 @@
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <getopt.h>
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
@ -775,11 +776,12 @@ namespace ms5611
MS5611 *g_dev;
void start();
void start(bool external_bus);
void test();
void reset();
void info();
void calibrate(unsigned altitude);
void usage();
/**
* MS5611 crc4 cribbed from the datasheet
@ -832,7 +834,7 @@ crc4(uint16_t *n_prom)
* Start the driver.
*/
void
start()
start(bool external_bus)
{
int fd;
prom_u prom_buf;
@ -845,7 +847,7 @@ start()
/* create the driver, try SPI first, fall back to I2C if unsuccessful */
if (MS5611_spi_interface != nullptr)
interface = MS5611_spi_interface(prom_buf);
interface = MS5611_spi_interface(prom_buf, external_bus);
if (interface == nullptr && (MS5611_i2c_interface != nullptr))
interface = MS5611_i2c_interface(prom_buf);
@ -1056,43 +1058,68 @@ calibrate(unsigned altitude)
exit(0);
}
void
usage()
{
warnx("missing command: try 'start', 'info', 'test', 'test2', 'reset', 'calibrate'");
warnx("options:");
warnx(" -X (external bus)");
}
} // namespace
int
ms5611_main(int argc, char *argv[])
{
bool external_bus = false;
int ch;
/* jump over start/off/etc and look at options first */
while ((ch = getopt(argc, argv, "X")) != EOF) {
switch (ch) {
case 'X':
external_bus = true;
break;
default:
ms5611::usage();
exit(0);
}
}
const char *verb = argv[optind];
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start"))
ms5611::start();
if (!strcmp(verb, "start"))
ms5611::start(external_bus);
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test"))
if (!strcmp(verb, "test"))
ms5611::test();
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset"))
if (!strcmp(verb, "reset"))
ms5611::reset();
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info"))
if (!strcmp(verb, "info"))
ms5611::info();
/*
* Perform MSL pressure calibration given an altitude in metres
*/
if (!strcmp(argv[1], "calibrate")) {
if (!strcmp(verb, "calibrate")) {
if (argc < 2)
errx(1, "missing altitude");
long altitude = strtol(argv[2], nullptr, 10);
long altitude = strtol(argv[optind+1], nullptr, 10);
ms5611::calibrate(altitude);
}

2
src/drivers/ms5611/ms5611.h
View File

@ -80,6 +80,6 @@ extern bool crc4(uint16_t *n_prom);
} /* namespace */
/* interface factories */
extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf) weak_function;
extern device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus) weak_function;
extern device::Device *MS5611_i2c_interface(ms5611::prom_u &prom_buf) weak_function;

14
src/drivers/ms5611/ms5611_spi.cpp
View File

@ -62,7 +62,7 @@
#ifdef PX4_SPIDEV_BARO
device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf);
device::Device *MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus);
class MS5611_SPI : public device::SPI
{
@ -115,9 +115,17 @@ private:
};
device::Device *
MS5611_spi_interface(ms5611::prom_u &prom_buf)
MS5611_spi_interface(ms5611::prom_u &prom_buf, bool external_bus)
{
return new MS5611_SPI(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf);
if (external_bus) {
#ifdef PX4_SPI_BUS_EXT
return new MS5611_SPI(PX4_SPI_BUS_EXT, (spi_dev_e)PX4_SPIDEV_EXT_BARO, prom_buf);
#else
return nullptr;
#endif
} else {
return new MS5611_SPI(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf);
}
}
MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) :

2
src/drivers/px4fmu/fmu.cpp
View File

@ -1784,7 +1784,7 @@ fmu_main(int argc, char *argv[])
}
if (!strcmp(verb, "id")) {
char id[12];
uint8_t id[12];
(void)get_board_serial(id);
errx(0, "Board serial:\n %02X%02X%02X%02X %02X%02X%02X%02X %02X%02X%02X%02X",

2
src/drivers/sf0x/module.mk
View File

@ -38,3 +38,5 @@
MODULE_COMMAND = sf0x
SRCS = sf0x.cpp
MAXOPTIMIZATION = -Os

142
src/lib/conversion/rotation.cpp
View File

@ -49,3 +49,145 @@ get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix)
rot_matrix->from_euler(roll, pitch, yaw);
}
#define HALF_SQRT_2 0.70710678118654757f
__EXPORT void
rotate_3f(enum Rotation rot, float &x, float &y, float &z)
{
float tmp;
switch (rot) {
case ROTATION_NONE:
case ROTATION_MAX:
return;
case ROTATION_YAW_45: {
tmp = HALF_SQRT_2*(x - y);
y = HALF_SQRT_2*(x + y);
x = tmp;
return;
}
case ROTATION_YAW_90: {
tmp = x; x = -y; y = tmp;
return;
}
case ROTATION_YAW_135: {
tmp = -HALF_SQRT_2*(x + y);
y = HALF_SQRT_2*(x - y);
x = tmp;
return;
}
case ROTATION_YAW_180:
x = -x; y = -y;
return;
case ROTATION_YAW_225: {
tmp = HALF_SQRT_2*(y - x);
y = -HALF_SQRT_2*(x + y);
x = tmp;
return;
}
case ROTATION_YAW_270: {
tmp = x; x = y; y = -tmp;
return;
}
case ROTATION_YAW_315: {
tmp = HALF_SQRT_2*(x + y);
y = HALF_SQRT_2*(y - x);
x = tmp;
return;
}
case ROTATION_ROLL_180: {
y = -y; z = -z;
return;
}
case ROTATION_ROLL_180_YAW_45: {
tmp = HALF_SQRT_2*(x + y);
y = HALF_SQRT_2*(x - y);
x = tmp; z = -z;
return;
}
case ROTATION_ROLL_180_YAW_90: {
tmp = x; x = y; y = tmp; z = -z;
return;
}
case ROTATION_ROLL_180_YAW_135: {
tmp = HALF_SQRT_2*(y - x);
y = HALF_SQRT_2*(y + x);
x = tmp; z = -z;
return;
}
case ROTATION_PITCH_180: {
x = -x; z = -z;
return;
}
case ROTATION_ROLL_180_YAW_225: {
tmp = -HALF_SQRT_2*(x + y);
y = HALF_SQRT_2*(y - x);
x = tmp; z = -z;
return;
}
case ROTATION_ROLL_180_YAW_270: {
tmp = x; x = -y; y = -tmp; z = -z;
return;
}
case ROTATION_ROLL_180_YAW_315: {
tmp = HALF_SQRT_2*(x - y);
y = -HALF_SQRT_2*(x + y);
x = tmp; z = -z;
return;
}
case ROTATION_ROLL_90: {
tmp = z; z = y; y = -tmp;
return;
}
case ROTATION_ROLL_90_YAW_45: {
tmp = z; z = y; y = -tmp;
tmp = HALF_SQRT_2*(x - y);
y = HALF_SQRT_2*(x + y);
x = tmp;
return;
}
case ROTATION_ROLL_90_YAW_90: {
tmp = z; z = y; y = -tmp;
tmp = x; x = -y; y = tmp;
return;
}
case ROTATION_ROLL_90_YAW_135: {
tmp = z; z = y; y = -tmp;
tmp = -HALF_SQRT_2*(x + y);
y = HALF_SQRT_2*(x - y);
x = tmp;
return;
}
case ROTATION_ROLL_270: {
tmp = z; z = -y; y = tmp;
return;
}
case ROTATION_ROLL_270_YAW_45: {
tmp = z; z = -y; y = tmp;
tmp = HALF_SQRT_2*(x - y);
y = HALF_SQRT_2*(x + y);
x = tmp;
return;
}
case ROTATION_ROLL_270_YAW_90: {
tmp = z; z = -y; y = tmp;
tmp = x; x = -y; y = tmp;
return;
}
case ROTATION_ROLL_270_YAW_135: {
tmp = z; z = -y; y = tmp;
tmp = -HALF_SQRT_2*(x + y);
y = HALF_SQRT_2*(x - y);
x = tmp;
return;
}
case ROTATION_PITCH_90: {
tmp = z; z = -x; x = tmp;
return;
}
case ROTATION_PITCH_270: {
tmp = z; z = x; x = -tmp;
return;
}
}
}

8
src/lib/conversion/rotation.h
View File

@ -118,4 +118,12 @@ const rot_lookup_t rot_lookup[] = {
__EXPORT void
get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix);
/**
* rotate a 3 element float vector in-place
*/
__EXPORT void
rotate_3f(enum Rotation rot, float &x, float &y, float &z);
#endif /* ROTATION_H_ */

2
src/modules/fw_att_control/module.mk
View File

@ -39,3 +39,5 @@ MODULE_COMMAND = fw_att_control
SRCS = fw_att_control_main.cpp \
fw_att_control_params.c
MODULE_STACKSIZE = 1200

2
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -1445,7 +1445,7 @@ FixedwingPositionControl::start()
_control_task = task_spawn_cmd("fw_pos_control_l1",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
3500,
2000,
(main_t)&FixedwingPositionControl::task_main_trampoline,
nullptr);

4
src/modules/fw_pos_control_l1/module.mk
View File

@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2013 PX4 Development Team. All rights reserved.
# Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@ -43,3 +43,5 @@ SRCS = fw_pos_control_l1_main.cpp \
mtecs/mTecs.cpp \
mtecs/limitoverride.cpp \
mtecs/mTecs_params.c
MODULE_STACKSIZE = 1200

2
src/modules/sensors/sensors.cpp
View File

@ -133,7 +133,7 @@
#endif
#define BATT_V_LOWPASS 0.001f
#define BATT_V_IGNORE_THRESHOLD 3.5f
#define BATT_V_IGNORE_THRESHOLD 4.8f
/**
* HACK - true temperature is much less than indicated temperature in baro,

8
src/modules/systemlib/board_serial.c
View File

@ -44,11 +44,11 @@
#include "board_config.h"
#include "board_serial.h"
int get_board_serial(char *serialid)
int get_board_serial(uint8_t *serialid)
{
const volatile unsigned *udid_ptr = (const unsigned *)UDID_START;
const volatile uint32_t *udid_ptr = (const uint32_t *)UDID_START;
union udid id;
val_read((unsigned *)&id, udid_ptr, sizeof(id));
val_read((uint32_t *)&id, udid_ptr, sizeof(id));
/* Copy the serial from the chips non-write memory and swap endianess */
@ -57,4 +57,4 @@ int get_board_serial(char *serialid)
serialid[8] = id.data[11]; serialid[9] = id.data[10]; serialid[10] = id.data[9]; serialid[11] = id.data[8];
return 0;
}
}

2
src/modules/systemlib/board_serial.h
View File

@ -44,6 +44,6 @@
__BEGIN_DECLS
__EXPORT int get_board_serial(char *serialid);
__EXPORT int get_board_serial(uint8_t *serialid);
__END_DECLS

2
src/modules/systemlib/otp.h
View File

@ -125,7 +125,7 @@ struct otp_lock {
#pragma pack(push, 1)
union udid {
uint32_t serial[3];
char data[12];
uint8_t data[12];
};
#pragma pack(pop)

1
src/systemcmds/mtd/24xxxx_mtd.c
View File

@ -161,6 +161,7 @@ static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock,
static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg);
void at24c_test(void);
int at24c_nuke(void);
/************************************************************************************
* Private Data

4
src/systemcmds/param/param.c
View File

@ -63,7 +63,7 @@ static void do_import(const char* param_file_name);
static void do_show(const char* search_string);
static void do_show_print(void *arg, param_t param);
static void do_set(const char* name, const char* val, bool fail_on_not_found);
static void do_compare(const char* name, const char* vals[], unsigned comparisons);
static void do_compare(const char* name, char* vals[], unsigned comparisons);
static void do_reset(void);
static void do_reset_nostart(void);
@ -351,7 +351,7 @@ do_set(const char* name, const char* val, bool fail_on_not_found)
}
static void
do_compare(const char* name, const char* vals[], unsigned comparisons)
do_compare(const char* name, char* vals[], unsigned comparisons)
{
int32_t i;
float f;