forked from Archive/PX4-Autopilot
Merge branch 'master' of https://github.com/PX4/Firmware
This commit is contained in:
commit
25b036c6a5
4
Makefile
4
Makefile
|
@ -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
2
NuttX
|
@ -1 +1 @@
|
|||
Subproject commit 2a94cc8e5babb7d5661aedc09201239d39644332
|
||||
Subproject commit 7e1b97bcf10d8495169eec355988ca5890bfd5df
|
|
@ -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
|
||||
|
|
|
@ -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"
|
||||
|
|
|
@ -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
|
||||
#
|
||||
|
|
|
@ -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
|
|
@ -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:"
|
||||
|
|
|
@ -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')))
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -42,6 +42,7 @@ MODULES += modules/uORB
|
|||
LIBRARIES += lib/mathlib/CMSIS
|
||||
MODULES += lib/mathlib
|
||||
MODULES += lib/mathlib/math/filter
|
||||
MODULES += lib/conversion
|
||||
|
||||
#
|
||||
# Libraries
|
||||
|
|
|
@ -39,3 +39,5 @@ MODULE_COMMAND = ardrone_interface
|
|||
SRCS = ardrone_interface.c \
|
||||
ardrone_motor_control.c
|
||||
MODULE_STACKSIZE = 1200
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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.
|
||||
*
|
||||
|
|
|
@ -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:
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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()
|
||||
|
|
|
@ -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);
|
||||
};
|
||||
|
||||
|
|
|
@ -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 */
|
||||
|
|
|
@ -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
|
||||
*/
|
||||
|
|
|
@ -46,10 +46,18 @@
|
|||
|
||||
#define PX4FLOW_DEVICE_PATH "/dev/px4flow"
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* Optical flow in NED body frame in SI units.
|
||||
*
|
||||
* @see http://en.wikipedia.org/wiki/International_System_of_Units
|
||||
*
|
||||
* @warning If possible the usage of the raw flow and performing rotation-compensation
|
||||
* using the autopilot angular rate estimate is recommended.
|
||||
*/
|
||||
struct px4flow_report {
|
||||
|
||||
|
@ -57,14 +65,18 @@ struct px4flow_report {
|
|||
|
||||
int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */
|
||||
int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */
|
||||
float flow_comp_x_m; /**< speed over ground in meters, rotation-compensated */
|
||||
float flow_comp_y_m; /**< speed over ground in meters, rotation-compensated */
|
||||
float flow_comp_x_m; /**< speed over ground in meters per second, rotation-compensated */
|
||||
float flow_comp_y_m; /**< speed over ground in meters per second, rotation-compensated */
|
||||
float ground_distance_m; /**< Altitude / distance to ground in meters */
|
||||
uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */
|
||||
uint8_t sensor_id; /**< id of the sensor emitting the flow value */
|
||||
|
||||
};
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/*
|
||||
* ObjDev tag for px4flow data.
|
||||
*/
|
||||
|
|
|
@ -50,6 +50,11 @@ enum RANGE_FINDER_TYPE {
|
|||
RANGE_FINDER_TYPE_LASER = 0,
|
||||
};
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* range finder report structure. Reads from the device must be in multiples of this
|
||||
* structure.
|
||||
|
@ -64,6 +69,10 @@ struct range_finder_report {
|
|||
uint8_t valid; /**< 1 == within sensor range, 0 = outside sensor range */
|
||||
};
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/*
|
||||
* ObjDev tag for raw range finder data.
|
||||
*/
|
||||
|
|
|
@ -67,6 +67,11 @@
|
|||
*/
|
||||
#define RC_INPUT_RSSI_MAX 255
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
/**
|
||||
* Input signal type, value is a control position from zero to 100
|
||||
* percent.
|
||||
|
@ -141,6 +146,10 @@ struct rc_input_values {
|
|||
rc_input_t values[RC_INPUT_MAX_CHANNELS];
|
||||
};
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/*
|
||||
* ObjDev tag for R/C inputs.
|
||||
*/
|
||||
|
|
|
@ -41,3 +41,5 @@ SRCS = frsky_data.c \
|
|||
frsky_telemetry.c
|
||||
|
||||
MODULE_STACKSIZE = 1200
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
|
|
@ -127,7 +127,7 @@ private:
|
|||
/**
|
||||
* Try to configure the GPS, handle outgoing communication to the GPS
|
||||
*/
|
||||
void config();
|
||||
void config();
|
||||
|
||||
/**
|
||||
* Trampoline to the worker task
|
||||
|
@ -486,6 +486,8 @@ GPS::print_info()
|
|||
warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type,
|
||||
_report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0);
|
||||
warnx("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt);
|
||||
warnx("vel: %.2fm/s, %.2fm/s, %.2fm/s", (double)_report_gps_pos.vel_n_m_s,
|
||||
(double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s);
|
||||
warnx("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.epv);
|
||||
warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
|
||||
warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
|
||||
|
|
|
@ -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 {
|
||||
|
|
|
@ -40,3 +40,5 @@ MODULE_COMMAND = hott_sensors
|
|||
SRCS = hott_sensors.cpp \
|
||||
../messages.cpp \
|
||||
../comms.cpp
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
|
|
@ -40,3 +40,5 @@ MODULE_COMMAND = hott_telemetry
|
|||
SRCS = hott_telemetry.cpp \
|
||||
../messages.cpp \
|
||||
../comms.cpp
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
|
|
@ -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'");
|
||||
|
|
|
@ -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'");
|
||||
}
|
|
@ -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
|
|
@ -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'");
|
||||
|
|
|
@ -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 ");
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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'");
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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) :
|
||||
|
|
|
@ -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",
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -38,3 +38,5 @@
|
|||
MODULE_COMMAND = sf0x
|
||||
|
||||
SRCS = sf0x.cpp
|
||||
|
||||
MAXOPTIMIZATION = -Os
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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_ */
|
||||
|
|
|
@ -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"
|
||||
|
@ -286,15 +288,22 @@ int commander_main(int argc, char *argv[])
|
|||
exit(0);
|
||||
}
|
||||
|
||||
/* commands needing the app to run below */
|
||||
if (!thread_running) {
|
||||
warnx("\tcommander not started");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (thread_running) {
|
||||
warnx("\tcommander is running");
|
||||
print_status();
|
||||
|
||||
} else {
|
||||
warnx("\tcommander not started");
|
||||
}
|
||||
print_status();
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "check")) {
|
||||
int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0);
|
||||
int checkres = prearm_check(&status, mavlink_fd_local);
|
||||
close(mavlink_fd_local);
|
||||
warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
|
@ -303,7 +312,7 @@ int commander_main(int argc, char *argv[])
|
|||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "2")) {
|
||||
if (!strcmp(argv[1], "disarm")) {
|
||||
arm_disarm(false, mavlink_fd, "command line");
|
||||
exit(0);
|
||||
}
|
||||
|
@ -324,6 +333,7 @@ void usage(const char *reason)
|
|||
|
||||
void print_status()
|
||||
{
|
||||
warnx("type: %s", (status.is_rotary_wing) ? "ROTARY" : "PLANE");
|
||||
warnx("usb powered: %s", (on_usb_power) ? "yes" : "no");
|
||||
|
||||
/* read all relevant states */
|
||||
|
@ -725,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;
|
||||
|
@ -742,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");
|
||||
|
@ -1486,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) {
|
||||
|
@ -1590,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;
|
||||
|
||||
|
|
|
@ -70,8 +70,6 @@
|
|||
#endif
|
||||
static const int ERROR = -1;
|
||||
|
||||
static int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd);
|
||||
|
||||
// This array defines the arming state transitions. The rows are the new state, and the columns
|
||||
// are the current state. Using new state and current state you can index into the array which
|
||||
// will be true for a valid transition or false for a invalid transition. In some cases even
|
||||
|
@ -622,12 +620,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
|
|||
int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
|
||||
{
|
||||
int ret;
|
||||
bool failed = false;
|
||||
|
||||
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL SENSOR MISSING");
|
||||
ret = fd;
|
||||
failed = true;
|
||||
goto system_eval;
|
||||
}
|
||||
|
||||
|
@ -635,6 +634,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
|
|||
|
||||
if (ret != OK) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION");
|
||||
failed = true;
|
||||
goto system_eval;
|
||||
}
|
||||
|
||||
|
@ -644,33 +644,31 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
|
|||
|
||||
if (ret == sizeof(acc)) {
|
||||
/* evaluate values */
|
||||
float accel_scale = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
|
||||
float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
|
||||
|
||||
if (accel_scale < 9.78f || accel_scale > 9.83f) {
|
||||
mavlink_log_info(mavlink_fd, "#audio: Accelerometer calibration recommended.");
|
||||
}
|
||||
|
||||
if (accel_scale > 30.0f /* m/s^2 */) {
|
||||
if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE");
|
||||
mavlink_log_info(mavlink_fd, "#audio: hold still while arming");
|
||||
/* this is frickin' fatal */
|
||||
ret = ERROR;
|
||||
failed = true;
|
||||
goto system_eval;
|
||||
} else {
|
||||
ret = OK;
|
||||
}
|
||||
} else {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ");
|
||||
/* this is frickin' fatal */
|
||||
ret = ERROR;
|
||||
failed = true;
|
||||
goto system_eval;
|
||||
}
|
||||
|
||||
if (!status->is_rotary_wing) {
|
||||
|
||||
/* accel done, close it */
|
||||
close(fd);
|
||||
fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0) {
|
||||
if (fd <= 0) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING");
|
||||
ret = fd;
|
||||
failed = true;
|
||||
goto system_eval;
|
||||
}
|
||||
|
||||
|
@ -679,20 +677,19 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
|
|||
ret = read(fd, &diff_pres, sizeof(diff_pres));
|
||||
|
||||
if (ret == sizeof(diff_pres)) {
|
||||
if (fabsf(diff_pres.differential_pressure_filtered_pa > 5.0f)) {
|
||||
if (fabsf(diff_pres.differential_pressure_filtered_pa > 6.0f)) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING");
|
||||
// XXX do not make this fatal yet
|
||||
ret = OK;
|
||||
}
|
||||
} else {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ");
|
||||
/* this is frickin' fatal */
|
||||
ret = ERROR;
|
||||
failed = true;
|
||||
goto system_eval;
|
||||
}
|
||||
}
|
||||
|
||||
system_eval:
|
||||
close(fd);
|
||||
return ret;
|
||||
return (failed);
|
||||
}
|
||||
|
|
|
@ -67,4 +67,6 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
|
|||
|
||||
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished);
|
||||
|
||||
int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd);
|
||||
|
||||
#endif /* STATE_MACHINE_HELPER_H_ */
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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(
|
||||
|
|
|
@ -0,0 +1 @@
|
|||
Subproject commit 8b65d755b8c4834f90a323172c25d91c45068e21
|
|
@ -39,3 +39,5 @@ MODULE_COMMAND = fw_att_control
|
|||
|
||||
SRCS = fw_att_control_main.cpp \
|
||||
fw_att_control_params.c
|
||||
|
||||
MODULE_STACKSIZE = 1200
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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[]);
|
||||
|
||||
};
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
}
|
|
@ -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;
|
||||
};
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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 \
|
||||
|
|
|
@ -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);
|
|
@ -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_ */
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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 */
|
||||
};
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -133,7 +133,7 @@
|
|||
#endif
|
||||
|
||||
#define BATT_V_LOWPASS 0.001f
|
||||
#define BATT_V_IGNORE_THRESHOLD 3.5f
|
||||
#define BATT_V_IGNORE_THRESHOLD 4.8f
|
||||
|
||||
/**
|
||||
* HACK - true temperature is much less than indicated temperature in baro,
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -44,6 +44,6 @@
|
|||
|
||||
__BEGIN_DECLS
|
||||
|
||||
__EXPORT int get_board_serial(char *serialid);
|
||||
__EXPORT int get_board_serial(uint8_t *serialid);
|
||||
|
||||
__END_DECLS
|
||||
|
|
|
@ -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)
|
||||
|
||||
|
|
|
@ -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 */
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
@ -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 */
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue