mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
Merge remote-tracking branch 'upstream/master' into ros_messagelayer_merge
Conflicts: src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp src/modules/uORB/topics/vehicle_attitude.h
This commit is contained in:
Thomas Gubler
commit
dcdde8ea88
2
NuttX
2
NuttX
@ -1 +1 @@
|
||||
Subproject commit dbcccb2455d759b789d549d25e1fbf489b2d3c83
|
||||
Subproject commit 574bac488f384ddaa344378e25653c27124a2b69
|
||||
@ -1,6 +1,6 @@
|
||||
## PX4 Flight Control Stack and Middleware ##
|
||||
|
||||
[](https://travis-ci.org/PX4/Firmware)
|
||||
[](https://travis-ci.org/PX4/Firmware) [](https://scan.coverity.com/projects/3966?tab=overview)
|
||||
|
||||
[](https://gitter.im/PX4/Firmware?utm_source=badge&utm_medium=badge&utm_campaign=pr-badge&utm_content=badge)
|
||||
|
||||
@ -21,7 +21,8 @@ Please refer to the [user documentation](https://pixhawk.org/users/start) for fl
|
||||
### Developers ###
|
||||
|
||||
Contributing guide:
|
||||
http://px4.io/dev/contributing
|
||||
* [CONTRIBUTING.md](https://github.com/PX4/Firmware/blob/master/CONTRIBUTING.md)
|
||||
* [PX4 Contribution Guide](http://px4.io/dev/contributing)
|
||||
|
||||
Developer guide:
|
||||
http://px4.io/dev/
|
||||
|
||||
@ -13,3 +13,12 @@ ekf_att_pos_estimator start
|
||||
#
|
||||
fw_att_control start
|
||||
fw_pos_control_l1 start
|
||||
|
||||
#
|
||||
# Start Land Detector
|
||||
#
|
||||
land_detector start fixedwing
|
||||
|
||||
#
|
||||
# Misc apps
|
||||
#
|
||||
|
||||
@ -8,7 +8,7 @@ then
|
||||
if ver hwcmp PX4FMU_V1
|
||||
then
|
||||
echo "Start sdlog2 at 50Hz"
|
||||
sdlog2 start -r 50 -a -b 4 -t
|
||||
sdlog2 start -r 40 -a -b 3 -t
|
||||
else
|
||||
echo "Start sdlog2 at 200Hz"
|
||||
sdlog2 start -r 200 -a -b 22 -t
|
||||
|
||||
@ -15,3 +15,8 @@ else
|
||||
mc_att_control_m start
|
||||
fi
|
||||
mc_pos_control start
|
||||
|
||||
#
|
||||
# Start Land Detector
|
||||
#
|
||||
land_detector start multicopter
|
||||
|
||||
@ -266,18 +266,19 @@ class uploader(object):
|
||||
self.__getSync()
|
||||
return value
|
||||
|
||||
def __drawProgressBar(self, progress, maxVal):
|
||||
def __drawProgressBar(self, label, progress, maxVal):
|
||||
if maxVal < progress:
|
||||
progress = maxVal
|
||||
|
||||
percent = (float(progress) / float(maxVal)) * 100.0
|
||||
|
||||
sys.stdout.write("\rprogress:[%-20s] %.2f%%" % ('='*int(percent/5.0), percent))
|
||||
sys.stdout.write("\r%s: [%-20s] %.1f%%" % (label, '='*int(percent/5.0), percent))
|
||||
sys.stdout.flush()
|
||||
|
||||
|
||||
# send the CHIP_ERASE command and wait for the bootloader to become ready
|
||||
def __erase(self):
|
||||
def __erase(self, label):
|
||||
print("\n", end='')
|
||||
self.__send(uploader.CHIP_ERASE
|
||||
+ uploader.EOC)
|
||||
|
||||
@ -288,15 +289,14 @@ class uploader(object):
|
||||
#Draw progress bar (erase usually takes about 9 seconds to complete)
|
||||
estimatedTimeRemaining = deadline-time.time()
|
||||
if estimatedTimeRemaining >= 9.0:
|
||||
self.__drawProgressBar(20.0-estimatedTimeRemaining, 9.0)
|
||||
self.__drawProgressBar(label, 20.0-estimatedTimeRemaining, 9.0)
|
||||
else:
|
||||
self.__drawProgressBar(10.0, 10.0)
|
||||
self.__drawProgressBar(label, 10.0, 10.0)
|
||||
sys.stdout.write(" (timeout: %d seconds) " % int(deadline-time.time()) )
|
||||
sys.stdout.flush()
|
||||
|
||||
if self.__trySync():
|
||||
self.__drawProgressBar(10.0, 10.0)
|
||||
sys.stdout.write("\nerase complete!\n")
|
||||
self.__drawProgressBar(label, 10.0, 10.0)
|
||||
return;
|
||||
|
||||
raise RuntimeError("timed out waiting for erase")
|
||||
@ -350,7 +350,8 @@ class uploader(object):
|
||||
return [seq[i:i+length] for i in range(0, len(seq), length)]
|
||||
|
||||
# upload code
|
||||
def __program(self, fw):
|
||||
def __program(self, label, fw):
|
||||
print("\n", end='')
|
||||
code = fw.image
|
||||
groups = self.__split_len(code, uploader.PROG_MULTI_MAX)
|
||||
|
||||
@ -361,31 +362,40 @@ class uploader(object):
|
||||
#Print upload progress (throttled, so it does not delay upload progress)
|
||||
uploadProgress += 1
|
||||
if uploadProgress % 256 == 0:
|
||||
self.__drawProgressBar(uploadProgress, len(groups))
|
||||
self.__drawProgressBar(100, 100)
|
||||
print("\nprogram complete!")
|
||||
self.__drawProgressBar(label, uploadProgress, len(groups))
|
||||
self.__drawProgressBar(label, 100, 100)
|
||||
|
||||
# verify code
|
||||
def __verify_v2(self, fw):
|
||||
def __verify_v2(self, label, fw):
|
||||
print("\n", end='')
|
||||
self.__send(uploader.CHIP_VERIFY
|
||||
+ uploader.EOC)
|
||||
self.__getSync()
|
||||
code = fw.image
|
||||
groups = self.__split_len(code, uploader.READ_MULTI_MAX)
|
||||
verifyProgress = 0
|
||||
for bytes in groups:
|
||||
verifyProgress += 1
|
||||
if verifyProgress % 256 == 0:
|
||||
self.__drawProgressBar(label, verifyProgress, len(groups))
|
||||
if (not self.__verify_multi(bytes)):
|
||||
raise RuntimeError("Verification failed")
|
||||
self.__drawProgressBar(label, 100, 100)
|
||||
|
||||
def __verify_v3(self, fw):
|
||||
expect_crc = fw.crc(self.fw_maxsize)
|
||||
def __verify_v3(self, label, fw):
|
||||
print("\n", end='')
|
||||
self.__drawProgressBar(label, 1, 100)
|
||||
expect_crc = fw.crc(self.fw_maxsize)
|
||||
self.__send(uploader.GET_CRC
|
||||
+ uploader.EOC)
|
||||
report_crc = self.__recv_int()
|
||||
self.__getSync()
|
||||
verifyProgress = 0
|
||||
if report_crc != expect_crc:
|
||||
print("Expected 0x%x" % expect_crc)
|
||||
print("Got 0x%x" % report_crc)
|
||||
raise RuntimeError("Program CRC failed")
|
||||
self.__drawProgressBar(label, 100, 100)
|
||||
|
||||
# get basic data about the board
|
||||
def identify(self):
|
||||
@ -439,19 +449,16 @@ class uploader(object):
|
||||
except Exception:
|
||||
# ignore bad character encodings
|
||||
pass
|
||||
print("erase...")
|
||||
self.__erase()
|
||||
|
||||
self.__erase("Erase ")
|
||||
self.__program("Program", fw)
|
||||
|
||||
print("program...")
|
||||
self.__program(fw)
|
||||
|
||||
print("verify...")
|
||||
if self.bl_rev == 2:
|
||||
self.__verify_v2(fw)
|
||||
self.__verify_v2("Verify ", fw)
|
||||
else:
|
||||
self.__verify_v3(fw)
|
||||
self.__verify_v3("Verify ", fw)
|
||||
|
||||
print("done, rebooting.")
|
||||
print("\nRebooting.\n")
|
||||
self.__reboot()
|
||||
self.port.close()
|
||||
|
||||
|
||||
@ -71,6 +71,7 @@ MODULES += modules/navigator
|
||||
MODULES += modules/mavlink
|
||||
MODULES += modules/gpio_led
|
||||
MODULES += modules/uavcan
|
||||
MODULES += modules/land_detector
|
||||
|
||||
#
|
||||
# Estimation modules (EKF/ SO3 / other filters)
|
||||
|
||||
@ -50,7 +50,7 @@ OBJDUMP = $(CROSSDEV)objdump
|
||||
|
||||
# Check if the right version of the toolchain is available
|
||||
#
|
||||
CROSSDEV_VER_SUPPORTED = 4.7.4 4.7.5 4.7.6 4.8.4
|
||||
CROSSDEV_VER_SUPPORTED = 4.7.4 4.7.5 4.7.6 4.8.4 4.9.3
|
||||
CROSSDEV_VER_FOUND = $(shell $(CC) -dumpversion)
|
||||
|
||||
ifeq (,$(findstring $(CROSSDEV_VER_FOUND), $(CROSSDEV_VER_SUPPORTED)))
|
||||
|
||||
@ -16,20 +16,3 @@ float32[4] q # Quaternion (NED)
|
||||
float32[3] g_comp # Compensated gravity vector
|
||||
bool R_valid # Rotation matrix valid
|
||||
bool q_valid # Quaternion valid
|
||||
|
||||
# secondary attitude for VTOL
|
||||
float32 roll_sec # Roll angle (rad, Tait-Bryan, NED)
|
||||
float32 pitch_sec # Pitch angle (rad, Tait-Bryan, NED)
|
||||
float32 yaw_sec # Yaw angle (rad, Tait-Bryan, NED)
|
||||
float32 rollspeed_sec # Roll angular speed (rad/s, Tait-Bryan, NED)
|
||||
float32 pitchspeed_sec # Pitch angular speed (rad/s, Tait-Bryan, NED)
|
||||
float32 yawspeed_sec # Yaw angular speed (rad/s, Tait-Bryan, NED)
|
||||
float32 rollacc_sec # Roll angular accelration (rad/s, Tait-Bryan, NED)
|
||||
float32 pitchacc_sec # Pitch angular acceleration (rad/s, Tait-Bryan, NED)
|
||||
float32 yawacc_sec # Yaw angular acceleration (rad/s, Tait-Bryan, NED)
|
||||
float32[3] rate_offsets_sec # Offsets of the body angular rates from zero
|
||||
float32[9] R_sec # Rotation matrix, body to world, (Tait-Bryan, NED)
|
||||
float32[4] q_sec # Quaternion (NED)
|
||||
float32[3] g_comp_sec # Compensated gravity vector
|
||||
bool R_valid_sec # Rotation matrix valid
|
||||
bool q_valid_sec # Quaternion valid
|
||||
|
||||
@ -417,7 +417,7 @@ CONFIG_PREALLOC_TIMERS=50
|
||||
# Stack and heap information
|
||||
#
|
||||
CONFIG_IDLETHREAD_STACKSIZE=3500
|
||||
CONFIG_USERMAIN_STACKSIZE=3000
|
||||
CONFIG_USERMAIN_STACKSIZE=2600
|
||||
CONFIG_PTHREAD_STACK_MIN=512
|
||||
CONFIG_PTHREAD_STACK_DEFAULT=2048
|
||||
|
||||
@ -719,11 +719,11 @@ CONFIG_SCHED_WORKQUEUE=y
|
||||
CONFIG_SCHED_HPWORK=y
|
||||
CONFIG_SCHED_WORKPRIORITY=192
|
||||
CONFIG_SCHED_WORKPERIOD=5000
|
||||
CONFIG_SCHED_WORKSTACKSIZE=2048
|
||||
CONFIG_SCHED_WORKSTACKSIZE=1800
|
||||
CONFIG_SCHED_LPWORK=y
|
||||
CONFIG_SCHED_LPWORKPRIORITY=50
|
||||
CONFIG_SCHED_LPWORKPERIOD=50000
|
||||
CONFIG_SCHED_LPWORKSTACKSIZE=2048
|
||||
CONFIG_SCHED_LPWORKSTACKSIZE=1800
|
||||
# CONFIG_LIB_KBDCODEC is not set
|
||||
# CONFIG_LIB_SLCDCODEC is not set
|
||||
|
||||
|
||||
@ -451,7 +451,7 @@ CONFIG_PREALLOC_TIMERS=50
|
||||
# Stack and heap information
|
||||
#
|
||||
CONFIG_IDLETHREAD_STACKSIZE=3500
|
||||
CONFIG_USERMAIN_STACKSIZE=3000
|
||||
CONFIG_USERMAIN_STACKSIZE=2600
|
||||
CONFIG_PTHREAD_STACK_MIN=512
|
||||
CONFIG_PTHREAD_STACK_DEFAULT=2048
|
||||
|
||||
@ -797,11 +797,11 @@ CONFIG_SCHED_WORKQUEUE=y
|
||||
CONFIG_SCHED_HPWORK=y
|
||||
CONFIG_SCHED_WORKPRIORITY=192
|
||||
CONFIG_SCHED_WORKPERIOD=5000
|
||||
CONFIG_SCHED_WORKSTACKSIZE=2000
|
||||
CONFIG_SCHED_WORKSTACKSIZE=1800
|
||||
CONFIG_SCHED_LPWORK=y
|
||||
CONFIG_SCHED_LPWORKPRIORITY=50
|
||||
CONFIG_SCHED_LPWORKPERIOD=50000
|
||||
CONFIG_SCHED_LPWORKSTACKSIZE=2000
|
||||
CONFIG_SCHED_LPWORKSTACKSIZE=1800
|
||||
# CONFIG_LIB_KBDCODEC is not set
|
||||
# CONFIG_LIB_SLCDCODEC is not set
|
||||
|
||||
|
||||
@ -41,9 +41,11 @@
|
||||
#include <stdint.h>
|
||||
#include <sys/ioctl.h>
|
||||
|
||||
#include "drv_device.h"
|
||||
#include "drv_sensor.h"
|
||||
#include "drv_orb_dev.h"
|
||||
|
||||
|
||||
#define MAG_DEVICE_PATH "/dev/mag"
|
||||
|
||||
/**
|
||||
|
||||
@ -88,8 +88,6 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud)
|
||||
|
||||
case 115200: speed = B115200; break;
|
||||
|
||||
warnx("try baudrate: %d\n", speed);
|
||||
|
||||
default:
|
||||
warnx("ERR: baudrate: %d\n", baud);
|
||||
return -EINVAL;
|
||||
|
||||
@ -66,6 +66,7 @@
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/device/ringbuffer.h>
|
||||
#include <drivers/drv_device.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/subsystem_info.h>
|
||||
@ -725,6 +726,9 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
debug("MAGIOCGEXTERNAL in main driver");
|
||||
return _interface->ioctl(cmd, dummy);
|
||||
|
||||
case DEVIOCGDEVICEID:
|
||||
return _interface->ioctl(cmd, dummy);
|
||||
|
||||
default:
|
||||
/* give it to the superclass */
|
||||
return CDev::ioctl(filp, cmd, arg);
|
||||
@ -1305,9 +1309,9 @@ struct hmc5883_bus_option {
|
||||
uint8_t busnum;
|
||||
HMC5883 *dev;
|
||||
} bus_options[] = {
|
||||
{ HMC5883_BUS_I2C_INTERNAL, "/dev/hmc5883_int", &HMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL },
|
||||
{ HMC5883_BUS_I2C_EXTERNAL, "/dev/hmc5883_ext", &HMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL },
|
||||
#ifdef PX4_I2C_BUS_ONBOARD
|
||||
{ HMC5883_BUS_I2C_EXTERNAL, "/dev/hmc5883_ext", &HMC5883_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL },
|
||||
{ HMC5883_BUS_I2C_INTERNAL, "/dev/hmc5883_int", &HMC5883_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL },
|
||||
#endif
|
||||
#ifdef PX4_SPIDEV_HMC
|
||||
{ HMC5883_BUS_SPI, "/dev/hmc5883_spi", &HMC5883_SPI_interface, PX4_SPI_BUS_SENSORS, NULL },
|
||||
|
||||
@ -53,6 +53,7 @@
|
||||
|
||||
#include <drivers/device/i2c.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <drivers/drv_device.h>
|
||||
|
||||
#include "hmc5883.h"
|
||||
|
||||
@ -90,6 +91,7 @@ HMC5883_I2C_interface(int bus)
|
||||
HMC5883_I2C::HMC5883_I2C(int bus) :
|
||||
I2C("HMC5883_I2C", nullptr, bus, HMC5883L_ADDRESS, 400000)
|
||||
{
|
||||
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
|
||||
}
|
||||
|
||||
HMC5883_I2C::~HMC5883_I2C()
|
||||
@ -117,6 +119,9 @@ HMC5883_I2C::ioctl(unsigned operation, unsigned &arg)
|
||||
return 0;
|
||||
}
|
||||
|
||||
case DEVIOCGDEVICEID:
|
||||
return CDev::ioctl(nullptr, operation, arg);
|
||||
|
||||
default:
|
||||
ret = -EINVAL;
|
||||
}
|
||||
|
||||
@ -53,6 +53,7 @@
|
||||
|
||||
#include <drivers/device/spi.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <drivers/drv_device.h>
|
||||
|
||||
#include "hmc5883.h"
|
||||
#include <board_config.h>
|
||||
@ -92,6 +93,7 @@ HMC5883_SPI_interface(int bus)
|
||||
HMC5883_SPI::HMC5883_SPI(int bus, spi_dev_e device) :
|
||||
SPI("HMC5883_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */)
|
||||
{
|
||||
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
|
||||
}
|
||||
|
||||
HMC5883_SPI::~HMC5883_SPI()
|
||||
@ -146,6 +148,9 @@ HMC5883_SPI::ioctl(unsigned operation, unsigned &arg)
|
||||
return 0;
|
||||
}
|
||||
|
||||
case DEVIOCGDEVICEID:
|
||||
return CDev::ioctl(nullptr, operation, arg);
|
||||
|
||||
default:
|
||||
{
|
||||
ret = -EINVAL;
|
||||
|
||||
@ -37,12 +37,12 @@
|
||||
* Shared defines for the ms5611 driver.
|
||||
*/
|
||||
|
||||
#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */
|
||||
#define ADDR_CMD_CONVERT_D1 0x48 /* write to this address to start temperature conversion */
|
||||
#define ADDR_CMD_CONVERT_D2 0x58 /* write to this address to start pressure conversion */
|
||||
#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */
|
||||
#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */
|
||||
#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */
|
||||
#define ADDR_RESET_CMD 0x1E /* write to this address to reset chip */
|
||||
#define ADDR_CMD_CONVERT_D1 0x48 /* write to this address to start pressure conversion */
|
||||
#define ADDR_CMD_CONVERT_D2 0x58 /* write to this address to start temperature conversion */
|
||||
#define ADDR_DATA 0x00 /* address of 3 bytes / 32bit pressure data */
|
||||
#define ADDR_PROM_SETUP 0xA0 /* address of 8x 2 bytes factory and calibration data */
|
||||
#define ADDR_PROM_C1 0xA2 /* address of 6x 2 bytes calibration data */
|
||||
|
||||
/* interface ioctls */
|
||||
#define IOCTL_RESET 2
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
|
||||
* Copyright (C) 2012-2015 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
|
||||
@ -1637,12 +1637,15 @@ sensor_reset(int ms)
|
||||
|
||||
fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
|
||||
|
||||
if (fd < 0)
|
||||
if (fd < 0) {
|
||||
errx(1, "open fail");
|
||||
}
|
||||
|
||||
if (ioctl(fd, GPIO_SENSOR_RAIL_RESET, ms) < 0)
|
||||
err(1, "servo arm failed");
|
||||
if (ioctl(fd, GPIO_SENSOR_RAIL_RESET, ms) < 0) {
|
||||
warnx("sensor rail reset failed");
|
||||
}
|
||||
|
||||
close(fd);
|
||||
}
|
||||
|
||||
void
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2012-2015 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
|
||||
@ -834,7 +834,7 @@ PX4IO::init()
|
||||
_task = task_spawn_cmd("px4io",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_ACTUATOR_OUTPUTS,
|
||||
2000,
|
||||
1800,
|
||||
(main_t)&PX4IO::task_main_trampoline,
|
||||
nullptr);
|
||||
|
||||
@ -1102,7 +1102,7 @@ PX4IO::io_set_control_state(unsigned group)
|
||||
uint16_t regs[_max_actuators];
|
||||
|
||||
/* get controls */
|
||||
bool changed;
|
||||
bool changed = false;
|
||||
|
||||
switch (group) {
|
||||
case 0:
|
||||
@ -1144,11 +1144,13 @@ PX4IO::io_set_control_state(unsigned group)
|
||||
break;
|
||||
}
|
||||
|
||||
if (!changed)
|
||||
if (!changed) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < _max_controls; i++)
|
||||
for (unsigned i = 0; i < _max_controls; i++) {
|
||||
regs[i] = FLOAT_TO_REG(controls.control[i]);
|
||||
}
|
||||
|
||||
/* copy values to registers in IO */
|
||||
return io_reg_set(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT, regs, _max_controls);
|
||||
@ -1773,12 +1775,12 @@ PX4IO::print_debug()
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
|
||||
int io_fd = -1;
|
||||
|
||||
if (io_fd < 0) {
|
||||
if (io_fd <= 0) {
|
||||
io_fd = ::open("/dev/ttyS0", O_RDONLY | O_NONBLOCK | O_NOCTTY);
|
||||
}
|
||||
|
||||
/* read IO's output */
|
||||
if (io_fd > 0) {
|
||||
if (io_fd >= 0) {
|
||||
pollfd fds[1];
|
||||
fds[0].fd = io_fd;
|
||||
fds[0].events = POLLIN;
|
||||
@ -2278,6 +2280,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
|
||||
|
||||
case PWM_SERVO_GET_DISABLE_LOCKDOWN:
|
||||
*(unsigned *)arg = _lockdown_override;
|
||||
break;
|
||||
|
||||
case PWM_SERVO_SET_FORCE_SAFETY_OFF:
|
||||
/* force safety swith off */
|
||||
@ -3003,11 +3006,14 @@ monitor(void)
|
||||
|
||||
fds[0].fd = 0;
|
||||
fds[0].events = POLLIN;
|
||||
poll(fds, 1, 2000);
|
||||
if (poll(fds, 1, 2000) < 0) {
|
||||
errx(1, "poll fail");
|
||||
}
|
||||
|
||||
if (fds[0].revents == POLLIN) {
|
||||
int c;
|
||||
read(0, &c, 1);
|
||||
/* control logic is to cancel with any key */
|
||||
char c;
|
||||
(void)read(0, &c, 1);
|
||||
|
||||
if (cancels-- == 0) {
|
||||
printf("\033[2J\033[H"); /* move cursor home and clear screen */
|
||||
@ -3069,12 +3075,13 @@ lockdown(int argc, char *argv[])
|
||||
|
||||
if (ret > 0) {
|
||||
|
||||
read(0, &c, 1);
|
||||
if (read(0, &c, 1) > 0) {
|
||||
|
||||
if (c != 'y') {
|
||||
exit(0);
|
||||
} else if (c == 'y') {
|
||||
break;
|
||||
if (c != 'y') {
|
||||
exit(0);
|
||||
} else if (c == 'y') {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -3237,7 +3244,13 @@ px4io_main(int argc, char *argv[])
|
||||
if (!strcmp(argv[1], "limit")) {
|
||||
|
||||
if ((argc > 2)) {
|
||||
g_dev->set_update_rate(atoi(argv[2]));
|
||||
int limitrate = atoi(argv[2]);
|
||||
|
||||
if (limitrate > 0) {
|
||||
g_dev->set_update_rate(limitrate);
|
||||
} else {
|
||||
errx(1, "invalid rate: %d", limitrate);
|
||||
}
|
||||
|
||||
} else {
|
||||
errx(1, "missing argument (50 - 500 Hz)");
|
||||
|
||||
@ -66,6 +66,7 @@ float LowPassFilter2p::apply(float sample)
|
||||
// no filtering
|
||||
return sample;
|
||||
}
|
||||
|
||||
// do the filtering
|
||||
float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
|
||||
if (isnan(delay_element_0) || isinf(delay_element_0)) {
|
||||
@ -82,7 +83,9 @@ float LowPassFilter2p::apply(float sample)
|
||||
}
|
||||
|
||||
float LowPassFilter2p::reset(float sample) {
|
||||
_delay_element_1 = _delay_element_2 = sample;
|
||||
float dval = sample / (_b0 + _b1 + _b2);
|
||||
_delay_element_1 = dval;
|
||||
_delay_element_2 = dval;
|
||||
return apply(sample);
|
||||
}
|
||||
|
||||
|
||||
@ -595,44 +595,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
|
||||
memcpy(&att.R[0], &R.data[0][0], sizeof(att.R));
|
||||
att.R_valid = true;
|
||||
|
||||
// compute secondary attitude
|
||||
math::Matrix<3, 3> R_adapted; //modified rotation matrix
|
||||
R_adapted = R;
|
||||
|
||||
//move z to x
|
||||
R_adapted(0, 0) = R(0, 2);
|
||||
R_adapted(1, 0) = R(1, 2);
|
||||
R_adapted(2, 0) = R(2, 2);
|
||||
//move x to z
|
||||
R_adapted(0, 2) = R(0, 0);
|
||||
R_adapted(1, 2) = R(1, 0);
|
||||
R_adapted(2, 2) = R(2, 0);
|
||||
|
||||
//change direction of pitch (convert to right handed system)
|
||||
R_adapted(0, 0) = -R_adapted(0, 0);
|
||||
R_adapted(1, 0) = -R_adapted(1, 0);
|
||||
R_adapted(2, 0) = -R_adapted(2, 0);
|
||||
math::Vector<3> euler_angles_sec; //adapted euler angles for fixed wing operation
|
||||
euler_angles_sec = R_adapted.to_euler();
|
||||
|
||||
att.roll_sec = euler_angles_sec(0);
|
||||
att.pitch_sec = euler_angles_sec(1);
|
||||
att.yaw_sec = euler_angles_sec(2);
|
||||
|
||||
memcpy(&att.R_sec[0], &R_adapted.data[0][0], sizeof(att.R_sec));
|
||||
|
||||
att.rollspeed_sec = -x_aposteriori[2];
|
||||
att.pitchspeed_sec = x_aposteriori[1];
|
||||
att.yawspeed_sec = x_aposteriori[0];
|
||||
att.rollacc_sec = -x_aposteriori[5];
|
||||
att.pitchacc_sec = x_aposteriori[4];
|
||||
att.yawacc_sec = x_aposteriori[3];
|
||||
|
||||
att.g_comp_sec[0] = -raw.accelerometer_m_s2[2] - (-acc(2));
|
||||
att.g_comp_sec[1] = raw.accelerometer_m_s2[1] - acc(1);
|
||||
att.g_comp_sec[2] = raw.accelerometer_m_s2[0] - acc(0);
|
||||
|
||||
|
||||
if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
|
||||
// Broadcast
|
||||
orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
|
||||
|
||||
@ -84,6 +84,7 @@
|
||||
#include <uORB/topics/geofence_result.h>
|
||||
#include <uORB/topics/telemetry_status.h>
|
||||
#include <uORB/topics/vtol_vehicle_status.h>
|
||||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
|
||||
#include <drivers/drv_led.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
@ -240,6 +241,13 @@ transition_result_t check_navigation_state_machine(struct vehicle_status_s *stat
|
||||
|
||||
transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy);
|
||||
|
||||
/**
|
||||
* @brief This function initializes the home position of the vehicle. This happens first time we get a good GPS fix and each
|
||||
* time the vehicle is armed with a good GPS fix.
|
||||
**/
|
||||
static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home,
|
||||
const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition);
|
||||
|
||||
/**
|
||||
* Loop that runs at a lower rate and priority for calibration and parameter tasks.
|
||||
*/
|
||||
@ -560,7 +568,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
|
||||
}
|
||||
break;
|
||||
|
||||
/* Flight termination */
|
||||
/* Flight termination */
|
||||
case VEHICLE_CMD_DO_FLIGHTTERMINATION: {
|
||||
if (cmd->param1 > 0.5f) {
|
||||
//XXX update state machine?
|
||||
@ -716,6 +724,53 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief This function initializes the home position of the vehicle. This happens first time we get a good GPS fix and each
|
||||
* time the vehicle is armed with a good GPS fix.
|
||||
**/
|
||||
static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home,
|
||||
const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition)
|
||||
{
|
||||
//Need global position fix to be able to set home
|
||||
if (!status.condition_global_position_valid) {
|
||||
return;
|
||||
}
|
||||
|
||||
//Ensure that the GPS accuracy is good enough for intializing home
|
||||
if (globalPosition.eph > eph_threshold || globalPosition.epv > epv_threshold) {
|
||||
return;
|
||||
}
|
||||
|
||||
//Set home position
|
||||
home.timestamp = hrt_absolute_time();
|
||||
home.lat = globalPosition.lat;
|
||||
home.lon = globalPosition.lon;
|
||||
home.alt = globalPosition.alt;
|
||||
|
||||
home.x = localPosition.x;
|
||||
home.y = localPosition.y;
|
||||
home.z = localPosition.z;
|
||||
|
||||
warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
|
||||
mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
|
||||
|
||||
/* announce new home position */
|
||||
if (homePub > 0) {
|
||||
orb_publish(ORB_ID(home_position), homePub, &home);
|
||||
|
||||
} else {
|
||||
homePub = orb_advertise(ORB_ID(home_position), &home);
|
||||
}
|
||||
|
||||
//Play tune first time we initialize HOME
|
||||
if (!status.condition_home_position_valid) {
|
||||
tune_positive(true);
|
||||
}
|
||||
|
||||
/* mark home position as set */
|
||||
status.condition_home_position_valid = true;
|
||||
}
|
||||
|
||||
int commander_thread_main(int argc, char *argv[])
|
||||
{
|
||||
/* not yet initialized */
|
||||
@ -904,7 +959,6 @@ int commander_thread_main(int argc, char *argv[])
|
||||
bool critical_battery_voltage_actions_done = false;
|
||||
|
||||
hrt_abstime last_idle_time = 0;
|
||||
hrt_abstime start_time = 0;
|
||||
|
||||
bool status_changed = true;
|
||||
bool param_init_forced = true;
|
||||
@ -964,6 +1018,11 @@ int commander_thread_main(int argc, char *argv[])
|
||||
struct vehicle_local_position_s local_position;
|
||||
memset(&local_position, 0, sizeof(local_position));
|
||||
|
||||
/* Subscribe to land detector */
|
||||
int land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
|
||||
struct vehicle_land_detected_s land_detector;
|
||||
memset(&land_detector, 0, sizeof(land_detector));
|
||||
|
||||
/*
|
||||
* The home position is set based on GPS only, to prevent a dependency between
|
||||
* position estimator and commander. RAW GPS is more than good enough for a
|
||||
@ -1035,7 +1094,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
commander_initialized = true;
|
||||
thread_running = true;
|
||||
|
||||
start_time = hrt_absolute_time();
|
||||
const hrt_abstime commander_boot_timestamp = hrt_absolute_time();
|
||||
|
||||
transition_result_t arming_ret;
|
||||
|
||||
@ -1096,8 +1155,8 @@ int commander_thread_main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
/* set vehicle_status.is_vtol flag */
|
||||
status.is_vtol = (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) ||
|
||||
(status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR);
|
||||
status.is_vtol = (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) ||
|
||||
(status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR);
|
||||
|
||||
/* check and update system / component ID */
|
||||
param_get(_param_system_id, &(status.system_id));
|
||||
@ -1259,12 +1318,12 @@ int commander_thread_main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
//Notify the user if the status of the safety switch changes
|
||||
if(safety.safety_switch_available && previous_safety_off != safety.safety_off) {
|
||||
if (safety.safety_switch_available && previous_safety_off != safety.safety_off) {
|
||||
|
||||
if(safety.safety_off) {
|
||||
if (safety.safety_off) {
|
||||
set_tune(TONE_NOTIFY_POSITIVE_TUNE);
|
||||
}
|
||||
else {
|
||||
|
||||
} else {
|
||||
tune_neutral(true);
|
||||
}
|
||||
|
||||
@ -1279,8 +1338,9 @@ int commander_thread_main(int argc, char *argv[])
|
||||
/* vtol status changed */
|
||||
orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status);
|
||||
status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab;
|
||||
|
||||
/* Make sure that this is only adjusted if vehicle realy is of type vtol*/
|
||||
if (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR || VEHICLE_TYPE_VTOL_QUADROTOR) {
|
||||
if ((status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) || (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR)) {
|
||||
status.is_rotary_wing = vtol_status.vtol_in_rw_mode;
|
||||
}
|
||||
}
|
||||
@ -1325,34 +1385,6 @@ int commander_thread_main(int argc, char *argv[])
|
||||
check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid),
|
||||
&status_changed);
|
||||
|
||||
/* update home position */
|
||||
if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed &&
|
||||
(global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) {
|
||||
|
||||
home.lat = global_position.lat;
|
||||
home.lon = global_position.lon;
|
||||
home.alt = global_position.alt;
|
||||
|
||||
home.x = local_position.x;
|
||||
home.y = local_position.y;
|
||||
home.z = local_position.z;
|
||||
|
||||
warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
|
||||
mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
|
||||
|
||||
/* announce new home position */
|
||||
if (home_pub > 0) {
|
||||
orb_publish(ORB_ID(home_position), home_pub, &home);
|
||||
|
||||
} else {
|
||||
home_pub = orb_advertise(ORB_ID(home_position), &home);
|
||||
}
|
||||
|
||||
/* mark home position as set */
|
||||
status.condition_home_position_valid = true;
|
||||
tune_positive(true);
|
||||
}
|
||||
|
||||
/* update condition_local_position_valid and condition_local_altitude_valid */
|
||||
/* hysteresis for EPH */
|
||||
bool local_eph_good;
|
||||
@ -1379,9 +1411,15 @@ int commander_thread_main(int argc, char *argv[])
|
||||
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid,
|
||||
&(status.condition_local_altitude_valid), &status_changed);
|
||||
|
||||
/* Update land detector */
|
||||
orb_check(land_detector_sub, &updated);
|
||||
if(updated) {
|
||||
orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector);
|
||||
}
|
||||
|
||||
if (status.condition_local_altitude_valid) {
|
||||
if (status.condition_landed != local_position.landed) {
|
||||
status.condition_landed = local_position.landed;
|
||||
if (status.condition_landed != land_detector.landed) {
|
||||
status.condition_landed = land_detector.landed;
|
||||
status_changed = true;
|
||||
|
||||
if (status.condition_landed) {
|
||||
@ -1401,7 +1439,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
|
||||
|
||||
/* only consider battery voltage if system has been running 2s and battery voltage is valid */
|
||||
if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
|
||||
if (hrt_absolute_time() > commander_boot_timestamp + 2000000 && battery.voltage_filtered_v > 0.0f) {
|
||||
status.battery_voltage = battery.voltage_filtered_v;
|
||||
status.battery_current = battery.current_a;
|
||||
status.condition_battery_voltage_valid = true;
|
||||
@ -1419,7 +1457,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(subsystem_info), subsys_sub, &info);
|
||||
|
||||
warnx("subsystem changed: %d\n", (int)info.subsystem_type);
|
||||
//warnx("subsystem changed: %d\n", (int)info.subsystem_type);
|
||||
|
||||
/* mark / unmark as present */
|
||||
if (info.present) {
|
||||
@ -1609,7 +1647,8 @@ int commander_thread_main(int argc, char *argv[])
|
||||
|
||||
} else {
|
||||
if (status.rc_signal_lost) {
|
||||
mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums",(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1000);
|
||||
mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums",
|
||||
(hrt_absolute_time() - status.rc_signal_lost_timestamp) / 1000);
|
||||
status_changed = true;
|
||||
}
|
||||
}
|
||||
@ -1710,9 +1749,9 @@ int commander_thread_main(int argc, char *argv[])
|
||||
|
||||
} else {
|
||||
if (!status.rc_signal_lost) {
|
||||
mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%llums)",hrt_absolute_time()/1000);
|
||||
mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%llums)", hrt_absolute_time() / 1000);
|
||||
status.rc_signal_lost = true;
|
||||
status.rc_signal_lost_timestamp=sp_man.timestamp;
|
||||
status.rc_signal_lost_timestamp = sp_man.timestamp;
|
||||
status_changed = true;
|
||||
}
|
||||
}
|
||||
@ -1859,42 +1898,23 @@ int commander_thread_main(int argc, char *argv[])
|
||||
}
|
||||
}
|
||||
|
||||
//Get current timestamp
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
hrt_abstime t1 = hrt_absolute_time();
|
||||
//First time home position update
|
||||
if (!status.condition_home_position_valid) {
|
||||
commander_set_home_position(home_pub, home, local_position, global_position);
|
||||
}
|
||||
|
||||
/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
|
||||
else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + 2000000) {
|
||||
commander_set_home_position(home_pub, home, local_position, global_position);
|
||||
}
|
||||
|
||||
/* print new state */
|
||||
if (arming_state_changed) {
|
||||
status_changed = true;
|
||||
mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]);
|
||||
|
||||
/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
|
||||
if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid &&
|
||||
(global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) {
|
||||
|
||||
// TODO remove code duplication
|
||||
home.lat = global_position.lat;
|
||||
home.lon = global_position.lon;
|
||||
home.alt = global_position.alt;
|
||||
|
||||
home.x = local_position.x;
|
||||
home.y = local_position.y;
|
||||
home.z = local_position.z;
|
||||
|
||||
warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
|
||||
mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
|
||||
|
||||
/* announce new home position */
|
||||
if (home_pub > 0) {
|
||||
orb_publish(ORB_ID(home_position), home_pub, &home);
|
||||
|
||||
} else {
|
||||
home_pub = orb_advertise(ORB_ID(home_position), &home);
|
||||
}
|
||||
|
||||
/* mark home position as set */
|
||||
status.condition_home_position_valid = true;
|
||||
}
|
||||
|
||||
arming_state_changed = false;
|
||||
}
|
||||
|
||||
@ -1915,11 +1935,14 @@ int commander_thread_main(int argc, char *argv[])
|
||||
|
||||
if (status.failsafe != failsafe_old) {
|
||||
status_changed = true;
|
||||
|
||||
if (status.failsafe) {
|
||||
mavlink_log_critical(mavlink_fd, "failsafe mode on");
|
||||
|
||||
} else {
|
||||
mavlink_log_critical(mavlink_fd, "failsafe mode off");
|
||||
}
|
||||
|
||||
failsafe_old = status.failsafe;
|
||||
}
|
||||
|
||||
@ -1932,13 +1955,13 @@ int commander_thread_main(int argc, char *argv[])
|
||||
/* publish states (armed, control mode, vehicle status) at least with 5 Hz */
|
||||
if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
|
||||
set_control_mode();
|
||||
control_mode.timestamp = t1;
|
||||
control_mode.timestamp = now;
|
||||
orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
|
||||
|
||||
status.timestamp = t1;
|
||||
status.timestamp = now;
|
||||
orb_publish(ORB_ID(vehicle_status), status_pub, &status);
|
||||
|
||||
armed.timestamp = t1;
|
||||
armed.timestamp = now;
|
||||
orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
|
||||
}
|
||||
|
||||
@ -1965,7 +1988,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) {
|
||||
|
||||
//Notify the user that it is safe to approach the vehicle
|
||||
if(arm_tune_played) {
|
||||
if (arm_tune_played) {
|
||||
tune_neutral(true);
|
||||
}
|
||||
|
||||
@ -2432,6 +2455,7 @@ set_control_mode()
|
||||
control_mode.flag_control_position_enabled = false;
|
||||
control_mode.flag_control_velocity_enabled = false;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
@ -2470,7 +2494,7 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
|
||||
{
|
||||
switch (result) {
|
||||
case VEHICLE_CMD_RESULT_ACCEPTED:
|
||||
tune_positive(true);
|
||||
tune_positive(true);
|
||||
break;
|
||||
|
||||
case VEHICLE_CMD_RESULT_DENIED:
|
||||
|
||||
@ -65,6 +65,7 @@ static const char *sensor_name = "mag";
|
||||
|
||||
int do_mag_calibration(int mavlink_fd)
|
||||
{
|
||||
int32_t device_id;
|
||||
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
|
||||
mavlink_log_info(mavlink_fd, "don't move system");
|
||||
|
||||
@ -88,6 +89,9 @@ int do_mag_calibration(int mavlink_fd)
|
||||
|
||||
/* erase old calibration */
|
||||
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
device_id = ioctl(fd, DEVIOCGDEVICEID, 0);
|
||||
|
||||
res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
|
||||
|
||||
if (res != OK) {
|
||||
@ -146,54 +150,60 @@ int do_mag_calibration(int mavlink_fd)
|
||||
|
||||
if (res == OK) {
|
||||
int sub_mag = orb_subscribe(ORB_ID(sensor_mag0));
|
||||
struct mag_report mag;
|
||||
|
||||
/* limit update rate to get equally spaced measurements over time (in ms) */
|
||||
orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
|
||||
if (sub_mag < 0) {
|
||||
mavlink_log_critical(mavlink_fd, "No mag found, abort");
|
||||
res = ERROR;
|
||||
} else {
|
||||
struct mag_report mag;
|
||||
|
||||
/* calibrate offsets */
|
||||
uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
|
||||
unsigned poll_errcount = 0;
|
||||
/* limit update rate to get equally spaced measurements over time (in ms) */
|
||||
orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
|
||||
|
||||
mavlink_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down");
|
||||
/* calibrate offsets */
|
||||
uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
|
||||
unsigned poll_errcount = 0;
|
||||
|
||||
calibration_counter = 0;
|
||||
mavlink_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down");
|
||||
|
||||
while (hrt_absolute_time() < calibration_deadline &&
|
||||
calibration_counter < calibration_maxcount) {
|
||||
calibration_counter = 0U;
|
||||
|
||||
/* wait blocking for new data */
|
||||
struct pollfd fds[1];
|
||||
fds[0].fd = sub_mag;
|
||||
fds[0].events = POLLIN;
|
||||
while (hrt_absolute_time() < calibration_deadline &&
|
||||
calibration_counter < calibration_maxcount) {
|
||||
|
||||
int poll_ret = poll(fds, 1, 1000);
|
||||
/* wait blocking for new data */
|
||||
struct pollfd fds[1];
|
||||
fds[0].fd = sub_mag;
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
if (poll_ret > 0) {
|
||||
orb_copy(ORB_ID(sensor_mag0), sub_mag, &mag);
|
||||
int poll_ret = poll(fds, 1, 1000);
|
||||
|
||||
x[calibration_counter] = mag.x;
|
||||
y[calibration_counter] = mag.y;
|
||||
z[calibration_counter] = mag.z;
|
||||
if (poll_ret > 0) {
|
||||
orb_copy(ORB_ID(sensor_mag0), sub_mag, &mag);
|
||||
|
||||
calibration_counter++;
|
||||
x[calibration_counter] = mag.x;
|
||||
y[calibration_counter] = mag.y;
|
||||
z[calibration_counter] = mag.z;
|
||||
|
||||
if (calibration_counter % (calibration_maxcount / 20) == 0) {
|
||||
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount);
|
||||
calibration_counter++;
|
||||
|
||||
if (calibration_counter % (calibration_maxcount / 20) == 0) {
|
||||
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount);
|
||||
}
|
||||
|
||||
} else {
|
||||
poll_errcount++;
|
||||
}
|
||||
|
||||
} else {
|
||||
poll_errcount++;
|
||||
if (poll_errcount > 1000) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
|
||||
res = ERROR;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (poll_errcount > 1000) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
|
||||
res = ERROR;
|
||||
break;
|
||||
}
|
||||
close(sub_mag);
|
||||
}
|
||||
|
||||
close(sub_mag);
|
||||
}
|
||||
|
||||
float sphere_x;
|
||||
@ -201,7 +211,7 @@ int do_mag_calibration(int mavlink_fd)
|
||||
float sphere_z;
|
||||
float sphere_radius;
|
||||
|
||||
if (res == OK) {
|
||||
if (res == OK && calibration_counter > (calibration_maxcount / 2)) {
|
||||
|
||||
/* sphere fit */
|
||||
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70);
|
||||
@ -253,6 +263,9 @@ int do_mag_calibration(int mavlink_fd)
|
||||
|
||||
if (res == OK) {
|
||||
/* set parameters */
|
||||
if (param_set(param_find("SENS_MAG_ID"), &(device_id))) {
|
||||
res = ERROR;
|
||||
}
|
||||
if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
|
||||
res = ERROR;
|
||||
}
|
||||
|
||||
@ -793,7 +793,7 @@ start(void)
|
||||
sem_init(&g_init_sema, 1, 0);
|
||||
|
||||
/* start the worker thread */
|
||||
if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2000, task_main, NULL)) <= 0) {
|
||||
if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1800, task_main, NULL)) <= 0) {
|
||||
warn("task start failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2013-2015 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
|
||||
@ -82,12 +82,11 @@
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <mathlib/math/filter/LowPassFilter2p.hpp>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
#include <platforms/px4_defines.h>
|
||||
|
||||
#include "estimator_23states.h"
|
||||
|
||||
|
||||
#include "estimator_22states.h"
|
||||
|
||||
/**
|
||||
* estimator app start / stop handling function
|
||||
@ -102,7 +101,7 @@ __EXPORT uint64_t getMicros();
|
||||
|
||||
static uint64_t IMUmsec = 0;
|
||||
static uint64_t IMUusec = 0;
|
||||
static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000;
|
||||
static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; // units: microseconds
|
||||
|
||||
uint32_t millis()
|
||||
{
|
||||
@ -223,8 +222,10 @@ private:
|
||||
float _baro_ref; /**< barometer reference altitude */
|
||||
float _baro_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */
|
||||
float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */
|
||||
hrt_abstime _last_debug_print = 0;
|
||||
|
||||
perf_counter_t _loop_perf; ///< loop performance counter
|
||||
perf_counter_t _loop_intvl; ///< loop rate counter
|
||||
perf_counter_t _perf_gyro; ///<local performance counter for gyro updates
|
||||
perf_counter_t _perf_mag; ///<local performance counter for mag updates
|
||||
perf_counter_t _perf_gps; ///<local performance counter for gps updates
|
||||
@ -291,10 +292,6 @@ private:
|
||||
|
||||
AttPosEKF *_ekf;
|
||||
|
||||
float _velocity_xy_filtered;
|
||||
float _velocity_z_filtered;
|
||||
float _airspeed_filtered;
|
||||
|
||||
/**
|
||||
* Update our local parameter cache.
|
||||
*/
|
||||
@ -400,6 +397,7 @@ FixedwingEstimator::FixedwingEstimator() :
|
||||
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")),
|
||||
_loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")),
|
||||
_perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")),
|
||||
_perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")),
|
||||
_perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")),
|
||||
@ -423,10 +421,7 @@ FixedwingEstimator::FixedwingEstimator() :
|
||||
_mavlink_fd(-1),
|
||||
_parameters{},
|
||||
_parameter_handles{},
|
||||
_ekf(nullptr),
|
||||
_velocity_xy_filtered(0.0f),
|
||||
_velocity_z_filtered(0.0f),
|
||||
_airspeed_filtered(0.0f)
|
||||
_ekf(nullptr)
|
||||
{
|
||||
|
||||
_last_run = hrt_absolute_time();
|
||||
@ -783,6 +778,11 @@ FixedwingEstimator::task_main()
|
||||
_gps.vel_e_m_s = 0.0f;
|
||||
_gps.vel_d_m_s = 0.0f;
|
||||
|
||||
// init lowpass filters for baro and gps altitude
|
||||
float _gps_alt_filt = 0, _baro_alt_filt = 0;
|
||||
float rc = 10.0f; // RC time constant of 1st order LPF in seconds
|
||||
hrt_abstime baro_last = 0;
|
||||
|
||||
_task_running = true;
|
||||
|
||||
while (!_task_should_exit) {
|
||||
@ -801,6 +801,7 @@ FixedwingEstimator::task_main()
|
||||
}
|
||||
|
||||
perf_begin(_loop_perf);
|
||||
perf_count(_loop_intvl);
|
||||
|
||||
/* only update parameters if they changed */
|
||||
if (fds[0].revents & POLLIN) {
|
||||
@ -1056,6 +1057,11 @@ FixedwingEstimator::task_main()
|
||||
_ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
|
||||
_ekf->gpsHgt = _gps.alt / 1e3f;
|
||||
|
||||
// update LPF
|
||||
_gps_alt_filt += (gps_elapsed / (rc + gps_elapsed)) * (_ekf->gpsHgt - _gps_alt_filt);
|
||||
|
||||
//warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)gps_elapsed);
|
||||
|
||||
// if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) {
|
||||
// _ekf->vneSigma = sqrtf(_gps.s_variance_m_s);
|
||||
// } else {
|
||||
@ -1071,7 +1077,6 @@ FixedwingEstimator::task_main()
|
||||
// warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m);
|
||||
|
||||
newDataGps = true;
|
||||
|
||||
last_gps = _gps.timestamp_position;
|
||||
|
||||
}
|
||||
@ -1083,15 +1088,15 @@ FixedwingEstimator::task_main()
|
||||
|
||||
if (baro_updated) {
|
||||
|
||||
hrt_abstime baro_last = _baro.timestamp;
|
||||
|
||||
orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro);
|
||||
|
||||
float baro_elapsed = (_baro.timestamp - baro_last) / 1e6f;
|
||||
baro_last = _baro.timestamp;
|
||||
|
||||
_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f));
|
||||
|
||||
_ekf->baroHgt = _baro.altitude;
|
||||
_baro_alt_filt += (baro_elapsed/(rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt);
|
||||
|
||||
if (!_baro_init) {
|
||||
_baro_ref = _baro.altitude;
|
||||
@ -1181,6 +1186,24 @@ FixedwingEstimator::task_main()
|
||||
|
||||
float initVelNED[3];
|
||||
|
||||
// maintain filtered baro and gps altitudes to calculate weather offset
|
||||
// baro sample rate is ~70Hz and measurement bandwidth is high
|
||||
// gps sample rate is 5Hz and altitude is assumed accurate when averaged over 30 seconds
|
||||
// maintain heavily filtered values for both baro and gps altitude
|
||||
// Assume the filtered output should be identical for both sensors
|
||||
_baro_gps_offset = _baro_alt_filt - _gps_alt_filt;
|
||||
// if (hrt_elapsed_time(&_last_debug_print) >= 5e6) {
|
||||
// _last_debug_print = hrt_absolute_time();
|
||||
// perf_print_counter(_perf_baro);
|
||||
// perf_reset(_perf_baro);
|
||||
// warnx("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f",
|
||||
// (double)_baro_gps_offset,
|
||||
// (double)_baro_alt_filt,
|
||||
// (double)_gps_alt_filt,
|
||||
// (double)_global_pos.alt,
|
||||
// (double)_local_pos.z);
|
||||
// }
|
||||
|
||||
/* Initialize the filter first */
|
||||
if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) {
|
||||
|
||||
@ -1196,7 +1219,11 @@ FixedwingEstimator::task_main()
|
||||
// Set up height correctly
|
||||
orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro);
|
||||
_baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame
|
||||
_baro_gps_offset = _baro.altitude - gps_alt;
|
||||
|
||||
// init filtered gps and baro altitudes
|
||||
_gps_alt_filt = gps_alt;
|
||||
_baro_alt_filt = _baro.altitude;
|
||||
|
||||
_ekf->baroHgt = _baro.altitude;
|
||||
_ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref));
|
||||
|
||||
@ -1372,10 +1399,15 @@ FixedwingEstimator::task_main()
|
||||
}
|
||||
|
||||
if (newRangeData) {
|
||||
_ekf->fuseRngData = true;
|
||||
_ekf->useRangeFinder = true;
|
||||
_ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f));
|
||||
_ekf->GroundEKF();
|
||||
|
||||
if (_ekf->Tnb.z.z > 0.9f) {
|
||||
// _ekf->rngMea is set in sensor readout already
|
||||
_ekf->fuseRngData = true;
|
||||
_ekf->fuseOptFlowData = false;
|
||||
_ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f));
|
||||
_ekf->OpticalFlowEKF();
|
||||
_ekf->fuseRngData = false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -1435,22 +1467,6 @@ FixedwingEstimator::task_main()
|
||||
_local_pos.v_z_valid = true;
|
||||
_local_pos.xy_global = true;
|
||||
|
||||
_velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy);
|
||||
_velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz);
|
||||
_airspeed_filtered = 0.95f*_airspeed_filtered + 0.05f*_airspeed.true_airspeed_m_s;
|
||||
|
||||
|
||||
/* crude land detector for fixedwing only,
|
||||
* TODO: adapt so that it works for both, maybe move to another location
|
||||
*/
|
||||
if (_velocity_xy_filtered < 5
|
||||
&& _velocity_z_filtered < 10
|
||||
&& _airspeed_filtered < 10) {
|
||||
_local_pos.landed = true;
|
||||
} else {
|
||||
_local_pos.landed = false;
|
||||
}
|
||||
|
||||
_local_pos.z_global = false;
|
||||
_local_pos.yaw = _att.yaw;
|
||||
|
||||
@ -1515,8 +1531,8 @@ FixedwingEstimator::task_main()
|
||||
|
||||
if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
|
||||
_wind.timestamp = _global_pos.timestamp;
|
||||
_wind.windspeed_north = _ekf->windSpdFiltNorth;
|
||||
_wind.windspeed_east = _ekf->windSpdFiltEast;
|
||||
_wind.windspeed_north = _ekf->states[14];
|
||||
_wind.windspeed_east = _ekf->states[15];
|
||||
// XXX we need to do something smart about the covariance here
|
||||
// but we default to the estimate covariance for now
|
||||
_wind.covariance_north = _ekf->P[14][14];
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -1,247 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include "estimator_utilities.h"
|
||||
|
||||
class AttPosEKF {
|
||||
|
||||
public:
|
||||
|
||||
AttPosEKF();
|
||||
~AttPosEKF();
|
||||
|
||||
/* ##############################################
|
||||
*
|
||||
* M A I N F I L T E R P A R A M E T E R S
|
||||
*
|
||||
* ########################################### */
|
||||
|
||||
/*
|
||||
* parameters are defined here and initialised in
|
||||
* the InitialiseParameters() (which is just 20 lines down)
|
||||
*/
|
||||
|
||||
float covTimeStepMax; // maximum time allowed between covariance predictions
|
||||
float covDelAngMax; // maximum delta angle between covariance predictions
|
||||
float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
|
||||
|
||||
float yawVarScale;
|
||||
float windVelSigma;
|
||||
float dAngBiasSigma;
|
||||
float dVelBiasSigma;
|
||||
float magEarthSigma;
|
||||
float magBodySigma;
|
||||
float gndHgtSigma;
|
||||
|
||||
float vneSigma;
|
||||
float vdSigma;
|
||||
float posNeSigma;
|
||||
float posDSigma;
|
||||
float magMeasurementSigma;
|
||||
float airspeedMeasurementSigma;
|
||||
|
||||
float gyroProcessNoise;
|
||||
float accelProcessNoise;
|
||||
|
||||
float EAS2TAS; // ratio f true to equivalent airspeed
|
||||
|
||||
void InitialiseParameters()
|
||||
{
|
||||
covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
|
||||
covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
|
||||
rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
|
||||
EAS2TAS = 1.0f;
|
||||
|
||||
yawVarScale = 1.0f;
|
||||
windVelSigma = 0.1f;
|
||||
dAngBiasSigma = 5.0e-7f;
|
||||
dVelBiasSigma = 1e-4f;
|
||||
magEarthSigma = 3.0e-4f;
|
||||
magBodySigma = 3.0e-4f;
|
||||
gndHgtSigma = 0.02f; // assume 2% terrain gradient 1-sigma
|
||||
|
||||
vneSigma = 0.2f;
|
||||
vdSigma = 0.3f;
|
||||
posNeSigma = 2.0f;
|
||||
posDSigma = 2.0f;
|
||||
|
||||
magMeasurementSigma = 0.05;
|
||||
airspeedMeasurementSigma = 1.4f;
|
||||
gyroProcessNoise = 1.4544411e-2f;
|
||||
accelProcessNoise = 0.5f;
|
||||
}
|
||||
|
||||
// Global variables
|
||||
float KH[n_states][n_states]; // intermediate result used for covariance updates
|
||||
float KHP[n_states][n_states]; // intermediate result used for covariance updates
|
||||
float P[n_states][n_states]; // covariance matrix
|
||||
float Kfusion[n_states]; // Kalman gains
|
||||
float states[n_states]; // state matrix
|
||||
float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
|
||||
uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
|
||||
|
||||
float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
|
||||
float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
|
||||
float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
|
||||
float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
|
||||
float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
|
||||
|
||||
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
|
||||
Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
|
||||
Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
|
||||
Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
|
||||
float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
|
||||
Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
|
||||
Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
|
||||
Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
|
||||
Vector3f dVelIMU;
|
||||
Vector3f dAngIMU;
|
||||
float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
|
||||
uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
|
||||
float innovVelPos[6]; // innovation output
|
||||
float varInnovVelPos[6]; // innovation variance output
|
||||
|
||||
float velNED[3]; // North, East, Down velocity obs (m/s)
|
||||
float posNE[2]; // North, East position obs (m)
|
||||
float hgtMea; // measured height (m)
|
||||
float posNED[3]; // North, East Down position (m)
|
||||
|
||||
float innovMag[3]; // innovation output
|
||||
float varInnovMag[3]; // innovation variance output
|
||||
Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
|
||||
float innovVtas; // innovation output
|
||||
float varInnovVtas; // innovation variance output
|
||||
float VtasMeas; // true airspeed measurement (m/s)
|
||||
float magDeclination;
|
||||
float latRef; // WGS-84 latitude of reference point (rad)
|
||||
float lonRef; // WGS-84 longitude of reference point (rad)
|
||||
float hgtRef; // WGS-84 height of reference point (m)
|
||||
Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
|
||||
uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
|
||||
|
||||
// GPS input data variables
|
||||
float gpsCourse;
|
||||
float gpsVelD;
|
||||
float gpsLat;
|
||||
float gpsLon;
|
||||
float gpsHgt;
|
||||
uint8_t GPSstatus;
|
||||
|
||||
// Baro input
|
||||
float baroHgt;
|
||||
|
||||
bool statesInitialised;
|
||||
|
||||
bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused
|
||||
bool fusePosData; // this boolean causes the posNE and velNED obs to be fused
|
||||
bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
|
||||
bool fuseMagData; // boolean true when magnetometer data is to be fused
|
||||
bool fuseVtasData; // boolean true when airspeed data is to be fused
|
||||
|
||||
bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying)
|
||||
bool staticMode; ///< boolean true if no position feedback is fused
|
||||
bool useAirspeed; ///< boolean true if airspeed data is being used
|
||||
bool useCompass; ///< boolean true if magnetometer data is being used
|
||||
|
||||
struct ekf_status_report current_ekf_state;
|
||||
struct ekf_status_report last_ekf_error;
|
||||
|
||||
bool numericalProtection;
|
||||
|
||||
unsigned storeIndex;
|
||||
|
||||
|
||||
void UpdateStrapdownEquationsNED();
|
||||
|
||||
void CovariancePrediction(float dt);
|
||||
|
||||
void FuseVelposNED();
|
||||
|
||||
void FuseMagnetometer();
|
||||
|
||||
void FuseAirspeed();
|
||||
|
||||
void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
|
||||
|
||||
void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
|
||||
|
||||
void quatNorm(float (&quatOut)[4], const float quatIn[4]);
|
||||
|
||||
// store staes along with system time stamp in msces
|
||||
void StoreStates(uint64_t timestamp_ms);
|
||||
|
||||
/**
|
||||
* Recall the state vector.
|
||||
*
|
||||
* Recalls the vector stored at closest time to the one specified by msec
|
||||
*
|
||||
* @return zero on success, integer indicating the number of invalid states on failure.
|
||||
* Does only copy valid states, if the statesForFusion vector was initialized
|
||||
* correctly by the caller, the result can be safely used, but is a mixture
|
||||
* time-wise where valid states were updated and invalid remained at the old
|
||||
* value.
|
||||
*/
|
||||
int RecallStates(float statesForFusion[n_states], uint64_t msec);
|
||||
|
||||
void ResetStoredStates();
|
||||
|
||||
void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]);
|
||||
|
||||
void calcEarthRateNED(Vector3f &omega, float latitude);
|
||||
|
||||
static void eul2quat(float (&quat)[4], const float (&eul)[3]);
|
||||
|
||||
static void quat2eul(float (&eul)[3], const float (&quat)[4]);
|
||||
|
||||
static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
|
||||
|
||||
static void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
|
||||
|
||||
static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
|
||||
|
||||
static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
|
||||
|
||||
static float sq(float valIn);
|
||||
|
||||
void OnGroundCheck();
|
||||
|
||||
void CovarianceInit();
|
||||
|
||||
void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
|
||||
|
||||
float ConstrainFloat(float val, float min, float max);
|
||||
|
||||
void ConstrainVariances();
|
||||
|
||||
void ConstrainStates();
|
||||
|
||||
void ForceSymmetry();
|
||||
|
||||
int CheckAndBound();
|
||||
|
||||
void ResetPosition();
|
||||
|
||||
void ResetVelocity();
|
||||
|
||||
void ZeroVariables();
|
||||
|
||||
void GetFilterState(struct ekf_status_report *state);
|
||||
|
||||
void GetLastErrorState(struct ekf_status_report *last_error);
|
||||
|
||||
bool StatesNaN(struct ekf_status_report *err_report);
|
||||
void FillErrorReport(struct ekf_status_report *err);
|
||||
|
||||
void InitializeDynamic(float (&initvelNED)[3], float declination);
|
||||
|
||||
protected:
|
||||
|
||||
bool FilterHealthy();
|
||||
|
||||
void ResetHeight(void);
|
||||
|
||||
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
|
||||
|
||||
};
|
||||
|
||||
uint32_t millis();
|
||||
|
||||
File diff suppressed because one or more lines are too long
@ -2,7 +2,7 @@
|
||||
|
||||
#include "estimator_utilities.h"
|
||||
|
||||
const unsigned int n_states = 23;
|
||||
const unsigned int n_states = 22;
|
||||
const unsigned int data_buffer_size = 50;
|
||||
|
||||
class AttPosEKF {
|
||||
@ -29,10 +29,6 @@ public:
|
||||
float covDelAngMax; // maximum delta angle between covariance predictions
|
||||
float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
|
||||
|
||||
float a1; // optical flow sensor misalgnment angle about X axis (rad)
|
||||
float a2; // optical flow sensor misalgnment angle about Y axis (rad)
|
||||
float a3; // optical flow sensor misalgnment angle about Z axis (rad)
|
||||
|
||||
float yawVarScale;
|
||||
float windVelSigma;
|
||||
float dAngBiasSigma;
|
||||
@ -59,9 +55,6 @@ public:
|
||||
covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
|
||||
rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
|
||||
EAS2TAS = 1.0f;
|
||||
a1 = 0.0f; // optical flow sensor misalgnment angle about X axis (rad)
|
||||
a2 = 0.0f; // optical flow sensor misalgnment angle about Y axis (rad)
|
||||
a3 = 0.0f; // optical flow sensor misalgnment angle about Z axis (rad)
|
||||
|
||||
yawVarScale = 1.0f;
|
||||
windVelSigma = 0.1f;
|
||||
@ -69,7 +62,6 @@ public:
|
||||
dVelBiasSigma = 1e-4f;
|
||||
magEarthSigma = 3.0e-4f;
|
||||
magBodySigma = 3.0e-4f;
|
||||
gndHgtSigma = 0.02f; // assume 2% terrain gradient 1-sigma
|
||||
|
||||
vneSigma = 0.2f;
|
||||
vdSigma = 0.3f;
|
||||
@ -82,12 +74,13 @@ public:
|
||||
accelProcessNoise = 0.5f;
|
||||
|
||||
gndHgtSigma = 0.1f; // terrain gradient 1-sigma
|
||||
R_LOS = 0.03f; // optical flow measurement noise variance (rad/sec)^2
|
||||
R_LOS = 0.3f; // optical flow measurement noise variance (rad/sec)^2
|
||||
flowInnovGate = 3.0f; // number of standard deviations applied to the optical flow innovation consistency check
|
||||
auxFlowInnovGate = 10.0f; // number of standard deviations applied to the optical flow innovation consistency check used by the auxiliary filter
|
||||
rngInnovGate = 10.0f; // number of standard deviations applied to the rnage finder innovation consistency check
|
||||
minFlowRng = 0.01f; //minimum range between ground and flow sensor
|
||||
moCompR_LOS = 0.2; // scaler from sensor gyro rate to uncertainty in LOS rate
|
||||
rngInnovGate = 5.0f; // number of standard deviations applied to the range finder innovation consistency check
|
||||
minFlowRng = 0.3f; //minimum range between ground and flow sensor
|
||||
moCompR_LOS = 0.0; // scaler from sensor gyro rate to uncertainty in LOS rate
|
||||
|
||||
}
|
||||
|
||||
struct mag_state_struct {
|
||||
@ -134,6 +127,7 @@ public:
|
||||
float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
|
||||
float statesAtRngTime[n_states]; // filter states at the effective measurement time
|
||||
float statesAtFlowTime[n_states]; // States at the effective optical flow measurement time
|
||||
float omegaAcrossFlowTime[3]; // angular rates at the effective optical flow measurement time
|
||||
|
||||
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
|
||||
Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
|
||||
@ -145,7 +139,7 @@ public:
|
||||
Vector3f lastGyroOffset; // Last gyro offset
|
||||
Vector3f delAngTotal;
|
||||
|
||||
Mat3f Tbn; // transformation matrix from body to NED coordinates
|
||||
Mat3f Tbn; // transformation matrix from body to NED coordinatesFuseOptFlow
|
||||
Mat3f Tnb; // transformation amtrix from NED to body coordinates
|
||||
|
||||
Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
|
||||
@ -157,10 +151,6 @@ public:
|
||||
float dtVelPosFilt; // average time between position / velocity fusion steps
|
||||
float dtHgtFilt; // average time between height measurement updates
|
||||
float dtGpsFilt; // average time between gps measurement updates
|
||||
float windSpdFiltNorth; // average wind speed north component
|
||||
float windSpdFiltEast; // average wind speed east component
|
||||
float windSpdFiltAltitude; // the last altitude used to filter wind speed
|
||||
float windSpdFiltClimb; // filtered climb rate
|
||||
uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
|
||||
float innovVelPos[6]; // innovation output
|
||||
float varInnovVelPos[6]; // innovation variance output
|
||||
@ -175,7 +165,8 @@ public:
|
||||
float innovMag[3]; // innovation output
|
||||
float varInnovMag[3]; // innovation variance output
|
||||
Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
|
||||
float losData[2]; // optical flow LOS rate measurements (rad/sec)
|
||||
float flowRadXYcomp[2]; // motion compensated optical flow angular rates(rad/sec)
|
||||
float flowRadXY[2]; // raw (non motion compensated) optical flow angular rates (rad/sec)
|
||||
float innovVtas; // innovation output
|
||||
float innovRng; ///< Range finder innovation
|
||||
float innovOptFlow[2]; // optical flow LOS innovations (rad/sec)
|
||||
@ -190,6 +181,8 @@ public:
|
||||
bool refSet; ///< flag to indicate if the reference position has been set
|
||||
Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
|
||||
unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
|
||||
uint32_t lastFixTime_ms; // Number of msec since last GPS Fix that was used
|
||||
uint32_t globalTimeStamp_ms; // time in msec of current prediction cycle
|
||||
|
||||
// GPS input data variables
|
||||
double gpsLat;
|
||||
@ -217,6 +210,7 @@ public:
|
||||
|
||||
bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying)
|
||||
bool staticMode; ///< boolean true if no position feedback is fused
|
||||
bool useGPS; // boolean true if GPS data is being used
|
||||
bool useAirspeed; ///< boolean true if airspeed data is being used
|
||||
bool useCompass; ///< boolean true if magnetometer data is being used
|
||||
bool useRangeFinder; ///< true when rangefinder is being used
|
||||
@ -267,11 +261,9 @@ void FuseMagnetometer();
|
||||
|
||||
void FuseAirspeed();
|
||||
|
||||
void FuseRangeFinder();
|
||||
|
||||
void FuseOptFlow();
|
||||
|
||||
void GroundEKF();
|
||||
void OpticalFlowEKF();
|
||||
|
||||
void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
|
||||
|
||||
@ -286,7 +278,7 @@ void StoreStates(uint64_t timestamp_ms);
|
||||
* Recall the state vector.
|
||||
*
|
||||
* Recalls the vector stored at closest time to the one specified by msec
|
||||
*
|
||||
*FuseOptFlow
|
||||
* @return zero on success, integer indicating the number of invalid states on failure.
|
||||
* Does only copy valid states, if the statesForFusion vector was initialized
|
||||
* correctly by the caller, the result can be safely used, but is a mixture
|
||||
@ -295,6 +287,8 @@ void StoreStates(uint64_t timestamp_ms);
|
||||
*/
|
||||
int RecallStates(float *statesForFusion, uint64_t msec);
|
||||
|
||||
void RecallOmega(float *omegaForFusion, uint64_t msec);
|
||||
|
||||
void ResetStoredStates();
|
||||
|
||||
void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]);
|
||||
@ -325,7 +319,7 @@ void CovarianceInit();
|
||||
|
||||
void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
|
||||
|
||||
float ConstrainFloat(float val, float min, float max);
|
||||
float ConstrainFloat(float val, float min, float maxf);
|
||||
|
||||
void ConstrainVariances();
|
||||
|
||||
@ -39,7 +39,7 @@ MODULE_COMMAND = ekf_att_pos_estimator
|
||||
|
||||
SRCS = ekf_att_pos_estimator_main.cpp \
|
||||
ekf_att_pos_estimator_params.c \
|
||||
estimator_23states.cpp \
|
||||
estimator_22states.cpp \
|
||||
estimator_utilities.cpp
|
||||
|
||||
EXTRACXXFLAGS = -Weffc++ -Wframe-larger-than=6100
|
||||
|
||||
125
src/modules/land_detector/FixedwingLandDetector.cpp
Normal file
125
src/modules/land_detector/FixedwingLandDetector.cpp
Normal file
@ -0,0 +1,125 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2015 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 FixedwingLandDetector.cpp
|
||||
* Land detection algorithm for fixedwings
|
||||
*
|
||||
* @author Johan Jansen <jnsn.johan@gmail.com>
|
||||
*/
|
||||
|
||||
#include "FixedwingLandDetector.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
FixedwingLandDetector::FixedwingLandDetector() : LandDetector(),
|
||||
_paramHandle(),
|
||||
_params(),
|
||||
_vehicleLocalPositionSub(-1),
|
||||
_vehicleLocalPosition({}),
|
||||
_airspeedSub(-1),
|
||||
_airspeed({}),
|
||||
_parameterSub(-1),
|
||||
_velocity_xy_filtered(0.0f),
|
||||
_velocity_z_filtered(0.0f),
|
||||
_airspeed_filtered(0.0f),
|
||||
_landDetectTrigger(0)
|
||||
{
|
||||
_paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX");
|
||||
_paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX");
|
||||
_paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX");
|
||||
}
|
||||
|
||||
void FixedwingLandDetector::initialize()
|
||||
{
|
||||
// Subscribe to local position and airspeed data
|
||||
_vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position));
|
||||
_airspeedSub = orb_subscribe(ORB_ID(airspeed));
|
||||
}
|
||||
|
||||
void FixedwingLandDetector::updateSubscriptions()
|
||||
{
|
||||
orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition);
|
||||
orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
|
||||
}
|
||||
|
||||
bool FixedwingLandDetector::update()
|
||||
{
|
||||
// First poll for new data from our subscriptions
|
||||
updateSubscriptions();
|
||||
|
||||
const uint64_t now = hrt_absolute_time();
|
||||
bool landDetected = false;
|
||||
|
||||
// TODO: reset filtered values on arming?
|
||||
_velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx *
|
||||
_vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy);
|
||||
_velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz);
|
||||
_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s;
|
||||
|
||||
// crude land detector for fixedwing
|
||||
if (_velocity_xy_filtered < _params.maxVelocity
|
||||
&& _velocity_z_filtered < _params.maxClimbRate
|
||||
&& _airspeed_filtered < _params.maxAirSpeed) {
|
||||
|
||||
// these conditions need to be stable for a period of time before we trust them
|
||||
if (now > _landDetectTrigger) {
|
||||
landDetected = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
// reset land detect trigger
|
||||
_landDetectTrigger = now + LAND_DETECTOR_TRIGGER_TIME;
|
||||
}
|
||||
|
||||
return landDetected;
|
||||
}
|
||||
|
||||
void FixedwingLandDetector::updateParameterCache(const bool force)
|
||||
{
|
||||
bool updated;
|
||||
parameter_update_s paramUpdate;
|
||||
|
||||
orb_check(_parameterSub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(parameter_update), _parameterSub, ¶mUpdate);
|
||||
}
|
||||
|
||||
if (updated || force) {
|
||||
param_get(_paramHandle.maxVelocity, &_params.maxVelocity);
|
||||
param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate);
|
||||
param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed);
|
||||
}
|
||||
}
|
||||
105
src/modules/land_detector/FixedwingLandDetector.h
Normal file
105
src/modules/land_detector/FixedwingLandDetector.h
Normal file
@ -0,0 +1,105 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2015 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 FixedwingLandDetector.h
|
||||
* Land detection algorithm for fixedwing
|
||||
*
|
||||
* @author Johan Jansen <jnsn.johan@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef __FIXED_WING_LAND_DETECTOR_H__
|
||||
#define __FIXED_WING_LAND_DETECTOR_H__
|
||||
|
||||
#include "LandDetector.h"
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
class FixedwingLandDetector : public LandDetector
|
||||
{
|
||||
public:
|
||||
FixedwingLandDetector();
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief blocking loop, should be run in a separate thread or task. Runs at 50Hz
|
||||
**/
|
||||
bool update() override;
|
||||
|
||||
/**
|
||||
* @brief Initializes the land detection algorithm
|
||||
**/
|
||||
void initialize() override;
|
||||
|
||||
/**
|
||||
* @brief polls all subscriptions and pulls any data that has changed
|
||||
**/
|
||||
void updateSubscriptions();
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief download and update local parameter cache
|
||||
**/
|
||||
void updateParameterCache(const bool force);
|
||||
|
||||
/**
|
||||
* @brief Handles for interesting parameters
|
||||
**/
|
||||
struct {
|
||||
param_t maxVelocity;
|
||||
param_t maxClimbRate;
|
||||
param_t maxAirSpeed;
|
||||
} _paramHandle;
|
||||
|
||||
struct {
|
||||
float maxVelocity;
|
||||
float maxClimbRate;
|
||||
float maxAirSpeed;
|
||||
} _params;
|
||||
|
||||
private:
|
||||
int _vehicleLocalPositionSub; /**< notification of local position */
|
||||
struct vehicle_local_position_s _vehicleLocalPosition; /**< the result from local position subscription */
|
||||
int _airspeedSub;
|
||||
struct airspeed_s _airspeed;
|
||||
int _parameterSub;
|
||||
|
||||
float _velocity_xy_filtered;
|
||||
float _velocity_z_filtered;
|
||||
float _airspeed_filtered;
|
||||
uint64_t _landDetectTrigger;
|
||||
};
|
||||
|
||||
#endif //__FIXED_WING_LAND_DETECTOR_H__
|
||||
124
src/modules/land_detector/LandDetector.cpp
Normal file
124
src/modules/land_detector/LandDetector.cpp
Normal file
@ -0,0 +1,124 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2015 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 LandDetector.cpp
|
||||
* Land detection algorithm
|
||||
*
|
||||
* @author Johan Jansen <jnsn.johan@gmail.com>
|
||||
* @author Morten Lysgaard <morten@lysgaard.no>
|
||||
*/
|
||||
|
||||
#include "LandDetector.h"
|
||||
#include <unistd.h> //usleep
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
LandDetector::LandDetector() :
|
||||
_landDetectedPub(-1),
|
||||
_landDetected({0, false}),
|
||||
_taskShouldExit(false),
|
||||
_taskIsRunning(false)
|
||||
{
|
||||
// ctor
|
||||
}
|
||||
|
||||
LandDetector::~LandDetector()
|
||||
{
|
||||
_taskShouldExit = true;
|
||||
close(_landDetectedPub);
|
||||
}
|
||||
|
||||
void LandDetector::shutdown()
|
||||
{
|
||||
_taskShouldExit = true;
|
||||
}
|
||||
|
||||
void LandDetector::start()
|
||||
{
|
||||
// make sure this method has not already been called by another thread
|
||||
if (isRunning()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// advertise the first land detected uORB
|
||||
_landDetected.timestamp = hrt_absolute_time();
|
||||
_landDetected.landed = false;
|
||||
_landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected);
|
||||
|
||||
// initialize land detection algorithm
|
||||
initialize();
|
||||
|
||||
// task is now running, keep doing so until shutdown() has been called
|
||||
_taskIsRunning = true;
|
||||
_taskShouldExit = false;
|
||||
|
||||
while (isRunning()) {
|
||||
|
||||
bool landDetected = update();
|
||||
|
||||
// publish if land detection state has changed
|
||||
if (_landDetected.landed != landDetected) {
|
||||
_landDetected.timestamp = hrt_absolute_time();
|
||||
_landDetected.landed = landDetected;
|
||||
|
||||
// publish the land detected broadcast
|
||||
orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected);
|
||||
}
|
||||
|
||||
// limit loop rate
|
||||
usleep(1000000 / LAND_DETECTOR_UPDATE_RATE);
|
||||
}
|
||||
|
||||
_taskIsRunning = false;
|
||||
_exit(0);
|
||||
}
|
||||
|
||||
bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void *buffer)
|
||||
{
|
||||
bool newData = false;
|
||||
|
||||
// check if there is new data to grab
|
||||
if (orb_check(handle, &newData) != OK) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!newData) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (orb_copy(meta, handle, buffer) != OK) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
104
src/modules/land_detector/LandDetector.h
Normal file
104
src/modules/land_detector/LandDetector.h
Normal file
@ -0,0 +1,104 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2015 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 LandDetector.h
|
||||
* Abstract interface for land detector algorithms
|
||||
*
|
||||
* @author Johan Jansen <jnsn.johan@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef __LAND_DETECTOR_H__
|
||||
#define __LAND_DETECTOR_H__
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
|
||||
class LandDetector
|
||||
{
|
||||
public:
|
||||
|
||||
LandDetector();
|
||||
virtual ~LandDetector();
|
||||
|
||||
/**
|
||||
* @return true if this task is currently running
|
||||
**/
|
||||
inline bool isRunning() const {return _taskIsRunning;}
|
||||
|
||||
/**
|
||||
* @brief Tells the Land Detector task that it should exit
|
||||
**/
|
||||
void shutdown();
|
||||
|
||||
/**
|
||||
* @brief Blocking function that should be called from it's own task thread. This method will
|
||||
* run the underlying algorithm at the desired update rate and publish if the landing state changes.
|
||||
**/
|
||||
void start();
|
||||
|
||||
protected:
|
||||
|
||||
/**
|
||||
* @brief Pure abstract method that must be overriden by sub-classes. This actually runs the underlying algorithm
|
||||
* @return true if a landing was detected and this should be broadcast to the rest of the system
|
||||
**/
|
||||
virtual bool update() = 0;
|
||||
|
||||
/**
|
||||
* @brief Pure abstract method that is called by this class once for initializing the uderlying algorithm (memory allocation,
|
||||
* uORB topic subscription, creating file descriptors, etc.)
|
||||
**/
|
||||
virtual void initialize() = 0;
|
||||
|
||||
/**
|
||||
* @brief Convinience function for polling uORB subscriptions
|
||||
* @return true if there was new data and it was successfully copied
|
||||
**/
|
||||
bool orb_update(const struct orb_metadata *meta, int handle, void *buffer);
|
||||
|
||||
static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE = 50; /**< Run algorithm at 50Hz */
|
||||
|
||||
static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold
|
||||
before triggering a land */
|
||||
|
||||
protected:
|
||||
orb_advert_t _landDetectedPub; /**< publisher for position in local frame */
|
||||
struct vehicle_land_detected_s _landDetected; /**< local vehicle position */
|
||||
|
||||
private:
|
||||
bool _taskShouldExit; /**< true if it is requested that this task should exit */
|
||||
bool _taskIsRunning; /**< task has reached main loop and is currently running */
|
||||
};
|
||||
|
||||
#endif //__LAND_DETECTOR_H__
|
||||
145
src/modules/land_detector/MulticopterLandDetector.cpp
Normal file
145
src/modules/land_detector/MulticopterLandDetector.cpp
Normal file
@ -0,0 +1,145 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2015 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 MulticopterLandDetector.cpp
|
||||
* Land detection algorithm for multicopters
|
||||
*
|
||||
* @author Johan Jansen <jnsn.johan@gmail.com>
|
||||
* @author Morten Lysgaard <morten@lysgaard.no>
|
||||
*/
|
||||
|
||||
#include "MulticopterLandDetector.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
|
||||
_paramHandle(),
|
||||
_params(),
|
||||
_vehicleGlobalPositionSub(-1),
|
||||
_sensorsCombinedSub(-1),
|
||||
_waypointSub(-1),
|
||||
_actuatorsSub(-1),
|
||||
_armingSub(-1),
|
||||
_parameterSub(-1),
|
||||
_vehicleGlobalPosition({}),
|
||||
_sensors({}),
|
||||
_waypoint({}),
|
||||
_actuators({}),
|
||||
_arming({}),
|
||||
_landTimer(0)
|
||||
{
|
||||
_paramHandle.maxRotation = param_find("LNDMC_Z_VEL_MAX");
|
||||
_paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX");
|
||||
_paramHandle.maxClimbRate = param_find("LNDMC_ROT_MAX");
|
||||
_paramHandle.maxThrottle = param_find("LNDMC_THR_MAX");
|
||||
}
|
||||
|
||||
void MulticopterLandDetector::initialize()
|
||||
{
|
||||
// subscribe to position, attitude, arming and velocity changes
|
||||
_vehicleGlobalPositionSub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
_sensorsCombinedSub = orb_subscribe(ORB_ID(sensor_combined));
|
||||
_waypointSub = orb_subscribe(ORB_ID(position_setpoint_triplet));
|
||||
_actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
|
||||
_armingSub = orb_subscribe(ORB_ID(actuator_armed));
|
||||
_parameterSub = orb_subscribe(ORB_ID(parameter_update));
|
||||
|
||||
// download parameters
|
||||
updateParameterCache(true);
|
||||
}
|
||||
|
||||
void MulticopterLandDetector::updateSubscriptions()
|
||||
{
|
||||
orb_update(ORB_ID(vehicle_global_position), _vehicleGlobalPositionSub, &_vehicleGlobalPosition);
|
||||
orb_update(ORB_ID(sensor_combined), _sensorsCombinedSub, &_sensors);
|
||||
orb_update(ORB_ID(position_setpoint_triplet), _waypointSub, &_waypoint);
|
||||
orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators);
|
||||
orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
|
||||
}
|
||||
|
||||
bool MulticopterLandDetector::update()
|
||||
{
|
||||
// first poll for new data from our subscriptions
|
||||
updateSubscriptions();
|
||||
|
||||
// only trigger flight conditions if we are armed
|
||||
if (!_arming.armed) {
|
||||
return true;
|
||||
}
|
||||
|
||||
const uint64_t now = hrt_absolute_time();
|
||||
|
||||
// check if we are moving vertically
|
||||
bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate;
|
||||
|
||||
// check if we are moving horizontally
|
||||
bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n
|
||||
+ _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > _params.maxVelocity;
|
||||
|
||||
// next look if all rotation angles are not moving
|
||||
bool rotating = sqrtf(_sensors.gyro_rad_s[0] * _sensors.gyro_rad_s[0] +
|
||||
_sensors.gyro_rad_s[1] * _sensors.gyro_rad_s[1] +
|
||||
_sensors.gyro_rad_s[2] * _sensors.gyro_rad_s[2]) > _params.maxRotation;
|
||||
|
||||
// check if thrust output is minimal (about half of default)
|
||||
bool minimalThrust = _actuators.control[3] <= _params.maxThrottle;
|
||||
|
||||
if (verticalMovement || rotating || !minimalThrust || horizontalMovement) {
|
||||
// sensed movement, so reset the land detector
|
||||
_landTimer = now;
|
||||
return false;
|
||||
}
|
||||
|
||||
return now - _landTimer > LAND_DETECTOR_TRIGGER_TIME;
|
||||
}
|
||||
|
||||
void MulticopterLandDetector::updateParameterCache(const bool force)
|
||||
{
|
||||
bool updated;
|
||||
parameter_update_s paramUpdate;
|
||||
|
||||
orb_check(_parameterSub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(parameter_update), _parameterSub, ¶mUpdate);
|
||||
}
|
||||
|
||||
if (updated || force) {
|
||||
param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate);
|
||||
param_get(_paramHandle.maxVelocity, &_params.maxVelocity);
|
||||
param_get(_paramHandle.maxRotation, &_params.maxRotation);
|
||||
param_get(_paramHandle.maxThrottle, &_params.maxThrottle);
|
||||
}
|
||||
}
|
||||
116
src/modules/land_detector/MulticopterLandDetector.h
Normal file
116
src/modules/land_detector/MulticopterLandDetector.h
Normal file
@ -0,0 +1,116 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2015 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 MulticopterLandDetector.h
|
||||
* Land detection algorithm for multicopters
|
||||
*
|
||||
* @author Johan Jansen <jnsn.johan@gmail.com>
|
||||
* @author Morten Lysgaard <morten@lysgaard.no>
|
||||
*/
|
||||
|
||||
#ifndef __MULTICOPTER_LAND_DETECTOR_H__
|
||||
#define __MULTICOPTER_LAND_DETECTOR_H__
|
||||
|
||||
#include "LandDetector.h"
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
class MulticopterLandDetector : public LandDetector
|
||||
{
|
||||
public:
|
||||
MulticopterLandDetector();
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief polls all subscriptions and pulls any data that has changed
|
||||
**/
|
||||
void updateSubscriptions();
|
||||
|
||||
/**
|
||||
* @brief Runs one iteration of the land detection algorithm
|
||||
**/
|
||||
bool update() override;
|
||||
|
||||
/**
|
||||
* @brief Initializes the land detection algorithm
|
||||
**/
|
||||
void initialize() override;
|
||||
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief download and update local parameter cache
|
||||
**/
|
||||
void updateParameterCache(const bool force);
|
||||
|
||||
/**
|
||||
* @brief Handles for interesting parameters
|
||||
**/
|
||||
struct {
|
||||
param_t maxClimbRate;
|
||||
param_t maxVelocity;
|
||||
param_t maxRotation;
|
||||
param_t maxThrottle;
|
||||
} _paramHandle;
|
||||
|
||||
struct {
|
||||
float maxClimbRate;
|
||||
float maxVelocity;
|
||||
float maxRotation;
|
||||
float maxThrottle;
|
||||
} _params;
|
||||
|
||||
private:
|
||||
int _vehicleGlobalPositionSub; /**< notification of global position */
|
||||
int _sensorsCombinedSub;
|
||||
int _waypointSub;
|
||||
int _actuatorsSub;
|
||||
int _armingSub;
|
||||
int _parameterSub;
|
||||
|
||||
struct vehicle_global_position_s _vehicleGlobalPosition; /**< the result from global position subscription */
|
||||
struct sensor_combined_s _sensors; /**< subscribe to sensor readings */
|
||||
struct position_setpoint_triplet_s _waypoint; /**< subscribe to autopilot navigation */
|
||||
struct actuator_controls_s _actuators;
|
||||
struct actuator_armed_s _arming;
|
||||
|
||||
uint64_t _landTimer; /**< timestamp in microseconds since a possible land was detected*/
|
||||
};
|
||||
|
||||
#endif //__MULTICOPTER_LAND_DETECTOR_H__
|
||||
214
src/modules/land_detector/land_detector_main.cpp
Normal file
214
src/modules/land_detector/land_detector_main.cpp
Normal file
@ -0,0 +1,214 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2015 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 land_detector_main.cpp
|
||||
* Land detection algorithm
|
||||
*
|
||||
* @author Johan Jansen <jnsn.johan@gmail.com>
|
||||
*/
|
||||
|
||||
#include <unistd.h> //usleep
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <errno.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <systemlib/systemlib.h> //Scheduler
|
||||
#include <systemlib/err.h> //print to console
|
||||
|
||||
#include "FixedwingLandDetector.h"
|
||||
#include "MulticopterLandDetector.h"
|
||||
|
||||
//Function prototypes
|
||||
static int land_detector_start(const char *mode);
|
||||
static void land_detector_stop();
|
||||
|
||||
/**
|
||||
* land detector app start / stop handling function
|
||||
* This makes the land detector module accessible from the nuttx shell
|
||||
* @ingroup apps
|
||||
*/
|
||||
extern "C" __EXPORT int land_detector_main(int argc, char *argv[]);
|
||||
|
||||
//Private variables
|
||||
static LandDetector *land_detector_task = nullptr;
|
||||
static int _landDetectorTaskID = -1;
|
||||
static char _currentMode[12];
|
||||
|
||||
/**
|
||||
* Deamon thread function
|
||||
**/
|
||||
static void land_detector_deamon_thread(int argc, char *argv[])
|
||||
{
|
||||
land_detector_task->start();
|
||||
}
|
||||
|
||||
/**
|
||||
* Stop the task, force killing it if it doesn't stop by itself
|
||||
**/
|
||||
static void land_detector_stop()
|
||||
{
|
||||
if (land_detector_task == nullptr || _landDetectorTaskID == -1) {
|
||||
errx(1, "not running");
|
||||
return;
|
||||
}
|
||||
|
||||
land_detector_task->shutdown();
|
||||
|
||||
//Wait for task to die
|
||||
int i = 0;
|
||||
|
||||
do {
|
||||
/* wait 20ms */
|
||||
usleep(20000);
|
||||
|
||||
/* if we have given up, kill it */
|
||||
if (++i > 50) {
|
||||
task_delete(_landDetectorTaskID);
|
||||
break;
|
||||
}
|
||||
} while (land_detector_task->isRunning());
|
||||
|
||||
|
||||
delete land_detector_task;
|
||||
land_detector_task = nullptr;
|
||||
_landDetectorTaskID = -1;
|
||||
errx(0, "land_detector has been stopped");
|
||||
}
|
||||
|
||||
/**
|
||||
* Start new task, fails if it is already running. Returns OK if successful
|
||||
**/
|
||||
static int land_detector_start(const char *mode)
|
||||
{
|
||||
if (land_detector_task != nullptr || _landDetectorTaskID != -1) {
|
||||
errx(1, "already running");
|
||||
return -1;
|
||||
}
|
||||
|
||||
//Allocate memory
|
||||
if (!strcmp(mode, "fixedwing")) {
|
||||
land_detector_task = new FixedwingLandDetector();
|
||||
|
||||
} else if (!strcmp(mode, "multicopter")) {
|
||||
land_detector_task = new MulticopterLandDetector();
|
||||
|
||||
} else {
|
||||
errx(1, "[mode] must be either 'fixedwing' or 'multicopter'");
|
||||
return -1;
|
||||
}
|
||||
|
||||
//Check if alloc worked
|
||||
if (land_detector_task == nullptr) {
|
||||
errx(1, "alloc failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
//Start new thread task
|
||||
_landDetectorTaskID = task_spawn_cmd("land_detector",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_DEFAULT,
|
||||
1200,
|
||||
(main_t)&land_detector_deamon_thread,
|
||||
nullptr);
|
||||
|
||||
if (_landDetectorTaskID < 0) {
|
||||
errx(1, "task start failed: %d", -errno);
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* avoid memory fragmentation by not exiting start handler until the task has fully started */
|
||||
const uint32_t timeout = hrt_absolute_time() + 5000000; //5 second timeout
|
||||
|
||||
while (!land_detector_task->isRunning()) {
|
||||
usleep(50000);
|
||||
printf(".");
|
||||
fflush(stdout);
|
||||
|
||||
if (hrt_absolute_time() > timeout) {
|
||||
err(1, "start failed - timeout");
|
||||
land_detector_stop();
|
||||
exit(1);
|
||||
}
|
||||
}
|
||||
|
||||
printf("\n");
|
||||
|
||||
//Remember current active mode
|
||||
strncpy(_currentMode, mode, 12);
|
||||
|
||||
exit(0);
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Main entry point for this module
|
||||
**/
|
||||
int land_detector_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
if (argc < 1) {
|
||||
warnx("usage: land_detector {start|stop|status} [mode]\nmode can either be 'fixedwing' or 'multicopter'");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (argc >= 2 && !strcmp(argv[1], "start")) {
|
||||
land_detector_start(argv[2]);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
land_detector_stop();
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
if (land_detector_task) {
|
||||
|
||||
if (land_detector_task->isRunning()) {
|
||||
warnx("running (%s)", _currentMode);
|
||||
|
||||
} else {
|
||||
errx(1, "exists, but not running (%s)", _currentMode);
|
||||
}
|
||||
|
||||
exit(0);
|
||||
|
||||
} else {
|
||||
errx(1, "not running");
|
||||
}
|
||||
}
|
||||
|
||||
warn("usage: land_detector {start|stop|status} [mode]\nmode can either be 'fixedwing' or 'multicopter'");
|
||||
return 1;
|
||||
}
|
||||
104
src/modules/land_detector/land_detector_params.c
Normal file
104
src/modules/land_detector/land_detector_params.c
Normal file
@ -0,0 +1,104 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2014, 2015 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 land_detector.c
|
||||
* Land detector algorithm parameters.
|
||||
*
|
||||
* @author Johan Jansen <jnsn.johan@gmail.com>
|
||||
*/
|
||||
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
/**
|
||||
* Multicopter max climb rate
|
||||
*
|
||||
* Maximum vertical velocity allowed to trigger a land (m/s up and down)
|
||||
*
|
||||
* @group Land Detector
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.30f);
|
||||
|
||||
/**
|
||||
* Multicopter max horizontal velocity
|
||||
*
|
||||
* Maximum horizontal velocity allowed to trigger a land (m/s)
|
||||
*
|
||||
* @group Land Detector
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.00f);
|
||||
|
||||
/**
|
||||
* Multicopter max rotation
|
||||
*
|
||||
* Maximum allowed around each axis to trigger a land (radians per second)
|
||||
*
|
||||
* @group Land Detector
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 0.20f);
|
||||
|
||||
/**
|
||||
* Multicopter max throttle
|
||||
*
|
||||
* Maximum actuator output on throttle before triggering a land
|
||||
*
|
||||
* @group Land Detector
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.20f);
|
||||
|
||||
/**
|
||||
* Fixedwing max horizontal velocity
|
||||
*
|
||||
* Maximum horizontal velocity allowed to trigger a land (m/s)
|
||||
*
|
||||
* @group Land Detector
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 0.20f);
|
||||
|
||||
/**
|
||||
* Fixedwing max climb rate
|
||||
*
|
||||
* Maximum vertical velocity allowed to trigger a land (m/s up and down)
|
||||
*
|
||||
* @group Land Detector
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.00f);
|
||||
|
||||
/**
|
||||
* Airspeed max
|
||||
*
|
||||
* Maximum airspeed allowed to trigger a land (m/s)
|
||||
*
|
||||
* @group Land Detector
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(LNDFW_AIRSPD_MAX, 10.00f);
|
||||
13
src/modules/land_detector/module.mk
Normal file
13
src/modules/land_detector/module.mk
Normal file
@ -0,0 +1,13 @@
|
||||
#
|
||||
# Land detector
|
||||
#
|
||||
|
||||
MODULE_COMMAND = land_detector
|
||||
|
||||
SRCS = land_detector_main.cpp \
|
||||
land_detector_params.c \
|
||||
LandDetector.cpp \
|
||||
MulticopterLandDetector.cpp \
|
||||
FixedwingLandDetector.cpp
|
||||
|
||||
EXTRACXXFLAGS = -Weffc++ -Os
|
||||
@ -92,6 +92,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
|
||||
_mavlink(parent),
|
||||
status{},
|
||||
hil_local_pos{},
|
||||
hil_land_detector{},
|
||||
_control_mode{},
|
||||
_global_pos_pub(-1),
|
||||
_local_pos_pub(-1),
|
||||
@ -118,6 +119,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
|
||||
_telemetry_status_pub(-1),
|
||||
_rc_pub(-1),
|
||||
_manual_pub(-1),
|
||||
_land_detector_pub(-1),
|
||||
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
|
||||
_hil_frames(0),
|
||||
_old_timestamp(0),
|
||||
@ -1353,9 +1355,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
|
||||
hil_local_pos.xy_global = true;
|
||||
hil_local_pos.z_global = true;
|
||||
|
||||
bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve?
|
||||
hil_local_pos.landed = landed;
|
||||
|
||||
if (_local_pos_pub < 0) {
|
||||
_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
|
||||
|
||||
@ -1364,6 +1363,22 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
|
||||
}
|
||||
}
|
||||
|
||||
/* land detector */
|
||||
{
|
||||
bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve?
|
||||
if(hil_land_detector.landed != landed) {
|
||||
hil_land_detector.landed = landed;
|
||||
hil_land_detector.timestamp = hrt_absolute_time();
|
||||
|
||||
if (_land_detector_pub < 0) {
|
||||
_land_detector_pub = orb_advertise(ORB_ID(vehicle_land_detected), &hil_land_detector);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(vehicle_land_detected), _land_detector_pub, &hil_land_detector);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* accelerometer */
|
||||
{
|
||||
struct accel_report accel;
|
||||
|
||||
@ -50,6 +50,7 @@
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
#include <uORB/topics/home_position.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/offboard_control_setpoint.h>
|
||||
@ -145,6 +146,7 @@ private:
|
||||
|
||||
mavlink_status_t status;
|
||||
struct vehicle_local_position_s hil_local_pos;
|
||||
struct vehicle_land_detected_s hil_land_detector;
|
||||
struct vehicle_control_mode_s _control_mode;
|
||||
orb_advert_t _global_pos_pub;
|
||||
orb_advert_t _local_pos_pub;
|
||||
@ -171,6 +173,7 @@ private:
|
||||
orb_advert_t _telemetry_status_pub;
|
||||
orb_advert_t _rc_pub;
|
||||
orb_advert_t _manual_pub;
|
||||
orb_advert_t _land_detector_pub;
|
||||
int _control_mode_sub;
|
||||
int _hil_frames;
|
||||
uint64_t _old_timestamp;
|
||||
|
||||
@ -247,9 +247,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
||||
float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted
|
||||
float surface_offset = 0.0f; // ground level offset from reference altitude
|
||||
float surface_offset_rate = 0.0f; // surface offset change rate
|
||||
float alt_avg = 0.0f;
|
||||
bool landed = true;
|
||||
hrt_abstime landed_time = 0;
|
||||
|
||||
hrt_abstime accel_timestamp = 0;
|
||||
hrt_abstime baro_timestamp = 0;
|
||||
@ -1069,36 +1066,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
||||
inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v);
|
||||
}
|
||||
|
||||
/* detect land */
|
||||
alt_avg += (- z_est[0] - alt_avg) * dt / params.land_t;
|
||||
float alt_disp2 = - z_est[0] - alt_avg;
|
||||
alt_disp2 = alt_disp2 * alt_disp2;
|
||||
float land_disp2 = params.land_disp * params.land_disp;
|
||||
/* get actual thrust output */
|
||||
float thrust = armed.armed ? actuator.control[3] : 0.0f;
|
||||
|
||||
if (landed) {
|
||||
if (alt_disp2 > land_disp2 || thrust > params.land_thr) {
|
||||
landed = false;
|
||||
landed_time = 0;
|
||||
}
|
||||
} else {
|
||||
if (alt_disp2 < land_disp2 && thrust < params.land_thr) {
|
||||
if (landed_time == 0) {
|
||||
landed_time = t; // land detected first time
|
||||
|
||||
} else {
|
||||
if (t > landed_time + params.land_t * 1000000.0f) {
|
||||
landed = true;
|
||||
landed_time = 0;
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
landed_time = 0;
|
||||
}
|
||||
}
|
||||
|
||||
if (verbose_mode) {
|
||||
/* print updates rate */
|
||||
if (t > updates_counter_start + updates_counter_len) {
|
||||
@ -1149,7 +1116,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
|
||||
local_pos.vy = y_est[1];
|
||||
local_pos.z = z_est[0];
|
||||
local_pos.vz = z_est[1];
|
||||
local_pos.landed = landed;
|
||||
local_pos.yaw = att.yaw;
|
||||
local_pos.dist_bottom_valid = dist_bottom_valid;
|
||||
local_pos.eph = eph;
|
||||
|
||||
@ -454,6 +454,10 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
|
||||
{
|
||||
bool result = false;
|
||||
|
||||
if (!(num_values) || !(values) || !(frame_len)) {
|
||||
return result;
|
||||
}
|
||||
|
||||
/* avoid racing with PPM updates */
|
||||
irqstate_t state = irqsave();
|
||||
|
||||
@ -468,7 +472,7 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
|
||||
if (*num_values > PX4IO_RC_INPUT_CHANNELS)
|
||||
*num_values = PX4IO_RC_INPUT_CHANNELS;
|
||||
|
||||
for (unsigned i = 0; i < *num_values; i++) {
|
||||
for (unsigned i = 0; ((i < *num_values) && (i < PPM_MAX_CHANNELS)); i++) {
|
||||
values[i] = ppm_buffer[i];
|
||||
}
|
||||
|
||||
@ -476,8 +480,7 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
|
||||
ppm_last_valid_decode = 0;
|
||||
|
||||
/* store PPM frame length */
|
||||
if (num_values)
|
||||
*frame_len = ppm_frame_length;
|
||||
*frame_len = ppm_frame_length;
|
||||
|
||||
/* good if we got any channels */
|
||||
result = (*num_values > 0);
|
||||
|
||||
@ -94,6 +94,7 @@
|
||||
#include <uORB/topics/servorail_status.h>
|
||||
#include <uORB/topics/wind_estimate.h>
|
||||
#include <uORB/topics/encoders.h>
|
||||
#include <uORB/topics/vtol_vehicle_status.h>
|
||||
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <systemlib/param/param.h>
|
||||
@ -1000,6 +1001,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
struct satellite_info_s sat_info;
|
||||
struct wind_estimate_s wind_estimate;
|
||||
struct encoders_s encoders;
|
||||
struct vtol_vehicle_status_s vtol_status;
|
||||
} buf;
|
||||
|
||||
memset(&buf, 0, sizeof(buf));
|
||||
@ -1019,6 +1021,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
struct log_GPS_s log_GPS;
|
||||
struct log_ATTC_s log_ATTC;
|
||||
struct log_STAT_s log_STAT;
|
||||
struct log_VTOL_s log_VTOL;
|
||||
struct log_RC_s log_RC;
|
||||
struct log_OUT0_s log_OUT0;
|
||||
struct log_AIRS_s log_AIRS;
|
||||
@ -1053,6 +1056,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
struct {
|
||||
int cmd_sub;
|
||||
int status_sub;
|
||||
int vtol_status_sub;
|
||||
int sensor_sub;
|
||||
int att_sub;
|
||||
int att_sp_sub;
|
||||
@ -1086,6 +1090,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
|
||||
subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
|
||||
subs.status_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
subs.vtol_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status));
|
||||
subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
|
||||
subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
|
||||
subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
|
||||
@ -1112,6 +1117,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
subs.system_power_sub = orb_subscribe(ORB_ID(system_power));
|
||||
subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status));
|
||||
subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate));
|
||||
|
||||
/* we need to rate-limit wind, as we do not need the full update rate */
|
||||
orb_set_interval(subs.wind_sub, 90);
|
||||
subs.encoders_sub = orb_subscribe(ORB_ID(encoders));
|
||||
@ -1218,6 +1224,13 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
LOGBUFFER_WRITE_AND_COUNT(STAT);
|
||||
}
|
||||
|
||||
/* --- VTOL VEHICLE STATUS --- */
|
||||
if(copy_if_updated(ORB_ID(vtol_vehicle_status), subs.vtol_status_sub, &buf.vtol_status)) {
|
||||
log_msg.msg_type = LOG_VTOL_MSG;
|
||||
log_msg.body.log_VTOL.airspeed_tot = buf.vtol_status.airspeed_tot;
|
||||
LOGBUFFER_WRITE_AND_COUNT(VTOL);
|
||||
}
|
||||
|
||||
/* --- GPS POSITION - UNIT #1 --- */
|
||||
if (gps_pos_updated) {
|
||||
|
||||
@ -1424,6 +1437,10 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
/* --- ATTITUDE --- */
|
||||
if (copy_if_updated(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att)) {
|
||||
log_msg.msg_type = LOG_ATT_MSG;
|
||||
log_msg.body.log_ATT.q_w = buf.att.q[0];
|
||||
log_msg.body.log_ATT.q_x = buf.att.q[1];
|
||||
log_msg.body.log_ATT.q_y = buf.att.q[2];
|
||||
log_msg.body.log_ATT.q_z = buf.att.q[3];
|
||||
log_msg.body.log_ATT.roll = buf.att.roll;
|
||||
log_msg.body.log_ATT.pitch = buf.att.pitch;
|
||||
log_msg.body.log_ATT.yaw = buf.att.yaw;
|
||||
@ -1434,18 +1451,6 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
log_msg.body.log_ATT.gy = buf.att.g_comp[1];
|
||||
log_msg.body.log_ATT.gz = buf.att.g_comp[2];
|
||||
LOGBUFFER_WRITE_AND_COUNT(ATT);
|
||||
// secondary attitude
|
||||
log_msg.msg_type = LOG_ATT2_MSG;
|
||||
log_msg.body.log_ATT.roll = buf.att.roll_sec;
|
||||
log_msg.body.log_ATT.pitch = buf.att.pitch_sec;
|
||||
log_msg.body.log_ATT.yaw = buf.att.yaw_sec;
|
||||
log_msg.body.log_ATT.roll_rate = buf.att.rollspeed_sec;
|
||||
log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed_sec;
|
||||
log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed_sec;
|
||||
log_msg.body.log_ATT.gx = buf.att.g_comp_sec[0];
|
||||
log_msg.body.log_ATT.gy = buf.att.g_comp_sec[1];
|
||||
log_msg.body.log_ATT.gz = buf.att.g_comp_sec[2];
|
||||
LOGBUFFER_WRITE_AND_COUNT(ATT);
|
||||
}
|
||||
|
||||
/* --- ATTITUDE SETPOINT --- */
|
||||
@ -1514,7 +1519,6 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
(buf.local_pos.v_z_valid ? 8 : 0) |
|
||||
(buf.local_pos.xy_global ? 16 : 0) |
|
||||
(buf.local_pos.z_global ? 32 : 0);
|
||||
log_msg.body.log_LPOS.landed = buf.local_pos.landed;
|
||||
log_msg.body.log_LPOS.ground_dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0);
|
||||
log_msg.body.log_LPOS.eph = buf.local_pos.eph;
|
||||
log_msg.body.log_LPOS.epv = buf.local_pos.epv;
|
||||
|
||||
@ -50,8 +50,11 @@
|
||||
#pragma pack(push, 1)
|
||||
/* --- ATT - ATTITUDE --- */
|
||||
#define LOG_ATT_MSG 2
|
||||
#define LOG_ATT2_MSG 41
|
||||
struct log_ATT_s {
|
||||
float q_w;
|
||||
float q_x;
|
||||
float q_y;
|
||||
float q_z;
|
||||
float roll;
|
||||
float pitch;
|
||||
float yaw;
|
||||
@ -113,7 +116,6 @@ struct log_LPOS_s {
|
||||
int32_t ref_lon;
|
||||
float ref_alt;
|
||||
uint8_t pos_flags;
|
||||
uint8_t landed;
|
||||
uint8_t ground_dist_flags;
|
||||
float eph;
|
||||
float epv;
|
||||
@ -427,6 +429,12 @@ struct log_ENCD_s {
|
||||
/* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */
|
||||
#define LOG_AIR1_MSG 40
|
||||
|
||||
/* --- VTOL - VTOL VEHICLE STATUS */
|
||||
#define LOG_VTOL_MSG 42
|
||||
struct log_VTOL_s {
|
||||
float airspeed_tot;
|
||||
};
|
||||
|
||||
/********** SYSTEM MESSAGES, ID > 0x80 **********/
|
||||
|
||||
/* --- TIME - TIME STAMP --- */
|
||||
@ -455,19 +463,20 @@ struct log_PARM_s {
|
||||
static const struct log_format_s log_formats[] = {
|
||||
/* business-level messages, ID < 0x80 */
|
||||
LOG_FORMAT_S(ATT, ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
|
||||
LOG_FORMAT_S(ATT2, ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
|
||||
LOG_FORMAT(ATT, "fffffffffffff", "qw,qx,qy,qz,Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
|
||||
LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
|
||||
LOG_FORMAT_S(IMU, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
|
||||
LOG_FORMAT_S(IMU1, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
|
||||
LOG_FORMAT_S(IMU2, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
|
||||
LOG_FORMAT_S(SENS, SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"),
|
||||
LOG_FORMAT_S(AIR1, SENS, "fffff", "BaroPa,BaroAlt,BaroTmp,DiffPres,DiffPresF"),
|
||||
LOG_FORMAT(LPOS, "ffffffffLLfBBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"),
|
||||
LOG_FORMAT(LPOS, "ffffffffLLfBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,GFlg,EPH,EPV"),
|
||||
LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
|
||||
LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
|
||||
LOG_FORMAT_S(ATTC, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
|
||||
LOG_FORMAT_S(ATC1, ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
|
||||
LOG_FORMAT(STAT, "BBBfBB", "MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"),
|
||||
LOG_FORMAT(VTOL, "f", "Arsp"),
|
||||
LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"),
|
||||
LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
|
||||
LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"),
|
||||
|
||||
@ -98,6 +98,12 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f);
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f);
|
||||
|
||||
/**
|
||||
* ID of Magnetometer the calibration is for.
|
||||
*
|
||||
* @group Sensor Calibration
|
||||
*/
|
||||
PARAM_DEFINE_INT32(SENS_MAG_ID, 0);
|
||||
|
||||
/**
|
||||
* Magnetometer X-axis offset
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2012-2015 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
|
||||
@ -1465,7 +1465,7 @@ Sensors::parameter_update_poll(bool forced)
|
||||
|
||||
/* this sensor is optional, abort without error */
|
||||
|
||||
if (fd > 0) {
|
||||
if (fd >= 0) {
|
||||
struct airspeed_scale airscale = {
|
||||
_parameters.diff_pres_offset_pa,
|
||||
1.0f,
|
||||
|
||||
@ -154,6 +154,7 @@ warn(const char *fmt, ...)
|
||||
|
||||
va_start(args, fmt);
|
||||
vwarn(fmt, args);
|
||||
va_end(args);
|
||||
}
|
||||
|
||||
void
|
||||
@ -169,6 +170,7 @@ warnc(int errcode, const char *fmt, ...)
|
||||
|
||||
va_start(args, fmt);
|
||||
vwarnc(errcode, fmt, args);
|
||||
va_end(args);
|
||||
}
|
||||
|
||||
void
|
||||
@ -184,6 +186,7 @@ warnx(const char *fmt, ...)
|
||||
|
||||
va_start(args, fmt);
|
||||
vwarnx(fmt, args);
|
||||
va_end(args);
|
||||
}
|
||||
|
||||
void
|
||||
|
||||
@ -83,6 +83,9 @@ ORB_DEFINE(sensor_combined, struct sensor_combined_s);
|
||||
#include "topics/vehicle_gps_position.h"
|
||||
ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s);
|
||||
|
||||
#include "topics/vehicle_land_detected.h"
|
||||
ORB_DEFINE(vehicle_land_detected, struct vehicle_land_detected_s);
|
||||
|
||||
#include "topics/satellite_info.h"
|
||||
ORB_DEFINE(satellite_info, struct satellite_info_s);
|
||||
|
||||
|
||||
63
src/modules/uORB/topics/vehicle_land_detected.h
Normal file
63
src/modules/uORB/topics/vehicle_land_detected.h
Normal file
@ -0,0 +1,63 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2015 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 vehicle_land_detected.h
|
||||
* Land detected uORB topic
|
||||
*
|
||||
* @author Johan Jansen <jnsn.johan@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef __TOPIC_VEHICLE_LANDED_H__
|
||||
#define __TOPIC_VEHICLE_LANDED_H__
|
||||
|
||||
#include "../uORB.h"
|
||||
|
||||
/**
|
||||
* @addtogroup topics
|
||||
* @{
|
||||
*/
|
||||
|
||||
struct vehicle_land_detected_s {
|
||||
uint64_t timestamp; /**< timestamp of the setpoint */
|
||||
bool landed; /**< true if vehicle is currently landed on the ground*/
|
||||
};
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/* register this as object request broker structure */
|
||||
ORB_DECLARE(vehicle_land_detected);
|
||||
|
||||
#endif //__TOPIC_VEHICLE_LANDED_H__
|
||||
@ -77,7 +77,6 @@ struct vehicle_local_position_s {
|
||||
double ref_lat; /**< Reference point latitude in degrees */
|
||||
double ref_lon; /**< Reference point longitude in degrees */
|
||||
float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */
|
||||
bool landed; /**< true if vehicle is landed */
|
||||
/* Distance to surface */
|
||||
float dist_bottom; /**< Distance to bottom surface (ground) */
|
||||
float dist_bottom_rate; /**< Distance to bottom surface (ground) change rate */
|
||||
|
||||
@ -55,6 +55,7 @@ struct vtol_vehicle_status_s {
|
||||
uint64_t timestamp; /**< Microseconds since system boot */
|
||||
bool vtol_in_rw_mode; /*true: vtol vehicle is in rotating wing mode */
|
||||
bool fw_permanent_stab; /**< In fw mode stabilize attitude even if in manual mode*/
|
||||
float airspeed_tot; /*< Estimated airspeed over control surfaces */
|
||||
};
|
||||
|
||||
/**
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Copyright (Cc) 2012-2015 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
|
||||
@ -230,7 +230,7 @@ ORBDevNode::open(struct file *filp)
|
||||
ret = CDev::open(filp);
|
||||
|
||||
if (ret != OK)
|
||||
free(sd);
|
||||
delete sd;
|
||||
|
||||
return ret;
|
||||
}
|
||||
@ -817,18 +817,6 @@ uorb_main(int argc, char *argv[])
|
||||
namespace
|
||||
{
|
||||
|
||||
void debug(const char *fmt, ...)
|
||||
{
|
||||
va_list ap;
|
||||
|
||||
va_start(ap, fmt);
|
||||
vfprintf(stderr, fmt, ap);
|
||||
va_end(ap);
|
||||
fprintf(stderr, "\n");
|
||||
fflush(stderr);
|
||||
usleep(100000);
|
||||
}
|
||||
|
||||
/**
|
||||
* Advertise a node; don't consider it an error if the node has
|
||||
* already been advertised.
|
||||
|
||||
@ -65,6 +65,8 @@
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
@ -105,7 +107,9 @@ private:
|
||||
int _params_sub; //parameter updates subscription
|
||||
int _manual_control_sp_sub; //manual control setpoint subscription
|
||||
int _armed_sub; //arming status subscription
|
||||
int _local_pos_sub; // sensor subscription
|
||||
int _airspeed_sub; // airspeed subscription
|
||||
int _battery_status_sub; // battery status subscription
|
||||
|
||||
int _actuator_inputs_mc; //topic on which the mc_att_controller publishes actuator inputs
|
||||
int _actuator_inputs_fw; //topic on which the fw_att_controller publishes actuator inputs
|
||||
@ -129,7 +133,9 @@ private:
|
||||
struct actuator_controls_s _actuators_mc_in; //actuator controls from mc_att_control
|
||||
struct actuator_controls_s _actuators_fw_in; //actuator controls from fw_att_control
|
||||
struct actuator_armed_s _armed; //actuator arming status
|
||||
struct vehicle_local_position_s _local_pos;
|
||||
struct airspeed_s _airspeed; // airspeed
|
||||
struct battery_status_s _batt_status; // battery status
|
||||
|
||||
struct {
|
||||
param_t idle_pwm_mc; //pwm value for idle in mc mode
|
||||
@ -139,6 +145,9 @@ private:
|
||||
float mc_airspeed_trim; // trim airspeed in multicopter mode
|
||||
float mc_airspeed_max; // max airpseed in multicopter mode
|
||||
float fw_pitch_trim; // trim for neutral elevon position in fw mode
|
||||
float power_max; // maximum power of one engine
|
||||
float prop_eff; // factor to calculate prop efficiency
|
||||
float arsp_lp_gain; // total airspeed estimate low pass gain
|
||||
} _params;
|
||||
|
||||
struct {
|
||||
@ -149,6 +158,9 @@ private:
|
||||
param_t mc_airspeed_trim;
|
||||
param_t mc_airspeed_max;
|
||||
param_t fw_pitch_trim;
|
||||
param_t power_max;
|
||||
param_t prop_eff;
|
||||
param_t arsp_lp_gain;
|
||||
} _params_handles;
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
@ -159,6 +171,7 @@ private:
|
||||
* to waste energy when gliding. */
|
||||
bool flag_idle_mc; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode"
|
||||
unsigned _motor_count; // number of motors
|
||||
float _airspeed_tot;
|
||||
|
||||
//*****************Member functions***********************************************************************
|
||||
|
||||
@ -172,7 +185,9 @@ private:
|
||||
void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output
|
||||
void vehicle_rates_sp_mc_poll();
|
||||
void vehicle_rates_sp_fw_poll();
|
||||
void vehicle_local_pos_poll(); // Check for changes in sensor values
|
||||
void vehicle_airspeed_poll(); // Check for changes in airspeed
|
||||
void vehicle_battery_poll(); // Check for battery updates
|
||||
void parameters_update_poll(); //Check if parameters have changed
|
||||
int parameters_update(); //Update local paraemter cache
|
||||
void fill_mc_att_control_output(); //write mc_att_control results to actuator message
|
||||
@ -182,6 +197,7 @@ private:
|
||||
void set_idle_fw();
|
||||
void set_idle_mc();
|
||||
void scale_mc_output();
|
||||
void calc_tot_airspeed(); // estimated airspeed seen by elevons
|
||||
};
|
||||
|
||||
namespace VTOL_att_control
|
||||
@ -205,7 +221,9 @@ VtolAttitudeControl::VtolAttitudeControl() :
|
||||
_params_sub(-1),
|
||||
_manual_control_sp_sub(-1),
|
||||
_armed_sub(-1),
|
||||
_local_pos_sub(-1),
|
||||
_airspeed_sub(-1),
|
||||
_battery_status_sub(-1),
|
||||
|
||||
//init publication handlers
|
||||
_actuators_0_pub(-1),
|
||||
@ -218,6 +236,7 @@ VtolAttitudeControl::VtolAttitudeControl() :
|
||||
{
|
||||
|
||||
flag_idle_mc = true;
|
||||
_airspeed_tot = 0.0f;
|
||||
|
||||
memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status));
|
||||
_vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/
|
||||
@ -234,7 +253,9 @@ VtolAttitudeControl::VtolAttitudeControl() :
|
||||
memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in));
|
||||
memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in));
|
||||
memset(&_armed, 0, sizeof(_armed));
|
||||
memset(&_local_pos,0,sizeof(_local_pos));
|
||||
memset(&_airspeed,0,sizeof(_airspeed));
|
||||
memset(&_batt_status,0,sizeof(_batt_status));
|
||||
|
||||
_params.idle_pwm_mc = PWM_LOWEST_MIN;
|
||||
_params.vtol_motor_count = 0;
|
||||
@ -247,6 +268,9 @@ VtolAttitudeControl::VtolAttitudeControl() :
|
||||
_params_handles.mc_airspeed_max = param_find("VT_MC_ARSPD_MAX");
|
||||
_params_handles.mc_airspeed_trim = param_find("VT_MC_ARSPD_TRIM");
|
||||
_params_handles.fw_pitch_trim = param_find("VT_FW_PITCH_TRIM");
|
||||
_params_handles.power_max = param_find("VT_POWER_MAX");
|
||||
_params_handles.prop_eff = param_find("VT_PROP_EFF");
|
||||
_params_handles.arsp_lp_gain = param_find("VT_ARSP_LP_GAIN");
|
||||
|
||||
/* fetch initial parameter values */
|
||||
parameters_update();
|
||||
@ -387,6 +411,19 @@ VtolAttitudeControl::vehicle_airspeed_poll() {
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Check for battery updates.
|
||||
*/
|
||||
void
|
||||
VtolAttitudeControl::vehicle_battery_poll() {
|
||||
bool updated;
|
||||
orb_check(_battery_status_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(battery_status), _battery_status_sub , &_batt_status);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Check for parameter updates.
|
||||
*/
|
||||
@ -405,6 +442,22 @@ VtolAttitudeControl::parameters_update_poll()
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Check for sensor updates.
|
||||
*/
|
||||
void
|
||||
VtolAttitudeControl::vehicle_local_pos_poll()
|
||||
{
|
||||
bool updated;
|
||||
/* Check if parameters have changed */
|
||||
orb_check(_local_pos_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub , &_local_pos);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Update parameters.
|
||||
*/
|
||||
@ -437,6 +490,18 @@ VtolAttitudeControl::parameters_update()
|
||||
param_get(_params_handles.fw_pitch_trim, &v);
|
||||
_params.fw_pitch_trim = v;
|
||||
|
||||
/* vtol maximum power engine can produce */
|
||||
param_get(_params_handles.power_max, &v);
|
||||
_params.power_max = v;
|
||||
|
||||
/* vtol propeller efficiency factor */
|
||||
param_get(_params_handles.prop_eff, &v);
|
||||
_params.prop_eff = v;
|
||||
|
||||
/* vtol total airspeed estimate low pass gain */
|
||||
param_get(_params_handles.arsp_lp_gain, &v);
|
||||
_params.arsp_lp_gain = v;
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
@ -555,7 +620,7 @@ void
|
||||
VtolAttitudeControl::scale_mc_output() {
|
||||
// scale around tuning airspeed
|
||||
float airspeed;
|
||||
|
||||
calc_tot_airspeed(); // estimate air velocity seen by elevons
|
||||
// if airspeed is not updating, we assume the normal average speed
|
||||
if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) ||
|
||||
hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
|
||||
@ -563,16 +628,12 @@ VtolAttitudeControl::scale_mc_output() {
|
||||
if (nonfinite) {
|
||||
perf_count(_nonfinite_input_perf);
|
||||
}
|
||||
} else {
|
||||
// prevent numerical drama by requiring 0.5 m/s minimal speed
|
||||
airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s);
|
||||
}
|
||||
|
||||
// Sanity check if airspeed is consistent with throttle
|
||||
if(_manual_control_sp.z >= 0.35f && airspeed < _params.mc_airspeed_trim) { // XXX magic number, should be hover throttle param
|
||||
airspeed = _params.mc_airspeed_trim;
|
||||
} else {
|
||||
airspeed = _airspeed_tot;
|
||||
airspeed = math::constrain(airspeed,_params.mc_airspeed_min, _params.mc_airspeed_max);
|
||||
}
|
||||
|
||||
_vtol_vehicle_status.airspeed_tot = airspeed; // save value for logging
|
||||
/*
|
||||
* For scaling our actuators using anything less than the min (close to stall)
|
||||
* speed doesn't make any sense - its the strongest reasonable deflection we
|
||||
@ -584,6 +645,23 @@ VtolAttitudeControl::scale_mc_output() {
|
||||
_actuators_mc_in.control[1] = math::constrain(_actuators_mc_in.control[1]*airspeed_scaling*airspeed_scaling,-1.0f,1.0f);
|
||||
}
|
||||
|
||||
void VtolAttitudeControl::calc_tot_airspeed() {
|
||||
float airspeed = math::max(1.0f, _airspeed.true_airspeed_m_s); // prevent numerical drama
|
||||
// calculate momentary power of one engine
|
||||
float P = _batt_status.voltage_filtered_v * _batt_status.current_a / _params.vtol_motor_count;
|
||||
P = math::constrain(P,1.0f,_params.power_max);
|
||||
// calculate prop efficiency
|
||||
float power_factor = 1.0f - P*_params.prop_eff/_params.power_max;
|
||||
float eta = (1.0f/(1 + expf(-0.4f * power_factor * airspeed)) - 0.5f)*2.0f;
|
||||
eta = math::constrain(eta,0.001f,1.0f); // live on the safe side
|
||||
// calculate induced airspeed by propeller
|
||||
float v_ind = (airspeed/eta - airspeed)*2.0f;
|
||||
// calculate total airspeed
|
||||
float airspeed_raw = airspeed + v_ind;
|
||||
// apply low-pass filter
|
||||
_airspeed_tot = _params.arsp_lp_gain * (_airspeed_tot - airspeed_raw) + airspeed_raw;
|
||||
}
|
||||
|
||||
void
|
||||
VtolAttitudeControl::task_main_trampoline(int argc, char *argv[])
|
||||
{
|
||||
@ -604,7 +682,9 @@ void VtolAttitudeControl::task_main()
|
||||
_params_sub = orb_subscribe(ORB_ID(parameter_update));
|
||||
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
|
||||
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
|
||||
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
|
||||
_battery_status_sub = orb_subscribe(ORB_ID(battery_status));
|
||||
|
||||
_actuator_inputs_mc = orb_subscribe(ORB_ID(actuator_controls_virtual_mc));
|
||||
_actuator_inputs_fw = orb_subscribe(ORB_ID(actuator_controls_virtual_fw));
|
||||
@ -674,7 +754,10 @@ void VtolAttitudeControl::task_main()
|
||||
vehicle_rates_sp_mc_poll();
|
||||
vehicle_rates_sp_fw_poll();
|
||||
parameters_update_poll();
|
||||
vehicle_local_pos_poll(); // Check for new sensor values
|
||||
vehicle_airspeed_poll();
|
||||
vehicle_battery_poll();
|
||||
|
||||
|
||||
if (_manual_control_sp.aux1 <= 0.0f) { /* vehicle is in mc mode */
|
||||
_vtol_vehicle_status.vtol_in_rw_mode = true;
|
||||
@ -688,7 +771,8 @@ void VtolAttitudeControl::task_main()
|
||||
if (fds[0].revents & POLLIN) {
|
||||
vehicle_manual_poll(); /* update remote input */
|
||||
orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in);
|
||||
// scale pitch control with airspeed
|
||||
|
||||
// scale pitch control with total airspeed
|
||||
scale_mc_output();
|
||||
|
||||
fill_mc_att_control_output();
|
||||
|
||||
@ -64,7 +64,7 @@ PARAM_DEFINE_INT32(VT_IDLE_PWM_MC,900);
|
||||
* @min 0.0
|
||||
* @group VTOL Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN,2.0f);
|
||||
PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN,10.0f);
|
||||
|
||||
/**
|
||||
* Maximum airspeed in multicopter mode
|
||||
@ -109,3 +109,36 @@ PARAM_DEFINE_INT32(VT_FW_PERM_STAB,0);
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(VT_FW_PITCH_TRIM,0.0f);
|
||||
|
||||
/**
|
||||
* Motor max power
|
||||
*
|
||||
* Indicates the maximum power the motor is able to produce. Used to calculate
|
||||
* propeller efficiency map.
|
||||
*
|
||||
* @min 1
|
||||
* @group VTOL Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(VT_POWER_MAX,120.0f);
|
||||
|
||||
/**
|
||||
* Propeller efficiency parameter
|
||||
*
|
||||
* Influences propeller efficiency at different power settings. Should be tuned beforehand.
|
||||
*
|
||||
* @min 0.5
|
||||
* @max 0.9
|
||||
* @group VTOL Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(VT_PROP_EFF,0.0f);
|
||||
|
||||
/**
|
||||
* Total airspeed estimate low-pass filter gain
|
||||
*
|
||||
* Gain for tuning the low-pass filter for the total airspeed estimate
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 0.99
|
||||
* @group VTOL Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN,0.3f);
|
||||
|
||||
|
||||
@ -58,6 +58,7 @@
|
||||
|
||||
#include "systemlib/systemlib.h"
|
||||
#include "systemlib/err.h"
|
||||
#include "systemlib/param/param.h"
|
||||
|
||||
__EXPORT int config_main(int argc, char *argv[]);
|
||||
|
||||
@ -264,8 +265,11 @@ do_mag(int argc, char *argv[])
|
||||
int srate = ioctl(fd, MAGIOCGSAMPLERATE, 0);
|
||||
int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
|
||||
int range = ioctl(fd, MAGIOCGRANGE, 0);
|
||||
int id = ioctl(fd, DEVIOCGDEVICEID,0);
|
||||
int32_t calibration_id = 0;
|
||||
param_get(param_find("SENS_MAG_ID"), &(calibration_id));
|
||||
|
||||
warnx("mag: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", srate, prate, range);
|
||||
warnx("mag: \n\tdevice id:\t%x\t(calibration is for device id %x)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", id, calibration_id, srate, prate, range);
|
||||
|
||||
close(fd);
|
||||
}
|
||||
|
||||
@ -84,6 +84,7 @@ int preflight_check_main(int argc, char *argv[])
|
||||
/* open text message output path */
|
||||
int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
|
||||
int ret;
|
||||
int32_t mag_devid,mag_calibration_devid;
|
||||
|
||||
/* give the system some time to sample the sensors in the background */
|
||||
usleep(150000);
|
||||
@ -96,6 +97,16 @@ int preflight_check_main(int argc, char *argv[])
|
||||
system_ok = false;
|
||||
goto system_eval;
|
||||
}
|
||||
|
||||
mag_devid = ioctl(fd, DEVIOCGDEVICEID,0);
|
||||
param_get(param_find("SENS_MAG_ID"), &(mag_calibration_devid));
|
||||
if (mag_devid != mag_calibration_devid){
|
||||
warnx("magnetometer calibration is for a different device - calibrate magnetometer first");
|
||||
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CAL ID");
|
||||
system_ok = false;
|
||||
goto system_eval;
|
||||
}
|
||||
|
||||
ret = ioctl(fd, MAGIOCSELFTEST, 0);
|
||||
|
||||
if (ret != OK) {
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user