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 stack_reduce

This commit is contained in:
Lorenz Meier 2014-07-12 20:10:07 +02:00
parent 2c4cf30857 8a6e69ed67
commit 91638b03bf
70 changed files with 3343 additions and 1241 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:

2
NuttX

@ -1 +1 @@
Subproject commit 2a94cc8e5babb7d5661aedc09201239d39644332
Subproject commit 7e1b97bcf10d8495169eec355988ca5890bfd5df

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

@ -11,28 +11,38 @@ 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 FMU_Q
set MIXER wingwing
# Provide ESC a constant 1000 us pulse
set PWM_OUTPUTS 4
set PWM_DISARMED 1000

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"

10
ROMFS/px4fmu_common/init.d/rcS
View File

@ -279,6 +279,11 @@ then
# Try to get an USB console
nshterm /dev/ttyACM0 &
#
# Start the datamanager
#
dataman start
#
# Start the Commander (needs to be this early for in-air-restarts)
#
@ -547,11 +552,6 @@ then
fi
fi
#
# Start the datamanager
#
dataman start
#
# Start the navigator
#

51
ROMFS/px4fmu_common/mixers/wingwing.mix Normal file
View File

@ -0,0 +1,51 @@
Delta-wing mixer for PX4FMU
===========================
Designed for Wing Wing Z-84
This file defines mixers suitable for controlling a delta wing aircraft using
PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU
servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is
assumed to be unused.
Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
(roll), 1 (pitch) and 3 (thrust).
See the README for more information on the scaler format.
Elevon mixers
-------------
Three scalers total (output, roll, pitch).
On the assumption that the two elevon servos are physically reversed, the pitch
input is inverted between the two servos.
The scaling factor for roll inputs is adjusted to implement differential travel
for the elevons.
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 -6000 -6000 0 -10000 10000
S: 0 1 6500 6500 0 -10000 10000
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 -6000 -6000 0 -10000 10000
S: 0 1 -6500 -6500 0 -10000 10000
Output 2
--------
This mixer is empty.
Z:
Motor speed mixer
-----------------
Two scalers total (output, thrust).
This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
range. Inputs below zero are treated as zero.
M: 1
O: 10000 10000 0 -10000 10000
S: 0 3 0 20000 -10000 -10000 10000

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

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

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 ");

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/px4io/px4io_i2c.cpp Normal file → Executable file
View File

@ -79,7 +79,7 @@ device::Device
}
PX4IO_I2C::PX4IO_I2C(int bus, uint8_t address) :
I2C("PX4IO_i2c", nullptr, bus, address, 320000)
I2C("PX4IO_i2c", nullptr, bus, address, 400000)
{
_retries = 3;
}

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_ */

35
src/modules/commander/commander.cpp
View File

@ -78,6 +78,7 @@
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/system_power.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/telemetry_status.h>
@ -92,6 +93,7 @@
#include <systemlib/cpuload.h>
#include <systemlib/rc_check.h>
#include <systemlib/state_table.h>
#include <dataman/dataman.h>
#include "px4_custom_mode.h"
#include "commander_helper.h"
@ -733,6 +735,11 @@ int commander_thread_main(int argc, char *argv[])
/* publish initial state */
status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
if (status_pub < 0) {
warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
warnx("exiting.");
exit(ERROR);
}
/* armed topic */
orb_advert_t armed_pub;
@ -750,10 +757,27 @@ int commander_thread_main(int argc, char *argv[])
struct home_position_s home;
memset(&home, 0, sizeof(home));
if (status_pub < 0) {
warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
warnx("exiting.");
exit(ERROR);
/* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */
orb_advert_t mission_pub = -1;
mission_s mission;
if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
if (mission.dataman_id >= 0 && mission.dataman_id <= 1) {
warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count, mission.current_seq);
mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d",
mission.dataman_id, mission.count, mission.current_seq);
} else {
warnx("reading mission state failed");
mavlink_log_info(mavlink_fd, "[cmd] reading mission state failed");
/* initialize mission state in dataman */
mission.dataman_id = 0;
mission.count = 0;
mission.current_seq = 0;
dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s));
}
mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission);
orb_publish(ORB_ID(offboard_mission), mission_pub, &mission);
}
mavlink_log_info(mavlink_fd, "[cmd] started");
@ -1494,7 +1518,7 @@ int commander_thread_main(int argc, char *argv[])
/* now set navigation state according to failsafe and main state */
bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled,
mission_result.mission_finished);
mission_result.finished);
// TODO handle mode changes by commands
if (main_state_changed) {
@ -1598,6 +1622,7 @@ int commander_thread_main(int argc, char *argv[])
close(diff_pres_sub);
close(param_changed_sub);
close(battery_sub);
close(mission_pub);
thread_running = false;

54
src/modules/dataman/dataman.c
View File

@ -62,6 +62,8 @@ __EXPORT int dataman_main(int argc, char *argv[]);
__EXPORT ssize_t dm_read(dm_item_t item, unsigned char index, void *buffer, size_t buflen);
__EXPORT ssize_t dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buffer, size_t buflen);
__EXPORT int dm_clear(dm_item_t item);
__EXPORT void dm_lock(dm_item_t item);
__EXPORT void dm_unlock(dm_item_t item);
__EXPORT int dm_restart(dm_reset_reason restart_type);
/** Types of function calls supported by the worker task */
@ -114,12 +116,17 @@ static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = {
DM_KEY_FENCE_POINTS_MAX,
DM_KEY_WAYPOINTS_OFFBOARD_0_MAX,
DM_KEY_WAYPOINTS_OFFBOARD_1_MAX,
DM_KEY_WAYPOINTS_ONBOARD_MAX
DM_KEY_WAYPOINTS_ONBOARD_MAX,
DM_KEY_MISSION_STATE_MAX
};
/* Table of offset for index 0 of each item type */
static unsigned int g_key_offsets[DM_KEY_NUM_KEYS];
/* Item type lock mutexes */
static sem_t *g_item_locks[DM_KEY_NUM_KEYS];
static sem_t g_sys_state_mutex;
/* The data manager store file handle and file name */
static int g_fd = -1, g_task_fd = -1;
static const char *k_data_manager_device_path = "/fs/microsd/dataman";
@ -139,22 +146,22 @@ static work_q_t g_work_q; /* pending work items. To be consumed by worker thread
sem_t g_work_queued_sema; /* To notify worker thread a work item has been queued */
sem_t g_init_sema;
static bool g_task_should_exit; /**< if true, dataman task should exit */
static bool g_task_should_exit; /**< if true, dataman task should exit */
#define DM_SECTOR_HDR_SIZE 4 /* data manager per item header overhead */
static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE; /* total item sorage space */
static void init_q(work_q_t *q)
{
sq_init(&(q->q)); /* Initialize the NuttX queue structure */
sq_init(&(q->q)); /* Initialize the NuttX queue structure */
sem_init(&(q->mutex), 1, 1); /* Queue is initially unlocked */
q->size = q->max_size = 0; /* Queue is initially empty */
q->size = q->max_size = 0; /* Queue is initially empty */
}
static inline void
destroy_q(work_q_t *q)
{
sem_destroy(&(q->mutex)); /* Destroy the queue lock */
sem_destroy(&(q->mutex)); /* Destroy the queue lock */
}
static inline void
@ -318,7 +325,9 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v
buffer[1] = persistence;
buffer[2] = 0;
buffer[3] = 0;
memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count);
if (count > 0) {
memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count);
}
count += DM_SECTOR_HDR_SIZE;
len = -1;
@ -568,6 +577,32 @@ dm_clear(dm_item_t item)
return enqueue_work_item_and_wait_for_result(work);
}
__EXPORT void
dm_lock(dm_item_t item)
{
/* Make sure data manager has been started and is not shutting down */
if ((g_fd < 0) || g_task_should_exit)
return;
if (item >= DM_KEY_NUM_KEYS)
return;
if (g_item_locks[item]) {
sem_wait(g_item_locks[item]);
}
}
__EXPORT void
dm_unlock(dm_item_t item)
{
/* Make sure data manager has been started and is not shutting down */
if ((g_fd < 0) || g_task_should_exit)
return;
if (item >= DM_KEY_NUM_KEYS)
return;
if (g_item_locks[item]) {
sem_post(g_item_locks[item]);
}
}
/* Tell the data manager about the type of the last reset */
__EXPORT int
dm_restart(dm_reset_reason reason)
@ -608,6 +643,12 @@ task_main(int argc, char *argv[])
for (unsigned i = 0; i < dm_number_of_funcs; i++)
g_func_counts[i] = 0;
/* Initialize the item type locks, for now only DM_KEY_MISSION_STATE supports locking */
sem_init(&g_sys_state_mutex, 1, 1); /* Initially unlocked */
for (unsigned i = 0; i < DM_KEY_NUM_KEYS; i++)
g_item_locks[i] = NULL;
g_item_locks[DM_KEY_MISSION_STATE] = &g_sys_state_mutex;
g_task_should_exit = false;
init_q(&g_work_q);
@ -743,6 +784,7 @@ task_main(int argc, char *argv[])
destroy_q(&g_work_q);
destroy_q(&g_free_q);
sem_destroy(&g_work_queued_sema);
sem_destroy(&g_sys_state_mutex);
return 0;
}

18
src/modules/dataman/dataman.h
View File

@ -53,16 +53,20 @@ extern "C" {
DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */
DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alernate between 0 and 1) */
DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */
DM_KEY_MISSION_STATE, /* Persistent mission state */
DM_KEY_NUM_KEYS /* Total number of item types defined */
} dm_item_t;
#define DM_KEY_WAYPOINTS_OFFBOARD(_id) (_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1)
/** The maximum number of instances for each item type */
enum {
DM_KEY_SAFE_POINTS_MAX = 8,
DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES,
DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED,
DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED,
DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED
DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED,
DM_KEY_MISSION_STATE_MAX = 1
};
/** Data persistence levels */
@ -101,6 +105,18 @@ extern "C" {
size_t buflen /* Length in bytes of data to retrieve */
);
/** Lock all items of this type */
__EXPORT void
dm_lock(
dm_item_t item /* The item type to clear */
);
/** Unlock all items of this type */
__EXPORT void
dm_unlock(
dm_item_t item /* The item type to clear */
);
/** Erase all items of this type */
__EXPORT int
dm_clear(

1
src/modules/ekf_att_pos_estimator/InertialNav Submodule

@ -0,0 +1 @@
Subproject commit 8b65d755b8c4834f90a323172c25d91c45068e21

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

772
src/modules/mavlink/mavlink_main.cpp
View File

@ -18,7 +18,6 @@
*
* 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,
@ -73,8 +72,6 @@
#include <mavlink/mavlink_log.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include "mavlink_bridge_header.h"
#include "mavlink_main.h"
@ -109,6 +106,8 @@ static struct file_operations fops;
*/
extern "C" __EXPORT int mavlink_main(int argc, char *argv[]);
extern mavlink_system_t mavlink_system;
static uint64_t last_write_success_times[6] = {0};
static uint64_t last_write_try_times[6] = {0};
@ -209,9 +208,6 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
}
}
}
static void usage(void);
@ -230,7 +226,9 @@ Mavlink::Mavlink() :
_main_loop_delay(1000),
_subscriptions(nullptr),
_streams(nullptr),
_mission_manager(nullptr),
_mission_pub(-1),
_mission_result_sub(-1),
_mode(MAVLINK_MODE_NORMAL),
_total_counter(0),
_verbose(false),
@ -253,8 +251,6 @@ Mavlink::Mavlink() :
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
_txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
{
_wpm = &_wpm_s;
mission.count = 0;
fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl;
_instance_id = Mavlink::instance_count();
@ -439,7 +435,7 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self)
}
void
Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self)
Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
{
Mavlink *inst;
@ -496,6 +492,7 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
// printf("logmsg: %s\n", txt);
struct mavlink_logmessage msg;
strncpy(msg.text, txt, sizeof(msg.text));
msg.severity = (unsigned char)cmd;
Mavlink *inst;
LL_FOREACH(_mavlink_instances, inst) {
@ -515,7 +512,6 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
void Mavlink::mavlink_update_system(void)
{
if (!_param_initialized) {
_param_system_id = param_find("MAV_SYS_ID");
_param_component_id = param_find("MAV_COMP_ID");
@ -729,9 +725,32 @@ Mavlink::set_hil_enabled(bool hil_enabled)
return ret;
}
extern mavlink_system_t mavlink_system;
void
Mavlink::send_message(const mavlink_message_t *msg)
{
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
int Mavlink::mavlink_pm_queued_send()
uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
mavlink_send_uart_bytes(_channel, buf, len);
}
void
Mavlink::handle_message(const mavlink_message_t *msg)
{
/* handle packet with mission manager */
_mission_manager->handle_message(msg);
/* handle packet with parameter component */
mavlink_pm_message_handler(_channel, msg);
if (get_forwarding_on()) {
/* forward any messages to other mavlink instances */
Mavlink::forward_message(msg, this);
}
}
int
Mavlink::mavlink_pm_queued_send()
{
if (_mavlink_param_queue_index < param_count()) {
mavlink_pm_send_param(param_for_index(_mavlink_param_queue_index));
@ -808,7 +827,7 @@ int Mavlink::mavlink_pm_send_param(param_t param)
mavlink_type,
param_count(),
param_get_index(param));
mavlink_missionlib_send_message(&tx_msg);
send_message(&tx_msg);
return OK;
}
@ -822,7 +841,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
(req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) {
/* Start sending parameters */
mavlink_pm_start_queued_send();
mavlink_missionlib_send_gcs_string("[mavlink pm] sending list");
send_statustext_info("[pm] sending list");
}
} break;
@ -846,7 +865,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
if (param == PARAM_INVALID) {
char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
sprintf(buf, "[pm] unknown: %s", name);
mavlink_missionlib_send_gcs_string(buf);
send_statustext_info(buf);
} else {
/* set and send parameter */
@ -882,653 +901,29 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
}
}
void Mavlink::publish_mission()
int
Mavlink::send_statustext_info(const char *string)
{
/* Initialize mission publication if necessary */
if (_mission_pub < 0) {
_mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission);
} else {
orb_publish(ORB_ID(offboard_mission), _mission_pub, &mission);
}
return send_statustext(MAVLINK_IOC_SEND_TEXT_INFO, string);
}
int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item)
{
/* only support global waypoints for now */
switch (mavlink_mission_item->frame) {
case MAV_FRAME_GLOBAL:
mission_item->lat = (double)mavlink_mission_item->x;
mission_item->lon = (double)mavlink_mission_item->y;
mission_item->altitude = mavlink_mission_item->z;
mission_item->altitude_is_relative = false;
break;
case MAV_FRAME_GLOBAL_RELATIVE_ALT:
mission_item->lat = (double)mavlink_mission_item->x;
mission_item->lon = (double)mavlink_mission_item->y;
mission_item->altitude = mavlink_mission_item->z;
mission_item->altitude_is_relative = true;
break;
case MAV_FRAME_LOCAL_NED:
case MAV_FRAME_LOCAL_ENU:
return MAV_MISSION_UNSUPPORTED_FRAME;
case MAV_FRAME_MISSION:
default:
return MAV_MISSION_ERROR;
}
switch (mavlink_mission_item->command) {
case MAV_CMD_NAV_TAKEOFF:
mission_item->pitch_min = mavlink_mission_item->param1;
break;
case MAV_CMD_DO_JUMP:
mission_item->do_jump_mission_index = mavlink_mission_item->param1;
mission_item->do_jump_current_count = 0;
mission_item->do_jump_repeat_count = mavlink_mission_item->param2;
break;
default:
mission_item->acceptance_radius = mavlink_mission_item->param2;
mission_item->time_inside = mavlink_mission_item->param1;
break;
}
mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
mission_item->loiter_radius = fabsf(mavlink_mission_item->param3);
mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
mission_item->autocontinue = mavlink_mission_item->autocontinue;
// mission_item->index = mavlink_mission_item->seq;
mission_item->origin = ORIGIN_MAVLINK;
/* reset DO_JUMP count */
mission_item->do_jump_current_count = 0;
return OK;
}
int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item)
{
if (mission_item->altitude_is_relative) {
mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
} else {
mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
}
switch (mission_item->nav_cmd) {
case NAV_CMD_TAKEOFF:
mavlink_mission_item->param1 = mission_item->pitch_min;
break;
case NAV_CMD_DO_JUMP:
mavlink_mission_item->param1 = mission_item->do_jump_mission_index;
mavlink_mission_item->param2 = mission_item->do_jump_repeat_count;
break;
default:
mavlink_mission_item->param2 = mission_item->acceptance_radius;
mavlink_mission_item->param1 = mission_item->time_inside;
break;
}
mavlink_mission_item->x = (float)mission_item->lat;
mavlink_mission_item->y = (float)mission_item->lon;
mavlink_mission_item->z = mission_item->altitude;
mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction;
mavlink_mission_item->command = mission_item->nav_cmd;
mavlink_mission_item->autocontinue = mission_item->autocontinue;
// mavlink_mission_item->seq = mission_item->index;
return OK;
}
void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state)
{
state->size = 0;
state->max_size = MAVLINK_WPM_MAX_WP_COUNT;
state->current_state = MAVLINK_WPM_STATE_IDLE;
state->current_partner_sysid = 0;
state->current_partner_compid = 0;
state->timestamp_lastaction = 0;
state->timestamp_last_send_setpoint = 0;
state->timestamp_last_send_request = 0;
state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT;
state->current_dataman_id = 0;
}
/*
* @brief Sends an waypoint ack message
*/
void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type)
{
mavlink_message_t msg;
mavlink_mission_ack_t wpa;
wpa.target_system = sysid;
wpa.target_component = compid;
wpa.type = type;
mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpa);
mavlink_missionlib_send_message(&msg);
if (_verbose) { warnx("Sent waypoint ack (%u) to ID %u", wpa.type, wpa.target_system); }
}
/*
* @brief Broadcasts the new target waypoint and directs the MAV to fly there
*
* This function broadcasts its new active waypoint sequence number and
* sends a message to the controller, advising it to fly to the coordinates
* of the waypoint with a given orientation
*
* @param seq The waypoint sequence number the MAV should fly to.
*/
void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq)
{
if (seq < _wpm->size) {
mavlink_message_t msg;
mavlink_mission_current_t wpc;
wpc.seq = seq;
mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc);
mavlink_missionlib_send_message(&msg);
} else if (seq == 0 && _wpm->size == 0) {
/* don't broadcast if no WPs */
} else {
mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds");
}
}
void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count)
{
mavlink_message_t msg;
mavlink_mission_count_t wpc;
wpc.target_system = sysid;
wpc.target_component = compid;
wpc.count = mission.count;
mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc);
mavlink_missionlib_send_message(&msg);
if (_verbose) { warnx("Sent waypoint count (%u) to ID %u", wpc.count, wpc.target_system); }
}
void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq)
{
struct mission_item_s mission_item;
ssize_t len = sizeof(struct mission_item_s);
dm_item_t dm_current;
if (_wpm->current_dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
if (dm_read(dm_current, seq, &mission_item, len) == len) {
/* create mission_item_s from mavlink_mission_item_t */
mavlink_mission_item_t wp;
map_mission_item_to_mavlink_mission_item(&mission_item, &wp);
mavlink_message_t msg;
wp.target_system = sysid;
wp.target_component = compid;
wp.seq = seq;
mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp);
mavlink_missionlib_send_message(&msg);
if (_verbose) { warnx("Sent waypoint %u to ID %u", wp.seq, wp.target_system); }
} else {
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
mavlink_missionlib_send_gcs_string("#audio: Unable to read from micro SD");
if (_verbose) { warnx("ERROR: could not read WP%u", seq); }
}
}
void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq)
{
if (seq < _wpm->max_size) {
mavlink_message_t msg;
mavlink_mission_request_t wpr;
wpr.target_system = sysid;
wpr.target_component = compid;
wpr.seq = seq;
mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpr);
mavlink_missionlib_send_message(&msg);
_wpm->timestamp_last_send_request = hrt_absolute_time();
if (_verbose) { warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); }
} else {
mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity");
}
}
/*
* @brief emits a message that a waypoint reached
*
* This function broadcasts a message that a waypoint is reached.
*
* @param seq The waypoint sequence number the MAV has reached.
*/
void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq)
{
mavlink_message_t msg;
mavlink_mission_item_reached_t wp_reached;
wp_reached.seq = seq;
mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp_reached);
mavlink_missionlib_send_message(&msg);
if (_verbose) { warnx("Sent waypoint %u reached message", wp_reached.seq); }
}
void Mavlink::mavlink_waypoint_eventloop(uint64_t now)
{
/* check for timed-out operations */
if (now - _wpm->timestamp_lastaction > _wpm->timeout && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
mavlink_missionlib_send_gcs_string("Operation timeout");
_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
_wpm->current_partner_sysid = 0;
_wpm->current_partner_compid = 0;
} else if (now - _wpm->timestamp_last_send_request > 500000 && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
/* try to get WP again after short timeout */
mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
}
}
void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
{
uint64_t now = hrt_absolute_time();
switch (msg->msgid) {
case MAVLINK_MSG_ID_MISSION_ACK: {
mavlink_mission_ack_t wpa;
mavlink_msg_mission_ack_decode(msg, &wpa);
if ((msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/)) {
_wpm->timestamp_lastaction = now;
if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) {
if (_wpm->current_wp_id == _wpm->size - 1) {
_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
_wpm->current_wp_id = 0;
}
}
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
}
break;
}
case MAVLINK_MSG_ID_MISSION_SET_CURRENT: {
mavlink_mission_set_current_t wpc;
mavlink_msg_mission_set_current_decode(msg, &wpc);
if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) {
_wpm->timestamp_lastaction = now;
if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
if (wpc.seq < _wpm->size) {
mission.current_index = wpc.seq;
publish_mission();
/* don't answer yet, wait for the navigator to respond, then publish the mission_result */
// mavlink_wpm_send_waypoint_current(wpc.seq);
} else {
mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
}
} else {
mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
}
}
break;
}
case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: {
mavlink_mission_request_list_t wprl;
mavlink_msg_mission_request_list_decode(msg, &wprl);
if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) {
_wpm->timestamp_lastaction = now;
if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
if (_wpm->size > 0) {
_wpm->current_state = MAVLINK_WPM_STATE_SENDLIST;
_wpm->current_wp_id = 0;
_wpm->current_partner_sysid = msg->sysid;
_wpm->current_partner_compid = msg->compid;
} else {
if (_verbose) { warnx("No waypoints send"); }
}
_wpm->current_count = _wpm->size;
mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, _wpm->current_count);
} else {
mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy");
}
}
break;
}
case MAVLINK_MSG_ID_MISSION_REQUEST: {
mavlink_mission_request_t wpr;
mavlink_msg_mission_request_decode(msg, &wpr);
if (msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) {
_wpm->timestamp_lastaction = now;
if (wpr.seq >= _wpm->size) {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list");
break;
}
/*
* Ensure that we are in the correct state and that the first request has id 0
* and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint)
*/
if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) {
if (wpr.seq == 0) {
if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u from %u changing to STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
_wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0");
break;
}
} else if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) {
if (wpr.seq == _wpm->current_wp_id) {
if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u (again) from %u staying in STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
} else if (wpr.seq == _wpm->current_wp_id + 1) {
if (_verbose) { warnx("Got ITEM_REQUEST of waypoint %u from %u staying in STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); }
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected");
break;
}
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
break;
}
_wpm->current_wp_id = wpr.seq;
_wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
if (wpr.seq < _wpm->size) {
mavlink_wpm_send_waypoint(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
} else {
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
mavlink_missionlib_send_gcs_string("ERROR: Waypoint out of bounds");
}
} else {
//we we're target but already communicating with someone else
if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid)) {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, _wpm->current_partner_sysid); }
}
}
break;
}
case MAVLINK_MSG_ID_MISSION_COUNT: {
mavlink_mission_count_t wpc;
mavlink_msg_mission_count_decode(msg, &wpc);
if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) {
_wpm->timestamp_lastaction = now;
if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
if (wpc.count > NUM_MISSIONS_SUPPORTED) {
if (_verbose) { warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); }
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_NO_SPACE);
break;
}
if (wpc.count == 0) {
mavlink_missionlib_send_gcs_string("WP COUNT 0");
break;
}
_wpm->current_state = MAVLINK_WPM_STATE_GETLIST;
_wpm->current_wp_id = 0;
_wpm->current_partner_sysid = msg->sysid;
_wpm->current_partner_compid = msg->compid;
_wpm->current_count = wpc.count;
mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
} else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
if (_wpm->current_wp_id == 0) {
mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN");
} else {
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy with WP");
}
} else {
mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy");
}
}
}
break;
case MAVLINK_MSG_ID_MISSION_ITEM: {
mavlink_mission_item_t wp;
mavlink_msg_mission_item_decode(msg, &wp);
if (wp.target_system == mavlink_system.sysid && wp.target_component == _mavlink_wpm_comp_id) {
_wpm->timestamp_lastaction = now;
/*
* ensure that we are in the correct state and that the first waypoint has id 0
* and the following waypoints have the correct ids
*/
if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
if (wp.seq != 0) {
mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP not 0");
break;
}
} else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
if (wp.seq >= _wpm->current_count) {
mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds");
break;
}
if (wp.seq != _wpm->current_wp_id) {
mavlink_missionlib_send_gcs_string("IGN: waypoint ID mismatch");
mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
break;
}
}
_wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS;
struct mission_item_s mission_item;
int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission_item);
if (ret != OK) {
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, ret);
_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
break;
}
ssize_t len = sizeof(struct mission_item_s);
dm_item_t dm_next;
if (_wpm->current_dataman_id == 0) {
dm_next = DM_KEY_WAYPOINTS_OFFBOARD_1;
mission.dataman_id = 1;
} else {
dm_next = DM_KEY_WAYPOINTS_OFFBOARD_0;
mission.dataman_id = 0;
}
if (dm_write(dm_next, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) {
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD");
_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
break;
}
// if (wp.current) {
// warnx("current is: %d", wp.seq);
// mission.current_index = wp.seq;
// }
// XXX ignore current set
mission.current_index = -1;
_wpm->current_wp_id = wp.seq + 1;
if (_wpm->current_wp_id == _wpm->current_count && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
if (_verbose) { warnx("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_count); }
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
mission.count = _wpm->current_count;
publish_mission();
_wpm->current_dataman_id = mission.dataman_id;
_wpm->size = _wpm->current_count;
_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
} else {
mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
}
}
break;
}
case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: {
mavlink_mission_clear_all_t wpca;
mavlink_msg_mission_clear_all_decode(msg, &wpca);
if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) {
if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) {
_wpm->timestamp_lastaction = now;
_wpm->size = 0;
/* prepare mission topic */
mission.dataman_id = -1;
mission.count = 0;
mission.current_index = -1;
publish_mission();
if (dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_0) == OK && dm_clear(DM_KEY_WAYPOINTS_OFFBOARD_1) == OK) {
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
} else {
mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
}
} else {
mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy");
if (_verbose) { warnx("IGN WP CLEAR CMD: Busy"); }
}
}
break;
}
default: {
/* other messages might should get caught by mavlink and others */
break;
}
}
}
void
Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg)
{
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
mavlink_send_uart_bytes(_channel, buf, len);
}
int
Mavlink::mavlink_missionlib_send_gcs_string(const char *string)
Mavlink::send_statustext_critical(const char *string)
{
return send_statustext(MAVLINK_IOC_SEND_TEXT_CRITICAL, string);
}
int
Mavlink::send_statustext_emergency(const char *string)
{
return send_statustext(MAVLINK_IOC_SEND_TEXT_EMERGENCY, string);
}
int
Mavlink::send_statustext(unsigned severity, const char *string)
{
const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
mavlink_statustext_t statustext;
statustext.severity = MAV_SEVERITY_INFO;
int i = 0;
@ -1546,11 +941,24 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string)
/* Enforce null termination */
statustext.text[i] = '\0';
/* Map severity */
switch (severity) {
case MAVLINK_IOC_SEND_TEXT_INFO:
statustext.severity = MAV_SEVERITY_INFO;
break;
case MAVLINK_IOC_SEND_TEXT_CRITICAL:
statustext.severity = MAV_SEVERITY_CRITICAL;
break;
case MAVLINK_IOC_SEND_TEXT_EMERGENCY:
statustext.severity = MAV_SEVERITY_EMERGENCY;
break;
}
mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text);
return OK;
} else {
return 1;
return ERROR;
}
}
@ -1702,7 +1110,7 @@ Mavlink::message_buffer_is_empty()
bool
Mavlink::message_buffer_write(void *ptr, int size)
Mavlink::message_buffer_write(const void *ptr, int size)
{
// bytes available to write
int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1;
@ -1769,7 +1177,7 @@ Mavlink::message_buffer_mark_read(int n)
}
void
Mavlink::pass_message(mavlink_message_t *msg)
Mavlink::pass_message(const mavlink_message_t *msg)
{
if (_passing_on) {
/* size is 8 bytes plus variable payload */
@ -1780,7 +1188,11 @@ Mavlink::pass_message(mavlink_message_t *msg)
}
}
float
Mavlink::get_rate_mult()
{
return _datarate / 1000.0f;
}
int
Mavlink::task_main(int argc, char *argv[])
@ -1902,8 +1314,6 @@ Mavlink::task_main(int argc, char *argv[])
break;
}
_mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate);
/* flush stdout in case MAVLink is about to take it over */
@ -1948,12 +1358,11 @@ Mavlink::task_main(int argc, char *argv[])
/* start the MAVLink receiver */
_receive_thread = MavlinkReceiver::receive_start(this);
/* initialize waypoint manager */
mavlink_wpm_init(_wpm);
_mission_result_sub = orb_subscribe(ORB_ID(mission_result));
int mission_result_sub = orb_subscribe(ORB_ID(mission_result));
struct mission_result_s mission_result;
memset(&mission_result, 0, sizeof(mission_result));
/* create mission manager */
_mission_manager = new MavlinkMissionManager(this);
_mission_manager->set_verbose(_verbose);
_task_running = true;
@ -1968,7 +1377,7 @@ Mavlink::task_main(int argc, char *argv[])
MavlinkCommandsStream commands_stream(this, _channel);
/* add default streams depending on mode and intervals depending on datarate */
float rate_mult = _datarate / 1000.0f;
float rate_mult = get_rate_mult();
configure_stream("HEARTBEAT", 1.0f);
@ -2003,7 +1412,6 @@ Mavlink::task_main(int argc, char *argv[])
/* don't send parameters on startup without request */
_mavlink_param_queue_index = param_count();
MavlinkRateLimiter slow_rate_limiter(2000000.0f / rate_mult);
MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult);
/* set main loop delay depending on data rate to minimize CPU overhead */
@ -2057,36 +1465,16 @@ Mavlink::task_main(int argc, char *argv[])
stream->update(t);
}
bool updated;
orb_check(mission_result_sub, &updated);
if (updated) {
orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
if (_verbose) { warnx("Got mission result: new current: %d", mission_result.index_current_mission); }
if (mission_result.mission_reached) {
mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index_reached);
}
mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission);
} else {
if (slow_rate_limiter.check(t)) {
mavlink_wpm_send_waypoint_current((uint16_t)mission_result.index_current_mission);
}
}
if (fast_rate_limiter.check(t)) {
mavlink_pm_queued_send();
mavlink_waypoint_eventloop(hrt_absolute_time());
_mission_manager->eventloop();
if (!mavlink_logbuffer_is_empty(&_logbuffer)) {
struct mavlink_logmessage msg;
int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg);
if (lb_ret == OK) {
mavlink_missionlib_send_gcs_string(msg.text);
send_statustext(msg.severity, msg.text);
}
}
}
@ -2138,11 +1526,11 @@ Mavlink::task_main(int argc, char *argv[])
}
}
perf_end(_loop_perf);
}
delete _mission_manager;
delete _subscribe_to_stream;
_subscribe_to_stream = nullptr;

131
src/modules/mavlink/mavlink_main.h
View File

@ -50,53 +50,13 @@
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include "mavlink_bridge_header.h"
#include "mavlink_orb_subscription.h"
#include "mavlink_stream.h"
#include "mavlink_messages.h"
// FIXME XXX - TO BE MOVED TO XML
enum MAVLINK_WPM_STATES {
MAVLINK_WPM_STATE_IDLE = 0,
MAVLINK_WPM_STATE_SENDLIST,
MAVLINK_WPM_STATE_SENDLIST_SENDWPS,
MAVLINK_WPM_STATE_GETLIST,
MAVLINK_WPM_STATE_GETLIST_GETWPS,
MAVLINK_WPM_STATE_GETLIST_GOTALL,
MAVLINK_WPM_STATE_ENUM_END
};
enum MAVLINK_WPM_CODES {
MAVLINK_WPM_CODE_OK = 0,
MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED,
MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED,
MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS,
MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED,
MAVLINK_WPM_CODE_ENUM_END
};
#define MAVLINK_WPM_MAX_WP_COUNT 255
#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds
#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint
#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000
struct mavlink_wpm_storage {
uint16_t size;
uint16_t max_size;
enum MAVLINK_WPM_STATES current_state;
int16_t current_wp_id; ///< Waypoint in current transmission
uint16_t current_count;
uint8_t current_partner_sysid;
uint8_t current_partner_compid;
uint64_t timestamp_lastaction;
uint64_t timestamp_last_send_setpoint;
uint64_t timestamp_last_send_request;
uint32_t timeout;
int current_dataman_id;
};
#include "mavlink_mission.h"
class Mavlink
@ -139,7 +99,7 @@ public:
static bool instance_exists(const char *device_name, Mavlink *self);
static void forward_message(mavlink_message_t *msg, Mavlink *self);
static void forward_message(const mavlink_message_t *msg, Mavlink *self);
static int get_uart_fd(unsigned index);
@ -178,11 +138,6 @@ public:
bool get_forwarding_on() { return _forwarding_on; }
/**
* Handle waypoint related messages.
*/
void mavlink_wpm_message_handler(const mavlink_message_t *msg);
static int start_helper(int argc, char *argv[]);
/**
@ -202,7 +157,11 @@ public:
*/
int set_hil_enabled(bool hil_enabled);
MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic);
void send_message(const mavlink_message_t *msg);
void handle_message(const mavlink_message_t *msg);
MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic);
int get_instance_id();
@ -215,14 +174,43 @@ public:
mavlink_channel_t get_channel();
void configure_stream_threadsafe(const char *stream_name, float rate);
void configure_stream_threadsafe(const char *stream_name, float rate);
bool _task_should_exit; /**< if true, mavlink task should exit */
int get_mavlink_fd() { return _mavlink_fd; }
MavlinkStream * get_streams() const { return _streams; }
/**
* Send a status text with loglevel INFO
*
* @param string the message to send (will be capped by mavlink max string length)
*/
int send_statustext_info(const char *string);
/**
* Send a status text with loglevel CRITICAL
*
* @param string the message to send (will be capped by mavlink max string length)
*/
int send_statustext_critical(const char *string);
/**
* Send a status text with loglevel EMERGENCY
*
* @param string the message to send (will be capped by mavlink max string length)
*/
int send_statustext_emergency(const char *string);
/**
* Send a status text with loglevel
*
* @param string the message to send (will be capped by mavlink max string length)
* @param severity the log level, one of
*/
int send_statustext(unsigned severity, const char *string);
MavlinkStream * get_streams() const { return _streams; }
float get_rate_mult();
/* Functions for waiting to start transmission until message received. */
void set_has_received_messages(bool received_messages) { _received_messages = received_messages; }
@ -231,15 +219,15 @@ public:
bool get_wait_to_transmit() { return _wait_to_transmit; }
bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
bool message_buffer_write(void *ptr, int size);
bool message_buffer_write(const void *ptr, int size);
void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
/**
* Count a transmision error
*/
void count_txerr();
void count_txerr();
protected:
Mavlink *next;
@ -262,22 +250,19 @@ private:
MavlinkOrbSubscription *_subscriptions;
MavlinkStream *_streams;
orb_advert_t _mission_pub;
struct mission_s mission;
MAVLINK_MODE _mode;
MavlinkMissionManager *_mission_manager;
uint8_t _mavlink_wpm_comp_id;
mavlink_channel_t _channel;
orb_advert_t _mission_pub;
int _mission_result_sub;
MAVLINK_MODE _mode;
mavlink_channel_t _channel;
struct mavlink_logbuffer _logbuffer;
unsigned int _total_counter;
pthread_t _receive_thread;
/* Allocate storage space for waypoints */
mavlink_wpm_storage _wpm_s;
mavlink_wpm_storage *_wpm;
bool _verbose;
bool _forwarding_on;
bool _passing_on;
@ -363,21 +348,6 @@ private:
void mavlink_update_system();
void mavlink_waypoint_eventloop(uint64_t now);
void mavlink_wpm_send_waypoint_reached(uint16_t seq);
void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq);
void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq);
void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count);
void mavlink_wpm_send_waypoint_current(uint16_t seq);
void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type);
void mavlink_wpm_init(mavlink_wpm_storage *state);
int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item);
int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item);
void publish_mission();
void mavlink_missionlib_send_message(mavlink_message_t *msg);
int mavlink_missionlib_send_gcs_string(const char *string);
int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
int configure_stream(const char *stream_name, const float rate);
@ -394,7 +364,7 @@ private:
void message_buffer_mark_read(int n);
void pass_message(mavlink_message_t *msg);
void pass_message(const mavlink_message_t *msg);
static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
@ -402,5 +372,4 @@ private:
* Main mavlink task.
*/
int task_main(int argc, char *argv[]);
};

13
src/modules/mavlink/mavlink_messages.cpp
View File

@ -170,6 +170,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
break;
case NAVIGATION_STATE_LAND:
/* fallthrough */
case NAVIGATION_STATE_DESCEND:
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED;
@ -190,6 +192,17 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
break;
case NAVIGATION_STATE_OFFBOARD:
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD;
break;
case NAVIGATION_STATE_MAX:
/* this is an unused case, ignore */
break;
}
*mavlink_custom_mode = custom_mode.data;

828
src/modules/mavlink/mavlink_mission.cpp Normal file
View File

@ -0,0 +1,828 @@
/****************************************************************************
*
* Copyright (c) 2012-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
* 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 mavlink_mission.cpp
* MAVLink mission manager implementation.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include "mavlink_mission.h"
#include "mavlink_main.h"
#include <math.h>
#include <lib/geo/geo.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <dataman/dataman.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) :
_mavlink(mavlink),
_state(MAVLINK_WPM_STATE_IDLE),
_time_last_recv(0),
_time_last_sent(0),
_action_timeout(MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT),
_retry_timeout(MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT),
_max_count(DM_KEY_WAYPOINTS_OFFBOARD_0_MAX),
_dataman_id(0),
_count(0),
_current_seq(0),
_transfer_dataman_id(0),
_transfer_count(0),
_transfer_seq(0),
_transfer_current_seq(0),
_transfer_partner_sysid(0),
_transfer_partner_compid(0),
_offboard_mission_sub(-1),
_mission_result_sub(-1),
_offboard_mission_pub(-1),
_slow_rate_limiter(2000000.0f / mavlink->get_rate_mult()),
_verbose(false),
_channel(mavlink->get_channel()),
_comp_id(MAV_COMP_ID_MISSIONPLANNER)
{
_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
_mission_result_sub = orb_subscribe(ORB_ID(mission_result));
init_offboard_mission();
}
MavlinkMissionManager::~MavlinkMissionManager()
{
close(_offboard_mission_pub);
close(_mission_result_sub);
}
void
MavlinkMissionManager::init_offboard_mission()
{
mission_s mission_state;
if (dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) == sizeof(mission_s)) {
_dataman_id = mission_state.dataman_id;
_count = mission_state.count;
_current_seq = mission_state.current_seq;
warnx("offboard mission init: dataman_id=%d, count=%u, current_seq=%d", _dataman_id, _count, _current_seq);
} else {
_dataman_id = 0;
_count = 0;
_current_seq = 0;
warnx("offboard mission init: ERROR, reading mission state failed");
}
}
/**
* Write new mission state to dataman and publish offboard_mission topic to notify navigator about changes.
*/
int
MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int seq)
{
struct mission_s mission;
mission.dataman_id = dataman_id;
mission.count = count;
mission.current_seq = seq;
/* update mission state in dataman */
int res = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s));
if (res == sizeof(mission_s)) {
/* update active mission state */
_dataman_id = dataman_id;
_count = count;
_current_seq = seq;
/* mission state saved successfully, publish offboard_mission topic */
if (_offboard_mission_pub < 0) {
_offboard_mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission);
} else {
orb_publish(ORB_ID(offboard_mission), _offboard_mission_pub, &mission);
}
return OK;
} else {
warnx("ERROR: can't save mission state");
_mavlink->send_statustext(MAV_SEVERITY_CRITICAL, "ERROR: can't save mission state");
return ERROR;
}
}
void
MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type)
{
mavlink_message_t msg;
mavlink_mission_ack_t wpa;
wpa.target_system = sysid;
wpa.target_component = compid;
wpa.type = type;
mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpa);
_mavlink->send_message(&msg);
if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); }
}
void
MavlinkMissionManager::send_mission_current(uint16_t seq)
{
if (seq < _count) {
mavlink_message_t msg;
mavlink_mission_current_t wpc;
wpc.seq = seq;
mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc);
_mavlink->send_message(&msg);
} else if (seq == 0 && _count == 0) {
/* don't broadcast if no WPs */
} else {
if (_verbose) { warnx("WPM: Send MISSION_CURRENT ERROR: seq %u out of bounds", seq); }
_mavlink->send_statustext_critical("ERROR: wp index out of bounds");
}
}
void
MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count)
{
_time_last_sent = hrt_absolute_time();
mavlink_message_t msg;
mavlink_mission_count_t wpc;
wpc.target_system = sysid;
wpc.target_component = compid;
wpc.count = _count;
mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc);
_mavlink->send_message(&msg);
if (_verbose) { warnx("WPM: Send MISSION_COUNT %u to ID %u", wpc.count, wpc.target_system); }
}
void
MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq)
{
dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_dataman_id);
struct mission_item_s mission_item;
if (dm_read(dm_item, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) {
_time_last_sent = hrt_absolute_time();
/* create mission_item_s from mavlink_mission_item_t */
mavlink_mission_item_t wp;
format_mavlink_mission_item(&mission_item, &wp);
mavlink_message_t msg;
wp.target_system = sysid;
wp.target_component = compid;
wp.seq = seq;
wp.current = (_current_seq == seq) ? 1 : 0;
mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp);
_mavlink->send_message(&msg);
if (_verbose) { warnx("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); }
} else {
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
_mavlink->send_statustext_critical("Unable to read from micro SD");
if (_verbose) { warnx("WPM: Send MISSION_ITEM ERROR: could not read seq %u from dataman ID %i", seq, _dataman_id); }
}
}
void
MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq)
{
if (seq < _max_count) {
_time_last_sent = hrt_absolute_time();
mavlink_message_t msg;
mavlink_mission_request_t wpr;
wpr.target_system = sysid;
wpr.target_component = compid;
wpr.seq = seq;
mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpr);
_mavlink->send_message(&msg);
if (_verbose) { warnx("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); }
} else {
_mavlink->send_statustext_critical("ERROR: Waypoint index exceeds list capacity");
if (_verbose) { warnx("WPM: Send MISSION_REQUEST ERROR: seq %u exceeds list capacity", seq); }
}
}
void
MavlinkMissionManager::send_mission_item_reached(uint16_t seq)
{
mavlink_message_t msg;
mavlink_mission_item_reached_t wp_reached;
wp_reached.seq = seq;
mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp_reached);
_mavlink->send_message(&msg);
if (_verbose) { warnx("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); }
}
void
MavlinkMissionManager::eventloop()
{
hrt_abstime now = hrt_absolute_time();
bool updated = false;
orb_check(_mission_result_sub, &updated);
if (updated) {
mission_result_s mission_result;
orb_copy(ORB_ID(mission_result), _mission_result_sub, &mission_result);
_current_seq = mission_result.seq_current;
if (_verbose) { warnx("WPM: got mission result, new current_seq: %d", _current_seq); }
if (mission_result.reached) {
send_mission_item_reached((uint16_t)mission_result.seq_reached);
}
send_mission_current(_current_seq);
} else {
if (_slow_rate_limiter.check(now)) {
send_mission_current(_current_seq);
}
}
/* check for timed-out operations */
if (_state != MAVLINK_WPM_STATE_IDLE && hrt_elapsed_time(&_time_last_recv) > _action_timeout) {
_mavlink->send_statustext_critical("Operation timeout");
if (_verbose) { warnx("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _state); }
_state = MAVLINK_WPM_STATE_IDLE;
} else if (_state == MAVLINK_WPM_STATE_GETLIST && hrt_elapsed_time(&_time_last_sent) > _retry_timeout) {
/* try to request item again after timeout */
send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq);
} else if (_state == MAVLINK_WPM_STATE_SENDLIST && hrt_elapsed_time(&_time_last_sent) > _retry_timeout) {
if (_transfer_seq == 0) {
/* try to send items count again after timeout */
send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _transfer_count);
} else {
/* try to send item again after timeout */
send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq - 1);
}
}
}
void
MavlinkMissionManager::handle_message(const mavlink_message_t *msg)
{
switch (msg->msgid) {
case MAVLINK_MSG_ID_MISSION_ACK:
handle_mission_ack(msg);
break;
case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
handle_mission_set_current(msg);
break;
case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
handle_mission_request_list(msg);
break;
case MAVLINK_MSG_ID_MISSION_REQUEST:
handle_mission_request(msg);
break;
case MAVLINK_MSG_ID_MISSION_COUNT:
handle_mission_count(msg);
break;
case MAVLINK_MSG_ID_MISSION_ITEM:
handle_mission_item(msg);
break;
case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
handle_mission_clear_all(msg);
break;
default:
break;
}
}
void
MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg)
{
mavlink_mission_ack_t wpa;
mavlink_msg_mission_ack_decode(msg, &wpa);
if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) {
if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) {
if (_state == MAVLINK_WPM_STATE_SENDLIST) {
_time_last_recv = hrt_absolute_time();
if (_transfer_seq == _count) {
if (_verbose) { warnx("WPM: MISSION_ACK OK all items sent, switch to state IDLE"); }
} else {
_mavlink->send_statustext_critical("WPM: ERR: not all items sent -> IDLE");
if (_verbose) { warnx("WPM: MISSION_ACK ERROR: not all items sent, switch to state IDLE anyway"); }
}
_state = MAVLINK_WPM_STATE_IDLE;
}
} else {
_mavlink->send_statustext_critical("REJ. WP CMD: partner id mismatch");
if (_verbose) { warnx("WPM: MISSION_ACK ERROR: rejected, partner ID mismatch"); }
}
}
}
void
MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg)
{
mavlink_mission_set_current_t wpc;
mavlink_msg_mission_set_current_decode(msg, &wpc);
if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) {
if (_state == MAVLINK_WPM_STATE_IDLE) {
_time_last_recv = hrt_absolute_time();
if (wpc.seq < _count) {
if (update_active_mission(_dataman_id, _count, wpc.seq) == OK) {
if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d OK", wpc.seq); }
} else {
if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d ERROR", wpc.seq); }
_mavlink->send_statustext_critical("WPM: WP CURR CMD: Error setting ID");
}
} else {
if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d ERROR: not in list", wpc.seq); }
_mavlink->send_statustext_critical("WPM: WP CURR CMD: Not in list");
}
} else {
if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: busy"); }
_mavlink->send_statustext_critical("WPM: IGN WP CURR CMD: Busy");
}
}
}
void
MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg)
{
mavlink_mission_request_list_t wprl;
mavlink_msg_mission_request_list_decode(msg, &wprl);
if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) {
if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) {
_time_last_recv = hrt_absolute_time();
if (_count > 0) {
_state = MAVLINK_WPM_STATE_SENDLIST;
_transfer_seq = 0;
_transfer_count = _count;
_transfer_partner_sysid = msg->sysid;
_transfer_partner_compid = msg->compid;
if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK, %u mission items to send", _transfer_count); }
} else {
if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty"); }
_mavlink->send_statustext_info("WPM: mission is empty");
}
send_mission_count(msg->sysid, msg->compid, _count);
} else {
if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST ERROR: busy"); }
_mavlink->send_statustext_critical("IGN REQUEST LIST: Busy");
}
}
}
void
MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg)
{
mavlink_mission_request_t wpr;
mavlink_msg_mission_request_decode(msg, &wpr);
if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) {
if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) {
if (_state == MAVLINK_WPM_STATE_SENDLIST) {
_time_last_recv = hrt_absolute_time();
/* _transfer_seq contains sequence of expected request */
if (wpr.seq == _transfer_seq && _transfer_seq < _transfer_count) {
if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u", wpr.seq, msg->sysid); }
_transfer_seq++;
} else if (wpr.seq == _transfer_seq - 1) {
if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u (again)", wpr.seq, msg->sysid); }
} else {
if (_transfer_seq > 0 && _transfer_seq < _transfer_count) {
if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, must be %i or %i", wpr.seq, msg->sysid, _transfer_seq - 1, _transfer_seq); }
} else if (_transfer_seq <= 0) {
if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid, _transfer_seq); }
} else {
if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid, _transfer_seq - 1); }
}
_state = MAVLINK_WPM_STATE_IDLE;
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
_mavlink->send_statustext_critical("WPM: REJ. CMD: Req. WP was unexpected");
return;
}
/* double check bounds in case of items count changed */
if (wpr.seq < _count) {
send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, wpr.seq);
} else {
if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u out of bound [%u, %u]", (unsigned)wpr.seq, (unsigned)wpr.seq, (unsigned)_count - 1); }
_state = MAVLINK_WPM_STATE_IDLE;
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
_mavlink->send_statustext_critical("WPM: REJ. CMD: Req. WP was unexpected");
}
} else if (_state == MAVLINK_WPM_STATE_IDLE) {
if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: no transfer"); }
_mavlink->send_statustext_critical("IGN MISSION_ITEM_REQUEST: No active transfer");
} else {
if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: busy (state %d).", _state); }
_mavlink->send_statustext_critical("WPM: REJ. CMD: Busy");
}
} else {
_mavlink->send_statustext_critical("WPM: REJ. CMD: partner id mismatch");
if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: rejected, partner ID mismatch"); }
}
}
}
void
MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
{
mavlink_mission_count_t wpc;
mavlink_msg_mission_count_decode(msg, &wpc);
if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) {
if (_state == MAVLINK_WPM_STATE_IDLE) {
_time_last_recv = hrt_absolute_time();
if (wpc.count > _max_count) {
if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, _max_count); }
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_NO_SPACE);
return;
}
if (wpc.count == 0) {
if (_verbose) { warnx("WPM: MISSION_COUNT 0, clearing waypoints list and staying in state MAVLINK_WPM_STATE_IDLE"); }
/* alternate dataman ID anyway to let navigator know about changes */
update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0);
_mavlink->send_statustext_info("WPM: COUNT 0: CLEAR MISSION");
// TODO send ACK?
return;
}
if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u, changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); }
_state = MAVLINK_WPM_STATE_GETLIST;
_transfer_seq = 0;
_transfer_partner_sysid = msg->sysid;
_transfer_partner_compid = msg->compid;
_transfer_count = wpc.count;
_transfer_dataman_id = _dataman_id == 0 ? 1 : 0; // use inactive storage for transmission
_transfer_current_seq = -1;
} else if (_state == MAVLINK_WPM_STATE_GETLIST) {
_time_last_recv = hrt_absolute_time();
if (_transfer_seq == 0) {
/* looks like our MISSION_REQUEST was lost, try again */
if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u (again)", wpc.count, msg->sysid); }
_mavlink->send_statustext_info("WP CMD OK TRY AGAIN");
} else {
if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, already receiving seq %u", _transfer_seq); }
_mavlink->send_statustext_critical("WPM: REJ. CMD: Busy");
return;
}
} else {
if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, state %i", _state); }
_mavlink->send_statustext_critical("WPM: IGN MISSION_COUNT: Busy");
return;
}
send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq);
}
}
void
MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
{
mavlink_mission_item_t wp;
mavlink_msg_mission_item_decode(msg, &wp);
if (wp.target_system == mavlink_system.sysid && wp.target_component == _comp_id) {
if (_state == MAVLINK_WPM_STATE_GETLIST) {
_time_last_recv = hrt_absolute_time();
if (wp.seq != _transfer_seq) {
if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _transfer_seq); }
/* don't send request here, it will be performed in eventloop after timeout */
return;
}
} else if (_state == MAVLINK_WPM_STATE_IDLE) {
if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: no transfer"); }
_mavlink->send_statustext_critical("IGN MISSION_ITEM: No transfer");
return;
} else {
if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: busy, state %i", _state); }
_mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy");
return;
}
struct mission_item_s mission_item;
int ret = parse_mavlink_mission_item(&wp, &mission_item);
if (ret != OK) {
if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); }
_mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy");
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, ret);
_state = MAVLINK_WPM_STATE_IDLE;
return;
}
dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_transfer_dataman_id);
if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) {
if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id); }
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
_mavlink->send_statustext_critical("Unable to write on micro SD");
_state = MAVLINK_WPM_STATE_IDLE;
return;
}
/* waypoint marked as current */
if (wp.current) {
_transfer_current_seq = wp.seq;
}
if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); }
_transfer_seq = wp.seq + 1;
if (_transfer_seq == _transfer_count) {
/* got all new mission items successfully */
if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, current_seq=%u, changing state to MAVLINK_WPM_STATE_IDLE", _transfer_count, _transfer_current_seq); }
_mavlink->send_statustext_info("WPM: Transfer complete.");
_state = MAVLINK_WPM_STATE_IDLE;
if (update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq) == OK) {
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED);
} else {
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
}
} else {
/* request next item */
send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq);
}
}
}
void
MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
{
mavlink_mission_clear_all_t wpca;
mavlink_msg_mission_clear_all_decode(msg, &wpca);
if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) {
if (_state == MAVLINK_WPM_STATE_IDLE) {
/* don't touch mission items storage itself, but only items count in mission state */
_time_last_recv = hrt_absolute_time();
if (update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0) == OK) {
if (_verbose) { warnx("WPM: CLEAR_ALL OK"); }
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED);
} else {
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
}
} else {
_mavlink->send_statustext_critical("WPM: IGN CLEAR CMD: Busy");
if (_verbose) { warnx("WPM: CLEAR_ALL IGNORED: busy"); }
}
}
}
int
MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item)
{
/* only support global waypoints for now */
switch (mavlink_mission_item->frame) {
case MAV_FRAME_GLOBAL:
mission_item->lat = (double)mavlink_mission_item->x;
mission_item->lon = (double)mavlink_mission_item->y;
mission_item->altitude = mavlink_mission_item->z;
mission_item->altitude_is_relative = false;
break;
case MAV_FRAME_GLOBAL_RELATIVE_ALT:
mission_item->lat = (double)mavlink_mission_item->x;
mission_item->lon = (double)mavlink_mission_item->y;
mission_item->altitude = mavlink_mission_item->z;
mission_item->altitude_is_relative = true;
break;
case MAV_FRAME_LOCAL_NED:
case MAV_FRAME_LOCAL_ENU:
return MAV_MISSION_UNSUPPORTED_FRAME;
case MAV_FRAME_MISSION:
default:
return MAV_MISSION_ERROR;
}
switch (mavlink_mission_item->command) {
case MAV_CMD_NAV_TAKEOFF:
mission_item->pitch_min = mavlink_mission_item->param1;
break;
case MAV_CMD_DO_JUMP:
mission_item->do_jump_mission_index = mavlink_mission_item->param1;
mission_item->do_jump_current_count = 0;
mission_item->do_jump_repeat_count = mavlink_mission_item->param2;
break;
default:
mission_item->acceptance_radius = mavlink_mission_item->param2;
mission_item->time_inside = mavlink_mission_item->param1;
break;
}
mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F);
mission_item->loiter_radius = fabsf(mavlink_mission_item->param3);
mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
mission_item->autocontinue = mavlink_mission_item->autocontinue;
// mission_item->index = mavlink_mission_item->seq;
mission_item->origin = ORIGIN_MAVLINK;
/* reset DO_JUMP count */
mission_item->do_jump_current_count = 0;
return MAV_MISSION_ACCEPTED;
}
int
MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item)
{
if (mission_item->altitude_is_relative) {
mavlink_mission_item->frame = MAV_FRAME_GLOBAL;
} else {
mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
}
switch (mission_item->nav_cmd) {
case NAV_CMD_TAKEOFF:
mavlink_mission_item->param1 = mission_item->pitch_min;
break;
case NAV_CMD_DO_JUMP:
mavlink_mission_item->param1 = mission_item->do_jump_mission_index;
mavlink_mission_item->param2 = mission_item->do_jump_repeat_count;
break;
default:
mavlink_mission_item->param2 = mission_item->acceptance_radius;
mavlink_mission_item->param1 = mission_item->time_inside;
break;
}
mavlink_mission_item->x = (float)mission_item->lat;
mavlink_mission_item->y = (float)mission_item->lon;
mavlink_mission_item->z = mission_item->altitude;
mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction;
mavlink_mission_item->command = mission_item->nav_cmd;
mavlink_mission_item->autocontinue = mission_item->autocontinue;
// mavlink_mission_item->seq = mission_item->index;
return OK;
}

177
src/modules/mavlink/mavlink_mission.h Normal file
View File

@ -0,0 +1,177 @@
/****************************************************************************
*
* Copyright (c) 2012-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
* 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 mavlink_mission.h
* MAVLink mission manager interface definition.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#pragma once
#include "mavlink_bridge_header.h"
#include "mavlink_rate_limiter.h"
#include <uORB/uORB.h>
// FIXME XXX - TO BE MOVED TO XML
enum MAVLINK_WPM_STATES {
MAVLINK_WPM_STATE_IDLE = 0,
MAVLINK_WPM_STATE_SENDLIST,
MAVLINK_WPM_STATE_GETLIST,
MAVLINK_WPM_STATE_ENUM_END
};
enum MAVLINK_WPM_CODES {
MAVLINK_WPM_CODE_OK = 0,
MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED,
MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED,
MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS,
MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED,
MAVLINK_WPM_CODE_ENUM_END
};
#define MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication action timeout in useconds
#define MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds
class Mavlink;
class MavlinkMissionManager {
public:
MavlinkMissionManager(Mavlink *parent);
~MavlinkMissionManager();
void init_offboard_mission();
int update_active_mission(int dataman_id, unsigned count, int seq);
void send_message(mavlink_message_t *msg);
/**
* @brief Sends an waypoint ack message
*/
void send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type);
/**
* @brief Broadcasts the new target waypoint and directs the MAV to fly there
*
* This function broadcasts its new active waypoint sequence number and
* sends a message to the controller, advising it to fly to the coordinates
* of the waypoint with a given orientation
*
* @param seq The waypoint sequence number the MAV should fly to.
*/
void send_mission_current(uint16_t seq);
void send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count);
void send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq);
void send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq);
/**
* @brief emits a message that a waypoint reached
*
* This function broadcasts a message that a waypoint is reached.
*
* @param seq The waypoint sequence number the MAV has reached.
*/
void send_mission_item_reached(uint16_t seq);
void eventloop();
void handle_message(const mavlink_message_t *msg);
void handle_mission_ack(const mavlink_message_t *msg);
void handle_mission_set_current(const mavlink_message_t *msg);
void handle_mission_request_list(const mavlink_message_t *msg);
void handle_mission_request(const mavlink_message_t *msg);
void handle_mission_count(const mavlink_message_t *msg);
void handle_mission_item(const mavlink_message_t *msg);
void handle_mission_clear_all(const mavlink_message_t *msg);
/**
* Parse mavlink MISSION_ITEM message to get mission_item_s.
*/
int parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item);
/**
* Format mission_item_s as mavlink MISSION_ITEM message.
*/
int format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item);
void set_verbose(bool v) { _verbose = v; }
private:
Mavlink* _mavlink;
enum MAVLINK_WPM_STATES _state; ///< Current state
uint64_t _time_last_recv;
uint64_t _time_last_sent;
uint32_t _action_timeout;
uint32_t _retry_timeout;
unsigned _max_count; ///< Maximum number of mission items
int _dataman_id; ///< Dataman storage ID for active mission
unsigned _count; ///< Count of items in active mission
int _current_seq; ///< Current item sequence in active mission
int _transfer_dataman_id; ///< Dataman storage ID for current transmission
unsigned _transfer_count; ///< Items count in current transmission
unsigned _transfer_seq; ///< Item sequence in current transmission
unsigned _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized)
unsigned _transfer_partner_sysid; ///< Partner system ID for current transmission
unsigned _transfer_partner_compid; ///< Partner component ID for current transmission
int _offboard_mission_sub;
int _mission_result_sub;
orb_advert_t _offboard_mission_pub;
MavlinkRateLimiter _slow_rate_limiter;
bool _verbose;
mavlink_channel_t _channel;
uint8_t _comp_id;
};

13
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -79,7 +79,6 @@ __BEGIN_DECLS
#include "mavlink_bridge_header.h"
#include "mavlink_receiver.h"
#include "mavlink_main.h"
#include "util.h"
__END_DECLS
@ -952,16 +951,8 @@ MavlinkReceiver::receive_thread(void *arg)
/* handle generic messages and commands */
handle_message(&msg);
/* handle packet with waypoint component */
_mavlink->mavlink_wpm_message_handler(&msg);
/* handle packet with parameter component */
_mavlink->mavlink_pm_message_handler(_mavlink->get_channel(), &msg);
if (_mavlink->get_forwarding_on()) {
/* forward any messages to other mavlink instances */
Mavlink::forward_message(&msg, _mavlink);
}
/* handle packet with parent object */
_mavlink->handle_message(&msg);
}
}
}

2
src/modules/mavlink/mavlink_receiver.h
View File

@ -105,8 +105,6 @@ public:
static void *start_helper(void *context);
private:
perf_counter_t _loop_perf; /**< loop performance counter */
Mavlink *_mavlink;
void handle_message(mavlink_message_t *msg);

1
src/modules/mavlink/module.mk
View File

@ -39,6 +39,7 @@ MODULE_COMMAND = mavlink
SRCS += mavlink_main.cpp \
mavlink.c \
mavlink_receiver.cpp \
mavlink_mission.cpp \
mavlink_orb_subscription.cpp \
mavlink_messages.cpp \
mavlink_stream.cpp \

53
src/modules/mavlink/util.h
View File

@ -1,53 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-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
* 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 util.h
* Utility and helper functions and data.
*/
#pragma once
/** MAVLink communications channel */
extern uint8_t chan;
/** Shutdown marker */
extern volatile bool thread_should_exit;
/** Waypoint storage */
extern mavlink_wpm_storage *wpm;
/**
* Translate the custom state into standard mavlink modes and state.
*/
extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);

111
src/modules/mavlink/waypoints.h
View File

@ -1,111 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2009-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
* 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 waypoints.h
* MAVLink waypoint protocol definition (BSD-relicensed).
*
* @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*/
#ifndef WAYPOINTS_H_
#define WAYPOINTS_H_
/* This assumes you have the mavlink headers on your include path
or in the same folder as this source file */
#include <v1.0/mavlink_types.h>
#include "mavlink_bridge_header.h"
#include <stdbool.h>
#include <uORB/topics/mission.h>
enum MAVLINK_WPM_STATES {
MAVLINK_WPM_STATE_IDLE = 0,
MAVLINK_WPM_STATE_SENDLIST,
MAVLINK_WPM_STATE_SENDLIST_SENDWPS,
MAVLINK_WPM_STATE_GETLIST,
MAVLINK_WPM_STATE_GETLIST_GETWPS,
MAVLINK_WPM_STATE_GETLIST_GOTALL,
MAVLINK_WPM_STATE_ENUM_END
};
enum MAVLINK_WPM_CODES {
MAVLINK_WPM_CODE_OK = 0,
MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED,
MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED,
MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS,
MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED,
MAVLINK_WPM_CODE_ENUM_END
};
#define MAVLINK_WPM_MAX_WP_COUNT 255
#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds
#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint
#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000
struct mavlink_wpm_storage {
uint16_t size;
uint16_t max_size;
enum MAVLINK_WPM_STATES current_state;
int16_t current_wp_id; ///< Waypoint in current transmission
uint16_t current_count;
uint8_t current_partner_sysid;
uint8_t current_partner_compid;
uint64_t timestamp_lastaction;
uint64_t timestamp_last_send_setpoint;
uint32_t timeout;
int current_dataman_id;
};
typedef struct mavlink_wpm_storage mavlink_wpm_storage;
int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item);
int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item);
void mavlink_wpm_init(mavlink_wpm_storage *state);
void mavlink_waypoint_eventloop(uint64_t now);
void mavlink_wpm_message_handler(const mavlink_message_t *msg);
extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
float param2, float param3, float param4, float param5_lat_x,
float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command);
static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
#endif /* WAYPOINTS_H_ */

206
src/modules/navigator/mission.cpp
View File

@ -61,6 +61,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
MissionBlock(navigator, name),
_param_onboard_enabled(this, "ONBOARD_EN"),
_param_takeoff_alt(this, "TAKEOFF_ALT"),
_param_dist_1wp(this, "DIST_1WP"),
_onboard_mission({0}),
_offboard_mission({0}),
_current_onboard_mission_index(-1),
@ -69,13 +70,12 @@ Mission::Mission(Navigator *navigator, const char *name) :
_takeoff(false),
_mission_result_pub(-1),
_mission_result({0}),
_mission_type(MISSION_TYPE_NONE)
_mission_type(MISSION_TYPE_NONE),
_inited(false),
_dist_1wp_ok(false)
{
/* load initial params */
updateParams();
/* set initial mission items */
on_inactive();
}
Mission::~Mission()
@ -85,16 +85,25 @@ Mission::~Mission()
void
Mission::on_inactive()
{
/* check anyway if missions have changed so that feedback to groundstation is given */
bool onboard_updated;
orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
if (onboard_updated) {
update_onboard_mission();
}
if (_inited) {
/* check if missions have changed so that feedback to ground station is given */
bool onboard_updated = false;
orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
if (onboard_updated) {
update_onboard_mission();
}
bool offboard_updated;
orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
if (offboard_updated) {
bool offboard_updated = false;
orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
if (offboard_updated) {
update_offboard_mission();
}
} else {
/* read mission topics on initialization */
_inited = true;
update_onboard_mission();
update_offboard_mission();
}
@ -113,13 +122,13 @@ void
Mission::on_active()
{
/* check if anything has changed */
bool onboard_updated;
bool onboard_updated = false;
orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
if (onboard_updated) {
update_onboard_mission();
}
bool offboard_updated;
bool offboard_updated = false;
orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
if (offboard_updated) {
update_offboard_mission();
@ -148,9 +157,9 @@ Mission::update_onboard_mission()
{
if (orb_copy(ORB_ID(onboard_mission), _navigator->get_onboard_mission_sub(), &_onboard_mission) == OK) {
/* accept the current index set by the onboard mission if it is within bounds */
if (_onboard_mission.current_index >=0
&& _onboard_mission.current_index < (int)_onboard_mission.count) {
_current_onboard_mission_index = _onboard_mission.current_index;
if (_onboard_mission.current_seq >=0
&& _onboard_mission.current_seq < (int)_onboard_mission.count) {
_current_onboard_mission_index = _onboard_mission.current_seq;
} else {
/* if less WPs available, reset to first WP */
if (_current_onboard_mission_index >= (int)_onboard_mission.count) {
@ -164,7 +173,7 @@ Mission::update_onboard_mission()
} else {
_onboard_mission.count = 0;
_onboard_mission.current_index = 0;
_onboard_mission.current_seq = 0;
_current_onboard_mission_index = 0;
}
}
@ -173,14 +182,12 @@ void
Mission::update_offboard_mission()
{
if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) {
warnx("offboard mission updated: dataman_id=%d, count=%d, current_seq=%d", _offboard_mission.dataman_id, _offboard_mission.count, _offboard_mission.current_seq);
/* determine current index */
if (_offboard_mission.current_index >= 0
&& _offboard_mission.current_index < (int)_offboard_mission.count) {
_current_offboard_mission_index = _offboard_mission.current_index;
if (_offboard_mission.current_seq >= 0 && _offboard_mission.current_seq < (int)_offboard_mission.count) {
_current_offboard_mission_index = _offboard_mission.current_seq;
} else {
/* if less WPs available, reset to first WP */
/* if less items available, reset to first item */
if (_current_offboard_mission_index >= (int)_offboard_mission.count) {
_current_offboard_mission_index = 0;
@ -193,23 +200,16 @@ Mission::update_offboard_mission()
/* Check mission feasibility, for now do not handle the return value,
* however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
dm_item_t dm_current;
dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
if (_offboard_mission.dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current,
(size_t)_offboard_mission.count,
_navigator->get_geofence(),
_navigator->get_home_position()->alt);
missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing,
dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
_navigator->get_home_position()->alt);
} else {
warnx("offboard mission update failed");
_offboard_mission.count = 0;
_offboard_mission.current_index = 0;
_offboard_mission.current_seq = 0;
_current_offboard_mission_index = 0;
}
@ -240,6 +240,69 @@ Mission::advance_mission()
}
}
bool
Mission::check_dist_1wp()
{
if (_dist_1wp_ok) {
/* always return true after at least one successful check */
return true;
}
/* check if first waypoint is not too far from home */
if (_param_dist_1wp.get() > 0.0f) {
if (_navigator->get_vstatus()->condition_home_position_valid) {
struct mission_item_s mission_item;
/* find first waypoint (with lat/lon) item in datamanager */
for (unsigned i = 0; i < _offboard_mission.count; i++) {
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id), i,
&mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) {
/* check only items with valid lat/lon */
if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
mission_item.nav_cmd == NAV_CMD_PATHPLANNING) {
/* check distance from home to item */
float dist_to_1wp = get_distance_to_next_waypoint(
mission_item.lat, mission_item.lon,
_navigator->get_home_position()->lat, _navigator->get_home_position()->lon);
if (dist_to_1wp < _param_dist_1wp.get()) {
_dist_1wp_ok = true;
return true;
} else {
/* item is too far from home */
mavlink_log_critical(_navigator->get_mavlink_fd(), "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)_param_dist_1wp.get());
return false;
}
}
} else {
/* error reading, mission is invalid */
mavlink_log_info(_navigator->get_mavlink_fd(), "error reading offboard mission");
return false;
}
}
/* no waypoints found in mission, then we will not fly far away */
_dist_1wp_ok = true;
return true;
} else {
mavlink_log_info(_navigator->get_mavlink_fd(), "no home position");
return false;
}
} else {
return true;
}
}
void
Mission::set_mission_items()
{
@ -260,7 +323,7 @@ Mission::set_mission_items()
_mission_type = MISSION_TYPE_ONBOARD;
/* try setting offboard mission item */
} else if (read_mission_item(false, true, &_mission_item)) {
} else if (check_dist_1wp() && read_mission_item(false, true, &_mission_item)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_OFFBOARD) {
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: offboard mission running");
@ -401,12 +464,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
mission = &_offboard_mission;
if (_offboard_mission.dataman_id == 0) {
dm_item = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_item = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
}
if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
@ -472,12 +530,55 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
return false;
}
void
Mission::save_offboard_mission_state()
{
mission_s mission_state;
/* lock MISSION_STATE item */
dm_lock(DM_KEY_MISSION_STATE);
/* read current state */
int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));
if (read_res == sizeof(mission_s)) {
/* data read successfully, check dataman ID and items count */
if (mission_state.dataman_id == _offboard_mission.dataman_id && mission_state.count == _offboard_mission.count) {
/* navigator may modify only sequence, write modified state only if it changed */
if (mission_state.current_seq != _current_offboard_mission_index) {
if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) {
warnx("ERROR: can't save mission state");
mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: can't save mission state");
}
}
}
} else {
/* invalid data, this must not happen and indicates error in offboard_mission publisher */
mission_state.dataman_id = _offboard_mission.dataman_id;
mission_state.count = _offboard_mission.count;
mission_state.current_seq = _current_offboard_mission_index;
warnx("ERROR: invalid mission state");
mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: invalid mission state");
/* write modified state only if changed */
if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) {
warnx("ERROR: can't save mission state");
mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: can't save mission state");
}
}
/* unlock MISSION_STATE item */
dm_unlock(DM_KEY_MISSION_STATE);
}
void
Mission::report_mission_item_reached()
{
if (_mission_type == MISSION_TYPE_OFFBOARD) {
_mission_result.mission_reached = true;
_mission_result.mission_index_reached = _current_offboard_mission_index;
_mission_result.reached = true;
_mission_result.seq_reached = _current_offboard_mission_index;
}
publish_mission_result();
}
@ -485,14 +586,17 @@ Mission::report_mission_item_reached()
void
Mission::report_current_offboard_mission_item()
{
_mission_result.index_current_mission = _current_offboard_mission_index;
warnx("current offboard mission index: %d", _current_offboard_mission_index);
_mission_result.seq_current = _current_offboard_mission_index;
publish_mission_result();
save_offboard_mission_state();
}
void
Mission::report_mission_finished()
{
_mission_result.mission_finished = true;
_mission_result.finished = true;
publish_mission_result();
}
@ -509,6 +613,6 @@ Mission::publish_mission_result()
_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
}
/* reset reached bool */
_mission_result.mission_reached = false;
_mission_result.mission_finished = false;
_mission_result.reached = false;
_mission_result.finished = false;
}

17
src/modules/navigator/mission.h
View File

@ -91,6 +91,12 @@ private:
*/
void advance_mission();
/**
* Check distance to first waypoint (with lat/lon)
* @return true only if it's not too far from home (< MIS_DIST_1WP)
*/
bool check_dist_1wp();
/**
* Set new mission items
*/
@ -102,6 +108,11 @@ private:
*/
bool read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item);
/**
* Save current offboard mission state to dataman
*/
void save_offboard_mission_state();
/**
* Report that a mission item has been reached
*/
@ -122,8 +133,9 @@ private:
*/
void publish_mission_result();
control::BlockParamFloat _param_onboard_enabled;
control::BlockParamInt _param_onboard_enabled;
control::BlockParamFloat _param_takeoff_alt;
control::BlockParamFloat _param_dist_1wp;
struct mission_s _onboard_mission;
struct mission_s _offboard_mission;
@ -142,6 +154,9 @@ private:
MISSION_TYPE_OFFBOARD
} _mission_type;
bool _inited;
bool _dist_1wp_ok;
MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */
};

21
src/modules/navigator/mission_params.c
View File

@ -58,12 +58,27 @@
*/
PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f);
/**
* Enable onboard mission
* Enable persistent onboard mission storage
*
* When enabled, missions that have been uploaded by the GCS are stored
* and reloaded after reboot persistently.
*
* @min 0
* @max 1
* @group Mission
*/
PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 0);
PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1);
/**
* Maximal horizontal distance from home to first waypoint
*
* Failsafe check to prevent running mission stored from previous flight at a new takeoff location.
* Set a value of zero or less to disable. The mission will not be started if the current
* waypoint is more distant than MIS_DIS_1WP from the current position.
*
* @min 0
* @max 250
* @group Mission
*/
PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 175);

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)

10
src/modules/uORB/topics/mission.h
View File

@ -98,11 +98,15 @@ struct mission_item_s {
unsigned do_jump_current_count; /**< count how many times the jump has been done */
};
/**
* This topic used to notify navigator about mission changes, mission itself and new mission state
* must be stored in dataman before publication.
*/
struct mission_s
{
int dataman_id; /**< default -1, there are two offboard storage places in the dataman: 0 or 1 */
unsigned count; /**< count of the missions stored in the datamanager */
int current_index; /**< default -1, start at the one changed latest */
int dataman_id; /**< default 0, there are two offboard storage places in the dataman: 0 or 1 */
unsigned count; /**< count of the missions stored in the dataman */
int current_seq; /**< default -1, start at the one changed latest */
};
/**

8
src/modules/uORB/topics/mission_result.h
View File

@ -53,10 +53,10 @@
struct mission_result_s
{
bool mission_reached; /**< true if mission has been reached */
unsigned mission_index_reached; /**< index of the mission which has been reached */
unsigned index_current_mission; /**< index of the current mission */
bool mission_finished; /**< true if mission has been completed */
unsigned seq_reached; /**< Sequence of the mission item which has been reached */
unsigned seq_current; /**< Sequence of the current mission item */
bool reached; /**< true if mission has been reached */
bool finished; /**< true if mission has been completed */
};
/**

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;

3
src/systemcmds/tests/test_bson.c
View File

@ -40,6 +40,7 @@
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <systemlib/err.h>
#include <systemlib/bson/tinybson.h>
@ -123,7 +124,7 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node)
warnx("FAIL: decoder: double1 type %d, expected %d", node->type, BSON_DOUBLE);
return 1;
}
if (node->d != sample_double) {
if (fabs(node->d - sample_double) > 1e-12) {
warnx("FAIL: decoder: double1 value %f, expected %f", node->d, sample_double);
return 1;
}