fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 07:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge master into attitude EKF update

Browse Source
This commit is contained in:
Lorenz Meier
2014-12-02 15:12:11 +01:00
parent 58aabe648a 8d1a0700f2
commit d5debc2f42
293 changed files with 14722 additions and 3032 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/airspeed/airspeed.cpp
+7 -5
View File
@@ -147,7 +147,7 @@ Airspeed::init()
_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp);
if (_airspeed_pub < 0)
warnx("failed to create airspeed sensor object. uORB started?");
warnx("uORB started?");
}
ret = OK;
@@ -159,13 +159,15 @@ out:
int
Airspeed::probe()
{
/* on initial power up the device needs more than one retry
for detection. Once it is running then retries aren't
needed
/* on initial power up the device may need more than one retry
for detection. Once it is running the number of retries can
be reduced
*/
_retries = 4;
int ret = measure();
_retries = 0;
// drop back to 2 retries once initialised
_retries = 2;
return ret;
}
src/drivers/ardrone_interface/ardrone_interface.c
+14 -22
View File
@@ -89,8 +89,8 @@ static void
usage(const char *reason)
{
if (reason)
fprintf(stderr, "%s\n", reason);
fprintf(stderr, "usage: ardrone_interface {start|stop|status} [-d <UART>]\n\n");
warnx("%s\n", reason);
warnx("usage: {start|stop|status} [-d <UART>]\n\n");
exit(1);
}
@@ -110,7 +110,7 @@ int ardrone_interface_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("ardrone_interface already running\n");
warnx("already running\n");
/* this is not an error */
exit(0);
}
@@ -132,9 +132,9 @@ int ardrone_interface_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\tardrone_interface is running\n");
warnx("running");
} else {
printf("\tardrone_interface not started\n");
warnx("not started");
}
exit(0);
}
@@ -158,7 +158,7 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin
/* Back up the original uart configuration to restore it after exit */
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
fprintf(stderr, "[ardrone_interface] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
warnx("ERR: TCGETATTR %s: %d", uart_name, termios_state);
close(uart);
return -1;
}
@@ -171,14 +171,14 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin
/* Set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
warnx("ERR: cfsetispeed %s: %d", uart_name, termios_state);
close(uart);
return -1;
}
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
warnx("ERR: tcsetattr: %s", uart_name);
close(uart);
return -1;
}
@@ -192,9 +192,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
char *device = "/dev/ttyS1";
/* welcome user */
printf("[ardrone_interface] Control started, taking over motors\n");
/* File descriptors */
int gpios;
@@ -237,7 +234,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
struct termios uart_config_original;
if (motor_test_mode) {
printf("[ardrone_interface] Motor test mode enabled, setting 10 %% thrust.\n");
warnx("setting 10 %% thrust.\n");
}
/* Led animation */
@@ -255,9 +252,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
printf("[ardrone_interface] Motors initialized - ready.\n");
fflush(stdout);
/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
ardrone_write = ardrone_open_uart(device, &uart_config_original);
@@ -265,7 +259,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
gpios = ar_multiplexing_init();
if (ardrone_write < 0) {
fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
warnx("No UART, exiting.");
thread_running = false;
exit(ERROR);
}
@@ -273,7 +267,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
/* initialize motors */
if (OK != ar_init_motors(ardrone_write, gpios)) {
close(ardrone_write);
fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
warnx("motor init fail");
thread_running = false;
exit(ERROR);
}
@@ -294,7 +288,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
gpios = ar_multiplexing_init();
if (ardrone_write < 0) {
fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
warnx("write fail");
thread_running = false;
exit(ERROR);
}
@@ -302,7 +296,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
/* initialize motors */
if (OK != ar_init_motors(ardrone_write, gpios)) {
close(ardrone_write);
fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
warnx("motor init fail");
thread_running = false;
exit(ERROR);
}
@@ -378,11 +372,9 @@ int ardrone_interface_thread_main(int argc, char *argv[])
int termios_state;
if ((termios_state = tcsetattr(ardrone_write, TCSANOW, &uart_config_original)) < 0) {
fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for (tcsetattr)\n");
warnx("ERR: tcsetattr");
}
printf("[ardrone_interface] Restored original UART config, exiting..\n");
/* close uarts */
close(ardrone_write);
ar_multiplexing_deinit(gpios);
src/drivers/ardrone_interface/ardrone_motor_control.c
+1 -1
View File
@@ -301,7 +301,7 @@ int ar_init_motors(int ardrone_uart, int gpios)
ardrone_write_motor_commands(ardrone_uart, 0, 0, 0, 0);
if (errcounter != 0) {
fprintf(stderr, "[ardrone_interface] init sequence incomplete, failed %d times", -errcounter);
warnx("Failed %d times", -errcounter);
fflush(stdout);
}
return errcounter;
src/drivers/boards/aerocore/aerocore_init.c
+13
View File
@@ -93,6 +93,19 @@
# endif
#endif
/*
* Ideally we'd be able to get these from up_internal.h,
* but since we want to be able to disable the NuttX use
* of leds for system indication at will and there is no
* separate switch, we need to build independent of the
* CONFIG_ARCH_LEDS configuration switch.
*/
__BEGIN_DECLS
extern void led_init(void);
extern void led_on(int led);
extern void led_off(int led);
__END_DECLS
/****************************************************************************
* Protected Functions
****************************************************************************/
src/drivers/boards/aerocore/board_config.h
+19
View File
@@ -171,6 +171,25 @@ __BEGIN_DECLS
extern void stm32_spiinitialize(void);
/****************************************************************************
* Name: nsh_archinitialize
*
* Description:
* Perform architecture specific initialization for NSH.
*
* CONFIG_NSH_ARCHINIT=y :
* Called from the NSH library
*
* CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, &&
* CONFIG_NSH_ARCHINIT=n :
* Called from board_initialize().
*
****************************************************************************/
#ifdef CONFIG_NSH_LIBRARY
int nsh_archinitialize(void);
#endif
#endif /* __ASSEMBLY__ */
__END_DECLS
src/drivers/boards/aerocore/module.mk
+2
View File
@@ -6,3 +6,5 @@ SRCS = aerocore_init.c \
aerocore_pwm_servo.c \
aerocore_spi.c \
aerocore_led.c
MAXOPTIMIZATION = -Os
src/drivers/boards/px4fmu-v1/board_config.h
+21
View File
@@ -209,6 +209,27 @@ __BEGIN_DECLS
extern void stm32_spiinitialize(void);
extern void stm32_usbinitialize(void);
/****************************************************************************
* Name: nsh_archinitialize
*
* Description:
* Perform architecture specific initialization for NSH.
*
* CONFIG_NSH_ARCHINIT=y :
* Called from the NSH library
*
* CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, &&
* CONFIG_NSH_ARCHINIT=n :
* Called from board_initialize().
*
****************************************************************************/
#ifdef CONFIG_NSH_LIBRARY
int nsh_archinitialize(void);
#endif
#endif /* __ASSEMBLY__ */
__END_DECLS
src/drivers/boards/px4fmu-v1/module.mk
+2
View File
@@ -8,3 +8,5 @@ SRCS = px4fmu_can.c \
px4fmu_spi.c \
px4fmu_usb.c \
px4fmu_led.c
MAXOPTIMIZATION = -Os
src/drivers/boards/px4fmu-v2/board_config.h
+21
View File
@@ -229,6 +229,27 @@ __BEGIN_DECLS
extern void stm32_spiinitialize(void);
extern void stm32_usbinitialize(void);
/****************************************************************************
* Name: nsh_archinitialize
*
* Description:
* Perform architecture specific initialization for NSH.
*
* CONFIG_NSH_ARCHINIT=y :
* Called from the NSH library
*
* CONFIG_BOARD_INITIALIZE=y, CONFIG_NSH_LIBRARY=y, &&
* CONFIG_NSH_ARCHINIT=n :
* Called from board_initialize().
*
****************************************************************************/
#ifdef CONFIG_NSH_LIBRARY
int nsh_archinitialize(void);
#endif
#endif /* __ASSEMBLY__ */
__END_DECLS
src/drivers/boards/px4fmu-v2/module.mk
+2
View File
@@ -8,3 +8,5 @@ SRCS = px4fmu_can.c \
px4fmu_spi.c \
px4fmu_usb.c \
px4fmu2_led.c
MAXOPTIMIZATION = -Os
src/drivers/boards/px4fmu-v2/px4fmu2_init.c
+13
View File
@@ -94,6 +94,19 @@
# endif
#endif
/*
* Ideally we'd be able to get these from up_internal.h,
* but since we want to be able to disable the NuttX use
* of leds for system indication at will and there is no
* separate switch, we need to build independent of the
* CONFIG_ARCH_LEDS configuration switch.
*/
__BEGIN_DECLS
extern void led_init(void);
extern void led_on(int led);
extern void led_off(int led);
__END_DECLS
/****************************************************************************
* Protected Functions
****************************************************************************/
src/drivers/boards/px4io-v1/module.mk
+2
View File
@@ -4,3 +4,5 @@
SRCS = px4io_init.c \
px4io_pwm_servo.c
MAXOPTIMIZATION = -Os
src/drivers/boards/px4io-v2/board_config.h
+1
View File
@@ -77,6 +77,7 @@
#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15)
#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
#define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11)
/* Safety switch button *******************************************************/
src/drivers/boards/px4io-v2/module.mk
+2
View File
@@ -4,3 +4,5 @@
SRCS = px4iov2_init.c \
px4iov2_pwm_servo.c
MAXOPTIMIZATION = -Os
src/drivers/boards/px4io-v2/px4iov2_init.c
+1
View File
@@ -108,6 +108,7 @@ __EXPORT void stm32_boardinitialize(void)
stm32_configgpio(GPIO_LED1);
stm32_configgpio(GPIO_LED2);
stm32_configgpio(GPIO_LED3);
stm32_configgpio(GPIO_LED4);
stm32_configgpio(GPIO_BTN_SAFETY);
src/drivers/device/device.cpp
+1
View File
@@ -97,6 +97,7 @@ Device::Device(const char *name,
/* setup a default device ID. When bus_type is UNKNOWN the
other fields are invalid */
_device_id.devid = 0;
_device_id.devid_s.bus_type = DeviceBusType_UNKNOWN;
_device_id.devid_s.bus = 0;
_device_id.devid_s.address = 0;
src/drivers/device/device.h
+2 -1
View File
@@ -130,7 +130,8 @@ public:
enum DeviceBusType {
DeviceBusType_UNKNOWN = 0,
DeviceBusType_I2C = 1,
DeviceBusType_SPI = 2
DeviceBusType_SPI = 2,
DeviceBusType_UAVCAN = 3,
};
/*
src/drivers/drv_gps.h
+2 -1
View File
@@ -57,7 +57,8 @@
typedef enum {
GPS_DRIVER_MODE_NONE = 0,
GPS_DRIVER_MODE_UBX,
GPS_DRIVER_MODE_MTK
GPS_DRIVER_MODE_MTK,
GPS_DRIVER_MODE_ASHTECH
} gps_driver_mode_t;
src/drivers/drv_pwm_output.h
+43 -3
View File
@@ -94,6 +94,11 @@ __BEGIN_DECLS
*/
#define PWM_LOWEST_MAX 1700
/**
* Do not output a channel with this value
*/
#define PWM_IGNORE_THIS_CHANNEL UINT16_MAX
/**
* Servo output signal type, value is actual servo output pulse
* width in microseconds.
@@ -112,6 +117,23 @@ struct pwm_output_values {
unsigned channel_count;
};
/**
* RC config values for a channel
*
* This allows for PX4IO_PAGE_RC_CONFIG values to be set without a
* param_get() dependency
*/
struct pwm_output_rc_config {
uint8_t channel;
uint16_t rc_min;
uint16_t rc_trim;
uint16_t rc_max;
uint16_t rc_dz;
uint16_t rc_assignment;
bool rc_reverse;
};
/*
* ORB tag for PWM outputs.
*/
@@ -160,7 +182,7 @@ ORB_DECLARE(output_pwm);
#define DSM2_BIND_PULSES 3 /* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */
#define DSMX_BIND_PULSES 7 /* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */
#define DSMX8_BIND_PULSES 10 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */
#define DSMX8_BIND_PULSES 9 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */
/** power up DSM receiver */
#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11)
@@ -200,10 +222,28 @@ ORB_DECLARE(output_pwm);
#define PWM_SERVO_GET_DISABLE_LOCKDOWN _IOC(_PWM_SERVO_BASE, 22)
/** force safety switch off (to disable use of safety switch) */
#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23)
#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23)
/** force failsafe mode (failsafe values are set immediately even if failsafe condition not met) */
#define PWM_SERVO_SET_FORCE_FAILSAFE _IOC(_PWM_SERVO_BASE, 24)
#define PWM_SERVO_SET_FORCE_FAILSAFE _IOC(_PWM_SERVO_BASE, 24)
/** make failsafe non-recoverable (termination) if it occurs */
#define PWM_SERVO_SET_TERMINATION_FAILSAFE _IOC(_PWM_SERVO_BASE, 25)
/** force safety switch on (to enable use of safety switch) */
#define PWM_SERVO_SET_FORCE_SAFETY_ON _IOC(_PWM_SERVO_BASE, 26)
/** set RC config for a channel. This takes a pointer to pwm_output_rc_config */
#define PWM_SERVO_SET_RC_CONFIG _IOC(_PWM_SERVO_BASE, 27)
/** set the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */
#define PWM_SERVO_SET_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 28)
/** clear the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */
#define PWM_SERVO_CLEAR_OVERRIDE_OK _IOC(_PWM_SERVO_BASE, 29)
/** setup OVERRIDE_IMMEDIATE behaviour on FMU fail */
#define PWM_SERVO_SET_OVERRIDE_IMMEDIATE _IOC(_PWM_SERVO_BASE, 30)
/*
*
src/drivers/drv_px4flow.h
-31
View File
@@ -46,37 +46,6 @@
#define PX4FLOW_DEVICE_PATH "/dev/px4flow"
/**
* @addtogroup topics
* @{
*/
/**
* Optical flow in NED body frame in SI units.
*
* @see http://en.wikipedia.org/wiki/International_System_of_Units
*
* @warning If possible the usage of the raw flow and performing rotation-compensation
* using the autopilot angular rate estimate is recommended.
*/
struct px4flow_report {
uint64_t timestamp; /**< in microseconds since system start */
int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */
int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */
float flow_comp_x_m; /**< speed over ground in meters per second, rotation-compensated */
float flow_comp_y_m; /**< speed over ground in meters per second, rotation-compensated */
float ground_distance_m; /**< Altitude / distance to ground in meters */
uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */
uint8_t sensor_id; /**< id of the sensor emitting the flow value */
};
/**
* @}
*/
/*
* ObjDev tag for px4flow data.
*/
src/drivers/drv_rc_input.h
+2 -1
View File
@@ -83,7 +83,8 @@ enum RC_INPUT_SOURCE {
RC_INPUT_SOURCE_PX4FMU_PPM,
RC_INPUT_SOURCE_PX4IO_PPM,
RC_INPUT_SOURCE_PX4IO_SPEKTRUM,
RC_INPUT_SOURCE_PX4IO_SBUS
RC_INPUT_SOURCE_PX4IO_SBUS,
RC_INPUT_SOURCE_PX4IO_ST24
};
/**
src/drivers/ets_airspeed/ets_airspeed.cpp
+6 -14
View File
@@ -155,7 +155,6 @@ ETSAirspeed::collect()
}
uint16_t diff_pres_pa_raw = val[1] << 8 | val[0];
uint16_t diff_pres_pa;
if (diff_pres_pa_raw == 0) {
// a zero value means the pressure sensor cannot give us a
// value. We need to return, and not report a value or the
@@ -166,28 +165,21 @@ ETSAirspeed::collect()
return -1;
}
if (diff_pres_pa_raw < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
diff_pres_pa = 0;
} else {
diff_pres_pa = diff_pres_pa_raw - _diff_pres_offset;
}
// The raw value still should be compensated for the known offset
diff_pres_pa_raw -= _diff_pres_offset;
// Track maximum differential pressure measured (so we can work out top speed).
if (diff_pres_pa > _max_differential_pressure_pa) {
_max_differential_pressure_pa = diff_pres_pa;
if (diff_pres_pa_raw > _max_differential_pressure_pa) {
_max_differential_pressure_pa = diff_pres_pa_raw;
}
differential_pressure_s report;
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.differential_pressure_pa = (float)diff_pres_pa;
// XXX we may want to smooth out the readings to remove noise.
report.differential_pressure_filtered_pa = (float)diff_pres_pa;
report.differential_pressure_raw_pa = (float)diff_pres_pa_raw;
report.differential_pressure_filtered_pa = diff_pres_pa_raw;
report.differential_pressure_raw_pa = diff_pres_pa_raw;
report.temperature = -1000.0f;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
@@ -369,7 +361,7 @@ test()
err(1, "immediate read failed");
warnx("single read");
warnx("diff pressure: %f pa", (double)report.differential_pressure_pa);
warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
@@ -394,7 +386,7 @@ test()
err(1, "periodic read failed");
warnx("periodic read %u", i);
warnx("diff pressure: %f pa", (double)report.differential_pressure_pa);
warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);
}
/* reset the sensor polling to its default rate */
src/drivers/frsky_telemetry/frsky_telemetry.c
+3 -6
View File
@@ -84,7 +84,7 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or
/* Back up the original UART configuration to restore it after exit */
int termios_state;
if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
warnx("ERROR get termios config %s: %d\n", uart_name, termios_state);
warnx("ERR: tcgetattr%s: %d\n", uart_name, termios_state);
close(uart);
return -1;
}
@@ -100,13 +100,13 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or
static const speed_t speed = B9600;
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
warnx("ERR: %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
close(uart);
return -1;
}
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
warnx("ERR: %s (tcsetattr)\n", uart_name);
close(uart);
return -1;
}
@@ -151,9 +151,6 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
}
}
/* Print welcome text */
warnx("FrSky telemetry interface starting...");
/* Open UART */
struct termios uart_config_original;
const int uart = frsky_open_uart(device_name, &uart_config_original);
src/drivers/gps/ashtech.cpp
+641
View File
@@ -0,0 +1,641 @@
#include "ashtech.h"
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <poll.h>
#include <math.h>
#include <string.h>
#include <assert.h>
#include <systemlib/err.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/satellite_info.h>
#include <drivers/drv_hrt.h>
#include <fcntl.h>
#include <math.h>
ASHTECH::ASHTECH(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info):
_fd(fd),
_satellite_info(satellite_info),
_gps_position(gps_position)
{
decode_init();
_decode_state = NME_DECODE_UNINIT;
_rx_buffer_bytes = 0;
}
ASHTECH::~ASHTECH()
{
}
/*
* All NMEA descriptions are taken from
* http://www.trimble.com/OEM_ReceiverHelp/V4.44/en/NMEA-0183messages_MessageOverview.html
*/
int ASHTECH::handle_message(int len)
{
char * endp;
if (len < 7) { return 0; }
int uiCalcComma = 0;
for (int i = 0 ; i < len; i++) {
if (_rx_buffer[i] == ',') { uiCalcComma++; }
}
char *bufptr = (char *)(_rx_buffer + 6);
if ((memcmp(_rx_buffer + 3, "ZDA,", 3) == 0) && (uiCalcComma == 6)) {
/*
UTC day, month, and year, and local time zone offset
An example of the ZDA message string is:
$GPZDA,172809.456,12,07,1996,00,00*45
ZDA message fields
Field Meaning
0 Message ID $GPZDA
1 UTC
2 Day, ranging between 01 and 31
3 Month, ranging between 01 and 12
4 Year
5 Local time zone offset from GMT, ranging from 00 through 13 hours
6 Local time zone offset from GMT, ranging from 00 through 59 minutes
7 The checksum data, always begins with *
Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT.
*/
double ashtech_time = 0.0;
int day = 0, month = 0, year = 0, local_time_off_hour __attribute__((unused)) = 0, local_time_off_min __attribute__((unused)) = 0;
if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { day = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { month = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { year = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { local_time_off_hour = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { local_time_off_min = strtol(bufptr, &endp, 10); bufptr = endp; }
int ashtech_hour = ashtech_time / 10000;
int ashtech_minute = (ashtech_time - ashtech_hour * 10000) / 100;
double ashtech_sec = ashtech_time - ashtech_hour * 10000 - ashtech_minute * 100;
/*
* convert to unix timestamp
*/
struct tm timeinfo;
timeinfo.tm_year = year - 1900;
timeinfo.tm_mon = month - 1;
timeinfo.tm_mday = day;
timeinfo.tm_hour = ashtech_hour;
timeinfo.tm_min = ashtech_minute;
timeinfo.tm_sec = int(ashtech_sec);
time_t epoch = mktime(&timeinfo);
_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
_gps_position->time_gps_usec += (uint64_t)((ashtech_sec - int(ashtech_sec)) * 1e6);
_gps_position->timestamp_time = hrt_absolute_time();
}
else if ((memcmp(_rx_buffer + 3, "GGA,", 3) == 0) && (uiCalcComma == 14)) {
/*
Time, position, and fix related data
An example of the GBS message string is:
$GPGGA,172814.0,3723.46587704,N,12202.26957864,W,2,6,1.2,18.893,M,-25.669,M,2.0,0031*4F
Note - The data string exceeds the ASHTECH standard length.
GGA message fields
Field Meaning
0 Message ID $GPGGA
1 UTC of position fix
2 Latitude
3 Direction of latitude:
N: North
S: South
4 Longitude
5 Direction of longitude:
E: East
W: West
6 GPS Quality indicator:
0: Fix not valid
1: GPS fix
2: Differential GPS fix, OmniSTAR VBS
4: Real-Time Kinematic, fixed integers
5: Real-Time Kinematic, float integers, OmniSTAR XP/HP or Location RTK
7 Number of SVs in use, range from 00 through to 24+
8 HDOP
9 Orthometric height (MSL reference)
10 M: unit of measure for orthometric height is meters
11 Geoid separation
12 M: geoid separation measured in meters
13 Age of differential GPS data record, Type 1 or Type 9. Null field when DGPS is not used.
14 Reference station ID, range 0000-4095. A null field when any reference station ID is selected and no corrections are received1.
15
The checksum data, always begins with *
Note - If a user-defined geoid model, or an inclined
*/
double ashtech_time __attribute__((unused)) = 0.0, lat = 0.0, lon = 0.0, alt = 0.0;
int num_of_sv __attribute__((unused)) = 0, fix_quality = 0;
double hdop __attribute__((unused)) = 99.9;
char ns = '?', ew = '?';
if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { lat = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); }
if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); }
if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { num_of_sv = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { hdop = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { alt = strtod(bufptr, &endp); bufptr = endp; }
if (ns == 'S') {
lat = -lat;
}
if (ew == 'W') {
lon = -lon;
}
_gps_position->lat = (int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000;
_gps_position->lon = (int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000;
_gps_position->alt = alt * 1000;
_rate_count_lat_lon++;
if (fix_quality <= 0) {
_gps_position->fix_type = 0;
} else {
/*
* in this NMEA message float integers (value 5) mode has higher value than fixed integers (value 4), whereas it provides lower quality,
* and since value 3 is not being used, I "moved" value 5 to 3 to add it to _gps_position->fix_type
*/
if (fix_quality == 5) { fix_quality = 3; }
/*
* fix quality 1 means just a normal 3D fix, so I'm subtracting 1 here. This way we'll have 3 for auto, 4 for DGPS, 5 for floats, 6 for fixed.
*/
_gps_position->fix_type = 3 + fix_quality - 1;
}
_gps_position->timestamp_position = hrt_absolute_time();
_gps_position->vel_m_s = 0; /**< GPS ground speed (m/s) */
_gps_position->vel_n_m_s = 0; /**< GPS ground speed in m/s */
_gps_position->vel_e_m_s = 0; /**< GPS ground speed in m/s */
_gps_position->vel_d_m_s = 0; /**< GPS ground speed in m/s */
_gps_position->cog_rad =
0; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */
_gps_position->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */
_gps_position->c_variance_rad = 0.1;
_gps_position->timestamp_velocity = hrt_absolute_time();
return 1;
} else if ((memcmp(_rx_buffer, "$PASHR,POS,", 11) == 0) && (uiCalcComma == 18)) {
/*
Example $PASHR,POS,2,10,125410.00,5525.8138702,N,03833.9587380,E,131.555,1.0,0.0,0.007,-0.001,2.0,1.0,1.7,1.0,*34
$PASHR,POS,d1,d2,m3,m4,c5,m6,c7,f8,f9,f10,f11,f12,f13,f14,f15,f16,s17*cc
Parameter Description Range
d1 Position mode 0: standalone
1: differential
2: RTK float
3: RTK fixed
5: Dead reckoning
9: SBAS (see NPT setting)
d2 Number of satellite used in position fix 0-99
m3 Current UTC time of position fix (hhmmss.ss) 000000.00-235959.99
m4 Latitude of position (ddmm.mmmmmm) 0-90 degrees 00-59.9999999 minutes
c5 Latitude sector N, S
m6 Longitude of position (dddmm.mmmmmm) 0-180 degrees 00-59.9999999 minutes
c7 Longitude sector E,W
f8 Altitude above ellipsoid +9999.000
f9 Differential age (data link age), seconds 0.0-600.0
f10 True track/course over ground in degrees 0.0-359.9
f11 Speed over ground in knots 0.0-999.9
f12 Vertical velocity in decimeters per second +999.9
f13 PDOP 0-99.9
f14 HDOP 0-99.9
f15 VDOP 0-99.9
f16 TDOP 0-99.9
s17 Reserved no data
*cc Checksum
*/
bufptr = (char *)(_rx_buffer + 10);
/*
* Ashtech would return empty space as coordinate (lat, lon or alt) if it doesn't have a fix yet
*/
int coordinatesFound = 0;
double ashtech_time __attribute__((unused)) = 0.0, lat = 0.0, lon = 0.0, alt = 0.0;
int num_of_sv __attribute__((unused)) = 0, fix_quality = 0;
double track_true = 0.0, ground_speed = 0.0 , age_of_corr __attribute__((unused)) = 0.0;
double hdop __attribute__((unused)) = 99.9, vdop __attribute__((unused)) = 99.9, pdop __attribute__((unused)) = 99.9, tdop __attribute__((unused)) = 99.9, vertic_vel = 0.0;
char ns = '?', ew = '?';
if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { num_of_sv = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') {
/*
* if a coordinate is skipped (i.e. no fix), it either won't get into this block (two commas in a row)
* or strtod won't find anything and endp will point exactly where bufptr is. The same is for lon and alt.
*/
lat = strtod(bufptr, &endp);
if (bufptr != endp) {coordinatesFound++;}
bufptr = endp;
}
if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); }
if (bufptr && *(++bufptr) != ',') {
lon = strtod(bufptr, &endp);
if (bufptr != endp) {coordinatesFound++;}
bufptr = endp;
}
if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); }
if (bufptr && *(++bufptr) != ',') {
alt = strtod(bufptr, &endp);
if (bufptr != endp) {coordinatesFound++;}
bufptr = endp;
}
if (bufptr && *(++bufptr) != ',') { age_of_corr = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { track_true = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { ground_speed = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { vertic_vel = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { pdop = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { hdop = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { vdop = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { tdop = strtod(bufptr, &endp); bufptr = endp; }
if (ns == 'S') {
lat = -lat;
}
if (ew == 'W') {
lon = -lon;
}
_gps_position->lat = (int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000;
_gps_position->lon = (int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000;
_gps_position->alt = alt * 1000;
_rate_count_lat_lon++;
if (coordinatesFound < 3) {
_gps_position->fix_type = 0;
} else {
_gps_position->fix_type = 3 + fix_quality;
}
_gps_position->timestamp_position = hrt_absolute_time();
double track_rad = track_true * M_PI / 180.0;
double velocity_ms = ground_speed / 1.9438445; /** knots to m/s */
double velocity_north = velocity_ms * cos(track_rad);
double velocity_east = velocity_ms * sin(track_rad);
_gps_position->vel_m_s = velocity_ms; /**< GPS ground speed (m/s) */
_gps_position->vel_n_m_s = velocity_north; /**< GPS ground speed in m/s */
_gps_position->vel_e_m_s = velocity_east; /**< GPS ground speed in m/s */
_gps_position->vel_d_m_s = -vertic_vel; /**< GPS ground speed in m/s */
_gps_position->cog_rad =
track_rad; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */
_gps_position->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */
_gps_position->c_variance_rad = 0.1;
_gps_position->timestamp_velocity = hrt_absolute_time();
return 1;
} else if ((memcmp(_rx_buffer + 3, "GST,", 3) == 0) && (uiCalcComma == 8)) {
/*
Position error statistics
An example of the GST message string is:
$GPGST,172814.0,0.006,0.023,0.020,273.6,0.023,0.020,0.031*6A
The Talker ID ($--) will vary depending on the satellite system used for the position solution:
$GP - GPS only
$GL - GLONASS only
$GN - Combined
GST message fields
Field Meaning
0 Message ID $GPGST
1 UTC of position fix
2 RMS value of the pseudorange residuals; includes carrier phase residuals during periods of RTK (float) and RTK (fixed) processing
3 Error ellipse semi-major axis 1 sigma error, in meters
4 Error ellipse semi-minor axis 1 sigma error, in meters
5 Error ellipse orientation, degrees from true north
6 Latitude 1 sigma error, in meters
7 Longitude 1 sigma error, in meters
8 Height 1 sigma error, in meters
9 The checksum data, always begins with *
*/
double ashtech_time __attribute__((unused)) = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0;
double min_err __attribute__((unused)) = 0.0, maj_err __attribute__((unused)) = 0.0, deg_from_north __attribute__((unused)) = 0.0, rms_err __attribute__((unused)) = 0.0;
if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { rms_err = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { maj_err = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { min_err = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { deg_from_north = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { lat_err = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { lon_err = strtod(bufptr, &endp); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { alt_err = strtod(bufptr, &endp); bufptr = endp; }
_gps_position->eph = sqrt(lat_err * lat_err + lon_err * lon_err);
_gps_position->epv = alt_err;
_gps_position->s_variance_m_s = 0;
_gps_position->timestamp_variance = hrt_absolute_time();
} else if ((memcmp(_rx_buffer + 3, "GSV,", 3) == 0)) {
/*
The GSV message string identifies the number of SVs in view, the PRN numbers, elevations, azimuths, and SNR values. An example of the GSV message string is:
$GPGSV,4,1,13,02,02,213,,03,-3,000,,11,00,121,,14,13,172,05*67
GSV message fields
Field Meaning
0 Message ID $GPGSV
1 Total number of messages of this type in this cycle
2 Message number
3 Total number of SVs visible
4 SV PRN number
5 Elevation, in degrees, 90 maximum
6 Azimuth, degrees from True North, 000 through 359
7 SNR, 00 through 99 dB (null when not tracking)
8-11 Information about second SV, same format as fields 4 through 7
12-15 Information about third SV, same format as fields 4 through 7
16-19 Information about fourth SV, same format as fields 4 through 7
20 The checksum data, always begins with *
*/
/*
* currently process only gps, because do not know what
* Global satellite ID I should use for non GPS sats
*/
bool bGPS = false;
if (memcmp(_rx_buffer, "$GP", 3) != 0) {
return 0;
} else {
bGPS = true;
}
int all_msg_num = 0, this_msg_num = 0, tot_sv_visible = 0;
struct gsv_sat {
int svid;
int elevation;
int azimuth;
int snr;
} sat[4];
memset(sat, 0, sizeof(sat));
if (bufptr && *(++bufptr) != ',') { all_msg_num = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { this_msg_num = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { tot_sv_visible = strtol(bufptr, &endp, 10); bufptr = endp; }
if ((this_msg_num < 1) || (this_msg_num > all_msg_num)) {
return 0;
}
if ((this_msg_num == 0) && (bGPS == true)) {
memset(_satellite_info->svid, 0, sizeof(_satellite_info->svid));
memset(_satellite_info->used, 0, sizeof(_satellite_info->used));
memset(_satellite_info->snr, 0, sizeof(_satellite_info->snr));
memset(_satellite_info->elevation, 0, sizeof(_satellite_info->elevation));
memset(_satellite_info->azimuth, 0, sizeof(_satellite_info->azimuth));
}
int end = 4;
if (this_msg_num == all_msg_num) {
end = tot_sv_visible - (this_msg_num - 1) * 4;
_gps_position->satellites_used = tot_sv_visible;
_satellite_info->count = SAT_INFO_MAX_SATELLITES;
_satellite_info->timestamp = hrt_absolute_time();
}
for (int y = 0 ; y < end ; y++) {
if (bufptr && *(++bufptr) != ',') { sat[y].svid = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { sat[y].elevation = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { sat[y].azimuth = strtol(bufptr, &endp, 10); bufptr = endp; }
if (bufptr && *(++bufptr) != ',') { sat[y].snr = strtol(bufptr, &endp, 10); bufptr = endp; }
_satellite_info->svid[y + (this_msg_num - 1) * 4] = sat[y].svid;
_satellite_info->used[y + (this_msg_num - 1) * 4] = ((sat[y].snr > 0) ? true : false);
_satellite_info->snr[y + (this_msg_num - 1) * 4] = sat[y].snr;
_satellite_info->elevation[y + (this_msg_num - 1) * 4] = sat[y].elevation;
_satellite_info->azimuth[y + (this_msg_num - 1) * 4] = sat[y].azimuth;
}
}
return 0;
}
int ASHTECH::receive(unsigned timeout)
{
{
/* poll descriptor */
pollfd fds[1];
fds[0].fd = _fd;
fds[0].events = POLLIN;
uint8_t buf[32];
/* timeout additional to poll */
uint64_t time_started = hrt_absolute_time();
int j = 0;
ssize_t bytes_count = 0;
while (true) {
/* pass received bytes to the packet decoder */
while (j < bytes_count) {
int l = 0;
if ((l = parse_char(buf[j])) > 0) {
/* return to configure during configuration or to the gps driver during normal work
* if a packet has arrived */
if (handle_message(l) > 0) {
return 1;
}
}
/* in case we keep trying but only get crap from GPS */
if (time_started + timeout * 1000 * 2 < hrt_absolute_time()) {
return -1;
}
j++;
}
/* everything is read */
j = bytes_count = 0;
/* then poll for new data */
int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout * 2);
if (ret < 0) {
/* something went wrong when polling */
return -1;
} else if (ret == 0) {
/* Timeout */
return -1;
} else if (ret > 0) {
/* if we have new data from GPS, go handle it */
if (fds[0].revents & POLLIN) {
/*
* We are here because poll says there is some data, so this
* won't block even on a blocking device. If more bytes are
* available, we'll go back to poll() again...
*/
bytes_count = ::read(_fd, buf, sizeof(buf));
}
}
}
}
}
#define HEXDIGIT_CHAR(d) ((char)((d) + (((d) < 0xA) ? '0' : 'A'-0xA)))
int ASHTECH::parse_char(uint8_t b)
{
int iRet = 0;
switch (_decode_state) {
/* First, look for sync1 */
case NME_DECODE_UNINIT:
if (b == '$') {
_decode_state = NME_DECODE_GOT_SYNC1;
_rx_buffer_bytes = 0;
_rx_buffer[_rx_buffer_bytes++] = b;
}
break;
case NME_DECODE_GOT_SYNC1:
if (b == '$') {
_decode_state = NME_DECODE_GOT_SYNC1;
_rx_buffer_bytes = 0;
} else if (b == '*') {
_decode_state = NME_DECODE_GOT_ASTERIKS;
}
if (_rx_buffer_bytes >= (sizeof(_rx_buffer) - 5)) {
_decode_state = NME_DECODE_UNINIT;
_rx_buffer_bytes = 0;
} else {
_rx_buffer[_rx_buffer_bytes++] = b;
}
break;
case NME_DECODE_GOT_ASTERIKS:
_rx_buffer[_rx_buffer_bytes++] = b;
_decode_state = NME_DECODE_GOT_FIRST_CS_BYTE;
break;
case NME_DECODE_GOT_FIRST_CS_BYTE:
_rx_buffer[_rx_buffer_bytes++] = b;
uint8_t checksum = 0;
uint8_t *buffer = _rx_buffer + 1;
uint8_t *bufend = _rx_buffer + _rx_buffer_bytes - 3;
for (; buffer < bufend; buffer++) { checksum ^= *buffer; }
if ((HEXDIGIT_CHAR(checksum >> 4) == *(_rx_buffer + _rx_buffer_bytes - 2)) &&
(HEXDIGIT_CHAR(checksum & 0x0F) == *(_rx_buffer + _rx_buffer_bytes - 1))) {
iRet = _rx_buffer_bytes;
}
_decode_state = NME_DECODE_UNINIT;
_rx_buffer_bytes = 0;
break;
}
return iRet;
}
void ASHTECH::decode_init(void)
{
}
/*
* ashtech board configuration script
*/
const char comm[] = "$PASHS,POP,20\r\n"\
"$PASHS,NME,ZDA,B,ON,3\r\n"\
"$PASHS,NME,GGA,B,OFF\r\n"\
"$PASHS,NME,GST,B,ON,3\r\n"\
"$PASHS,NME,POS,B,ON,0.05\r\n"\
"$PASHS,NME,GSV,B,ON,3\r\n"\
"$PASHS,SPD,A,8\r\n"\
"$PASHS,SPD,B,9\r\n";
int ASHTECH::configure(unsigned &baudrate)
{
/* try different baudrates */
const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200};
for (unsigned int baud_i = 0; baud_i < sizeof(baudrates_to_try) / sizeof(baudrates_to_try[0]); baud_i++) {
baudrate = baudrates_to_try[baud_i];
set_baudrate(_fd, baudrate);
write(_fd, (uint8_t *)comm, sizeof(comm));
}
set_baudrate(_fd, 115200);
return 0;
}
src/drivers/gps/ashtech.h
+96
View File
@@ -0,0 +1,96 @@
/****************************************************************************
*
* Copyright (C) 2013. All rights reserved.
* Author: Boriskin Aleksey <a.d.boriskin@gmail.com>
* Kistanov Alexander <akistanov@gramant.ru>
*
* 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 ASHTECH protocol definitions */
#ifndef ASHTECH_H_
#define ASHTECH_H_
#include "gps_helper.h"
#ifndef RECV_BUFFER_SIZE
#define RECV_BUFFER_SIZE 512
#define SAT_INFO_MAX_SATELLITES 20
#endif
class ASHTECH : public GPS_Helper
{
enum ashtech_decode_state_t {
NME_DECODE_UNINIT,
NME_DECODE_GOT_SYNC1,
NME_DECODE_GOT_ASTERIKS,
NME_DECODE_GOT_FIRST_CS_BYTE
};
int _fd;
struct satellite_info_s *_satellite_info;
struct vehicle_gps_position_s *_gps_position;
int ashtechlog_fd;
ashtech_decode_state_t _decode_state;
uint8_t _rx_buffer[RECV_BUFFER_SIZE];
uint16_t _rx_buffer_bytes;
bool _parse_error; /** parse error flag */
char *_parse_pos; /** parse position */
bool _gsv_in_progress; /**< Indicates that gsv data parsing is in progress */
/* int _satellites_count; **< Number of satellites info parsed. */
uint8_t count; /**< Number of satellites in satellite info */
uint8_t svid[SAT_INFO_MAX_SATELLITES]; /**< Space vehicle ID [1..255], see scheme below */
uint8_t used[SAT_INFO_MAX_SATELLITES]; /**< 0: Satellite not used, 1: used for navigation */
uint8_t elevation[SAT_INFO_MAX_SATELLITES]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
uint8_t azimuth[SAT_INFO_MAX_SATELLITES]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
uint8_t snr[SAT_INFO_MAX_SATELLITES]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */
public:
ASHTECH(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info);
~ASHTECH();
int receive(unsigned timeout);
int configure(unsigned &baudrate);
void decode_init(void);
int handle_message(int len);
int parse_char(uint8_t b);
/** Read int ASHTECH parameter */
int32_t read_int();
/** Read float ASHTECH parameter */
double read_float();
/** Read char ASHTECH parameter */
char read_char();
};
#endif /* ASHTECH_H_ */
src/drivers/gps/gps.cpp
+17 -1
View File
@@ -69,6 +69,7 @@
#include "ubx.h"
#include "mtk.h"
#include "ashtech.h"
#define TIMEOUT_5HZ 500
@@ -273,7 +274,6 @@ GPS::task_main_trampoline(void *arg)
void
GPS::task_main()
{
log("starting");
/* open the serial port */
_serial_fd = ::open(_port, O_RDWR);
@@ -341,6 +341,10 @@ GPS::task_main()
_Helper = new MTK(_serial_fd, &_report_gps_pos);
break;
case GPS_DRIVER_MODE_ASHTECH:
_Helper = new ASHTECH(_serial_fd, &_report_gps_pos, _p_report_sat_info);
break;
default:
break;
}
@@ -402,6 +406,10 @@ GPS::task_main()
mode_str = "MTK";
break;
case GPS_DRIVER_MODE_ASHTECH:
mode_str = "ASHTECH";
break;
default:
break;
}
@@ -429,6 +437,10 @@ GPS::task_main()
break;
case GPS_DRIVER_MODE_MTK:
_mode = GPS_DRIVER_MODE_ASHTECH;
break;
case GPS_DRIVER_MODE_ASHTECH:
_mode = GPS_DRIVER_MODE_UBX;
break;
@@ -475,6 +487,10 @@ GPS::print_info()
warnx("protocol: MTK");
break;
case GPS_DRIVER_MODE_ASHTECH:
warnx("protocol: ASHTECH");
break;
default:
break;
}
src/drivers/gps/module.mk
+1
View File
@@ -40,6 +40,7 @@ MODULE_COMMAND = gps
SRCS = gps.cpp \
gps_helper.cpp \
mtk.cpp \
ashtech.cpp \
ubx.cpp
MODULE_STACKSIZE = 1200
src/drivers/gps/ubx.cpp
+12
View File
@@ -189,6 +189,18 @@ UBX::configure(unsigned &baudrate)
return 1;
}
#ifdef UBX_CONFIGURE_SBAS
/* send a SBAS message to set the SBAS options */
memset(&_buf.payload_tx_cfg_sbas, 0, sizeof(_buf.payload_tx_cfg_sbas));
_buf.payload_tx_cfg_sbas.mode = UBX_TX_CFG_SBAS_MODE;
send_message(UBX_MSG_CFG_SBAS, _buf.raw, sizeof(_buf.payload_tx_cfg_sbas));
if (wait_for_ack(UBX_MSG_CFG_SBAS, UBX_CONFIG_TIMEOUT, true) < 0) {
return 1;
}
#endif
/* configure message rates */
/* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */
src/drivers/gps/ubx.h
+17
View File
@@ -73,6 +73,7 @@
#define UBX_ID_CFG_MSG 0x01
#define UBX_ID_CFG_RATE 0x08
#define UBX_ID_CFG_NAV5 0x24
#define UBX_ID_CFG_SBAS 0x16
#define UBX_ID_MON_VER 0x04
#define UBX_ID_MON_HW 0x09
@@ -89,6 +90,7 @@
#define UBX_MSG_CFG_MSG ((UBX_CLASS_CFG) | UBX_ID_CFG_MSG << 8)
#define UBX_MSG_CFG_RATE ((UBX_CLASS_CFG) | UBX_ID_CFG_RATE << 8)
#define UBX_MSG_CFG_NAV5 ((UBX_CLASS_CFG) | UBX_ID_CFG_NAV5 << 8)
#define UBX_MSG_CFG_SBAS ((UBX_CLASS_CFG) | UBX_ID_CFG_SBAS << 8)
#define UBX_MSG_MON_HW ((UBX_CLASS_MON) | UBX_ID_MON_HW << 8)
#define UBX_MSG_MON_VER ((UBX_CLASS_MON) | UBX_ID_MON_VER << 8)
@@ -128,6 +130,11 @@
#define UBX_TX_CFG_NAV5_DYNMODEL 7 /**< 0 Portable, 2 Stationary, 3 Pedestrian, 4 Automotive, 5 Sea, 6 Airborne <1g, 7 Airborne <2g, 8 Airborne <4g */
#define UBX_TX_CFG_NAV5_FIXMODE 2 /**< 1 2D only, 2 3D only, 3 Auto 2D/3D */
/* TX CFG-SBAS message contents */
#define UBX_TX_CFG_SBAS_MODE_ENABLED 1 /**< SBAS enabled */
#define UBX_TX_CFG_SBAS_MODE_DISABLED 0 /**< SBAS disabled */
#define UBX_TX_CFG_SBAS_MODE UBX_TX_CFG_SBAS_MODE_DISABLED /**< SBAS enabled or disabled */
/* TX CFG-MSG message contents */
#define UBX_TX_CFG_MSG_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
#define UBX_TX_CFG_MSG_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
@@ -383,6 +390,15 @@ typedef struct {
uint32_t reserved4;
} ubx_payload_tx_cfg_nav5_t;
/* tx cfg-sbas */
typedef struct {
uint8_t mode;
uint8_t usage;
uint8_t maxSBAS;
uint8_t scanmode2;
uint32_t scanmode1;
} ubx_payload_tx_cfg_sbas_t;
/* Tx CFG-MSG */
typedef struct {
union {
@@ -413,6 +429,7 @@ typedef union {
ubx_payload_tx_cfg_prt_t payload_tx_cfg_prt;
ubx_payload_tx_cfg_rate_t payload_tx_cfg_rate;
ubx_payload_tx_cfg_nav5_t payload_tx_cfg_nav5;
ubx_payload_tx_cfg_sbas_t payload_tx_cfg_sbas;
ubx_payload_tx_cfg_msg_t payload_tx_cfg_msg;
uint8_t raw[];
} ubx_buf_t;
src/drivers/hil/hil.cpp
+2 -3
View File
@@ -392,7 +392,8 @@ HIL::task_main()
if (fds[0].revents & POLLIN) {
/* get controls - must always do this to avoid spinning */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1), _t_actuators, &_controls);
/* can we mix? */
if (_mixers != nullptr) {
@@ -441,8 +442,6 @@ HIL::task_main()
/* make sure servos are off */
// up_pwm_servo_deinit();
log("stopping");
/* note - someone else is responsible for restoring the GPIO config */
/* tell the dtor that we are exiting */
src/drivers/hmc5883/hmc5883.cpp
+44 -44
View File
@@ -1049,11 +1049,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
* LSM/Ga, giving 1.16 and 1.08 */
float expected_cal[3] = { 1.16f, 1.08f, 1.08f };
warnx("starting mag scale calibration");
/* start the sensor polling at 50 Hz */
if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {
warn("failed to set 2Hz poll rate");
warn("FAILED: SENSORIOCSPOLLRATE 2Hz");
ret = 1;
goto out;
}
@@ -1061,25 +1059,25 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
/* Set to 2.5 Gauss. We ask for 3 to get the right part of
* the chained if statement above. */
if (OK != ioctl(filp, MAGIOCSRANGE, 3)) {
warnx("failed to set 2.5 Ga range");
warnx("FAILED: MAGIOCSRANGE 3.3 Ga");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) {
warnx("failed to enable sensor calibration mode");
warnx("FAILED: MAGIOCEXSTRAP 1");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) {
warn("WARNING: failed to get scale / offsets for mag");
warn("FAILED: MAGIOCGSCALE 1");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
warn("WARNING: failed to set null scale / offsets for mag");
warn("FAILED: MAGIOCSSCALE 1");
ret = 1;
goto out;
}
@@ -1094,7 +1092,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
ret = ::poll(&fds, 1, 2000);
if (ret != 1) {
warn("timed out waiting for sensor data");
warn("ERROR: TIMEOUT 1");
goto out;
}
@@ -1102,7 +1100,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
sz = ::read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
warn("periodic read failed");
warn("ERROR: READ 1");
ret = -EIO;
goto out;
}
@@ -1118,7 +1116,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
ret = ::poll(&fds, 1, 2000);
if (ret != 1) {
warn("timed out waiting for sensor data");
warn("ERROR: TIMEOUT 2");
goto out;
}
@@ -1126,7 +1124,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
sz = ::read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
warn("periodic read failed");
warn("ERROR: READ 2");
ret = -EIO;
goto out;
}
@@ -1142,33 +1140,19 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
sum_excited[1] += cal[1];
sum_excited[2] += cal[2];
}
//warnx("periodic read %u", i);
//warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
//warnx("cal: %.6f %.6f %.6f", (double)cal[0], (double)cal[1], (double)cal[2]);
}
if (good_count < 5) {
warn("failed calibration");
ret = -EIO;
goto out;
}
#if 0
warnx("measurement avg: %.6f %.6f %.6f",
(double)sum_excited[0]/good_count,
(double)sum_excited[1]/good_count,
(double)sum_excited[2]/good_count);
#endif
float scaling[3];
scaling[0] = sum_excited[0] / good_count;
scaling[1] = sum_excited[1] / good_count;
scaling[2] = sum_excited[2] / good_count;
warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
/* set scaling in device */
mscale_previous.x_scale = scaling[0];
mscale_previous.y_scale = scaling[1];
@@ -1179,29 +1163,26 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
out:
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
warn("failed to set new scale / offsets for mag");
warn("FAILED: MAGIOCSSCALE 2");
}
/* set back to normal mode */
/* Set to 1.1 Gauss */
if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
warnx("failed to set 1.1 Ga range");
warnx("FAILED: MAGIOCSRANGE 1.1 Ga");
}
if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
warnx("failed to disable sensor calibration mode");
warnx("FAILED: MAGIOCEXSTRAP 0");
}
if (ret == OK) {
if (!check_scale()) {
warnx("mag scale calibration successfully finished.");
} else {
warnx("mag scale calibration finished with invalid results.");
if (check_scale()) {
/* failed */
warnx("FAILED: SCALE");
ret = ERROR;
}
} else {
warnx("mag scale calibration failed.");
}
return ret;
@@ -1368,7 +1349,7 @@ HMC5883 *g_dev_ext = nullptr;
void start(int bus, enum Rotation rotation);
void test(int bus);
void reset(int bus);
void info(int bus);
int info(int bus);
int calibrate(int bus);
void usage();
@@ -1614,17 +1595,23 @@ reset(int bus)
/**
* Print a little info about the driver.
*/
void
int
info(int bus)
{
HMC5883 *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext);
if (g_dev == nullptr)
errx(1, "driver not running");
int ret = 1;
printf("state @ %p\n", g_dev);
g_dev->print_info();
HMC5883 *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext);
if (g_dev == nullptr) {
warnx("not running on bus %d", bus);
} else {
exit(0);
warnx("running on bus: %d (%s)\n", bus, ((PX4_I2C_BUS_ONBOARD) ? "onboard" : "offboard"));
g_dev->print_info();
ret = 0;
}
return ret;
}
void
@@ -1704,8 +1691,21 @@ hmc5883_main(int argc, char *argv[])
/*
* Print driver information.
*/
if (!strcmp(verb, "info") || !strcmp(verb, "status"))
hmc5883::info(bus);
if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
if (bus == -1) {
int ret = 0;
if (hmc5883::info(PX4_I2C_BUS_ONBOARD)) {
ret = 1;
}
if (hmc5883::info(PX4_I2C_BUS_EXPANSION)) {
ret = 1;
}
exit(ret);
} else {
exit(hmc5883::info(bus));
}
}
/*
* Autocalibrate the scaling
src/drivers/hmc5883/module.mk
+2
View File
@@ -42,3 +42,5 @@ SRCS = hmc5883.cpp
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++
MAXOPTIMIZATION = -Os
src/drivers/hott/comms.cpp
+4 -4
View File
@@ -55,7 +55,7 @@ open_uart(const char *device)
const int uart = open(device, O_RDWR | O_NOCTTY);
if (uart < 0) {
err(1, "Error opening port: %s", device);
err(1, "ERR: opening %s", device);
}
/* Back up the original uart configuration to restore it after exit */
@@ -63,7 +63,7 @@ open_uart(const char *device)
struct termios uart_config_original;
if ((termios_state = tcgetattr(uart, &uart_config_original)) < 0) {
close(uart);
err(1, "Error getting baudrate / termios config for %s: %d", device, termios_state);
err(1, "ERR: %s: %d", device, termios_state);
}
/* Fill the struct for the new configuration */
@@ -76,13 +76,13 @@ open_uart(const char *device)
/* Set baud rate */
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
close(uart);
err(1, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)",
err(1, "ERR: %s: %d (cfsetispeed, cfsetospeed)",
device, termios_state);
}
if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
close(uart);
err(1, "Error setting baudrate / termios config for %s (tcsetattr)", device);
err(1, "ERR: %s (tcsetattr)", device);
}
/* Activate single wire mode */
src/drivers/hott/hott_sensors/hott_sensors.cpp
+3 -3
View File
@@ -204,7 +204,7 @@ hott_sensors_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("deamon already running");
warnx("already running");
exit(0);
}
@@ -225,10 +225,10 @@ hott_sensors_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("daemon is running");
warnx("is running");
} else {
warnx("daemon not started");
warnx("not started");
}
exit(0);
src/drivers/hott/hott_telemetry/hott_telemetry.cpp
+3 -3
View File
@@ -230,7 +230,7 @@ hott_telemetry_main(int argc, char *argv[])
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("deamon already running");
warnx("already running");
exit(0);
}
@@ -251,10 +251,10 @@ hott_telemetry_main(int argc, char *argv[])
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("daemon is running");
warnx("is running");
} else {
warnx("daemon not started");
warnx("not started");
}
exit(0);
src/drivers/hott/messages.cpp
+7 -7
View File
@@ -115,9 +115,9 @@ publish_gam_message(const uint8_t *buffer)
esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT;
esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10;
esc.esc[0].esc_temperature = msg.temperature1 - 20;
esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff));
esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff));
esc.esc[0].esc_temperature = static_cast<float>(msg.temperature1) - 20.0F;
esc.esc[0].esc_voltage = static_cast<float>((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff)) * 0.1F;
esc.esc[0].esc_current = static_cast<float>((msg.current_H << 8) | (msg.current_L & 0xff)) * 0.1F;
/* announce the esc if needed, just publish else */
if (_esc_pub > 0) {
@@ -186,18 +186,18 @@ build_gam_response(uint8_t *buffer, size_t *size)
msg.gam_sensor_id = GAM_SENSOR_ID;
msg.sensor_text_id = GAM_SENSOR_TEXT_ID;
msg.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20);
msg.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20.0F);
msg.temperature2 = 20; // 0 deg. C.
uint16_t voltage = (uint16_t)(esc.esc[0].esc_voltage);
const uint16_t voltage = (uint16_t)(esc.esc[0].esc_voltage * 10.0F);
msg.main_voltage_L = (uint8_t)voltage & 0xff;
msg.main_voltage_H = (uint8_t)(voltage >> 8) & 0xff;
uint16_t current = (uint16_t)(esc.esc[0].esc_current);
const uint16_t current = (uint16_t)(esc.esc[0].esc_current * 10.0F);
msg.current_L = (uint8_t)current & 0xff;
msg.current_H = (uint8_t)(current >> 8) & 0xff;
uint16_t rpm = (uint16_t)(esc.esc[0].esc_rpm * 0.1f);
const uint16_t rpm = (uint16_t)(esc.esc[0].esc_rpm * 0.1f);
msg.rpm_L = (uint8_t)rpm & 0xff;
msg.rpm_H = (uint8_t)(rpm >> 8) & 0xff;
src/drivers/l3gd20/l3gd20.cpp
+8 -1
View File
@@ -176,6 +176,7 @@ static const int ERROR = -1;
#define L3G4200D_DEFAULT_RATE 800
#define L3GD20_DEFAULT_RANGE_DPS 2000
#define L3GD20_DEFAULT_FILTER_FREQ 30
#define L3GD20_TEMP_OFFSET_CELSIUS 40
#ifndef SENSOR_BOARD_ROTATION_DEFAULT
#define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG
@@ -856,7 +857,7 @@ L3GD20::measure()
#pragma pack(push, 1)
struct {
uint8_t cmd;
uint8_t temp;
int8_t temp;
uint8_t status;
int16_t x;
int16_t y;
@@ -930,6 +931,8 @@ L3GD20::measure()
report.z_raw = raw_report.z;
report.temperature_raw = raw_report.temp;
report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
@@ -938,6 +941,8 @@ L3GD20::measure()
report.y = _gyro_filter_y.apply(report.y);
report.z = _gyro_filter_z.apply(report.z);
report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp;
// apply user specified rotation
rotate_3f(_rotation, report.x, report.y, report.z);
@@ -1091,9 +1096,11 @@ test()
warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x);
warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y);
warnx("gyro z: \t% 9.5f\trad/s", (double)g_report.z);
warnx("temp: \t%d\tC", (int)g_report.temperature);
warnx("gyro x: \t%d\traw", (int)g_report.x_raw);
warnx("gyro y: \t%d\traw", (int)g_report.y_raw);
warnx("gyro z: \t%d\traw", (int)g_report.z_raw);
warnx("temp: \t%d\traw", (int)g_report.temperature_raw);
warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s,
(int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f));
src/drivers/l3gd20/module.mk
+2
View File
@@ -8,3 +8,5 @@ SRCS = l3gd20.cpp
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++
MAXOPTIMIZATION = -Os
src/drivers/ll40ls/ll40ls.cpp
+187 -59
View File
@@ -73,14 +73,19 @@
/* Configuration Constants */
#define LL40LS_BUS PX4_I2C_BUS_EXPANSION
#define LL40LS_BASEADDR 0x42 /* 7-bit address */
#define LL40LS_DEVICE_PATH "/dev/ll40ls"
#define LL40LS_BASEADDR 0x62 /* 7-bit address */
#define LL40LS_BASEADDR_OLD 0x42 /* previous 7-bit address */
#define LL40LS_DEVICE_PATH_INT "/dev/ll40ls_int"
#define LL40LS_DEVICE_PATH_EXT "/dev/ll40ls_ext"
/* LL40LS Registers addresses */
#define LL40LS_MEASURE_REG 0x00 /* Measure range register */
#define LL40LS_MSRREG_ACQUIRE 0x04 /* Value to initiate a measurement, varies based on sensor revision */
#define LL40LS_MSRREG_ACQUIRE 0x04 /* Value to initiate a measurement, varies based on sensor revision */
#define LL40LS_DISTHIGH_REG 0x8F /* High byte of distance register, auto increment */
#define LL40LS_WHO_AM_I_REG 0x11
#define LL40LS_WHO_AM_I_REG_VAL 0xCA
#define LL40LS_SIGNAL_STRENGTH_REG 0x5b
/* Device limits */
#define LL40LS_MIN_DISTANCE (0.00f)
@@ -101,7 +106,7 @@ static const int ERROR = -1;
class LL40LS : public device::I2C
{
public:
LL40LS(int bus = LL40LS_BUS, int address = LL40LS_BASEADDR);
LL40LS(int bus, const char *path, int address = LL40LS_BASEADDR);
virtual ~LL40LS();
virtual int init();
@@ -116,6 +121,7 @@ public:
protected:
virtual int probe();
virtual int read_reg(uint8_t reg, uint8_t &val);
private:
float _min_distance;
@@ -132,6 +138,10 @@ private:
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
uint16_t _last_distance;
/**< the bus the device is connected to */
int _bus;
/**
* Test whether the device supported by the driver is present at a
@@ -188,8 +198,8 @@ private:
*/
extern "C" __EXPORT int ll40ls_main(int argc, char *argv[]);
LL40LS::LL40LS(int bus, int address) :
I2C("LL40LS", LL40LS_DEVICE_PATH, bus, address, 100000),
LL40LS::LL40LS(int bus, const char *path, int address) :
I2C("LL40LS", path, bus, address, 100000),
_min_distance(LL40LS_MIN_DISTANCE),
_max_distance(LL40LS_MAX_DISTANCE),
_reports(nullptr),
@@ -200,10 +210,12 @@ LL40LS::LL40LS(int bus, int address) :
_range_finder_topic(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_read")),
_comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows"))
_buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")),
_last_distance(0),
_bus(bus)
{
// up the retries since the device misses the first measure attempts
I2C::_retries = 3;
_retries = 3;
// enable debug() calls
_debug_enabled = false;
@@ -270,9 +282,51 @@ out:
return ret;
}
int
LL40LS::read_reg(uint8_t reg, uint8_t &val)
{
return transfer(&reg, 1, &val, 1);
}
int
LL40LS::probe()
{
// cope with both old and new I2C bus address
const uint8_t addresses[2] = {LL40LS_BASEADDR, LL40LS_BASEADDR_OLD};
// more retries for detection
_retries = 10;
for (uint8_t i=0; i<sizeof(addresses); i++) {
uint8_t val=0, who_am_i=0;
// set the I2C bus address
set_address(addresses[i]);
if (read_reg(LL40LS_WHO_AM_I_REG, who_am_i) == OK && who_am_i == LL40LS_WHO_AM_I_REG_VAL) {
// it is responding correctly to a WHO_AM_I
goto ok;
}
if (read_reg(LL40LS_SIGNAL_STRENGTH_REG, val) == OK && val != 0) {
// very likely to be a ll40ls. px4flow does not
// respond to this
goto ok;
}
debug("WHO_AM_I byte mismatch 0x%02x should be 0x%02x val=0x%02x\n",
(unsigned)who_am_i,
LL40LS_WHO_AM_I_REG_VAL,
(unsigned)val);
}
// not found on any address
return -EIO;
ok:
_retries = 3;
// start a measurement
return measure();
}
@@ -521,6 +575,8 @@ LL40LS::collect()
float si_units = distance * 0.01f; /* cm to m */
struct range_finder_report report;
_last_distance = distance;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
@@ -648,6 +704,8 @@ LL40LS::print_info()
perf_print_counter(_buffer_overflows);
printf("poll interval: %u ticks\n", _measure_ticks);
_reports->print_info("report queue");
printf("distance: %ucm (0x%04x)\n",
(unsigned)_last_distance, (unsigned)_last_distance);
}
/**
@@ -662,55 +720,89 @@ namespace ll40ls
#endif
const int ERROR = -1;
LL40LS *g_dev;
LL40LS *g_dev_int;
LL40LS *g_dev_ext;
void start();
void stop();
void test();
void reset();
void info();
void start(int bus);
void stop(int bus);
void test(int bus);
void reset(int bus);
void info(int bus);
void usage();
/**
* Start the driver.
*/
void
start()
start(int bus)
{
int fd;
if (g_dev != nullptr) {
errx(1, "already started");
/* create the driver, attempt expansion bus first */
if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) {
if (g_dev_ext != nullptr)
errx(0, "already started external");
g_dev_ext = new LL40LS(PX4_I2C_BUS_EXPANSION, LL40LS_DEVICE_PATH_EXT);
if (g_dev_ext != nullptr && OK != g_dev_ext->init()) {
delete g_dev_ext;
g_dev_ext = nullptr;
}
}
/* create the driver */
g_dev = new LL40LS(LL40LS_BUS);
#ifdef PX4_I2C_BUS_ONBOARD
/* if this failed, attempt onboard sensor */
if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) {
if (g_dev_int != nullptr)
errx(0, "already started internal");
g_dev_int = new LL40LS(PX4_I2C_BUS_ONBOARD, LL40LS_DEVICE_PATH_INT);
if (g_dev_int != nullptr && OK != g_dev_int->init()) {
/* tear down the failing onboard instance */
delete g_dev_int;
g_dev_int = nullptr;
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init()) {
goto fail;
if (bus == PX4_I2C_BUS_ONBOARD) {
goto fail;
}
}
if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) {
goto fail;
}
}
#endif
/* set the poll rate to default, starts automatic data collection */
fd = open(LL40LS_DEVICE_PATH, O_RDONLY);
if (g_dev_int != nullptr) {
int fd = open(LL40LS_DEVICE_PATH_INT, O_RDONLY);
if (fd == -1) {
goto fail;
}
int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
close(fd);
if (ret < 0) {
goto fail;
}
}
if (fd < 0) {
goto fail;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail;
}
if (g_dev_ext != nullptr) {
int fd = open(LL40LS_DEVICE_PATH_EXT, O_RDONLY);
if (fd == -1) {
goto fail;
}
int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
close(fd);
if (ret < 0) {
goto fail;
}
}
exit(0);
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) {
delete g_dev_int;
g_dev_int = nullptr;
}
if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) {
delete g_dev_ext;
g_dev_ext = nullptr;
}
errx(1, "driver start failed");
@@ -719,11 +811,12 @@ fail:
/**
* Stop the driver
*/
void stop()
void stop(int bus)
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
LL40LS **g_dev = (bus == PX4_I2C_BUS_ONBOARD?&g_dev_int:&g_dev_ext);
if (*g_dev != nullptr) {
delete *g_dev;
*g_dev = nullptr;
} else {
errx(1, "driver not running");
@@ -738,16 +831,17 @@ void stop()
* and automatic modes.
*/
void
test()
test(int bus)
{
struct range_finder_report report;
ssize_t sz;
int ret;
const char *path = (bus==PX4_I2C_BUS_ONBOARD?LL40LS_DEVICE_PATH_INT:LL40LS_DEVICE_PATH_EXT);
int fd = open(LL40LS_DEVICE_PATH, O_RDONLY);
int fd = open(path, O_RDONLY);
if (fd < 0) {
err(1, "%s open failed (try 'll40ls start' if the driver is not running", LL40LS_DEVICE_PATH);
err(1, "%s open failed (try 'll40ls start' if the driver is not running", path);
}
/* do a simple demand read */
@@ -803,9 +897,10 @@ test()
* Reset the driver.
*/
void
reset()
reset(int bus)
{
int fd = open(LL40LS_DEVICE_PATH, O_RDONLY);
const char *path = (bus==PX4_I2C_BUS_ONBOARD?LL40LS_DEVICE_PATH_INT:LL40LS_DEVICE_PATH_EXT);
int fd = open(path, O_RDONLY);
if (fd < 0) {
err(1, "failed ");
@@ -826,8 +921,9 @@ reset()
* Print a little info about the driver.
*/
void
info()
info(int bus)
{
LL40LS *g_dev = (bus == PX4_I2C_BUS_ONBOARD?g_dev_int:g_dev_ext);
if (g_dev == nullptr) {
errx(1, "driver not running");
}
@@ -838,44 +934,76 @@ info()
exit(0);
}
void
usage()
{
warnx("missing command: try 'start', 'stop', 'info', 'test', 'reset', 'info'");
warnx("options:");
warnx(" -X only external bus");
#ifdef PX4_I2C_BUS_ONBOARD
warnx(" -I only internal bus");
#endif
}
} // namespace
int
ll40ls_main(int argc, char *argv[])
{
int ch;
int bus = -1;
while ((ch = getopt(argc, argv, "XI")) != EOF) {
switch (ch) {
#ifdef PX4_I2C_BUS_ONBOARD
case 'I':
bus = PX4_I2C_BUS_ONBOARD;
break;
#endif
case 'X':
bus = PX4_I2C_BUS_EXPANSION;
break;
default:
ll40ls::usage();
exit(0);
}
}
const char *verb = argv[optind];
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start")) {
ll40ls::start();
if (!strcmp(verb, "start")) {
ll40ls::start(bus);
}
/*
* Stop the driver
*/
if (!strcmp(argv[1], "stop")) {
ll40ls::stop();
if (!strcmp(verb, "stop")) {
ll40ls::stop(bus);
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test")) {
ll40ls::test();
if (!strcmp(verb, "test")) {
ll40ls::test(bus);
}
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset")) {
ll40ls::reset();
if (!strcmp(verb, "reset")) {
ll40ls::reset(bus);
}
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
ll40ls::info();
if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
ll40ls::info(bus);
}
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
src/drivers/lsm303d/lsm303d.cpp
+4 -2
View File
@@ -1519,8 +1519,10 @@ LSM303D::measure()
{
// if the accel doesn't have any data ready then re-schedule
// for 100 microseconds later. This ensures we don't double
// read a value and then miss the next value
if (stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) {
// read a value and then miss the next value.
// Note that DRDY is not available when the lsm303d is
// connected on the external bus
if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) {
perf_count(_accel_reschedules);
hrt_call_delay(&_accel_call, 100);
return;
src/drivers/lsm303d/module.mk
+2
View File
@@ -8,3 +8,5 @@ SRCS = lsm303d.cpp
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++
MAXOPTIMIZATION = -Os
src/drivers/meas_airspeed/meas_airspeed.cpp
+9 -30
View File
@@ -228,44 +228,23 @@ MEASAirspeed::collect()
// the raw value still should be compensated for the known offset
diff_press_pa_raw -= _diff_pres_offset;
float diff_press_pa = fabsf(diff_press_pa_raw);
/*
note that we return both the absolute value with offset
applied and a raw value without the offset applied. This
makes it possible for higher level code to detect if the
user has the tubes connected backwards, and also makes it
possible to correctly use offsets calculated by a higher
level airspeed driver.
With the above calculation the MS4525 sensor will produce a
positive number when the top port is used as a dynamic port
and bottom port is used as the static port
Also note that the _diff_pres_offset is applied before the
fabsf() not afterwards. It needs to be done this way to
prevent a bias at low speeds, but this also means that when
setting a offset you must set it based on the raw value, not
the offset value
*/
struct differential_pressure_s report;
/* track maximum differential pressure measured (so we can work out top speed). */
if (diff_press_pa > _max_differential_pressure_pa) {
_max_differential_pressure_pa = diff_press_pa;
if (diff_press_pa_raw > _max_differential_pressure_pa) {
_max_differential_pressure_pa = diff_press_pa_raw;
}
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.temperature = temperature;
report.differential_pressure_pa = diff_press_pa;
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa);
/* the dynamics of the filter can make it overshoot into the negative range */
if (report.differential_pressure_filtered_pa < 0.0f) {
report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa);
}
report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw);
report.differential_pressure_raw_pa = diff_press_pa_raw;
report.max_differential_pressure_pa = _max_differential_pressure_pa;
@@ -345,7 +324,7 @@ MEASAirspeed::cycle()
/**
correct for 5V rail voltage if the system_power ORB topic is
available
See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of
offset versus voltage for 3 sensors
*/
@@ -394,7 +373,7 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
if (voltage_diff < -1.0f) {
voltage_diff = -1.0f;
}
temperature -= voltage_diff * temp_slope;
temperature -= voltage_diff * temp_slope;
#endif // CONFIG_ARCH_BOARD_PX4FMU_V2
}
@@ -523,7 +502,7 @@ test()
}
warnx("single read");
warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
warnx("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
@@ -540,7 +519,7 @@ test()
ret = poll(&fds, 1, 2000);
if (ret != 1) {
errx(1, "timed out waiting for sensor data");
errx(1, "timed out");
}
/* now go get it */
@@ -551,7 +530,7 @@ test()
}
warnx("periodic read %u", i);
warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
warnx("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa);
warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature);
}
src/drivers/mkblctrl/mkblctrl.cpp
+3 -3
View File
@@ -600,8 +600,8 @@ MK::task_main()
esc.esc[i].esc_address = (uint8_t) BLCTRL_BASE_ADDR + i;
esc.esc[i].esc_vendor = ESC_VENDOR_MIKROKOPTER;
esc.esc[i].esc_version = (uint16_t) Motor[i].Version;
esc.esc[i].esc_voltage = (uint16_t) 0;
esc.esc[i].esc_current = (uint16_t) Motor[i].Current;
esc.esc[i].esc_voltage = 0.0F;
esc.esc[i].esc_current = static_cast<float>(Motor[i].Current) * 0.1F;
esc.esc[i].esc_rpm = (uint16_t) 0;
esc.esc[i].esc_setpoint = (float) Motor[i].SetPoint_PX4;
@@ -614,7 +614,7 @@ MK::task_main()
esc.esc[i].esc_setpoint_raw = (uint16_t) Motor[i].SetPoint;
}
esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature;
esc.esc[i].esc_temperature = static_cast<float>(Motor[i].Temperature);
esc.esc[i].esc_state = (uint16_t) Motor[i].State;
esc.esc[i].esc_errorcount = (uint16_t) 0;
src/drivers/mpu6000/module.mk
+2
View File
@@ -42,3 +42,5 @@ SRCS = mpu6000.cpp
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++
MAXOPTIMIZATION = -Os
src/drivers/mpu6000/mpu6000.cpp
+18 -10
View File
@@ -229,6 +229,7 @@ private:
perf_counter_t _gyro_reads;
perf_counter_t _sample_perf;
perf_counter_t _bad_transfers;
perf_counter_t _good_transfers;
math::LowPassFilter2p _accel_filter_x;
math::LowPassFilter2p _accel_filter_y;
@@ -404,6 +405,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
_gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")),
_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")),
_bad_transfers(perf_alloc(PC_COUNT, "mpu6000_bad_transfers")),
_good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")),
_accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
@@ -456,6 +458,7 @@ MPU6000::~MPU6000()
perf_free(_accel_reads);
perf_free(_gyro_reads);
perf_free(_bad_transfers);
perf_free(_good_transfers);
}
int
@@ -910,12 +913,14 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
// adjust filters
float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq();
float sample_rate = 1.0e6f/ticks;
_set_dlpf_filter(cutoff_freq_hz);
_accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
_accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
_accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq();
_set_dlpf_filter(cutoff_freq_hz_gyro);
_gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
_gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
_gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
@@ -968,11 +973,9 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
return _accel_filter_x.get_cutoff_freq();
case ACCELIOCSLOWPASS:
if (arg == 0) {
// allow disabling of on-chip filter using
// zero as desired filter frequency
_set_dlpf_filter(0);
}
// set hardware filtering
_set_dlpf_filter(arg);
// set software filtering
_accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
_accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
_accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
@@ -1053,14 +1056,11 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
case GYROIOCGLOWPASS:
return _gyro_filter_x.get_cutoff_freq();
case GYROIOCSLOWPASS:
// set hardware filtering
_set_dlpf_filter(arg);
_gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
_gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
_gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
if (arg == 0) {
// allow disabling of on-chip filter using 0
// as desired frequency
_set_dlpf_filter(0);
}
return OK;
case GYROIOCSSCALE:
@@ -1282,8 +1282,14 @@ MPU6000::measure()
// all zero data - probably a SPI bus error
perf_count(_bad_transfers);
perf_end(_sample_perf);
// note that we don't call reset() here as a reset()
// costs 20ms with interrupts disabled. That means if
// the mpu6k does go bad it would cause a FMU failure,
// regardless of whether another sensor is available,
return;
}
perf_count(_good_transfers);
/*
@@ -1402,6 +1408,8 @@ MPU6000::print_info()
perf_print_counter(_sample_perf);
perf_print_counter(_accel_reads);
perf_print_counter(_gyro_reads);
perf_print_counter(_bad_transfers);
perf_print_counter(_good_transfers);
_accel_reports->print_info("accel queue");
_gyro_reports->print_info("gyro queue");
}
src/drivers/px4flow/px4flow.cpp
+329 -231
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -37,7 +37,7 @@
*
* Driver for the PX4FLOW module connected via I2C.
*/
#include <nuttx/config.h>
#include <drivers/device/i2c.h>
@@ -68,20 +68,18 @@
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
//#include <uORB/topics/optical_flow.h>
#include <uORB/topics/optical_flow.h>
#include <board_config.h>
/* Configuration Constants */
#define PX4FLOW_BUS PX4_I2C_BUS_EXPANSION
#define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84
//range 0x42 - 0x49
/* PX4FLOW Registers addresses */
#define PX4FLOW_REG 0x00 /* Measure Register */
#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz
#define PX4FLOW_REG 0x16 /* Measure Register 22*/
#define PX4FLOW_CONVERSION_INTERVAL 20000 //in microseconds! 20000 = 50 Hz 100000 = 10Hz
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
@@ -92,40 +90,54 @@ static const int ERROR = -1;
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
//struct i2c_frame
//{
// uint16_t frame_count;
// int16_t pixel_flow_x_sum;
// int16_t pixel_flow_y_sum;
// int16_t flow_comp_m_x;
// int16_t flow_comp_m_y;
// int16_t qual;
// int16_t gyro_x_rate;
// int16_t gyro_y_rate;
// int16_t gyro_z_rate;
// uint8_t gyro_range;
// uint8_t sonar_timestamp;
// int16_t ground_distance;
//};
//
//struct i2c_frame f;
struct i2c_frame {
uint16_t frame_count;
int16_t pixel_flow_x_sum;
int16_t pixel_flow_y_sum;
int16_t flow_comp_m_x;
int16_t flow_comp_m_y;
int16_t qual;
int16_t gyro_x_rate;
int16_t gyro_y_rate;
int16_t gyro_z_rate;
uint8_t gyro_range;
uint8_t sonar_timestamp;
int16_t ground_distance;
};
struct i2c_frame f;
class PX4FLOW : public device::I2C
typedef struct i2c_integral_frame {
uint16_t frame_count_since_last_readout;
int16_t pixel_flow_x_integral;
int16_t pixel_flow_y_integral;
int16_t gyro_x_rate_integral;
int16_t gyro_y_rate_integral;
int16_t gyro_z_rate_integral;
uint32_t integration_timespan;
uint32_t time_since_last_sonar_update;
uint16_t ground_distance;
int16_t gyro_temperature;
uint8_t qual;
} __attribute__((packed));
struct i2c_integral_frame f_integral;
class PX4FLOW: public device::I2C
{
public:
PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS);
PX4FLOW(int bus, int address = I2C_FLOW_ADDRESS);
virtual ~PX4FLOW();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
* Diagnostics - print some basic information about the driver.
*/
void print_info();
protected:
virtual int probe();
@@ -136,51 +148,50 @@ private:
bool _sensor_ok;
int _measure_ticks;
bool _collect_phase;
orb_advert_t _px4flow_topic;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
/**
* Test whether the device supported by the driver is present at a
* specific address.
*
* @param address The I2C bus address to probe.
* @return True if the device is present.
*/
* Test whether the device supported by the driver is present at a
* specific address.
*
* @param address The I2C bus address to probe.
* @return True if the device is present.
*/
int probe_address(uint8_t address);
/**
* Initialise the automatic measurement state machine and start it.
*
* @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors.
*/
* Initialise the automatic measurement state machine and start it.
*
* @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors.
*/
void start();
/**
* Stop the automatic measurement state machine.
*/
* Stop the automatic measurement state machine.
*/
void stop();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void cycle();
int measure();
int collect();
/**
* Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
* Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
};
/*
@@ -189,7 +200,7 @@ private:
extern "C" __EXPORT int px4flow_main(int argc, char *argv[]);
PX4FLOW::PX4FLOW(int bus, int address) :
I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000),//400khz
I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000), //400khz
_reports(nullptr),
_sensor_ok(false),
_measure_ticks(0),
@@ -200,8 +211,8 @@ PX4FLOW::PX4FLOW(int bus, int address) :
_buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows"))
{
// enable debug() calls
_debug_enabled = true;
_debug_enabled = false;
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
}
@@ -212,8 +223,9 @@ PX4FLOW::~PX4FLOW()
stop();
/* free any existing reports */
if (_reports != nullptr)
if (_reports != nullptr) {
delete _reports;
}
}
int
@@ -222,22 +234,16 @@ PX4FLOW::init()
int ret = ERROR;
/* do I2C init (and probe) first */
if (I2C::init() != OK)
if (I2C::init() != OK) {
goto out;
}
/* allocate basic report buffers */
_reports = new RingBuffer(2, sizeof(px4flow_report));
_reports = new RingBuffer(2, sizeof(optical_flow_s));
if (_reports == nullptr)
if (_reports == nullptr) {
goto out;
/* get a publish handle on the px4flow topic */
struct px4flow_report zero_report;
memset(&zero_report, 0, sizeof(zero_report));
_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report);
if (_px4flow_topic < 0)
debug("failed to create px4flow object. Did you start uOrb?");
}
ret = OK;
/* sensor is ok, but we don't really know if it is within range */
@@ -249,6 +255,17 @@ out:
int
PX4FLOW::probe()
{
uint8_t val[22];
// to be sure this is not a ll40ls Lidar (which can also be on
// 0x42) we check if a 22 byte transfer works from address
// 0. The ll40ls gives an error for that, whereas the flow
// happily returns some data
if (transfer(nullptr, 0, &val[0], 22) != OK) {
return -EIO;
}
// that worked, so start a measurement cycle
return measure();
}
@@ -260,20 +277,20 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_measure_ticks = 0;
return OK;
/* external signalling (DRDY) not supported */
/* external signalling (DRDY) not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
@@ -283,13 +300,14 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
_measure_ticks = USEC2TICK(PX4FLOW_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */
if (want_start)
if (want_start) {
start();
}
return OK;
}
/* adjust to a legal polling interval in Hz */
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
@@ -298,15 +316,17 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL))
if (ticks < USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) {
return -EINVAL;
}
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
if (want_start)
if (want_start) {
start();
}
return OK;
}
@@ -314,33 +334,37 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
}
case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0)
if (_measure_ticks == 0) {
return SENSOR_POLLRATE_MANUAL;
}
return (1000 / _measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
return -EINVAL;
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) {
return -EINVAL;
}
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return -ENOMEM;
return OK;
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _reports->size();
case SENSORIOCRESET:
/* XXX implement this */
return -EINVAL;
default:
/* give it to the superclass */
return I2C::ioctl(filp, cmd, arg);
@@ -350,13 +374,14 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
ssize_t
PX4FLOW::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct px4flow_report);
struct px4flow_report *rbuf = reinterpret_cast<struct px4flow_report *>(buffer);
unsigned count = buflen / sizeof(struct optical_flow_s);
struct optical_flow_s *rbuf = reinterpret_cast<struct optical_flow_s *>(buffer);
int ret = 0;
/* buffer must be large enough */
if (count < 1)
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is enabled */
if (_measure_ticks > 0) {
@@ -387,9 +412,6 @@ PX4FLOW::read(struct file *filp, char *buffer, size_t buflen)
break;
}
/* wait for it to complete */
usleep(PX4FLOW_CONVERSION_INTERVAL);
/* run the collection phase */
if (OK != collect()) {
ret = -EIO;
@@ -417,68 +439,110 @@ PX4FLOW::measure()
uint8_t cmd = PX4FLOW_REG;
ret = transfer(&cmd, 1, nullptr, 0);
if (OK != ret)
{
if (OK != ret) {
perf_count(_comms_errors);
log("i2c::transfer returned %d", ret);
printf("i2c::transfer flow returned %d");
debug("i2c::transfer returned %d", ret);
return ret;
}
ret = OK;
return ret;
}
int
PX4FLOW::collect()
{
int ret = -EIO;
int ret = -EIO;
/* read from the sensor */
uint8_t val[22] = {0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0};
uint8_t val[47] = { 0 };
perf_begin(_sample_perf);
ret = transfer(nullptr, 0, &val[0], 22);
if (ret < 0)
{
log("error reading from sensor: %d", ret);
if (PX4FLOW_REG == 0x00) {
ret = transfer(nullptr, 0, &val[0], 47); // read 47 bytes (22+25 : frame1 + frame2)
}
if (PX4FLOW_REG == 0x16) {
ret = transfer(nullptr, 0, &val[0], 25); // read 25 bytes (only frame2)
}
if (ret < 0) {
debug("error reading from sensor: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
// f.frame_count = val[1] << 8 | val[0];
// f.pixel_flow_x_sum= val[3] << 8 | val[2];
// f.pixel_flow_y_sum= val[5] << 8 | val[4];
// f.flow_comp_m_x= val[7] << 8 | val[6];
// f.flow_comp_m_y= val[9] << 8 | val[8];
// f.qual= val[11] << 8 | val[10];
// f.gyro_x_rate= val[13] << 8 | val[12];
// f.gyro_y_rate= val[15] << 8 | val[14];
// f.gyro_z_rate= val[17] << 8 | val[16];
// f.gyro_range= val[18];
// f.sonar_timestamp= val[19];
// f.ground_distance= val[21] << 8 | val[20];
int16_t flowcx = val[7] << 8 | val[6];
int16_t flowcy = val[9] << 8 | val[8];
int16_t gdist = val[21] << 8 | val[20];
if (PX4FLOW_REG == 0) {
f.frame_count = val[1] << 8 | val[0];
f.pixel_flow_x_sum = val[3] << 8 | val[2];
f.pixel_flow_y_sum = val[5] << 8 | val[4];
f.flow_comp_m_x = val[7] << 8 | val[6];
f.flow_comp_m_y = val[9] << 8 | val[8];
f.qual = val[11] << 8 | val[10];
f.gyro_x_rate = val[13] << 8 | val[12];
f.gyro_y_rate = val[15] << 8 | val[14];
f.gyro_z_rate = val[17] << 8 | val[16];
f.gyro_range = val[18];
f.sonar_timestamp = val[19];
f.ground_distance = val[21] << 8 | val[20];
f_integral.frame_count_since_last_readout = val[23] << 8 | val[22];
f_integral.pixel_flow_x_integral = val[25] << 8 | val[24];
f_integral.pixel_flow_y_integral = val[27] << 8 | val[26];
f_integral.gyro_x_rate_integral = val[29] << 8 | val[28];
f_integral.gyro_y_rate_integral = val[31] << 8 | val[30];
f_integral.gyro_z_rate_integral = val[33] << 8 | val[32];
f_integral.integration_timespan = val[37] << 24 | val[36] << 16
| val[35] << 8 | val[34];
f_integral.time_since_last_sonar_update = val[41] << 24 | val[40] << 16
| val[39] << 8 | val[38];
f_integral.ground_distance = val[43] << 8 | val[42];
f_integral.gyro_temperature = val[45] << 8 | val[44];
f_integral.qual = val[46];
}
if (PX4FLOW_REG == 0x16) {
f_integral.frame_count_since_last_readout = val[1] << 8 | val[0];
f_integral.pixel_flow_x_integral = val[3] << 8 | val[2];
f_integral.pixel_flow_y_integral = val[5] << 8 | val[4];
f_integral.gyro_x_rate_integral = val[7] << 8 | val[6];
f_integral.gyro_y_rate_integral = val[9] << 8 | val[8];
f_integral.gyro_z_rate_integral = val[11] << 8 | val[10];
f_integral.integration_timespan = val[15] << 24 | val[14] << 16 | val[13] << 8 | val[12];
f_integral.time_since_last_sonar_update = val[19] << 24 | val[18] << 16 | val[17] << 8 | val[16];
f_integral.ground_distance = val[21] << 8 | val[20];
f_integral.gyro_temperature = val[23] << 8 | val[22];
f_integral.qual = val[24];
}
struct optical_flow_s report;
struct px4flow_report report;
report.flow_comp_x_m = float(flowcx)/1000.0f;
report.flow_comp_y_m = float(flowcy)/1000.0f;
report.flow_raw_x= val[3] << 8 | val[2];
report.flow_raw_y= val[5] << 8 | val[4];
report.ground_distance_m =float(gdist)/1000.0f;
report.quality= val[10];
report.sensor_id = 0;
report.timestamp = hrt_absolute_time();
report.pixel_flow_x_integral = static_cast<float>(f_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians
report.pixel_flow_y_integral = static_cast<float>(f_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians
report.frame_count_since_last_readout = f_integral.frame_count_since_last_readout;
report.ground_distance_m = static_cast<float>(f_integral.ground_distance) / 1000.0f;//convert to meters
report.quality = f_integral.qual; //0:bad ; 255 max quality
report.gyro_x_rate_integral = static_cast<float>(f_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians
report.gyro_y_rate_integral = static_cast<float>(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians
report.gyro_z_rate_integral = static_cast<float>(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians
report.integration_timespan = f_integral.integration_timespan; //microseconds
report.time_since_last_sonar_update = f_integral.time_since_last_sonar_update;//microseconds
report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius
report.sensor_id = 0;
/* publish it */
orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report);
if (_px4flow_topic < 0) {
_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report);
} else {
/* publish it */
orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report);
}
/* post a report to the ring */
if (_reports->force(&report)) {
@@ -503,17 +567,19 @@ PX4FLOW::start()
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1);
/* notify about state change */
struct subsystem_info_s info = {
true,
true,
true,
SUBSYSTEM_TYPE_OPTICALFLOW};
SUBSYSTEM_TYPE_OPTICALFLOW
};
static orb_advert_t pub = -1;
if (pub > 0) {
orb_publish(ORB_ID(subsystem_info), pub, &info);
} else {
pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
@@ -536,49 +602,21 @@ PX4FLOW::cycle_trampoline(void *arg)
void
PX4FLOW::cycle()
{
/* collection phase? */
if (_collect_phase) {
/* perform collection */
if (OK != collect()) {
log("collection error");
/* restart the measurement state machine */
start();
return;
}
/* next phase is measurement */
_collect_phase = false;
/*
* Is there a collect->measure gap?
*/
if (_measure_ticks > USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) {
/* schedule a fresh cycle call when we are ready to measure again */
work_queue(HPWORK,
&_work,
(worker_t)&PX4FLOW::cycle_trampoline,
this,
_measure_ticks - USEC2TICK(PX4FLOW_CONVERSION_INTERVAL));
return;
}
if (OK != measure()) {
debug("measure error");
}
/* measurement phase */
if (OK != measure())
log("measure error");
/* perform collection */
if (OK != collect()) {
debug("collection error");
/* restart the measurement state machine */
start();
return;
}
/* next phase is collection */
_collect_phase = true;
work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this,
_measure_ticks);
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&PX4FLOW::cycle_trampoline,
this,
USEC2TICK(PX4FLOW_CONVERSION_INTERVAL));
}
void
@@ -619,33 +657,64 @@ start()
{
int fd;
if (g_dev != nullptr)
if (g_dev != nullptr) {
errx(1, "already started");
}
/* create the driver */
g_dev = new PX4FLOW(PX4FLOW_BUS);
g_dev = new PX4FLOW(PX4_I2C_BUS_EXPANSION);
if (g_dev == nullptr)
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init())
goto fail;
if (OK != g_dev->init()) {
#ifdef PX4_I2C_BUS_ESC
delete g_dev;
/* try 2nd bus */
g_dev = new PX4FLOW(PX4_I2C_BUS_ESC);
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init()) {
#endif
delete g_dev;
/* try 3rd bus */
g_dev = new PX4FLOW(PX4_I2C_BUS_ONBOARD);
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init()) {
goto fail;
}
#ifdef PX4_I2C_BUS_ESC
}
#endif
}
/* set the poll rate to default, starts automatic data collection */
fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY);
if (fd < 0)
if (fd < 0) {
goto fail;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0)
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0) {
goto fail;
}
exit(0);
fail:
if (g_dev != nullptr)
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
@@ -656,17 +725,17 @@ fail:
/**
* Stop the driver
*/
void stop()
void
stop()
{
if (g_dev != nullptr)
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
else
{
} else {
errx(1, "driver not running");
}
exit(0);
}
@@ -678,33 +747,38 @@ void stop()
void
test()
{
struct px4flow_report report;
struct optical_flow_s report;
ssize_t sz;
int ret;
int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY);
if (fd < 0)
if (fd < 0) {
err(1, "%s open failed (try 'px4flow start' if the driver is not running", PX4FLOW_DEVICE_PATH);
}
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report))
// err(1, "immediate read failed");
{
warnx("immediate read failed");
}
warnx("single read");
warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m);
warnx("flowy: %0.2f m/s", (double)report.flow_comp_y_m);
warnx("time: %lld", report.timestamp);
warnx("pixel_flow_x_integral: %i", f_integral.pixel_flow_x_integral);
warnx("pixel_flow_y_integral: %i", f_integral.pixel_flow_y_integral);
warnx("framecount_integral: %u",
f_integral.frame_count_since_last_readout);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
errx(1, "failed to set 2Hz poll rate");
/* start the sensor polling at 10Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 10)) {
errx(1, "failed to set 10Hz poll rate");
}
/* read the sensor 5x and report each value */
for (unsigned i = 0; i < 5; i++) {
for (unsigned i = 0; i < 10; i++) {
struct pollfd fds;
/* wait for data to be ready */
@@ -712,19 +786,34 @@ test()
fds.events = POLLIN;
ret = poll(&fds, 1, 2000);
if (ret != 1)
if (ret != 1) {
errx(1, "timed out waiting for sensor data");
}
/* now go get it */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report))
if (sz != sizeof(report)) {
err(1, "periodic read failed");
}
warnx("periodic read %u", i);
warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m);
warnx("flowy: %0.2f m/s", (double)report.flow_comp_y_m);
warnx("time: %lld", report.timestamp);
warnx("framecount_total: %u", f.frame_count);
warnx("framecount_integral: %u",
f_integral.frame_count_since_last_readout);
warnx("pixel_flow_x_integral: %i", f_integral.pixel_flow_x_integral);
warnx("pixel_flow_y_integral: %i", f_integral.pixel_flow_y_integral);
warnx("gyro_x_rate_integral: %i", f_integral.gyro_x_rate_integral);
warnx("gyro_y_rate_integral: %i", f_integral.gyro_y_rate_integral);
warnx("gyro_z_rate_integral: %i", f_integral.gyro_z_rate_integral);
warnx("integration_timespan [us]: %u", f_integral.integration_timespan);
warnx("ground_distance: %0.2f m",
(double) f_integral.ground_distance / 1000);
warnx("time since last sonar update [us]: %i",
f_integral.time_since_last_sonar_update);
warnx("quality integration average : %i", f_integral.qual);
warnx("quality : %i", f.qual);
}
@@ -740,14 +829,17 @@ reset()
{
int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY);
if (fd < 0)
if (fd < 0) {
err(1, "failed ");
}
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
err(1, "driver reset failed");
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "driver poll restart failed");
}
exit(0);
}
@@ -758,8 +850,9 @@ reset()
void
info()
{
if (g_dev == nullptr)
if (g_dev == nullptr) {
errx(1, "driver not running");
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
@@ -775,32 +868,37 @@ px4flow_main(int argc, char *argv[])
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start"))
if (!strcmp(argv[1], "start")) {
px4flow::start();
/*
* Stop the driver
*/
if (!strcmp(argv[1], "stop"))
px4flow::stop();
}
/*
* Stop the driver
*/
if (!strcmp(argv[1], "stop")) {
px4flow::stop();
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test"))
if (!strcmp(argv[1], "test")) {
px4flow::test();
}
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset"))
if (!strcmp(argv[1], "reset")) {
px4flow::reset();
}
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
px4flow::info();
}
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
}
src/drivers/px4fmu/fmu.cpp
+4 -1
View File
@@ -829,6 +829,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case PWM_SERVO_SET_ARM_OK:
case PWM_SERVO_CLEAR_ARM_OK:
case PWM_SERVO_SET_FORCE_SAFETY_OFF:
case PWM_SERVO_SET_FORCE_SAFETY_ON:
// these are no-ops, as no safety switch
break;
@@ -1272,7 +1273,9 @@ PX4FMU::write(file *filp, const char *buffer, size_t len)
memcpy(values, buffer, count * 2);
for (uint8_t i = 0; i < count; i++) {
up_pwm_servo_set(i, values[i]);
if (values[i] != PWM_IGNORE_THIS_CHANNEL) {
up_pwm_servo_set(i, values[i]);
}
}
return count * 2;
src/drivers/px4fmu/module.mk
+2
View File
@@ -8,3 +8,5 @@ SRCS = fmu.cpp
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++
MAXOPTIMIZATION = -Os
src/drivers/px4io/module.mk
+2
View File
@@ -48,3 +48,5 @@ INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/com
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++
MAXOPTIMIZATION = -Os
src/drivers/px4io/px4io.cpp
+158 -53
View File
@@ -295,6 +295,7 @@ private:
float _battery_amp_bias; ///< current sensor bias
float _battery_mamphour_total;///< amp hours consumed so far
uint64_t _battery_last_timestamp;///< last amp hour calculation timestamp
bool _cb_flighttermination; ///< true if the flight termination circuit breaker is enabled
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
bool _dsm_vcc_ctl; ///< true if relay 1 controls DSM satellite RX power
@@ -515,7 +516,8 @@ PX4IO::PX4IO(device::Device *interface) :
_battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor
_battery_amp_bias(0),
_battery_mamphour_total(0),
_battery_last_timestamp(0)
_battery_last_timestamp(0),
_cb_flighttermination(true)
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
, _dsm_vcc_ctl(false)
#endif
@@ -815,6 +817,11 @@ PX4IO::init()
}
/* set safety to off if circuit breaker enabled */
if (circuit_breaker_enabled("CBRK_IO_SAFETY", CBRK_IO_SAFETY_KEY)) {
(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
}
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
@@ -1051,6 +1058,9 @@ PX4IO::task_main()
}
}
/* Update Circuit breakers */
_cb_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY);
}
}
@@ -1150,43 +1160,54 @@ PX4IO::io_set_arming_state()
actuator_armed_s armed; ///< system armed state
vehicle_control_mode_s control_mode; ///< vehicle_control_mode
orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed);
orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode);
int have_armed = orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed);
int have_control_mode = orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode);
uint16_t set = 0;
uint16_t clear = 0;
if (armed.armed) {
set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
if (have_armed == OK) {
if (armed.armed) {
set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
} else {
clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
}
} else {
clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
if (armed.lockdown && !_lockdown_override) {
set |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
} else {
clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
}
/* Do not set failsafe if circuit breaker is enabled */
if (armed.force_failsafe && !_cb_flighttermination) {
set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
} else {
clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
}
// XXX this is for future support in the commander
// but can be removed if unneeded
// if (armed.termination_failsafe) {
// set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
// } else {
// clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE;
// }
if (armed.ready_to_arm) {
set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
} else {
clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
}
}
if (armed.lockdown && !_lockdown_override) {
set |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
} else {
clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
}
if (armed.force_failsafe) {
set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
} else {
clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
}
if (armed.ready_to_arm) {
set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
} else {
clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
}
if (control_mode.flag_external_manual_override_ok) {
set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
} else {
clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
if (have_control_mode == OK) {
if (control_mode.flag_external_manual_override_ok) {
set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
} else {
clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
}
}
return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set);
@@ -1231,30 +1252,43 @@ PX4IO::io_set_rc_config()
* for compatibility reasons with existing
* autopilots / GCS'.
*/
/* ROLL */
param_get(param_find("RC_MAP_ROLL"), &ichan);
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
input_map[ichan - 1] = 0;
}
/* PITCH */
param_get(param_find("RC_MAP_PITCH"), &ichan);
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
input_map[ichan - 1] = 1;
}
/* YAW */
param_get(param_find("RC_MAP_YAW"), &ichan);
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
input_map[ichan - 1] = 2;
}
/* THROTTLE */
param_get(param_find("RC_MAP_THROTTLE"), &ichan);
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
input_map[ichan - 1] = 3;
}
param_get(param_find("RC_MAP_MODE_SW"), &ichan);
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
/* FLAPS */
param_get(param_find("RC_MAP_FLAPS"), &ichan);
if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
input_map[ichan - 1] = 4;
}
/* MAIN MODE SWITCH */
param_get(param_find("RC_MAP_MODE_SW"), &ichan);
if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
/* use out of normal bounds index to indicate special channel */
input_map[ichan - 1] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH;
}
/*
* Iterate all possible RC inputs.
@@ -1603,6 +1637,9 @@ PX4IO::io_publish_raw_rc()
} else if (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) {
rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SBUS;
} else if (_status & PX4IO_P_STATUS_FLAGS_RC_ST24) {
rc_val.input_source = RC_INPUT_SOURCE_PX4IO_ST24;
} else {
rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN;
@@ -1627,10 +1664,6 @@ PX4IO::io_publish_raw_rc()
int
PX4IO::io_publish_pwm_outputs()
{
/* if no FMU comms(!) just don't publish */
if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK))
return OK;
/* data we are going to fetch */
actuator_outputs_s outputs;
outputs.timestamp = hrt_absolute_time();
@@ -1920,13 +1953,15 @@ PX4IO::print_status(bool extended_status)
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM));
uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
uint16_t io_status_flags = flags;
printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n",
printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s%s\n",
flags,
((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""),
((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"),
((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"),
((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""),
((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) ? " DSM" : ""),
((flags & PX4IO_P_STATUS_FLAGS_RC_ST24) ? " ST24" : ""),
((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""),
((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"),
((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PWM_PASSTHROUGH" : ""),
@@ -2029,7 +2064,7 @@ PX4IO::print_status(bool extended_status)
((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "")
);
uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
printf("arming 0x%04x%s%s%s%s%s%s%s%s\n",
printf("arming 0x%04x%s%s%s%s%s%s%s%s%s%s\n",
arming,
((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"),
((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"),
@@ -2038,7 +2073,9 @@ PX4IO::print_status(bool extended_status)
((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""),
((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : ""),
((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : "")
((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : ""),
((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : ""),
((arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) ? " OVERRIDE_IMMEDIATE" : "")
);
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
@@ -2163,7 +2200,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
if (pwm->channel_count > _max_actuators)
/* fail with error */
return E2BIG;
return -E2BIG;
/* copy values to registers in IO */
ret = io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm->values, pwm->channel_count);
@@ -2182,7 +2219,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
if (pwm->channel_count > _max_actuators)
/* fail with error */
return E2BIG;
return -E2BIG;
/* copy values to registers in IO */
ret = io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, pwm->channel_count);
@@ -2201,7 +2238,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
if (pwm->channel_count > _max_actuators)
/* fail with error */
return E2BIG;
return -E2BIG;
/* copy values to registers in IO */
ret = io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, pwm->channel_count);
@@ -2220,7 +2257,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
if (pwm->channel_count > _max_actuators)
/* fail with error */
return E2BIG;
return -E2BIG;
/* copy values to registers in IO */
ret = io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, pwm->channel_count);
@@ -2251,6 +2288,11 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
break;
case PWM_SERVO_SET_FORCE_SAFETY_ON:
/* force safety switch on */
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_ON, PX4IO_FORCE_SAFETY_MAGIC);
break;
case PWM_SERVO_SET_FORCE_FAILSAFE:
/* force failsafe mode instantly */
if (arg == 0) {
@@ -2262,6 +2304,30 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
}
break;
case PWM_SERVO_SET_TERMINATION_FAILSAFE:
/* if failsafe occurs, do not allow the system to recover */
if (arg == 0) {
/* clear termination failsafe flag */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE, 0);
} else {
/* set termination failsafe flag */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE);
}
break;
case PWM_SERVO_SET_OVERRIDE_IMMEDIATE:
/* control whether override on FMU failure is
immediate or waits for override threshold on mode
switch */
if (arg == 0) {
/* clear override immediate flag */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE, 0);
} else {
/* set override immediate flag */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE);
}
break;
case DSM_BIND_START:
/* only allow DSM2, DSM-X and DSM-X with more than 7 channels */
@@ -2439,6 +2505,9 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
} else if (status & PX4IO_P_STATUS_FLAGS_RC_SBUS) {
rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SBUS;
} else if (status & PX4IO_P_STATUS_FLAGS_RC_ST24) {
rc_val->input_source = RC_INPUT_SOURCE_PX4IO_ST24;
} else {
rc_val->input_source = RC_INPUT_SOURCE_UNKNOWN;
}
@@ -2520,6 +2589,42 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
break;
case PWM_SERVO_SET_RC_CONFIG: {
/* enable setting of RC configuration without relying
on param_get()
*/
struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg;
if (config->channel >= RC_INPUT_MAX_CHANNELS) {
/* fail with error */
return -E2BIG;
}
/* copy values to registers in IO */
uint16_t regs[PX4IO_P_RC_CONFIG_STRIDE];
uint16_t offset = config->channel * PX4IO_P_RC_CONFIG_STRIDE;
regs[PX4IO_P_RC_CONFIG_MIN] = config->rc_min;
regs[PX4IO_P_RC_CONFIG_CENTER] = config->rc_trim;
regs[PX4IO_P_RC_CONFIG_MAX] = config->rc_max;
regs[PX4IO_P_RC_CONFIG_DEADZONE] = config->rc_dz;
regs[PX4IO_P_RC_CONFIG_ASSIGNMENT] = config->rc_assignment;
regs[PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
if (config->rc_reverse) {
regs[PX4IO_P_RC_CONFIG_OPTIONS] |= PX4IO_P_RC_CONFIG_OPTIONS_REVERSE;
}
ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE);
break;
}
case PWM_SERVO_SET_OVERRIDE_OK:
/* set the 'OVERRIDE OK' bit */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK);
break;
case PWM_SERVO_CLEAR_OVERRIDE_OK:
/* clear the 'OVERRIDE OK' bit */
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0);
break;
default:
/* see if the parent class can make any use of it */
ret = CDev::ioctl(filep, cmd, arg);
src/drivers/rgbled/rgbled.cpp
+7 -7
View File
@@ -121,7 +121,7 @@ private:
/* for now, we only support one RGBLED */
namespace
{
RGBLED *g_rgbled;
RGBLED *g_rgbled = nullptr;
}
void rgbled_usage();
@@ -680,15 +680,15 @@ rgbled_main(int argc, char *argv[])
ret = ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF);
close(fd);
/* delete the rgbled object if stop was requested, in addition to turning off the LED. */
if (!strcmp(verb, "stop")) {
delete g_rgbled;
g_rgbled = nullptr;
exit(0);
}
exit(ret);
}
if (!strcmp(verb, "stop")) {
delete g_rgbled;
g_rgbled = nullptr;
exit(0);
}
if (!strcmp(verb, "rgb")) {
if (argc < 5) {
errx(1, "Usage: rgbled rgb <red> <green> <blue>");
src/drivers/sf0x/module.mk
+2 -1
View File
@@ -37,6 +37,7 @@
MODULE_COMMAND = sf0x
SRCS = sf0x.cpp
SRCS = sf0x.cpp \
sf0x_parser.cpp
MAXOPTIMIZATION = -Os
src/drivers/sf0x/sf0x.cpp
+18 -88
View File
@@ -72,6 +72,8 @@
#include <board_config.h>
#include "sf0x_parser.h"
/* Configuration Constants */
/* oddly, ERROR is not defined for c++ */
@@ -120,6 +122,7 @@ private:
int _fd;
char _linebuf[10];
unsigned _linebuf_index;
enum SF0X_PARSE_STATE _parse_state;
hrt_abstime _last_read;
orb_advert_t _range_finder_topic;
@@ -186,6 +189,7 @@ SF0X::SF0X(const char *port) :
_collect_phase(false),
_fd(-1),
_linebuf_index(0),
_parse_state(SF0X_PARSE_STATE0_UNSYNC),
_last_read(0),
_range_finder_topic(-1),
_consecutive_fail_count(0),
@@ -200,12 +204,6 @@ SF0X::SF0X(const char *port) :
warnx("FAIL: laser fd");
}
/* tell it to stop auto-triggering */
char stop_auto = ' ';
(void)::write(_fd, &stop_auto, 1);
usleep(100);
(void)::write(_fd, &stop_auto, 1);
struct termios uart_config;
int termios_state;
@@ -520,22 +518,15 @@ SF0X::collect()
/* clear buffer if last read was too long ago */
uint64_t read_elapsed = hrt_elapsed_time(&_last_read);
if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
_linebuf_index = 0;
} else if (_linebuf_index > 0) {
/* increment to next read position */
_linebuf_index++;
}
/* the buffer for read chars is buflen minus null termination */
unsigned readlen = sizeof(_linebuf) - 1;
char readbuf[sizeof(_linebuf)];
unsigned readlen = sizeof(readbuf) - 1;
/* read from the sensor (uart buffer) */
ret = ::read(_fd, &_linebuf[_linebuf_index], readlen - _linebuf_index);
ret = ::read(_fd, &readbuf[0], readlen);
if (ret < 0) {
_linebuf[sizeof(_linebuf) - 1] = '\0';
debug("read err: %d lbi: %d buf: %s", ret, (int)_linebuf_index, _linebuf);
debug("read err: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
@@ -550,84 +541,23 @@ SF0X::collect()
return -EAGAIN;
}
/* we did increment the index to the next position already, so just add the additional fields */
_linebuf_index += (ret - 1);
_last_read = hrt_absolute_time();
if (_linebuf_index < 1) {
/* we need at least the two end bytes to make sense of this string */
return -EAGAIN;
} else if (_linebuf[_linebuf_index - 1] != '\r' || _linebuf[_linebuf_index] != '\n') {
if (_linebuf_index >= readlen - 1) {
/* we have a full buffer, but no line ending - abort */
_linebuf_index = 0;
perf_count(_comms_errors);
return -ENOMEM;
} else {
/* incomplete read, reschedule ourselves */
return -EAGAIN;
}
}
char *end;
float si_units;
bool valid;
/* enforce line ending */
unsigned lend = (_linebuf_index < (sizeof(_linebuf) - 1)) ? _linebuf_index : (sizeof(_linebuf) - 1);
_linebuf[lend] = '\0';
if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') {
si_units = -1.0f;
valid = false;
} else {
/* we need to find a dot in the string, as we're missing the meters part else */
valid = false;
/* wipe out partially read content from last cycle(s), check for dot */
for (unsigned i = 0; i < (lend - 2); i++) {
if (_linebuf[i] == '\n') {
char buf[sizeof(_linebuf)];
memcpy(buf, &_linebuf[i+1], (lend + 1) - (i + 1));
memcpy(_linebuf, buf, (lend + 1) - (i + 1));
}
/* we need a digit before the dot and a dot for a valid number */
if (i > 0 && _linebuf[i] == '.') {
valid = true;
}
}
if (valid) {
si_units = strtod(_linebuf, &end);
/* we require at least 3 characters for a valid number */
if (end > _linebuf + 3) {
valid = true;
} else {
si_units = -1.0f;
valid = false;
}
bool valid = false;
for (int i = 0; i < ret; i++) {
if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &si_units)) {
valid = true;
}
}
debug("val (float): %8.4f, raw: %s, valid: %s\n", (double)si_units, _linebuf, ((valid) ? "OK" : "NO"));
/* done with this chunk, resetting - even if invalid */
_linebuf_index = 0;
/* if its invalid, there is no reason to forward the value */
if (!valid) {
perf_count(_comms_errors);
return -EINVAL;
return -EAGAIN;
}
debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO"));
struct range_finder_report report;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
@@ -709,12 +639,12 @@ SF0X::cycle()
int collect_ret = collect();
if (collect_ret == -EAGAIN) {
/* reschedule to grab the missing bits, time to transmit 10 bytes @9600 bps */
/* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */
work_queue(HPWORK,
&_work,
(worker_t)&SF0X::cycle_trampoline,
this,
USEC2TICK(1100));
USEC2TICK(1042 * 8));
return;
}
src/drivers/sf0x/sf0x_parser.cpp
+155
View File
@@ -0,0 +1,155 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file sf0x_parser.cpp
* @author Lorenz Meier <lm@inf.ethz.ch>
*
* Driver for the Lightware SF0x laser rangefinder series
*/
#include "sf0x_parser.h"
#include <string.h>
#include <stdlib.h>
//#define SF0X_DEBUG
#ifdef SF0X_DEBUG
#include <stdio.h>
const char *parser_state[] = {
"0_UNSYNC",
"1_SYNC",
"2_GOT_DIGIT0",
"3_GOT_DOT",
"4_GOT_DIGIT1",
"5_GOT_DIGIT2",
"6_GOT_CARRIAGE_RETURN"
};
#endif
int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist)
{
int ret = -1;
char *end;
switch (*state) {
case SF0X_PARSE_STATE0_UNSYNC:
if (c == '\n') {
*state = SF0X_PARSE_STATE1_SYNC;
(*parserbuf_index) = 0;
}
break;
case SF0X_PARSE_STATE1_SYNC:
if (c >= '0' && c <= '9') {
*state = SF0X_PARSE_STATE2_GOT_DIGIT0;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
}
break;
case SF0X_PARSE_STATE2_GOT_DIGIT0:
if (c >= '0' && c <= '9') {
*state = SF0X_PARSE_STATE2_GOT_DIGIT0;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
} else if (c == '.') {
*state = SF0X_PARSE_STATE3_GOT_DOT;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
} else {
*state = SF0X_PARSE_STATE0_UNSYNC;
}
break;
case SF0X_PARSE_STATE3_GOT_DOT:
if (c >= '0' && c <= '9') {
*state = SF0X_PARSE_STATE4_GOT_DIGIT1;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
} else {
*state = SF0X_PARSE_STATE0_UNSYNC;
}
break;
case SF0X_PARSE_STATE4_GOT_DIGIT1:
if (c >= '0' && c <= '9') {
*state = SF0X_PARSE_STATE5_GOT_DIGIT2;
parserbuf[*parserbuf_index] = c;
(*parserbuf_index)++;
} else {
*state = SF0X_PARSE_STATE0_UNSYNC;
}
break;
case SF0X_PARSE_STATE5_GOT_DIGIT2:
if (c == '\r') {
*state = SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN;
} else {
*state = SF0X_PARSE_STATE0_UNSYNC;
}
break;
case SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN:
if (c == '\n') {
parserbuf[*parserbuf_index] = '\0';
*dist = strtod(parserbuf, &end);
*state = SF0X_PARSE_STATE1_SYNC;
*parserbuf_index = 0;
ret = 0;
} else {
*state = SF0X_PARSE_STATE0_UNSYNC;
}
break;
}
#ifdef SF0X_DEBUG
printf("state: SF0X_PARSE_STATE%s\n", parser_state[*state]);
#endif
return ret;
}
src/drivers/sf0x/sf0x_parser.h
+51
View File
@@ -0,0 +1,51 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file sf0x_parser.cpp
* @author Lorenz Meier <lm@inf.ethz.ch>
*
* Declarations of parser for the Lightware SF0x laser rangefinder series
*/
enum SF0X_PARSE_STATE {
SF0X_PARSE_STATE0_UNSYNC = 0,
SF0X_PARSE_STATE1_SYNC,
SF0X_PARSE_STATE2_GOT_DIGIT0,
SF0X_PARSE_STATE3_GOT_DOT,
SF0X_PARSE_STATE4_GOT_DIGIT1,
SF0X_PARSE_STATE5_GOT_DIGIT2,
SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN
};
int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist);
src/drivers/stm32/adc/module.mk
+2
View File
@@ -40,3 +40,5 @@ MODULE_COMMAND = adc
SRCS = adc.cpp
INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
MAXOPTIMIZATION = -Os
src/drivers/stm32/module.mk
+2
View File
@@ -41,3 +41,5 @@ SRCS = drv_hrt.c \
drv_pwm_servo.c
INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
MAXOPTIMIZATION = -Os
src/drivers/stm32/tone_alarm/module.mk
+2
View File
@@ -40,3 +40,5 @@ MODULE_COMMAND = tone_alarm
SRCS = tone_alarm.cpp
INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
MAXOPTIMIZATION = -Os
src/drivers/trone/module.mk
+44
View File
@@ -0,0 +1,44 @@
############################################################################
#
# Copyright (c) 2014 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Makefile to build the TeraRanger One range finder driver
#
MODULE_COMMAND = trone
SRCS = trone.cpp
MODULE_STACKSIZE = 1200
MAXOPTIMIZATION = -Os
src/drivers/trone/trone.cpp
+913
View File
@@ -0,0 +1,913 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file trone.cpp
* @author Luis Rodrigues
*
* Driver for the TeraRanger One range finders connected via I2C.
*/
#include <nuttx/config.h>
#include <drivers/device/i2c.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_range_finder.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/subsystem_info.h>
#include <board_config.h>
/* Configuration Constants */
#define TRONE_BUS PX4_I2C_BUS_EXPANSION
#define TRONE_BASEADDR 0x30 /* 7-bit address */
#define TRONE_DEVICE_PATH "/dev/trone"
/* TRONE Registers addresses */
#define TRONE_MEASURE_REG 0x00 /* Measure range register */
/* Device limits */
#define TRONE_MIN_DISTANCE (0.20f)
#define TRONE_MAX_DISTANCE (14.00f)
#define TRONE_CONVERSION_INTERVAL 50000 /* 50ms */
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
#ifndef CONFIG_SCHED_WORKQUEUE
# error This requires CONFIG_SCHED_WORKQUEUE.
#endif
class TRONE : public device::I2C
{
public:
TRONE(int bus = TRONE_BUS, int address = TRONE_BASEADDR);
virtual ~TRONE();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
protected:
virtual int probe();
private:
float _min_distance;
float _max_distance;
work_s _work;
RingBuffer *_reports;
bool _sensor_ok;
int _measure_ticks;
bool _collect_phase;
int _class_instance;
orb_advert_t _range_finder_topic;
perf_counter_t _sample_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
/**
* Test whether the device supported by the driver is present at a
* specific address.
*
* @param address The I2C bus address to probe.
* @return True if the device is present.
*/
int probe_address(uint8_t address);
/**
* Initialise the automatic measurement state machine and start it.
*
* @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors.
*/
void start();
/**
* Stop the automatic measurement state machine.
*/
void stop();
/**
* Set the min and max distance thresholds if you want the end points of the sensors
* range to be brought in at all, otherwise it will use the defaults TRONE_MIN_DISTANCE
* and TRONE_MAX_DISTANCE
*/
void set_minimum_distance(float min);
void set_maximum_distance(float max);
float get_minimum_distance();
float get_maximum_distance();
/**
* Perform a poll cycle; collect from the previous measurement
* and start a new one.
*/
void cycle();
int measure();
int collect();
/**
* Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void cycle_trampoline(void *arg);
};
static const uint8_t crc_table[] = {
0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31,
0x24, 0x23, 0x2a, 0x2d, 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65,
0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, 0xe0, 0xe7, 0xee, 0xe9,
0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd,
0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1,
0xb4, 0xb3, 0xba, 0xbd, 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2,
0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, 0xb7, 0xb0, 0xb9, 0xbe,
0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a,
0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16,
0x03, 0x04, 0x0d, 0x0a, 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42,
0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, 0x89, 0x8e, 0x87, 0x80,
0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4,
0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8,
0xdd, 0xda, 0xd3, 0xd4, 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c,
0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, 0x19, 0x1e, 0x17, 0x10,
0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34,
0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f,
0x6a, 0x6d, 0x64, 0x63, 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b,
0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, 0xae, 0xa9, 0xa0, 0xa7,
0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83,
0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef,
0xfa, 0xfd, 0xf4, 0xf3
};
uint8_t crc8(uint8_t *p, uint8_t len){
uint16_t i;
uint16_t crc = 0x0;
while (len--) {
i = (crc ^ *p++) & 0xFF;
crc = (crc_table[i] ^ (crc << 8)) & 0xFF;
}
return crc & 0xFF;
}
/*
* Driver 'main' command.
*/
extern "C" __EXPORT int trone_main(int argc, char *argv[]);
TRONE::TRONE(int bus, int address) :
I2C("TRONE", TRONE_DEVICE_PATH, bus, address, 100000),
_min_distance(TRONE_MIN_DISTANCE),
_max_distance(TRONE_MAX_DISTANCE),
_reports(nullptr),
_sensor_ok(false),
_measure_ticks(0),
_collect_phase(false),
_class_instance(-1),
_range_finder_topic(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "trone_read")),
_comms_errors(perf_alloc(PC_COUNT, "trone_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "trone_buffer_overflows"))
{
// up the retries since the device misses the first measure attempts
I2C::_retries = 3;
// enable debug() calls
_debug_enabled = false;
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
}
TRONE::~TRONE()
{
/* make sure we are truly inactive */
stop();
/* free any existing reports */
if (_reports != nullptr) {
delete _reports;
}
if (_class_instance != -1) {
unregister_class_devname(RANGE_FINDER_DEVICE_PATH, _class_instance);
}
// free perf counters
perf_free(_sample_perf);
perf_free(_comms_errors);
perf_free(_buffer_overflows);
}
int
TRONE::init()
{
int ret = ERROR;
/* do I2C init (and probe) first */
if (I2C::init() != OK) {
goto out;
}
/* allocate basic report buffers */
_reports = new RingBuffer(2, sizeof(range_finder_report));
if (_reports == nullptr) {
goto out;
}
_class_instance = register_class_devname(RANGE_FINDER_DEVICE_PATH);
if (_class_instance == CLASS_DEVICE_PRIMARY) {
/* get a publish handle on the range finder topic */
struct range_finder_report rf_report;
measure();
_reports->get(&rf_report);
_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report);
if (_range_finder_topic < 0) {
debug("failed to create sensor_range_finder object. Did you start uOrb?");
}
}
ret = OK;
/* sensor is ok, but we don't really know if it is within range */
_sensor_ok = true;
out:
return ret;
}
int
TRONE::probe()
{
return measure();
}
void
TRONE::set_minimum_distance(float min)
{
_min_distance = min;
}
void
TRONE::set_maximum_distance(float max)
{
_max_distance = max;
}
float
TRONE::get_minimum_distance()
{
return _min_distance;
}
float
TRONE::get_maximum_distance()
{
return _max_distance;
}
int
TRONE::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_measure_ticks = 0;
return OK;
/* external signalling (DRDY) not supported */
case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(TRONE_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0);
/* convert hz to tick interval via microseconds */
unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (ticks < USEC2TICK(TRONE_CONVERSION_INTERVAL)) {
return -EINVAL;
}
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
}
}
case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0) {
return SENSOR_POLLRATE_MANUAL;
}
return (1000 / _measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) {
return -EINVAL;
}
irqstate_t flags = irqsave();
if (!_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _reports->size();
case SENSORIOCRESET:
/* XXX implement this */
return -EINVAL;
case RANGEFINDERIOCSETMINIUMDISTANCE: {
set_minimum_distance(*(float *)arg);
return 0;
}
break;
case RANGEFINDERIOCSETMAXIUMDISTANCE: {
set_maximum_distance(*(float *)arg);
return 0;
}
break;
default:
/* give it to the superclass */
return I2C::ioctl(filp, cmd, arg);
}
}
ssize_t
TRONE::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(struct range_finder_report);
struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer);
int ret = 0;
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is enabled */
if (_measure_ticks > 0) {
/*
* While there is space in the caller's buffer, and reports, copy them.
* Note that we may be pre-empted by the workq thread while we are doing this;
* we are careful to avoid racing with them.
*/
while (count--) {
if (_reports->get(rbuf)) {
ret += sizeof(*rbuf);
rbuf++;
}
}
/* if there was no data, warn the caller */
return ret ? ret : -EAGAIN;
}
/* manual measurement - run one conversion */
do {
_reports->flush();
/* trigger a measurement */
if (OK != measure()) {
ret = -EIO;
break;
}
/* wait for it to complete */
usleep(TRONE_CONVERSION_INTERVAL);
/* run the collection phase */
if (OK != collect()) {
ret = -EIO;
break;
}
/* state machine will have generated a report, copy it out */
if (_reports->get(rbuf)) {
ret = sizeof(*rbuf);
}
} while (0);
return ret;
}
int
TRONE::measure()
{
int ret;
/*
* Send the command to begin a measurement.
*/
const uint8_t cmd = TRONE_MEASURE_REG;
ret = transfer(&cmd, sizeof(cmd), nullptr, 0);
if (OK != ret) {
perf_count(_comms_errors);
log("i2c::transfer returned %d", ret);
return ret;
}
ret = OK;
return ret;
}
int
TRONE::collect()
{
int ret = -EIO;
/* read from the sensor */
uint8_t val[3] = {0, 0, 0};
perf_begin(_sample_perf);
ret = transfer(nullptr, 0, &val[0], 3);
if (ret < 0) {
log("error reading from sensor: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
}
uint16_t distance = (val[0] << 8) | val[1];
float si_units = distance * 0.001f; /* mm to m */
struct range_finder_report report;
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
report.timestamp = hrt_absolute_time();
report.error_count = perf_event_count(_comms_errors);
report.distance = si_units;
report.valid = crc8(val, 2) == val[2] && si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
/* publish it, if we are the primary */
if (_range_finder_topic >= 0) {
orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
}
if (_reports->force(&report)) {
perf_count(_buffer_overflows);
}
/* notify anyone waiting for data */
poll_notify(POLLIN);
ret = OK;
perf_end(_sample_perf);
return ret;
}
void
TRONE::start()
{
/* reset the report ring and state machine */
_collect_phase = false;
_reports->flush();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&TRONE::cycle_trampoline, this, 1);
/* notify about state change */
struct subsystem_info_s info = {
true,
true,
true,
SUBSYSTEM_TYPE_RANGEFINDER
};
static orb_advert_t pub = -1;
if (pub > 0) {
orb_publish(ORB_ID(subsystem_info), pub, &info);
} else {
pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
}
void
TRONE::stop()
{
work_cancel(HPWORK, &_work);
}
void
TRONE::cycle_trampoline(void *arg)
{
TRONE *dev = (TRONE *)arg;
dev->cycle();
}
void
TRONE::cycle()
{
/* collection phase? */
if (_collect_phase) {
/* perform collection */
if (OK != collect()) {
log("collection error");
/* restart the measurement state machine */
start();
return;
}
/* next phase is measurement */
_collect_phase = false;
/*
* Is there a collect->measure gap?
*/
if (_measure_ticks > USEC2TICK(TRONE_CONVERSION_INTERVAL)) {
/* schedule a fresh cycle call when we are ready to measure again */
work_queue(HPWORK,
&_work,
(worker_t)&TRONE::cycle_trampoline,
this,
_measure_ticks - USEC2TICK(TRONE_CONVERSION_INTERVAL));
return;
}
}
/* measurement phase */
if (OK != measure()) {
log("measure error");
}
/* next phase is collection */
_collect_phase = true;
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&TRONE::cycle_trampoline,
this,
USEC2TICK(TRONE_CONVERSION_INTERVAL));
}
void
TRONE::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
perf_print_counter(_buffer_overflows);
printf("poll interval: %u ticks\n", _measure_ticks);
_reports->print_info("report queue");
}
/**
* Local functions in support of the shell command.
*/
namespace trone
{
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
const int ERROR = -1;
TRONE *g_dev;
void start();
void stop();
void test();
void reset();
void info();
/**
* Start the driver.
*/
void
start()
{
int fd;
if (g_dev != nullptr) {
errx(1, "already started");
}
/* create the driver */
g_dev = new TRONE(TRONE_BUS);
if (g_dev == nullptr) {
goto fail;
}
if (OK != g_dev->init()) {
goto fail;
}
/* set the poll rate to default, starts automatic data collection */
fd = open(TRONE_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
goto fail;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail;
}
exit(0);
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
errx(1, "driver start failed");
}
/**
* Stop the driver
*/
void stop()
{
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
} else {
errx(1, "driver not running");
}
exit(0);
}
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
void
test()
{
struct range_finder_report report;
ssize_t sz;
int ret;
int fd = open(TRONE_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
err(1, "%s open failed (try 'trone start' if the driver is not running", TRONE_DEVICE_PATH);
}
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
err(1, "immediate read failed");
}
warnx("single read");
warnx("measurement: %0.2f m", (double)report.distance);
warnx("time: %lld", report.timestamp);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
errx(1, "failed to set 2Hz poll rate");
}
/* read the sensor 50x and report each value */
for (unsigned i = 0; i < 50; i++) {
struct pollfd fds;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = poll(&fds, 1, 2000);
if (ret != 1) {
errx(1, "timed out waiting for sensor data");
}
/* now go get it */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
err(1, "periodic read failed");
}
warnx("periodic read %u", i);
warnx("valid %u", report.valid);
warnx("measurement: %0.3f", (double)report.distance);
warnx("time: %lld", report.timestamp);
}
/* reset the sensor polling to default rate */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
errx(1, "failed to set default poll rate");
}
errx(0, "PASS");
}
/**
* Reset the driver.
*/
void
reset()
{
int fd = open(TRONE_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
err(1, "failed ");
}
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
err(1, "driver reset failed");
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "driver poll restart failed");
}
exit(0);
}
/**
* Print a little info about the driver.
*/
void
info()
{
if (g_dev == nullptr) {
errx(1, "driver not running");
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
exit(0);
}
} // namespace
int
trone_main(int argc, char *argv[])
{
/*
* Start/load the driver.
*/
if (!strcmp(argv[1], "start")) {
trone::start();
}
/*
* Stop the driver
*/
if (!strcmp(argv[1], "stop")) {
trone::stop();
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test")) {
trone::test();
}
/*
* Reset the driver.
*/
if (!strcmp(argv[1], "reset")) {
trone::reset();
}
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
trone::info();
}
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
}
src/examples/flow_position_estimator/flow_position_estimator_main.c
+2 -2
View File
@@ -308,8 +308,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
if (vehicle_liftoff || params.debug)
{
/* copy flow */
flow_speed[0] = flow.flow_comp_x_m;
flow_speed[1] = flow.flow_comp_y_m;
flow_speed[0] = flow.pixel_flow_x_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m;
flow_speed[1] = flow.pixel_flow_y_integral / (flow.integration_timespan / 1e6f) * flow.ground_distance_m;
flow_speed[2] = 0.0f;
/* convert to bodyframe velocity */
src/lib/conversion/module.mk
+2
View File
@@ -36,3 +36,5 @@
#
SRCS = rotation.cpp
MAXOPTIMIZATION = -Os
src/lib/conversion/rotation.h
+3 -1
View File
@@ -74,6 +74,7 @@ enum Rotation {
ROTATION_ROLL_270_YAW_135 = 23,
ROTATION_PITCH_90 = 24,
ROTATION_PITCH_270 = 25,
ROTATION_ROLL_270_YAW_270 = 26,
ROTATION_MAX
};
@@ -109,7 +110,8 @@ const rot_lookup_t rot_lookup[] = {
{270, 0, 90 },
{270, 0, 135 },
{ 0, 90, 0 },
{ 0, 270, 0 }
{ 0, 270, 0 },
{270, 0, 270 }
};
/**
src/lib/external_lgpl/tecs/tecs.cpp
+16 -13
View File
@@ -236,9 +236,9 @@ void TECS::_update_height_demand(float demand, float state)
// // _hgt_rate_dem);
_hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last;
_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT;
_hgt_dem_adj_last = _hgt_dem_adj;
_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p;
// Limit height rate of change
if (_hgt_rate_dem > _maxClimbRate) {
_hgt_rate_dem = _maxClimbRate;
@@ -252,6 +252,11 @@ void TECS::_update_height_demand(float demand, float state)
void TECS::_detect_underspeed(void)
{
if (!_detect_underspeed_enabled) {
_underspeed = false;
return;
}
if (((_integ5_state < _TASmin * 0.9f) && (_throttle_dem >= _THRmaxf * 0.95f)) || ((_integ3_state < _hgt_dem_adj) && _underspeed)) {
_underspeed = true;
@@ -294,7 +299,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
// Calculate throttle demand
// If underspeed condition is set, then demand full throttle
if (_underspeed) {
_throttle_dem_unc = 1.0f;
_throttle_dem = 1.0f;
} else {
// Calculate gain scaler from specific energy error to throttle
@@ -316,31 +321,29 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr;
}
// Calculate PD + FF throttle
// Calculate PD + FF throttle and constrain to avoid blow-up of the integrator later
_throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
// Rate limit PD + FF throttle
// Calculate the throttle increment from the specified slew time
if (fabsf(_throttle_slewrate) > 0.01f) {
float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate;
_throttle_dem = constrain(_throttle_dem,
_last_throttle_dem - thrRateIncr,
_last_throttle_dem + thrRateIncr);
_last_throttle_dem - thrRateIncr,
_last_throttle_dem + thrRateIncr);
}
// Ensure _last_throttle_dem is always initialized properly
// Also: The throttle_slewrate limit is only applied to throttle_dem, but does not limit the integrator!!
_last_throttle_dem = _throttle_dem;
// Calculate integrator state upper and lower limits
// Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand
// Set to a value that will allow 0.1 (10%) throttle saturation to allow for noise on the demand
float integ_max = (_THRmaxf - _throttle_dem + 0.1f);
float integ_min = (_THRminf - _throttle_dem - 0.1f);
// Calculate integrator state, constraining state
// Set integrator to a max throttle value dduring climbout
// Set integrator to a max throttle value during climbout
_integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr;
if (_climbOutDem) {
@@ -358,10 +361,10 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
} else {
_throttle_dem = ff_throttle;
}
}
// Constrain throttle demand
_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
// Constrain throttle demand
_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
}
}
void TECS::_detect_bad_descent(void)
src/lib/external_lgpl/tecs/tecs.h
+16 -3
View File
@@ -47,6 +47,7 @@ public:
_rollComp(0.0f),
_spdWeight(0.5f),
_heightrate_p(0.0f),
_heightrate_ff(0.0f),
_speedrate_p(0.0f),
_throttle_dem(0.0f),
_pitch_dem(0.0f),
@@ -66,6 +67,9 @@ public:
_hgt_dem_prev(0.0f),
_TAS_dem_adj(0.0f),
_STEdotErrLast(0.0f),
_underspeed(false),
_detect_underspeed_enabled(true),
_badDescent(false),
_climbOutDem(false),
_SPE_dem(0.0f),
_SKE_dem(0.0f),
@@ -217,10 +221,18 @@ public:
_heightrate_p = heightrate_p;
}
void set_heightrate_ff(float heightrate_ff) {
_heightrate_ff = heightrate_ff;
}
void set_speedrate_p(float speedrate_p) {
_speedrate_p = speedrate_p;
}
void set_detect_underspeed_enabled(bool enabled) {
_detect_underspeed_enabled = enabled;
}
private:
struct tecs_state _tecs_state;
@@ -249,6 +261,7 @@ private:
float _rollComp;
float _spdWeight;
float _heightrate_p;
float _heightrate_ff;
float _speedrate_p;
// throttle demand in the range from 0.0 to 1.0
@@ -323,15 +336,15 @@ private:
// Underspeed condition
bool _underspeed;
// Underspeed detection enabled
bool _detect_underspeed_enabled;
// Bad descent condition caused by unachievable airspeed demand
bool _badDescent;
// climbout mode
bool _climbOutDem;
// throttle demand before limiting
float _throttle_dem_unc;
// pitch demand before limiting
float _pitch_dem_unc;
src/lib/geo/geo.c
+10 -7
View File
@@ -362,8 +362,12 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d
crosstrack_error->distance = 0.0f;
crosstrack_error->bearing = 0.0f;
dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
// Return error if arguments are bad
if (lat_now == 0.0 || lon_now == 0.0 || lat_start == 0.0 || lon_start == 0.0 || lat_end == 0.0d || lon_end == 0.0d) { return return_value; }
if (dist_to_end < 0.1f) {
return ERROR;
}
bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
@@ -377,7 +381,6 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d
return return_value;
}
dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
crosstrack_error->distance = (dist_to_end) * sinf(bearing_diff);
if (sin(bearing_diff) >= 0) {
@@ -414,10 +417,10 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
crosstrack_error->bearing = 0.0f;
// Return error if arguments are bad
if (lat_now == 0.0 || lon_now == 0.0 || lat_center == 0.0 || lon_center == 0.0 || radius == 0.0f) { return return_value; }
if (radius < 0.1f) { return return_value; }
if (arc_sweep >= 0) {
if (arc_sweep >= 0.0f) {
bearing_sector_start = arc_start_bearing;
bearing_sector_end = arc_start_bearing + arc_sweep;
@@ -463,8 +466,8 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
double start_disp_x = (double)radius * sin(arc_start_bearing);
double start_disp_y = (double)radius * cos(arc_start_bearing);
double end_disp_x = (double)radius * sin(_wrapPI((double)(arc_start_bearing + arc_sweep)));
double end_disp_y = (double)radius * cos(_wrapPI((double)(arc_start_bearing + arc_sweep)));
double end_disp_x = (double)radius * sin(_wrap_pi((double)(arc_start_bearing + arc_sweep)));
double end_disp_y = (double)radius * cos(_wrap_pi((double)(arc_start_bearing + arc_sweep)));
double lon_start = lon_now + start_disp_x / 111111.0;
double lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0;
double lon_end = lon_now + end_disp_x / 111111.0;
@@ -484,7 +487,7 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
}
crosstrack_error->bearing = _wrapPI((double)crosstrack_error->bearing);
crosstrack_error->bearing = _wrap_pi((double)crosstrack_error->bearing);
return_value = OK;
return return_value;
}
src/lib/launchdetection/CatapultLaunchMethod.cpp
+52 -18
View File
@@ -49,9 +49,11 @@ CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) :
SuperBlock(parent, "CAT"),
last_timestamp(hrt_absolute_time()),
integrator(0.0f),
launchDetected(false),
threshold_accel(this, "A"),
threshold_time(this, "T")
state(LAUNCHDETECTION_RES_NONE),
thresholdAccel(this, "A"),
thresholdTime(this, "T"),
motorDelay(this, "MDEL"),
pitchMaxPreThrottle(this, "PMAX")
{
}
@@ -65,34 +67,66 @@ void CatapultLaunchMethod::update(float accel_x)
float dt = (float)hrt_elapsed_time(&last_timestamp) * 1e-6f;
last_timestamp = hrt_absolute_time();
if (accel_x > threshold_accel.get()) {
integrator += accel_x * dt;
// warnx("*** integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f",
// (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt);
if (integrator > threshold_accel.get() * threshold_time.get()) {
launchDetected = true;
switch (state) {
case LAUNCHDETECTION_RES_NONE:
/* Detect a acceleration that is longer and stronger as the minimum given by the params */
if (accel_x > thresholdAccel.get()) {
integrator += dt;
if (integrator > thresholdTime.get()) {
if (motorDelay.get() > 0.0f) {
state = LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL;
warnx("Launch detected: state: enablecontrol, waiting %.2fs until using full"
" throttle", (double)motorDelay.get());
} else {
/* No motor delay set: go directly to enablemotors state */
state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
warnx("Launch detected: state: enablemotors (delay not activated)");
}
}
} else {
/* reset */
reset();
}
break;
case LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL:
/* Vehicle is currently controlling attitude but not with full throttle. Waiting undtil delay is
* over to allow full throttle */
motorDelayCounter += dt;
if (motorDelayCounter > motorDelay.get()) {
warnx("Launch detected: state enablemotors");
state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
}
break;
default:
break;
} else {
// warnx("integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f",
// (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt);
/* reset integrator */
integrator = 0.0f;
launchDetected = false;
}
}
bool CatapultLaunchMethod::getLaunchDetected()
LaunchDetectionResult CatapultLaunchMethod::getLaunchDetected() const
{
return launchDetected;
return state;
}
void CatapultLaunchMethod::reset()
{
integrator = 0.0f;
launchDetected = false;
motorDelayCounter = 0.0f;
state = LAUNCHDETECTION_RES_NONE;
}
float CatapultLaunchMethod::getPitchMax(float pitchMaxDefault) {
/* If motor is turned on do not impose the extra limit on maximum pitch */
if (state == LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
return pitchMaxDefault;
} else {
return pitchMaxPreThrottle.get();
}
}
}
src/lib/launchdetection/CatapultLaunchMethod.h
+11 -4
View File
@@ -57,16 +57,23 @@ public:
~CatapultLaunchMethod();
void update(float accel_x);
bool getLaunchDetected();
LaunchDetectionResult getLaunchDetected() const;
void reset();
float getPitchMax(float pitchMaxDefault);
private:
hrt_abstime last_timestamp;
float integrator;
bool launchDetected;
float motorDelayCounter;
control::BlockParamFloat threshold_accel;
control::BlockParamFloat threshold_time;
LaunchDetectionResult state;
control::BlockParamFloat thresholdAccel;
control::BlockParamFloat thresholdTime;
control::BlockParamFloat motorDelay;
control::BlockParamFloat pitchMaxPreThrottle; /**< Upper pitch limit before throttle is turned on.
Can be used to make sure that the AC does not climb
too much while attached to a bungee */
};
src/lib/launchdetection/LaunchDetector.cpp
+41 -6
View File
@@ -46,6 +46,7 @@ namespace launchdetection
LaunchDetector::LaunchDetector() :
SuperBlock(NULL, "LAUN"),
activeLaunchDetectionMethodIndex(-1),
launchdetection_on(this, "ALL_ON"),
throttlePreTakeoff(this, "THR_PRE")
{
@@ -65,7 +66,14 @@ LaunchDetector::~LaunchDetector()
void LaunchDetector::reset()
{
/* Reset all detectors */
launchMethods[0]->reset();
for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
launchMethods[i]->reset();
}
/* Reset active launchdetector */
activeLaunchDetectionMethodIndex = -1;
}
void LaunchDetector::update(float accel_x)
@@ -77,17 +85,44 @@ void LaunchDetector::update(float accel_x)
}
}
bool LaunchDetector::getLaunchDetected()
LaunchDetectionResult LaunchDetector::getLaunchDetected()
{
if (launchdetection_on.get() == 1) {
for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
if(launchMethods[i]->getLaunchDetected()) {
return true;
if (activeLaunchDetectionMethodIndex < 0) {
/* None of the active launchmethods has detected a launch, check all launchmethods */
for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
if(launchMethods[i]->getLaunchDetected() != LAUNCHDETECTION_RES_NONE) {
warnx("selecting launchmethod %d", i);
activeLaunchDetectionMethodIndex = i; // from now on only check this method
return launchMethods[i]->getLaunchDetected();
}
}
} else {
return launchMethods[activeLaunchDetectionMethodIndex]->getLaunchDetected();
}
}
return false;
return LAUNCHDETECTION_RES_NONE;
}
float LaunchDetector::getPitchMax(float pitchMaxDefault) {
if (!launchdetection_on.get()) {
return pitchMaxDefault;
}
/* if a lauchdetectionmethod is active or only one exists return the pitch limit from this method,
* otherwise use the default limit */
if (activeLaunchDetectionMethodIndex < 0) {
if (sizeof(launchMethods)/sizeof(LaunchMethod) > 1) {
return pitchMaxDefault;
} else {
return launchMethods[0]->getPitchMax(pitchMaxDefault);
}
} else {
return launchMethods[activeLaunchDetectionMethodIndex]->getPitchMax(pitchMaxDefault);
}
}
}
src/lib/launchdetection/LaunchDetector.h
+10 -1
View File
@@ -59,14 +59,23 @@ public:
void reset();
void update(float accel_x);
bool getLaunchDetected();
LaunchDetectionResult getLaunchDetected();
bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); };
float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); }
/* Returns a maximum pitch in deg. Different launch methods may impose upper pitch limits during launch */
float getPitchMax(float pitchMaxDefault);
// virtual bool getLaunchDetected();
protected:
private:
int activeLaunchDetectionMethodIndex; /**< holds a index to the launchMethod in the array launchMethods
which detected a Launch. If no launchMethod has detected a launch yet the
value is -1. Once one launchMetthod has detected a launch only this
method is checked for further adavancing in the state machine (e.g. when
to power up the motors) */
LaunchMethod* launchMethods[1];
control::BlockParamInt launchdetection_on;
control::BlockParamFloat throttlePreTakeoff;
src/lib/launchdetection/LaunchMethod.h
+15 -1
View File
@@ -44,13 +44,27 @@
namespace launchdetection
{
enum LaunchDetectionResult {
LAUNCHDETECTION_RES_NONE = 0, /**< No launch has been detected */
LAUNCHDETECTION_RES_DETECTED_ENABLECONTROL = 1, /**< Launch has been detected, the controller should
control the attitude. However any motors should not throttle
up and still be set to 'throttlePreTakeoff'.
For instance this is used to have a delay for the motor
when launching a fixed wing aircraft from a bungee */
LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS = 2 /**< Launch has been detected, teh controller should control
attitude and also throttle up the motors. */
};
class LaunchMethod
{
public:
virtual void update(float accel_x) = 0;
virtual bool getLaunchDetected() = 0;
virtual LaunchDetectionResult getLaunchDetected() const = 0;
virtual void reset() = 0;
/* Returns a upper pitch limit if required, otherwise returns pitchMaxDefault */
virtual float getPitchMax(float pitchMaxDefault) = 0;
protected:
private:
};
src/lib/launchdetection/launchdetection_params.c
+25
View File
@@ -77,6 +77,31 @@ PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f);
*/
PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f);
/**
* Motor delay
*
* Delay between starting attitude control and powering up the throttle (giving throttle control to the controller)
* Before this timespan is up the throttle will be set to LAUN_THR_PRE, set to 0 to deactivate
*
* @unit seconds
* @min 0
* @group Launch detection
*/
PARAM_DEFINE_FLOAT(LAUN_CAT_MDEL, 0.0f);
/**
* Maximum pitch before the throttle is powered up (during motor delay phase)
*
* This is an extra limit for the maximum pitch which is imposed in the phase before the throttle turns on.
* This allows to limit the maximum pitch angle during a bungee launch (make the launch less steep).
*
* @unit deg
* @min 0
* @max 45
* @group Launch detection
*/
PARAM_DEFINE_FLOAT(LAUN_CAT_PMAX, 30.0f);
/**
* Throttle setting while detecting launch.
*
src/lib/rc/module.mk
+40
View File
@@ -0,0 +1,40 @@
############################################################################
#
# Copyright (c) 2014 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Yuntec ST24 transmitter protocol decoder
#
SRCS = st24.c
MAXOPTIMIZATION = -Os
src/lib/rc/st24.c
+253
View File
@@ -0,0 +1,253 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*
* @file st24.h
*
* RC protocol implementation for Yuneec ST24 transmitter.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#include <stdbool.h>
#include <stdio.h>
#include "st24.h"
enum ST24_DECODE_STATE {
ST24_DECODE_STATE_UNSYNCED = 0,
ST24_DECODE_STATE_GOT_STX1,
ST24_DECODE_STATE_GOT_STX2,
ST24_DECODE_STATE_GOT_LEN,
ST24_DECODE_STATE_GOT_TYPE,
ST24_DECODE_STATE_GOT_DATA
};
const char *decode_states[] = {"UNSYNCED",
"GOT_STX1",
"GOT_STX2",
"GOT_LEN",
"GOT_TYPE",
"GOT_DATA"
};
/* define range mapping here, -+100% -> 1000..2000 */
#define ST24_RANGE_MIN 0.0f
#define ST24_RANGE_MAX 4096.0f
#define ST24_TARGET_MIN 1000.0f
#define ST24_TARGET_MAX 2000.0f
/* pre-calculate the floating point stuff as far as possible at compile time */
#define ST24_SCALE_FACTOR ((ST24_TARGET_MAX - ST24_TARGET_MIN) / (ST24_RANGE_MAX - ST24_RANGE_MIN))
#define ST24_SCALE_OFFSET (int)(ST24_TARGET_MIN - (ST24_SCALE_FACTOR * ST24_RANGE_MIN + 0.5f))
static enum ST24_DECODE_STATE _decode_state = ST24_DECODE_STATE_UNSYNCED;
static unsigned _rxlen;
static ReceiverFcPacket _rxpacket;
uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len)
{
uint8_t i, crc ;
crc = 0;
while (len--) {
for (i = 0x80; i != 0; i >>= 1) {
if ((crc & 0x80) != 0) {
crc <<= 1;
crc ^= 0x07;
} else {
crc <<= 1;
}
if ((*ptr & i) != 0) {
crc ^= 0x07;
}
}
ptr++;
}
return (crc);
}
int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels,
uint16_t max_chan_count)
{
int ret = 1;
switch (_decode_state) {
case ST24_DECODE_STATE_UNSYNCED:
if (byte == ST24_STX1) {
_decode_state = ST24_DECODE_STATE_GOT_STX1;
} else {
ret = 3;
}
break;
case ST24_DECODE_STATE_GOT_STX1:
if (byte == ST24_STX2) {
_decode_state = ST24_DECODE_STATE_GOT_STX2;
} else {
_decode_state = ST24_DECODE_STATE_UNSYNCED;
}
break;
case ST24_DECODE_STATE_GOT_STX2:
/* ensure no data overflow failure or hack is possible */
if ((unsigned)byte <= sizeof(_rxpacket.length) + sizeof(_rxpacket.type) + sizeof(_rxpacket.st24_data)) {
_rxpacket.length = byte;
_rxlen = 0;
_decode_state = ST24_DECODE_STATE_GOT_LEN;
} else {
_decode_state = ST24_DECODE_STATE_UNSYNCED;
}
break;
case ST24_DECODE_STATE_GOT_LEN:
_rxpacket.type = byte;
_rxlen++;
_decode_state = ST24_DECODE_STATE_GOT_TYPE;
break;
case ST24_DECODE_STATE_GOT_TYPE:
_rxpacket.st24_data[_rxlen - 1] = byte;
_rxlen++;
if (_rxlen == (_rxpacket.length - 1)) {
_decode_state = ST24_DECODE_STATE_GOT_DATA;
}
break;
case ST24_DECODE_STATE_GOT_DATA:
_rxpacket.crc8 = byte;
_rxlen++;
if (st24_common_crc8((uint8_t *) & (_rxpacket.length), _rxlen) == _rxpacket.crc8) {
ret = 0;
/* decode the actual packet */
switch (_rxpacket.type) {
case ST24_PACKET_TYPE_CHANNELDATA12: {
ChannelData12 *d = (ChannelData12 *)_rxpacket.st24_data;
*rssi = d->rssi;
*rx_count = d->packet_count;
/* this can lead to rounding of the strides */
*channel_count = (max_chan_count < 12) ? max_chan_count : 12;
unsigned stride_count = (*channel_count * 3) / 2;
unsigned chan_index = 0;
for (unsigned i = 0; i < stride_count; i += 3) {
channels[chan_index] = ((uint16_t)d->channel[i] << 4);
channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4);
/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
chan_index++;
channels[chan_index] = ((uint16_t)d->channel[i + 2]);
channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8);
/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
chan_index++;
}
}
break;
case ST24_PACKET_TYPE_CHANNELDATA24: {
ChannelData24 *d = (ChannelData24 *)&_rxpacket.st24_data;
*rssi = d->rssi;
*rx_count = d->packet_count;
/* this can lead to rounding of the strides */
*channel_count = (max_chan_count < 24) ? max_chan_count : 24;
unsigned stride_count = (*channel_count * 3) / 2;
unsigned chan_index = 0;
for (unsigned i = 0; i < stride_count; i += 3) {
channels[chan_index] = ((uint16_t)d->channel[i] << 4);
channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4);
/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
chan_index++;
channels[chan_index] = ((uint16_t)d->channel[i + 2]);
channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8);
/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
chan_index++;
}
}
break;
case ST24_PACKET_TYPE_TRANSMITTERGPSDATA: {
// ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket.st24_data;
/* we silently ignore this data for now, as it is unused */
ret = 2;
}
break;
default:
ret = 2;
break;
}
} else {
/* decoding failed */
ret = 4;
}
_decode_state = ST24_DECODE_STATE_UNSYNCED;
break;
}
return ret;
}
src/lib/rc/st24.h
+163
View File
@@ -0,0 +1,163 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file st24.h
*
* RC protocol definition for Yuneec ST24 transmitter
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
#pragma once
#include <stdint.h>
__BEGIN_DECLS
#define ST24_DATA_LEN_MAX 64
#define ST24_STX1 0x55
#define ST24_STX2 0x55
enum ST24_PACKET_TYPE {
ST24_PACKET_TYPE_CHANNELDATA12 = 0,
ST24_PACKET_TYPE_CHANNELDATA24,
ST24_PACKET_TYPE_TRANSMITTERGPSDATA
};
#pragma pack(push, 1)
typedef struct {
uint8_t header1; ///< 0x55 for a valid packet
uint8_t header2; ///< 0x55 for a valid packet
uint8_t length; ///< length includes type, data, and crc = sizeof(type)+sizeof(data[payload_len])+sizeof(crc8)
uint8_t type; ///< from enum ST24_PACKET_TYPE
uint8_t st24_data[ST24_DATA_LEN_MAX];
uint8_t crc8; ///< crc8 checksum, calculated by st24_common_crc8 and including fields length, type and st24_data
} ReceiverFcPacket;
/**
* RC Channel data (12 channels).
*
* This is incoming from the ST24
*/
typedef struct {
uint16_t t; ///< packet counter or clock
uint8_t rssi; ///< signal strength
uint8_t packet_count; ///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate)
uint8_t channel[18]; ///< channel data, 12 channels (12 bit numbers)
} ChannelData12;
/**
* RC Channel data (12 channels).
*
*/
typedef struct {
uint16_t t; ///< packet counter or clock
uint8_t rssi; ///< signal strength
uint8_t packet_count; ///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate)
uint8_t channel[36]; ///< channel data, 24 channels (12 bit numbers)
} ChannelData24;
/**
* Telemetry packet
*
* This is outgoing to the ST24
*
* imuStatus:
* 8 bit total
* bits 0-2 for status
* - value 0 is FAILED
* - value 1 is INITIALIZING
* - value 2 is RUNNING
* - values 3 through 7 are reserved
* bits 3-7 are status for sensors (0 or 1)
* - mpu6050
* - accelerometer
* - primary gyro x
* - primary gyro y
* - primary gyro z
*
* pressCompassStatus
* 8 bit total
* bits 0-3 for compass status
* - value 0 is FAILED
* - value 1 is INITIALIZING
* - value 2 is RUNNING
* - value 3 - 15 are reserved
* bits 4-7 for pressure status
* - value 0 is FAILED
* - value 1 is INITIALIZING
* - value 2 is RUNNING
* - value 3 - 15 are reserved
*
*/
typedef struct {
uint16_t t; ///< packet counter or clock
int32_t lat; ///< lattitude (degrees) +/- 90 deg
int32_t lon; ///< longitude (degrees) +/- 180 deg
int32_t alt; ///< 0.01m resolution, altitude (meters)
int16_t vx, vy, vz; ///< velocity 0.01m res, +/-320.00 North-East- Down
uint8_t nsat; ///<number of satellites
uint8_t voltage; ///< 25.4V voltage = 5 + 255*0.1 = 30.5V, min=5V
uint8_t current; ///< 0.5A resolution
int16_t roll, pitch, yaw; ///< 0.01 degree resolution
uint8_t motorStatus; ///< 1 bit per motor for status 1=good, 0= fail
uint8_t imuStatus; ///< inertial measurement unit status
uint8_t pressCompassStatus; ///< baro / compass status
} TelemetryData;
#pragma pack(pop)
/**
* CRC8 implementation for ST24 protocol
*
* @param prt Pointer to the data to CRC
* @param len number of bytes to accumulate in the checksum
* @return the checksum of these bytes over len
*/
uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len);
/**
* Decoder for ST24 protocol
*
* @param byte current char to read
* @param rssi pointer to a byte where the RSSI value is written back to
* @param rx_count pointer to a byte where the receive count of packets signce last wireless frame is written back to
* @param channels pointer to a datastructure of size max_chan_count where channel values (12 bit) are written back to
* @param max_chan_count maximum channels to decode - if more channels are decoded, the last n are skipped and success (0) is returned
* @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 2 for unknown packet, 3 for out of sync, 4 for checksum error
*/
__EXPORT int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count,
uint16_t *channels, uint16_t max_chan_count);
__END_DECLS
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
-4
View File
@@ -208,10 +208,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
float debugOutput[4] = { 0.0f };
// print text
printf("Extended Kalman Filter Attitude Estimator initialized..\n\n");
fflush(stdout);
int overloadcounter = 19;
/* Initialize filter */
src/modules/bottle_drop/bottle_drop.cpp
+906
View File
@@ -0,0 +1,906 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* 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 bottle_drop.cpp
*
* Bottle drop module for Outback Challenge 2014, Team Swiss Fang
*
* @author Dominik Juchli <juchlid@ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <time.h>
#include <sys/ioctl.h>
#include <drivers/device/device.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/parameter_update.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <dataman/dataman.h>
#include <mathlib/mathlib.h>
#include <mavlink/mavlink_log.h>
/**
* bottle_drop app start / stop handling function
*
* @ingroup apps
*/
extern "C" __EXPORT int bottle_drop_main(int argc, char *argv[]);
class BottleDrop
{
public:
/**
* Constructor
*/
BottleDrop();
/**
* Destructor, also kills task.
*/
~BottleDrop();
/**
* Start the task.
*
* @return OK on success.
*/
int start();
/**
* Display status.
*/
void status();
void open_bay();
void close_bay();
void drop();
void lock_release();
private:
bool _task_should_exit; /**< if true, task should exit */
int _main_task; /**< handle for task */
int _mavlink_fd;
int _command_sub;
int _wind_estimate_sub;
struct vehicle_command_s _command;
struct vehicle_global_position_s _global_pos;
map_projection_reference_s ref;
orb_advert_t _actuator_pub;
struct actuator_controls_s _actuators;
bool _drop_approval;
hrt_abstime _doors_opened;
hrt_abstime _drop_time;
float _alt_clearance;
struct position_s {
double lat; ///< degrees
double lon; ///< degrees
float alt; ///< m
} _target_position, _drop_position;
enum DROP_STATE {
DROP_STATE_INIT = 0,
DROP_STATE_TARGET_VALID,
DROP_STATE_TARGET_SET,
DROP_STATE_BAY_OPEN,
DROP_STATE_DROPPED,
DROP_STATE_BAY_CLOSED
} _drop_state;
struct mission_s _onboard_mission;
orb_advert_t _onboard_mission_pub;
void task_main();
void handle_command(struct vehicle_command_s *cmd);
void answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESULT result);
/**
* Set the actuators
*/
int actuators_publish();
/**
* Shim for calling task_main from task_create.
*/
static void task_main_trampoline(int argc, char *argv[]);
};
namespace bottle_drop
{
BottleDrop *g_bottle_drop;
}
BottleDrop::BottleDrop() :
_task_should_exit(false),
_main_task(-1),
_mavlink_fd(-1),
_command_sub(-1),
_wind_estimate_sub(-1),
_command {},
_global_pos {},
ref {},
_actuator_pub(-1),
_actuators {},
_drop_approval(false),
_doors_opened(0),
_drop_time(0),
_alt_clearance(70.0f),
_target_position {},
_drop_position {},
_drop_state(DROP_STATE_INIT),
_onboard_mission {},
_onboard_mission_pub(-1)
{
}
BottleDrop::~BottleDrop()
{
if (_main_task != -1) {
/* task wakes up every 100ms or so at the longest */
_task_should_exit = true;
/* wait for a second for the task to quit at our request */
unsigned i = 0;
do {
/* wait 20ms */
usleep(20000);
/* if we have given up, kill it */
if (++i > 50) {
task_delete(_main_task);
break;
}
} while (_main_task != -1);
}
bottle_drop::g_bottle_drop = nullptr;
}
int
BottleDrop::start()
{
ASSERT(_main_task == -1);
/* start the task */
_main_task = task_spawn_cmd("bottle_drop",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT + 15,
1500,
(main_t)&BottleDrop::task_main_trampoline,
nullptr);
if (_main_task < 0) {
warn("task start failed");
return -errno;
}
return OK;
}
void
BottleDrop::status()
{
warnx("drop state: %d", _drop_state);
}
void
BottleDrop::open_bay()
{
_actuators.control[0] = -1.0f;
_actuators.control[1] = 1.0f;
if (_doors_opened == 0) {
_doors_opened = hrt_absolute_time();
}
warnx("open doors");
actuators_publish();
usleep(500 * 1000);
}
void
BottleDrop::close_bay()
{
// closed door and locked survival kit
_actuators.control[0] = 1.0f;
_actuators.control[1] = -1.0f;
_doors_opened = 0;
actuators_publish();
// delay until the bay is closed
usleep(500 * 1000);
}
void
BottleDrop::drop()
{
// update drop actuator, wait 0.5s until the doors are open before dropping
hrt_abstime starttime = hrt_absolute_time();
// force the door open if we have to
if (_doors_opened == 0) {
open_bay();
}
while (hrt_elapsed_time(&_doors_opened) < 500 * 1000 && hrt_elapsed_time(&starttime) < 2000000) {
usleep(50000);
warnx("delayed by door!");
}
_actuators.control[2] = 1.0f;
_drop_time = hrt_absolute_time();
actuators_publish();
warnx("dropping now");
// Give it time to drop
usleep(1000 * 1000);
}
void
BottleDrop::lock_release()
{
_actuators.control[2] = -1.0f;
actuators_publish();
warnx("closing release");
}
int
BottleDrop::actuators_publish()
{
_actuators.timestamp = hrt_absolute_time();
// lazily publish _actuators only once available
if (_actuator_pub > 0) {
return orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &_actuators);
} else {
_actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators);
if (_actuator_pub > 0) {
return OK;
} else {
return -1;
}
}
}
void
BottleDrop::task_main()
{
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(_mavlink_fd, "[bottle_drop] started");
_command_sub = orb_subscribe(ORB_ID(vehicle_command));
_wind_estimate_sub = orb_subscribe(ORB_ID(wind_estimate));
bool updated = false;
float z_0; // ground properties
float turn_radius; // turn radius of the UAV
float precision; // Expected precision of the UAV
float ground_distance = _alt_clearance; // Replace by closer estimate in loop
// constant
float g = CONSTANTS_ONE_G; // constant of gravity [m/s^2]
float m = 0.5f; // mass of bottle [kg]
float rho = 1.2f; // air density [kg/m^3]
float A = ((0.063f * 0.063f) / 4.0f * M_PI_F); // Bottle cross section [m^2]
float dt_freefall_prediction = 0.01f; // step size of the free fall prediction [s]
// Has to be estimated by experiment
float cd = 0.86f; // Drag coefficient for a cylinder with a d/l ratio of 1/3 []
float t_signal =
0.084f; // Time span between sending the signal and the bottle top reaching level height with the bottom of the plane [s]
float t_door =
0.7f; // The time the system needs to open the door + safety, is also the time the palyload needs to safely escape the shaft [s]
// Definition
float h_0; // height over target
float az; // acceleration in z direction[m/s^2]
float vz; // velocity in z direction [m/s]
float z; // fallen distance [m]
float h; // height over target [m]
float ax; // acceleration in x direction [m/s^2]
float vx; // ground speed in x direction [m/s]
float x; // traveled distance in x direction [m]
float vw; // wind speed [m/s]
float vrx; // relative velocity in x direction [m/s]
float v; // relative speed vector [m/s]
float Fd; // Drag force [N]
float Fdx; // Drag force in x direction [N]
float Fdz; // Drag force in z direction [N]
float x_drop, y_drop; // coordinates of the drop point in reference to the target (projection of NED)
float x_t, y_t; // coordinates of the target in reference to the target x_t = 0, y_t = 0 (projection of NED)
float x_l, y_l; // local position in projected coordinates
float x_f, y_f; // to-be position of the UAV after dt_runs seconds in projected coordinates
double x_f_NED, y_f_NED; // to-be position of the UAV after dt_runs seconds in NED
float distance_open_door; // The distance the UAV travels during its doors open [m]
float approach_error = 0.0f; // The error in radians between current ground vector and desired ground vector
float distance_real = 0; // The distance between the UAVs position and the drop point [m]
float future_distance = 0; // The distance between the UAVs to-be position and the drop point [m]
unsigned counter = 0;
param_t param_gproperties = param_find("BD_GPROPERTIES");
param_t param_turn_radius = param_find("BD_TURNRADIUS");
param_t param_precision = param_find("BD_PRECISION");
param_t param_cd = param_find("BD_OBJ_CD");
param_t param_mass = param_find("BD_OBJ_MASS");
param_t param_surface = param_find("BD_OBJ_SURFACE");
param_get(param_precision, &precision);
param_get(param_turn_radius, &turn_radius);
param_get(param_gproperties, &z_0);
param_get(param_cd, &cd);
param_get(param_mass, &m);
param_get(param_surface, &A);
int vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
struct parameter_update_s update;
memset(&update, 0, sizeof(update));
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
struct mission_item_s flight_vector_s {};
struct mission_item_s flight_vector_e {};
flight_vector_s.nav_cmd = NAV_CMD_WAYPOINT;
flight_vector_s.acceptance_radius = 50; // TODO: make parameter
flight_vector_s.autocontinue = true;
flight_vector_s.altitude_is_relative = false;
flight_vector_e.nav_cmd = NAV_CMD_WAYPOINT;
flight_vector_e.acceptance_radius = 50; // TODO: make parameter
flight_vector_e.autocontinue = true;
flight_vector_s.altitude_is_relative = false;
struct wind_estimate_s wind;
// wakeup source(s)
struct pollfd fds[1];
// Setup of loop
fds[0].fd = _command_sub;
fds[0].events = POLLIN;
// Whatever state the bay is in, we want it closed on startup
lock_release();
close_bay();
while (!_task_should_exit) {
/* wait for up to 100ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50);
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("poll error %d, %d", pret, errno);
continue;
}
/* vehicle commands updated */
if (fds[0].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_command), _command_sub, &_command);
handle_command(&_command);
}
orb_check(vehicle_global_position_sub, &updated);
if (updated) {
/* copy global position */
orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos);
}
if (_global_pos.timestamp == 0) {
continue;
}
const unsigned sleeptime_us = 9500;
hrt_abstime last_run = hrt_absolute_time();
float dt_runs = sleeptime_us / 1e6f;
// switch to faster updates during the drop
while (_drop_state > DROP_STATE_INIT) {
// Get wind estimate
orb_check(_wind_estimate_sub, &updated);
if (updated) {
orb_copy(ORB_ID(wind_estimate), _wind_estimate_sub, &wind);
}
// Get vehicle position
orb_check(vehicle_global_position_sub, &updated);
if (updated) {
// copy global position
orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos);
}
// Get parameter updates
orb_check(parameter_update_sub, &updated);
if (updated) {
// copy global position
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
// update all parameters
param_get(param_gproperties, &z_0);
param_get(param_turn_radius, &turn_radius);
param_get(param_precision, &precision);
}
orb_check(_command_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_command), _command_sub, &_command);
handle_command(&_command);
}
float windspeed_norm = sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east);
float groundspeed_body = sqrtf(_global_pos.vel_n * _global_pos.vel_n + _global_pos.vel_e * _global_pos.vel_e);
ground_distance = _global_pos.alt - _target_position.alt;
// Distance to drop position and angle error to approach vector
// are relevant in all states greater than target valid (which calculates these positions)
if (_drop_state > DROP_STATE_TARGET_VALID) {
distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, _drop_position.lon));
float ground_direction = atan2f(_global_pos.vel_e, _global_pos.vel_n);
float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon);
approach_error = _wrap_pi(ground_direction - approach_direction);
if (counter % 90 == 0) {
mavlink_log_info(_mavlink_fd, "drop distance %u, heading error %u", (unsigned)distance_real, (unsigned)math::degrees(approach_error));
}
}
switch (_drop_state) {
case DROP_STATE_TARGET_VALID:
{
az = g; // acceleration in z direction[m/s^2]
vz = 0; // velocity in z direction [m/s]
z = 0; // fallen distance [m]
h_0 = _global_pos.alt - _target_position.alt; // height over target at start[m]
h = h_0; // height over target [m]
ax = 0; // acceleration in x direction [m/s^2]
vx = groundspeed_body;// XXX project // ground speed in x direction [m/s]
x = 0; // traveled distance in x direction [m]
vw = 0; // wind speed [m/s]
vrx = 0; // relative velocity in x direction [m/s]
v = groundspeed_body; // relative speed vector [m/s]
Fd = 0; // Drag force [N]
Fdx = 0; // Drag force in x direction [N]
Fdz = 0; // Drag force in z direction [N]
// Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x
while (h > 0.05f) {
// z-direction
vz = vz + az * dt_freefall_prediction;
z = z + vz * dt_freefall_prediction;
h = h_0 - z;
// x-direction
vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0);
vx = vx + ax * dt_freefall_prediction;
x = x + vx * dt_freefall_prediction;
vrx = vx + vw;
// drag force
v = sqrtf(vz * vz + vrx * vrx);
Fd = 0.5f * rho * A * cd * (v * v);
Fdx = Fd * vrx / v;
Fdz = Fd * vz / v;
// acceleration
az = g - Fdz / m;
ax = -Fdx / m;
}
// compute drop vector
x = groundspeed_body * t_signal + x;
x_t = 0.0f;
y_t = 0.0f;
float wind_direction_n, wind_direction_e;
if (windspeed_norm < 0.5f) { // If there is no wind, an arbitrarily direction is chosen
wind_direction_n = 1.0f;
wind_direction_e = 0.0f;
} else {
wind_direction_n = wind.windspeed_north / windspeed_norm;
wind_direction_e = wind.windspeed_east / windspeed_norm;
}
x_drop = x_t + x * wind_direction_n;
y_drop = y_t + x * wind_direction_e;
map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon);
_drop_position.alt = _target_position.alt + _alt_clearance;
// Compute flight vector
map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_e,
&(flight_vector_s.lat), &(flight_vector_s.lon));
flight_vector_s.altitude = _drop_position.alt;
map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_n, y_drop - turn_radius * wind_direction_e,
&flight_vector_e.lat, &flight_vector_e.lon);
flight_vector_e.altitude = _drop_position.alt;
// Save WPs in datamanager
const ssize_t len = sizeof(struct mission_item_s);
if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) {
warnx("ERROR: could not save onboard WP");
}
if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 1, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_e, len) != len) {
warnx("ERROR: could not save onboard WP");
}
_onboard_mission.count = 2;
_onboard_mission.current_seq = 0;
if (_onboard_mission_pub > 0) {
orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
} else {
_onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &_onboard_mission);
}
float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon);
mavlink_log_critical(_mavlink_fd, "position set, approach heading: %u", (unsigned)distance_real, (unsigned)math::degrees(approach_direction + M_PI_F));
_drop_state = DROP_STATE_TARGET_SET;
}
break;
case DROP_STATE_TARGET_SET:
{
float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon);
if (distance_wp2 < distance_real) {
_onboard_mission.current_seq = 0;
orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
} else {
// We're close enough - open the bay
distance_open_door = math::max(10.0f, 3.0f * fabsf(t_door * groundspeed_body));
if (isfinite(distance_real) && distance_real < distance_open_door &&
fabsf(approach_error) < math::radians(20.0f)) {
open_bay();
_drop_state = DROP_STATE_BAY_OPEN;
mavlink_log_info(_mavlink_fd, "#audio: opening bay");
}
}
}
break;
case DROP_STATE_BAY_OPEN:
{
if (_drop_approval) {
map_projection_project(&ref, _global_pos.lat, _global_pos.lon, &x_l, &y_l);
x_f = x_l + _global_pos.vel_n * dt_runs;
y_f = y_l + _global_pos.vel_e * dt_runs;
map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED);
future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon);
if (isfinite(distance_real) &&
(distance_real < precision) && ((distance_real < future_distance))) {
drop();
_drop_state = DROP_STATE_DROPPED;
mavlink_log_info(_mavlink_fd, "#audio: payload dropped");
} else {
float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon);
if (distance_wp2 < distance_real) {
_onboard_mission.current_seq = 0;
orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
}
}
}
}
break;
case DROP_STATE_DROPPED:
/* 2s after drop, reset and close everything again */
if ((hrt_elapsed_time(&_doors_opened) > 2 * 1000 * 1000)) {
_drop_state = DROP_STATE_INIT;
_drop_approval = false;
lock_release();
close_bay();
mavlink_log_info(_mavlink_fd, "#audio: closing bay");
// remove onboard mission
_onboard_mission.current_seq = -1;
_onboard_mission.count = 0;
orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
}
break;
}
counter++;
// update_actuators();
// run at roughly 100 Hz
usleep(sleeptime_us);
dt_runs = hrt_elapsed_time(&last_run) / 1e6f;
last_run = hrt_absolute_time();
}
}
warnx("exiting.");
_main_task = -1;
_exit(0);
}
void
BottleDrop::handle_command(struct vehicle_command_s *cmd)
{
switch (cmd->command) {
case VEHICLE_CMD_CUSTOM_0:
/*
* param1 and param2 set to 1: open and drop
* param1 set to 1: open
* else: close (and don't drop)
*/
if (cmd->param1 > 0.5f && cmd->param2 > 0.5f) {
open_bay();
drop();
mavlink_log_critical(_mavlink_fd, "drop bottle");
} else if (cmd->param1 > 0.5f) {
open_bay();
mavlink_log_critical(_mavlink_fd, "opening bay");
} else {
lock_release();
close_bay();
mavlink_log_critical(_mavlink_fd, "closing bay");
}
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
break;
case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY:
switch ((int)(cmd->param1 + 0.5f)) {
case 0:
_drop_approval = false;
mavlink_log_critical(_mavlink_fd, "got drop position, no approval");
break;
case 1:
_drop_approval = true;
mavlink_log_critical(_mavlink_fd, "got drop position and approval");
break;
default:
_drop_approval = false;
warnx("param1 val unknown");
break;
}
// XXX check all fields (2-3)
_alt_clearance = cmd->param4;
_target_position.lat = cmd->param5;
_target_position.lon = cmd->param6;
_target_position.alt = cmd->param7;
_drop_state = DROP_STATE_TARGET_VALID;
mavlink_log_info(_mavlink_fd, "got target: %8.4f, %8.4f, %8.4f", (double)_target_position.lat,
(double)_target_position.lon, (double)_target_position.alt);
map_projection_init(&ref, _target_position.lat, _target_position.lon);
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
break;
case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY:
if (cmd->param1 < 0) {
// Clear internal states
_drop_approval = false;
_drop_state = DROP_STATE_INIT;
// Abort if mission is present
_onboard_mission.current_seq = -1;
if (_onboard_mission_pub > 0) {
orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
}
} else {
switch ((int)(cmd->param1 + 0.5f)) {
case 0:
_drop_approval = false;
break;
case 1:
_drop_approval = true;
mavlink_log_info(_mavlink_fd, "#audio: got drop approval");
break;
default:
_drop_approval = false;
break;
// XXX handle other values
}
}
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
break;
default:
break;
}
}
void
BottleDrop::answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESULT result)
{
switch (result) {
case VEHICLE_CMD_RESULT_ACCEPTED:
break;
case VEHICLE_CMD_RESULT_DENIED:
mavlink_log_critical(_mavlink_fd, "command denied: %u", cmd->command);
break;
case VEHICLE_CMD_RESULT_FAILED:
mavlink_log_critical(_mavlink_fd, "command failed: %u", cmd->command);
break;
case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
mavlink_log_critical(_mavlink_fd, "command temporarily rejected: %u", cmd->command);
break;
case VEHICLE_CMD_RESULT_UNSUPPORTED:
mavlink_log_critical(_mavlink_fd, "command unsupported: %u", cmd->command);
break;
default:
break;
}
}
void
BottleDrop::task_main_trampoline(int argc, char *argv[])
{
bottle_drop::g_bottle_drop->task_main();
}
static void usage()
{
errx(1, "usage: bottle_drop {start|stop|status}");
}
int bottle_drop_main(int argc, char *argv[])
{
if (argc < 2) {
usage();
}
if (!strcmp(argv[1], "start")) {
if (bottle_drop::g_bottle_drop != nullptr) {
errx(1, "already running");
}
bottle_drop::g_bottle_drop = new BottleDrop;
if (bottle_drop::g_bottle_drop == nullptr) {
errx(1, "alloc failed");
}
if (OK != bottle_drop::g_bottle_drop->start()) {
delete bottle_drop::g_bottle_drop;
bottle_drop::g_bottle_drop = nullptr;
err(1, "start failed");
}
return 0;
}
if (bottle_drop::g_bottle_drop == nullptr) {
errx(1, "not running");
}
if (!strcmp(argv[1], "stop")) {
delete bottle_drop::g_bottle_drop;
bottle_drop::g_bottle_drop = nullptr;
} else if (!strcmp(argv[1], "status")) {
bottle_drop::g_bottle_drop->status();
} else if (!strcmp(argv[1], "drop")) {
bottle_drop::g_bottle_drop->drop();
} else if (!strcmp(argv[1], "open")) {
bottle_drop::g_bottle_drop->open_bay();
} else if (!strcmp(argv[1], "close")) {
bottle_drop::g_bottle_drop->close_bay();
} else if (!strcmp(argv[1], "lock")) {
bottle_drop::g_bottle_drop->lock_release();
} else {
usage();
}
return 0;
}
src/modules/bottle_drop/bottle_drop_params.c
+131
View File
@@ -0,0 +1,131 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* 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 bottle_drop_params.c
* Bottle drop parameters
*
* @author Dominik Juchli <juchlid@ethz.ch>
*/
#include <nuttx/config.h>
#include <systemlib/param/param.h>
/**
* Ground drag property
*
* This parameter encodes the ground drag coefficient and the corresponding
* decrease in wind speed from the plane altitude to ground altitude.
*
* @unit unknown
* @min 0.001
* @max 0.1
* @group Payload drop
*/
PARAM_DEFINE_FLOAT(BD_GPROPERTIES, 0.03f);
/**
* Plane turn radius
*
* The planes known minimal turn radius - use a higher value
* to make the plane maneuver more distant from the actual drop
* position. This is to ensure the wings are level during the drop.
*
* @unit meter
* @min 30.0
* @max 500.0
* @group Payload drop
*/
PARAM_DEFINE_FLOAT(BD_TURNRADIUS, 120.0f);
/**
* Drop precision
*
* If the system is closer than this distance on passing over the
* drop position, it will release the payload. This is a safeguard
* to prevent a drop out of the required accuracy.
*
* @unit meter
* @min 1.0
* @max 80.0
* @group Payload drop
*/
PARAM_DEFINE_FLOAT(BD_PRECISION, 30.0f);
/**
* Payload drag coefficient of the dropped object
*
* The drag coefficient (cd) is the typical drag
* constant for air. It is in general object specific,
* but the closest primitive shape to the actual object
* should give good results:
* http://en.wikipedia.org/wiki/Drag_coefficient
*
* @unit meter
* @min 0.08
* @max 1.5
* @group Payload drop
*/
PARAM_DEFINE_FLOAT(BD_OBJ_CD, 0.1f);
/**
* Payload mass
*
* A typical small toy ball:
* 0.025 kg
*
* OBC water bottle:
* 0.6 kg
*
* @unit kilogram
* @min 0.001
* @max 5.0
* @group Payload drop
*/
PARAM_DEFINE_FLOAT(BD_OBJ_MASS, 0.6f);
/**
* Payload front surface area
*
* A typical small toy ball:
* (0.045 * 0.045) / 4.0 * pi = 0.001590 m^2
*
* OBC water bottle:
* (0.063 * 0.063) / 4.0 * pi = 0.003117 m^2
*
* @unit m^2
* @min 0.001
* @max 0.5
* @group Payload drop
*/
PARAM_DEFINE_FLOAT(BD_OBJ_SURFACE, 0.00311724531f);
src/modules/bottle_drop/module.mk
+45
View File
@@ -0,0 +1,45 @@
############################################################################
#
# Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# 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.
#
############################################################################
#
# Daemon application
#
MODULE_COMMAND = bottle_drop
SRCS = bottle_drop.cpp \
bottle_drop_params.c
MAXOPTIMIZATION = -Os
MODULE_STACKSIZE = 1200
src/modules/commander/accelerometer_calibration.cpp
+32 -18
View File
@@ -83,7 +83,7 @@
* | accel_T[1][i] |
* [ accel_T[2][i] ]
*
* b = [ accel_corr_ref[0][i] ] // One measurement per axis is enough
* b = [ accel_corr_ref[0][i] ] // One measurement per side is enough
* | accel_corr_ref[2][i] |
* [ accel_corr_ref[4][i] ]
*
@@ -162,6 +162,11 @@ int do_accel_calibration(int mavlink_fd)
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "You need to put the system on all six sides");
sleep(3);
mavlink_log_info(mavlink_fd, "Follow the instructions on the screen");
sleep(5);
struct accel_scale accel_scale = {
0.0f,
1.0f,
@@ -258,7 +263,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
const int samples_num = 2500;
float accel_ref[6][3];
bool data_collected[6] = { false, false, false, false, false, false };
const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" };
const char *orientation_strs[6] = { "back", "front", "left", "right", "up", "down" };
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
@@ -287,29 +292,37 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
break;
}
mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s",
(!data_collected[0]) ? "x+ " : "",
(!data_collected[1]) ? "x- " : "",
(!data_collected[2]) ? "y+ " : "",
(!data_collected[3]) ? "y- " : "",
(!data_collected[4]) ? "z+ " : "",
(!data_collected[5]) ? "z- " : "");
/* inform user which axes are still needed */
mavlink_log_info(mavlink_fd, "pending: %s%s%s%s%s%s",
(!data_collected[5]) ? "down " : "",
(!data_collected[0]) ? "back " : "",
(!data_collected[1]) ? "front " : "",
(!data_collected[2]) ? "left " : "",
(!data_collected[3]) ? "right " : "",
(!data_collected[4]) ? "up " : "");
/* allow user enough time to read the message */
sleep(3);
int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
if (orient < 0) {
res = ERROR;
break;
}
if (data_collected[orient]) {
mavlink_log_info(mavlink_fd, "%s done, rotate to a different axis", orientation_strs[orient]);
mavlink_log_info(mavlink_fd, "invalid motion, hold still...");
sleep(3);
continue;
}
mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]);
/* inform user about already handled side */
if (data_collected[orient]) {
mavlink_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]);
sleep(4);
continue;
}
mavlink_log_info(mavlink_fd, "Hold still, starting to measure %s side", orientation_strs[orient]);
sleep(1);
read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num);
mavlink_log_info(mavlink_fd, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient],
mavlink_log_info(mavlink_fd, "result for %s side: [ %.2f %.2f %.2f ]", orientation_strs[orient],
(double)accel_ref[orient][0],
(double)accel_ref[orient][1],
(double)accel_ref[orient][2]);
@@ -400,7 +413,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
/* is still now */
if (t_still == 0) {
/* first time */
mavlink_log_info(mavlink_fd, "detected rest position, waiting...");
mavlink_log_info(mavlink_fd, "detected rest position, hold still...");
t_still = t;
t_timeout = t + timeout;
@@ -418,6 +431,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
/* not still, reset still start time */
if (t_still != 0) {
mavlink_log_info(mavlink_fd, "detected motion, hold still...");
sleep(3);
t_still = 0;
}
}
src/modules/commander/airspeed_calibration.cpp
+25 -14
View File
@@ -61,6 +61,15 @@ static const int ERROR = -1;
static const char *sensor_name = "dpress";
#define HUMAN_ASPD_CAL_FAILED_MSG "Calibration failed, see http://px4.io/help/aspd"
static void feedback_calibration_failed(int mavlink_fd)
{
sleep(5);
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
mavlink_log_critical(mavlink_fd, HUMAN_ASPD_CAL_FAILED_MSG);
}
int do_airspeed_calibration(int mavlink_fd)
{
/* give directions */
@@ -99,7 +108,7 @@ int do_airspeed_calibration(int mavlink_fd)
float analog_scaling = 0.0f;
param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
if (fabsf(analog_scaling) < 0.1f) {
mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]");
mavlink_log_critical(mavlink_fd, "No airspeed sensor, see http://px4.io/help/aspd");
close(diff_pres_sub);
return ERROR;
}
@@ -138,7 +147,7 @@ int do_airspeed_calibration(int mavlink_fd)
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
feedback_calibration_failed(mavlink_fd);
close(diff_pres_sub);
return ERROR;
}
@@ -175,16 +184,18 @@ int do_airspeed_calibration(int mavlink_fd)
}
} else {
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
feedback_calibration_failed(mavlink_fd);
close(diff_pres_sub);
return ERROR;
}
mavlink_log_critical(mavlink_fd, "Offset of %d Pa, create airflow now!", (int)diff_pres_offset);
mavlink_log_critical(mavlink_fd, "Offset of %d Pascal", (int)diff_pres_offset);
/* wait 500 ms to ensure parameter propagated through the system */
usleep(500 * 1000);
mavlink_log_critical(mavlink_fd, "Create airflow now");
calibration_counter = 0;
const unsigned maxcount = 3000;
@@ -204,18 +215,18 @@ int do_airspeed_calibration(int mavlink_fd)
calibration_counter++;
if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
if (calibration_counter % 100 == 0) {
mavlink_log_critical(mavlink_fd, "Missing airflow! (%d, wanted: 50 Pa, #h101)",
(int)diff_pres.differential_pressure_raw_pa);
if (calibration_counter % 500 == 0) {
mavlink_log_info(mavlink_fd, "Create air pressure! (got %d, wanted: 50 Pa)",
(int)diff_pres.differential_pressure_raw_pa);
}
continue;
}
/* do not allow negative values */
if (diff_pres.differential_pressure_raw_pa < 0.0f) {
mavlink_log_info(mavlink_fd, "negative pressure: ERROR (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa);
mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!");
close(diff_pres_sub);
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
@@ -233,24 +244,24 @@ int do_airspeed_calibration(int mavlink_fd)
close(diff_pres_sub);
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
feedback_calibration_failed(mavlink_fd);
return ERROR;
} else {
mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)",
mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
break;
}
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
feedback_calibration_failed(mavlink_fd);
close(diff_pres_sub);
return ERROR;
}
}
if (calibration_counter == maxcount) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
feedback_calibration_failed(mavlink_fd);
close(diff_pres_sub);
return ERROR;
}
src/modules/commander/commander.cpp
+482 -142
View File
File diff suppressed because it is too large Load Diff
src/modules/commander/commander_params.c
+66
View File
@@ -105,3 +105,69 @@ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
* @max 1
*/
PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0);
/** Datalink loss time threshold
*
* After this amount of seconds without datalink the data link lost mode triggers
*
* @group commander
* @unit second
* @min 0
* @max 30
*/
PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10);
/** Datalink regain time threshold
*
* After a data link loss: after this this amount of seconds with a healthy datalink the 'datalink loss'
* flag is set back to false
*
* @group commander
* @unit second
* @min 0
* @max 30
*/
PARAM_DEFINE_INT32(COM_DL_REG_T, 0);
/** Engine Failure Throttle Threshold
*
* Engine failure triggers only above this throttle value
*
* @group commander
* @min 0.0f
* @max 1.0f
*/
PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f);
/** Engine Failure Current/Throttle Threshold
*
* Engine failure triggers only below this current/throttle value
*
* @group commander
* @min 0.0f
* @max 7.0f
*/
PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f);
/** Engine Failure Time Threshold
*
* Engine failure triggers only if the throttle threshold and the
* current to throttle threshold are violated for this time
*
* @group commander
* @unit second
* @min 0.0f
* @max 7.0f
*/
PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f);
/** RC loss time threshold
*
* After this amount of seconds without RC connection the rc lost flag is set to true
*
* @group commander
* @unit second
* @min 0
* @max 35
*/
PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5);
src/modules/commander/commander_tests/commander_tests.cpp
+1 -3
View File
@@ -48,7 +48,5 @@ extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]);
int commander_tests_main(int argc, char *argv[])
{
stateMachineHelperTest();
return 0;
return stateMachineHelperTest() ? 0 : -1;
}
src/modules/commander/commander_tests/state_machine_helper_test.cpp
+5 -8
View File
@@ -49,7 +49,7 @@ public:
StateMachineHelperTest();
virtual ~StateMachineHelperTest();
virtual void runTests(void);
virtual bool run_tests(void);
private:
bool armingStateTransitionTest();
@@ -488,16 +488,13 @@ bool StateMachineHelperTest::isSafeTest(void)
return true;
}
void StateMachineHelperTest::runTests(void)
bool StateMachineHelperTest::run_tests(void)
{
ut_run_test(armingStateTransitionTest);
ut_run_test(mainStateTransitionTest);
ut_run_test(isSafeTest);
return (_tests_failed == 0);
}
void stateMachineHelperTest(void)
{
StateMachineHelperTest* test = new StateMachineHelperTest();
test->runTests();
test->printResults();
}
ut_declare_test(stateMachineHelperTest, StateMachineHelperTest)
src/modules/commander/commander_tests/state_machine_helper_test.h
+1 -1
View File
@@ -39,6 +39,6 @@
#ifndef STATE_MACHINE_HELPER_TEST_H_
#define STATE_MACHINE_HELPER_TEST_
void stateMachineHelperTest(void);
bool stateMachineHelperTest(void);
#endif /* STATE_MACHINE_HELPER_TEST_H_ */
src/modules/commander/gyro_calibration.cpp
+4 -1
View File
@@ -63,7 +63,10 @@ static const char *sensor_name = "gyro";
int do_gyro_calibration(int mavlink_fd)
{
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "don't move system");
mavlink_log_info(mavlink_fd, "HOLD STILL");
/* wait for the user to respond */
sleep(2);
struct gyro_scale gyro_scale = {
0.0f,
src/modules/commander/mag_calibration.cpp
+1 -1
View File
@@ -155,7 +155,7 @@ int do_mag_calibration(int mavlink_fd)
uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
unsigned poll_errcount = 0;
mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis");
mavlink_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down");
calibration_counter = 0;
src/modules/commander/state_machine_helper.cpp
+62 -43
View File
@@ -182,12 +182,19 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
// Fail transition if power levels on the avionics rail
// are measured but are insufficient
if (status->condition_power_input_valid && ((status->avionics_power_rail_voltage > 0.0f) &&
(status->avionics_power_rail_voltage < 4.9f))) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
valid_transition = false;
if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) {
// Check avionics rail voltages
if (status->avionics_power_rail_voltage < 4.75f) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
valid_transition = false;
} else if (status->avionics_power_rail_voltage < 4.9f) {
mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
} else if (status->avionics_power_rail_voltage > 5.4f) {
mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
}
}
}
@@ -436,7 +443,8 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
/**
* Check failsafe and main status and set navigation status for navigator accordingly
*/
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished)
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished,
const bool stay_in_failsafe)
{
navigation_state_t nav_state_old = status->nav_state;
@@ -450,11 +458,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
case MAIN_STATE_ALTCTL:
case MAIN_STATE_POSCTL:
/* require RC for all manual modes */
if (status->rc_signal_lost && armed) {
if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RTL;
status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
@@ -489,25 +497,31 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
break;
case MAIN_STATE_AUTO_MISSION:
/* go into failsafe
* - if either the datalink is enabled and lost as well as RC is lost
* - if there is no datalink and the mission is finished */
if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) ||
(!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) {
status->failsafe = true;
* - if commanded to do so
* - if we have an engine failure
* - depending on datalink, RC and if the mission is finished */
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RTL;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
status->nav_state = NAVIGATION_STATE_DESCEND;
} else {
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
/* first look at the commands */
if (status->engine_failure_cmd) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (status->data_link_lost_cmd) {
status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
} else if (status->gps_failure_cmd) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
} else if (status->rc_signal_lost_cmd) {
status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
/* also go into failsafe if just datalink is lost */
} else if (status->data_link_lost && data_link_loss_enabled) {
/* finished handling commands which have priority, now handle failures */
} else if (status->gps_failure) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
} else if (status->engine_failure) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
/* datalink loss enabled:
* check for datalink lost: this should always trigger RTGS */
} else if (data_link_loss_enabled && status->data_link_lost) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
@@ -520,24 +534,15 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
/* don't bother if RC is lost and mission is not yet finished */
} else if (status->rc_signal_lost) {
/* this mode is ok, we don't need RC for missions */
status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
} else {
/* everything is perfect */
status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
}
break;
case MAIN_STATE_AUTO_LOITER:
/* go into failsafe if datalink and RC is lost */
if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) {
/* datalink loss disabled:
* check if both, RC and datalink are lost during the mission
* or RC is lost after the mission is finished: this should always trigger RCRECOVER */
} else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) ||
(status->rc_signal_lost && mission_finished))) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->nav_state = NAVIGATION_STATE_AUTO_RTL;
status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
@@ -546,6 +551,16 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
status->nav_state = NAVIGATION_STATE_TERMINATION;
}
/* stay where you are if you should stay in failsafe, otherwise everything is perfect */
} else if (!stay_in_failsafe){
status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
}
break;
case MAIN_STATE_AUTO_LOITER:
/* go into failsafe on a engine failure */
if (status->engine_failure) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
/* also go into failsafe if just datalink is lost */
} else if (status->data_link_lost && data_link_loss_enabled) {
status->failsafe = true;
@@ -586,8 +601,12 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
break;
case MAIN_STATE_AUTO_RTL:
/* require global position and home */
if ((!status->condition_global_position_valid || !status->condition_home_position_valid)) {
/* require global position and home, also go into failsafe on an engine failure */
if (status->engine_failure) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if ((!status->condition_global_position_valid ||
!status->condition_home_position_valid)) {
status->failsafe = true;
if (status->condition_local_position_valid) {
@@ -687,7 +706,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
}
if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) {
mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION MISSING");
mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
// XXX do not make this fatal yet
}
}
src/modules/commander/state_machine_helper.h
+1 -1
View File
@@ -63,7 +63,7 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state
transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished);
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe);
int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd);
src/modules/controllib/module.mk
+2
View File
@@ -39,3 +39,5 @@ SRCS = test_params.c \
block/BlockParam.cpp \
uorb/blocks.cpp \
blocks.cpp
MAXOPTIMIZATION = -Os
src/modules/dataman/dataman.c
+6 -10
View File
@@ -629,9 +629,6 @@ task_main(int argc, char *argv[])
{
work_q_item_t *work;
/* inform about start */
warnx("Initializing..");
/* Initialize global variables */
g_key_offsets[0] = 0;
@@ -694,16 +691,15 @@ task_main(int argc, char *argv[])
if (sys_restart_val == DM_INIT_REASON_POWER_ON) {
warnx("Power on restart");
_restart(DM_INIT_REASON_POWER_ON);
}
else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) {
} else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) {
warnx("In flight restart");
_restart(DM_INIT_REASON_IN_FLIGHT);
}
else
} else {
warnx("Unknown restart");
}
else
}
} else {
warnx("Unknown restart");
}
/* We use two file descriptors, one for the caller context and one for the worker thread */
/* They are actually the same but we need to some way to reject caller request while the */
@@ -797,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_MAX - 5, 2000, task_main, NULL)) <= 0) {
if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2000, task_main, NULL)) <= 0) {
warn("task start failed");
return -1;
}
src/modules/dataman/module.mk
+2
View File
@@ -40,3 +40,5 @@ MODULE_COMMAND = dataman
SRCS = dataman.c
MODULE_STACKSIZE = 1200
MAXOPTIMIZATION = -Os
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+78 -29
View File
@@ -58,6 +58,7 @@
#include <drivers/drv_accel.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_baro.h>
#include <drivers/drv_range_finder.h>
#ifdef SENSOR_COMBINED_SUB
#include <uORB/topics/sensor_combined.h>
#endif
@@ -96,7 +97,10 @@ extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]);
__EXPORT uint32_t millis();
__EXPORT uint64_t getMicros();
static uint64_t IMUmsec = 0;
static uint64_t IMUusec = 0;
static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000;
uint32_t millis()
@@ -104,6 +108,11 @@ uint32_t millis()
return IMUmsec;
}
uint64_t getMicros()
{
return IMUusec;
}
class FixedwingEstimator
{
public:
@@ -171,6 +180,7 @@ private:
#else
int _sensor_combined_sub;
#endif
int _distance_sub; /**< distance measurement */
int _airspeed_sub; /**< airspeed subscription */
int _baro_sub; /**< barometer subscription */
int _gps_sub; /**< GPS subscription */
@@ -196,7 +206,8 @@ private:
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
struct vehicle_local_position_s _local_pos; /**< local vehicle position */
struct vehicle_gps_position_s _gps; /**< GPS position */
struct wind_estimate_s _wind; /**< Wind estimate */
struct wind_estimate_s _wind; /**< wind estimate */
struct range_finder_report _distance; /**< distance estimate */
struct gyro_scale _gyro_offsets;
struct accel_scale _accel_offsets;
@@ -226,6 +237,7 @@ private:
hrt_abstime _filter_start_time;
hrt_abstime _last_sensor_timestamp;
hrt_abstime _last_run;
hrt_abstime _distance_last_valid;
bool _gyro_valid;
bool _accel_valid;
bool _mag_valid;
@@ -342,6 +354,7 @@ FixedwingEstimator::FixedwingEstimator() :
#else
_sensor_combined_sub(-1),
#endif
_distance_sub(-1),
_airspeed_sub(-1),
_baro_sub(-1),
_gps_sub(-1),
@@ -369,6 +382,7 @@ FixedwingEstimator::FixedwingEstimator() :
_local_pos({}),
_gps({}),
_wind({}),
_distance{},
_gyro_offsets({}),
_accel_offsets({}),
@@ -399,6 +413,7 @@ FixedwingEstimator::FixedwingEstimator() :
_filter_start_time(0),
_last_sensor_timestamp(0),
_last_run(0),
_distance_last_valid(0),
_gyro_valid(false),
_accel_valid(false),
_mag_valid(false),
@@ -549,6 +564,7 @@ FixedwingEstimator::parameters_update()
_ekf->gyroProcessNoise = _parameters.gyro_pnoise;
_ekf->accelProcessNoise = _parameters.acc_pnoise;
_ekf->airspeedMeasurementSigma = _parameters.eas_noise;
_ekf->rngFinderPitch = 0.0f; // XXX base on SENS_BOARD_Y_OFF
}
return OK;
@@ -581,12 +597,12 @@ FixedwingEstimator::check_filter_state()
const char* const feedback[] = { 0,
"NaN in states, resetting",
"stale IMU data, resetting",
"stale sensor data, resetting",
"got initial position lock",
"excessive gyro offsets",
"GPS velocity divergence",
"velocity diverted, check accel config",
"excessive covariances",
"unknown condition"};
"unknown condition, resetting"};
// Print out error condition
if (check) {
@@ -597,8 +613,11 @@ FixedwingEstimator::check_filter_state()
warn_index = max_warn_index;
}
warnx("reset: %s", feedback[warn_index]);
mavlink_log_critical(_mavlink_fd, "[ekf] re-init: %s", feedback[warn_index]);
// Do not warn about accel offset if we have no position updates
if (!(warn_index == 5 && _ekf->staticMode)) {
warnx("reset: %s", feedback[warn_index]);
mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]);
}
}
struct estimator_status_report rep;
@@ -704,6 +723,7 @@ FixedwingEstimator::task_main()
/*
* do subscriptions
*/
_distance_sub = orb_subscribe(ORB_ID(sensor_range_finder));
_baro_sub = orb_subscribe(ORB_ID(sensor_baro0));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
@@ -753,6 +773,7 @@ FixedwingEstimator::task_main()
bool newHgtData = false;
bool newAdsData = false;
bool newDataMag = false;
bool newRangeData = false;
float posNED[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m)
@@ -765,7 +786,7 @@ FixedwingEstimator::task_main()
while (!_task_should_exit) {
/* wait for up to 500ms for data */
/* wait for up to 100ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit, etc. */
@@ -850,7 +871,8 @@ FixedwingEstimator::task_main()
}
_last_sensor_timestamp = _gyro.timestamp;
IMUmsec = _gyro.timestamp / 1e3f;
IMUmsec = _gyro.timestamp / 1e3;
IMUusec = _gyro.timestamp;
float deltaT = (_gyro.timestamp - _last_run) / 1e6f;
_last_run = _gyro.timestamp;
@@ -914,7 +936,8 @@ FixedwingEstimator::task_main()
// Copy gyro and accel
_last_sensor_timestamp = _sensor_combined.timestamp;
IMUmsec = _sensor_combined.timestamp / 1e3f;
IMUmsec = _sensor_combined.timestamp / 1e3;
IMUusec = _sensor_combined.timestamp;
float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;
@@ -994,8 +1017,6 @@ FixedwingEstimator::task_main()
if (gps_updated) {
last_gps = _gps.timestamp_position;
orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
perf_count(_perf_gps);
@@ -1008,11 +1029,17 @@ FixedwingEstimator::task_main()
_gps_start_time = hrt_absolute_time();
/* check if we had a GPS outage for a long time */
if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) {
float gps_elapsed = hrt_elapsed_time(&last_gps) / 1e6f;
const float pos_reset_threshold = 5.0f; // seconds
/* timeout of 5 seconds */
if (gps_elapsed > pos_reset_threshold) {
_ekf->ResetPosition();
_ekf->ResetVelocity();
_ekf->ResetStoredStates();
}
_ekf->updateDtGpsFilt(math::constrain((_gps.timestamp_position - last_gps) / 1e6f, 0.01f, pos_reset_threshold));
/* fuse GPS updates */
@@ -1044,6 +1071,8 @@ FixedwingEstimator::task_main()
newDataGps = true;
last_gps = _gps.timestamp_position;
}
}
@@ -1052,8 +1081,15 @@ FixedwingEstimator::task_main()
orb_check(_baro_sub, &baro_updated);
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;
_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f));
_ekf->baroHgt = _baro.altitude;
if (!_baro_init) {
@@ -1114,6 +1150,19 @@ FixedwingEstimator::task_main()
newDataMag = false;
}
orb_check(_distance_sub, &newRangeData);
if (newRangeData) {
orb_copy(ORB_ID(sensor_range_finder), _distance_sub, &_distance);
if (_distance.valid) {
_ekf->rngMea = _distance.distance;
_distance_last_valid = _distance.timestamp;
} else {
newRangeData = false;
}
}
/*
* CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY
*/
@@ -1197,6 +1246,7 @@ FixedwingEstimator::task_main()
} else if (_ekf->statesInitialised) {
// We're apparently initialized in this case now
// check (and reset the filter as needed)
int check = check_filter_state();
if (check) {
@@ -1206,21 +1256,7 @@ FixedwingEstimator::task_main()
// Run the strapdown INS equations every IMU update
_ekf->UpdateStrapdownEquationsNED();
#if 0
// debug code - could be tunred into a filter mnitoring/watchdog function
float tempQuat[4];
for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j];
quat2eul(eulerEst, tempQuat);
for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j];
if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi;
if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi;
#endif
// store the predicted states for subsequent use by measurement fusion
_ekf->StoreStates(IMUmsec);
// Check if on ground - status is used by covariance prediction
@@ -1334,6 +1370,13 @@ FixedwingEstimator::task_main()
_ekf->fuseVtasData = false;
}
if (newRangeData) {
_ekf->fuseRngData = true;
_ekf->useRangeFinder = true;
_ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f));
_ekf->GroundEKF();
}
// Output results
math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
@@ -1447,6 +1490,10 @@ FixedwingEstimator::task_main()
_global_pos.vel_d = _local_pos.vz;
}
/* terrain altitude */
_global_pos.terrain_alt = _ekf->hgtRef - _ekf->flowStates[1];
_global_pos.terrain_alt_valid = (_distance_last_valid > 0) &&
(hrt_elapsed_time(&_distance_last_valid) < 20 * 1000 * 1000);
_global_pos.yaw = _local_pos.yaw;
@@ -1467,8 +1514,10 @@ FixedwingEstimator::task_main()
if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
_wind.timestamp = _global_pos.timestamp;
_wind.windspeed_north = _ekf->states[14];
_wind.windspeed_east = _ekf->states[15];
_wind.windspeed_north = _ekf->windSpdFiltNorth;
_wind.windspeed_east = _ekf->windSpdFiltEast;
// 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];
_wind.covariance_east = _ekf->P[15][15];
@@ -1511,7 +1560,7 @@ FixedwingEstimator::start()
_estimator_task = task_spawn_cmd("ekf_att_pos_estimator",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40,
5000,
7500,
(main_t)&FixedwingEstimator::task_main_trampoline,
nullptr);
src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+515 -306
View File
@@ -2,6 +2,11 @@
#include <string.h>
#include <stdio.h>
#include <stdarg.h>
#include <math.h>
#ifndef M_PI_F
#define M_PI_F ((float)M_PI)
#endif
#define EKF_COVARIANCE_DIVERGED 1.0e8f
@@ -38,13 +43,14 @@ AttPosEKF::AttPosEKF() :
resetStates{},
storedStates{},
statetimeStamp{},
lastVelPosFusion(millis()),
statesAtVelTime{},
statesAtPosTime{},
statesAtHgtTime{},
statesAtMagMeasTime{},
statesAtVtasMeasTime{},
statesAtRngTime{},
statesAtOptFlowTime{},
statesAtFlowTime{},
correctedDelAng(),
correctedDelVel(),
summedDelAng(),
@@ -59,7 +65,16 @@ AttPosEKF::AttPosEKF() :
accel(),
dVelIMU(),
dAngIMU(),
dtIMU(0),
dtIMU(0.005f),
dtIMUfilt(0.005f),
dtVelPos(0.01f),
dtVelPosFilt(0.01f),
dtHgtFilt(0.01f),
dtGpsFilt(0.1f),
windSpdFiltNorth(0.0f),
windSpdFiltEast(0.0f),
windSpdFiltAltitude(0.0f),
windSpdFiltClimb(0.0f),
fusionModeGPS(0),
innovVelPos{},
varInnovVelPos{},
@@ -103,13 +118,14 @@ AttPosEKF::AttPosEKF() :
inhibitWindStates(true),
inhibitMagStates(true),
inhibitGndHgtState(true),
inhibitGndState(true),
inhibitScaleState(true),
onGround(true),
staticMode(true),
useAirspeed(true),
useCompass(true),
useRangeFinder(false),
useRangeFinder(true),
useOpticalFlow(false),
ekfDiverged(false),
@@ -117,7 +133,24 @@ AttPosEKF::AttPosEKF() :
current_ekf_state{},
last_ekf_error{},
numericalProtection(true),
storeIndex(0)
storeIndex(0),
storedOmega{},
Popt{},
flowStates{},
prevPosN(0.0f),
prevPosE(0.0f),
auxFlowObsInnov{},
auxFlowObsInnovVar{},
fScaleFactorVar(0.0f),
Tnb_flow{},
R_LOS(0.0f),
auxFlowTestRatio{},
auxRngTestRatio(0.0f),
flowInnovGate(0.0f),
auxFlowInnovGate(0.0f),
rngInnovGate(0.0f),
minFlowRng(0.0f),
moCompR_LOS(0.0f)
{
memset(&last_ekf_error, 0, sizeof(last_ekf_error));
memset(&current_ekf_state, 0, sizeof(current_ekf_state));
@@ -260,6 +293,9 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
// Constrain states (to protect against filter divergence)
ConstrainStates();
// update filtered IMU time step length
dtIMUfilt = 0.99f * dtIMUfilt + 0.01f * dtIMU;
}
void AttPosEKF::CovariancePrediction(float dt)
@@ -312,7 +348,7 @@ void AttPosEKF::CovariancePrediction(float dt)
} else {
for (uint8_t i=16; i<=21; i++) processNoise[i] = 0;
}
if (!inhibitGndHgtState) {
if (!inhibitGndState) {
processNoise[22] = dt * sqrtf(sq(states[4]) + sq(states[5])) * gndHgtSigma;
} else {
processNoise[22] = 0;
@@ -962,6 +998,21 @@ void AttPosEKF::CovariancePrediction(float dt)
ConstrainVariances();
}
void AttPosEKF::updateDtGpsFilt(float dt)
{
dtGpsFilt = ConstrainFloat(dt, 0.001f, 2.0f) * 0.05f + dtGpsFilt * 0.95f;
}
void AttPosEKF::updateDtHgtFilt(float dt)
{
dtHgtFilt = ConstrainFloat(dt, 0.001f, 2.0f) * 0.05f + dtHgtFilt * 0.95f;
}
void AttPosEKF::updateDtVelPosFilt(float dt)
{
dtVelPosFilt = ConstrainFloat(dt, 0.0005f, 2.0f) * 0.05f + dtVelPosFilt * 0.95f;
}
void AttPosEKF::FuseVelposNED()
{
@@ -998,6 +1049,18 @@ void AttPosEKF::FuseVelposNED()
// associated with sequential fusion
if (fuseVelData || fusePosData || fuseHgtData)
{
uint64_t tNow = getMicros();
updateDtVelPosFilt((tNow - lastVelPosFusion) / 1e6f);
lastVelPosFusion = tNow;
// scaler according to the number of repetitions of the
// same measurement in one fusion step
float gpsVarianceScaler = dtGpsFilt / dtVelPosFilt;
// scaler according to the number of repetitions of the
// same measurement in one fusion step
float hgtVarianceScaler = dtHgtFilt / dtVelPosFilt;
// set the GPS data timeout depending on whether airspeed data is present
if (useAirspeed) horizRetryTime = gpsRetryTime;
else horizRetryTime = gpsRetryTimeNoTAS;
@@ -1010,12 +1073,12 @@ void AttPosEKF::FuseVelposNED()
// Estimate the GPS Velocity, GPS horiz position and height measurement variances.
velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring
posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring
R_OBS[0] = sq(vneSigma) + sq(velErr);
R_OBS[0] = gpsVarianceScaler * sq(vneSigma) + sq(velErr);
R_OBS[1] = R_OBS[0];
R_OBS[2] = sq(vdSigma) + sq(velErr);
R_OBS[3] = sq(posNeSigma) + sq(posErr);
R_OBS[2] = gpsVarianceScaler * sq(vdSigma) + sq(velErr);
R_OBS[3] = gpsVarianceScaler * sq(posNeSigma) + sq(posErr);
R_OBS[4] = R_OBS[3];
R_OBS[5] = sq(posDSigma) + sq(posErr);
R_OBS[5] = hgtVarianceScaler * sq(posDSigma) + sq(posErr);
// calculate innovations and check GPS data validity using an innovation consistency check
if (fuseVelData)
@@ -1173,7 +1236,7 @@ void AttPosEKF::FuseVelposNED()
}
}
// Don't update terrain state if inhibited
if (inhibitGndHgtState) {
if (inhibitGndState) {
Kfusion[22] = 0;
}
@@ -1356,7 +1419,7 @@ void AttPosEKF::FuseMagnetometer()
Kfusion[i] = 0;
}
}
if (!inhibitGndHgtState) {
if (!inhibitGndState) {
Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]);
} else {
Kfusion[22] = 0;
@@ -1430,11 +1493,6 @@ void AttPosEKF::FuseMagnetometer()
Kfusion[20] = 0;
Kfusion[21] = 0;
}
if (!inhibitGndHgtState) {
Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]);
} else {
Kfusion[22] = 0;
}
varInnovMag[1] = 1.0f/SK_MY[0];
innovMag[1] = MagPred[1] - magData.y;
}
@@ -1505,11 +1563,6 @@ void AttPosEKF::FuseMagnetometer()
Kfusion[20] = 0;
Kfusion[21] = 0;
}
if (!inhibitGndHgtState) {
Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] + P[22][3]*SH_MAG[0] - P[22][1]*SK_MZ[2] + P[22][2]*SK_MZ[3] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[5] - P[22][17]*SK_MZ[4]);
} else {
Kfusion[22] = 0;
}
varInnovMag[2] = 1.0f/SK_MZ[0];
innovMag[2] = MagPred[2] - magData.z;
@@ -1616,6 +1669,32 @@ void AttPosEKF::FuseAirspeed()
// Perform fusion of True Airspeed measurement
if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f))
{
float altDiff = fabsf(windSpdFiltAltitude - hgtMea);
if (isfinite(windSpdFiltClimb)) {
windSpdFiltClimb = ((1.0f - 0.0002f) * windSpdFiltClimb) + (0.0002f * states[6]);
} else {
windSpdFiltClimb = states[6];
}
if (altDiff < 20.0f) {
// Lowpass the output of the wind estimate - we want a long-term
// stable estimate, but not start to load into the overall dynamics
// of the system (which adjusting covariances would do)
// Change filter coefficient based on altitude change rate
float windFiltCoeff = ConstrainFloat(fabsf(windSpdFiltClimb) / 1000.0f, 0.00005f, 0.2f);
windSpdFiltNorth = ((1.0f - windFiltCoeff) * windSpdFiltNorth) + (windFiltCoeff * vwn);
windSpdFiltEast = ((1.0f - windFiltCoeff) * windSpdFiltEast) + (windFiltCoeff * vwe);
} else {
windSpdFiltNorth = vwn;
windSpdFiltEast = vwe;
}
windSpdFiltAltitude = hgtMea;
// Calculate observation jacobians
SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f;
@@ -1675,11 +1754,6 @@ void AttPosEKF::FuseAirspeed()
Kfusion[i] = 0;
}
}
if (!inhibitGndHgtState) {
Kfusion[22] = SK_TAS*(P[22][4]*SH_TAS[2] - P[22][14]*SH_TAS[2] + P[22][5]*SH_TAS[1] - P[22][15]*SH_TAS[1] + P[22][6]*vd*SH_TAS[0]);
} else {
Kfusion[22] = 0;
}
varInnovVtas = 1.0f/SK_TAS;
// Calculate the measurement innovation
@@ -1811,8 +1885,11 @@ void AttPosEKF::FuseRangeFinder()
rngPred = (ptd - pd)/cosRngTilt;
innovRng = rngPred - rngMea;
// Check the innovation for consistency and don't fuse if > 5Sigma
if ((innovRng*innovRng*SK_RNG[0]) < 25)
// calculate the innovation consistency test ratio
auxRngTestRatio = sq(innovRng) / (sq(rngInnovGate) * varInnovRng);
// Check the innovation for consistency and don't fuse if out of bounds
if (auxRngTestRatio < 1.0f)
{
// correct the state vector
states[22] = states[22] - Kfusion[22] * innovRng;
@@ -1827,285 +1904,387 @@ void AttPosEKF::FuseRangeFinder()
void AttPosEKF::FuseOptFlow()
{
static uint8_t obsIndex;
static float SH_LOS[13];
static float SKK_LOS[15];
static float SK_LOS[2];
static float q0 = 0.0f;
static float q1 = 0.0f;
static float q2 = 0.0f;
static float q3 = 1.0f;
static float vn = 0.0f;
static float ve = 0.0f;
static float vd = 0.0f;
static float pd = 0.0f;
static float ptd = 0.0f;
static float R_LOS = 0.01f;
static float losPred[2];
// static uint8_t obsIndex;
// static float SH_LOS[13];
// static float SKK_LOS[15];
// static float SK_LOS[2];
// static float q0 = 0.0f;
// static float q1 = 0.0f;
// static float q2 = 0.0f;
// static float q3 = 1.0f;
// static float vn = 0.0f;
// static float ve = 0.0f;
// static float vd = 0.0f;
// static float pd = 0.0f;
// static float ptd = 0.0f;
// static float R_LOS = 0.01f;
// static float losPred[2];
// Transformation matrix from nav to body axes
Mat3f Tnb_local;
// Transformation matrix from body to sensor axes
// assume camera is aligned with Z body axis plus a misalignment
// defined by 3 small angles about X, Y and Z body axis
Mat3f Tbs;
Tbs.x.y = a3;
Tbs.y.x = -a3;
Tbs.x.z = -a2;
Tbs.z.x = a2;
Tbs.y.z = a1;
Tbs.z.y = -a1;
// Transformation matrix from navigation to sensor axes
Mat3f Tns;
float H_LOS[n_states];
for (uint8_t i = 0; i < n_states; i++) {
H_LOS[i] = 0.0f;
// // Transformation matrix from nav to body axes
// Mat3f Tnb_local;
// // Transformation matrix from body to sensor axes
// // assume camera is aligned with Z body axis plus a misalignment
// // defined by 3 small angles about X, Y and Z body axis
// Mat3f Tbs;
// Tbs.x.y = a3;
// Tbs.y.x = -a3;
// Tbs.x.z = -a2;
// Tbs.z.x = a2;
// Tbs.y.z = a1;
// Tbs.z.y = -a1;
// // Transformation matrix from navigation to sensor axes
// Mat3f Tns;
// float H_LOS[n_states];
// for (uint8_t i = 0; i < n_states; i++) {
// H_LOS[i] = 0.0f;
// }
// Vector3f velNED_local;
// Vector3f relVelSensor;
// // Perform sequential fusion of optical flow measurements only when in the air and tilt is less than 30 deg.
// if (useOpticalFlow && (fuseOptFlowData || obsIndex == 1) && !onGround && Tbs.z.z > 0.866f && rngMea > 5.0f && rngMea < 39.0f)
// {
// // Sequential fusion of XY components to spread processing load across
// // two prediction time steps.
// // Calculate observation jacobians and Kalman gains
// if (fuseOptFlowData)
// {
// // Copy required states to local variable names
// q0 = statesAtOptFlowTime[0];
// q1 = statesAtOptFlowTime[1];
// q2 = statesAtOptFlowTime[2];
// q3 = statesAtOptFlowTime[3];
// vn = statesAtOptFlowTime[4];
// ve = statesAtOptFlowTime[5];
// vd = statesAtOptFlowTime[6];
// pd = statesAtOptFlowTime[9];
// ptd = statesAtOptFlowTime[22];
// velNED_local.x = vn;
// velNED_local.y = ve;
// velNED_local.z = vd;
// // calculate rotation from NED to body axes
// float q00 = sq(q0);
// float q11 = sq(q1);
// float q22 = sq(q2);
// float q33 = sq(q3);
// float q01 = q0 * q1;
// float q02 = q0 * q2;
// float q03 = q0 * q3;
// float q12 = q1 * q2;
// float q13 = q1 * q3;
// float q23 = q2 * q3;
// Tnb_local.x.x = q00 + q11 - q22 - q33;
// Tnb_local.y.y = q00 - q11 + q22 - q33;
// Tnb_local.z.z = q00 - q11 - q22 + q33;
// Tnb_local.y.x = 2*(q12 - q03);
// Tnb_local.z.x = 2*(q13 + q02);
// Tnb_local.x.y = 2*(q12 + q03);
// Tnb_local.z.y = 2*(q23 - q01);
// Tnb_local.x.z = 2*(q13 - q02);
// Tnb_local.y.z = 2*(q23 + q01);
// // calculate transformation from NED to sensor axes
// Tns = Tbs*Tnb_local;
// // calculate range from ground plain to centre of sensor fov assuming flat earth
// float range = ConstrainFloat(((ptd - pd)/Tns.z.z),0.5f,100.0f);
// // calculate relative velocity in sensor frame
// relVelSensor = Tns*velNED_local;
// // divide velocity by range and include angular rate effects to get predicted angular LOS rates relative to X and Y axes
// losPred[0] = relVelSensor.y/range;
// losPred[1] = -relVelSensor.x/range;
// //printf("relVelSensor.x=%5.1f, relVelSensor.y=%5.1f\n", relVelSensor.x, relVelSensor.y);
// //printf("Xpred=%5.2f, Xmea=%5.2f, Ypred=%5.2f, Ymea=%5.2f, delAng.x=%4.4f, delAng.y=%4.4f\n", losPred[0], losData[0], losPred[1], losData[1], delAng.x, delAng.y);
// //printf("omegaX=%5.2f, omegaY=%5.2f, velY=%5.1f velX=%5.1f\n, range=%5.1f\n", delAngRel.x/dt, delAngRel.y/dt, relVelSensor.y, relVelSensor.x, range);
// // Calculate observation jacobians
// SH_LOS[0] = a1*(2*q0*q1 + 2*q2*q3) + a2*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3);
// SH_LOS[1] = vd*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)) - ve*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + vn*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3));
// SH_LOS[2] = ve*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - vd*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)) + vn*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3));
// SH_LOS[3] = 1/(pd - ptd);
// SH_LOS[4] = 2*q1 - 2*a2*q3 + 2*a3*q2;
// SH_LOS[5] = 2*a2*q2 - 2*q0 + 2*a3*q3;
// SH_LOS[6] = 2*q2 + 2*a2*q0 - 2*a3*q1;
// SH_LOS[7] = 1/sq(pd - ptd);
// SH_LOS[8] = 2*q2 + 2*a1*q3 - 2*a3*q1;
// SH_LOS[9] = 2*q3 - 2*a1*q2 + 2*a3*q0;
// SH_LOS[10] = 2*a1*q1 - 2*q0 + 2*a3*q3;
// SH_LOS[11] = 2*q3 + 2*a2*q1 + 2*a3*q0;
// SH_LOS[12] = 2*q1 + 2*a1*q0 + 2*a3*q2;
// for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0;
// H_LOS[0] = - SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]);
// H_LOS[1] = - SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]);
// H_LOS[2] = SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]) - SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3);
// H_LOS[3] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]);
// H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3));
// H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3));
// H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3));
// H_LOS[9] = SH_LOS[0]*SH_LOS[2]*SH_LOS[7];
// H_LOS[22] = -SH_LOS[0]*SH_LOS[2]*SH_LOS[7];
// // Calculate Kalman gain
// SKK_LOS[0] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3);
// SKK_LOS[1] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3);
// SKK_LOS[2] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3);
// SKK_LOS[3] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3);
// SKK_LOS[4] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3);
// SKK_LOS[5] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3);
// SKK_LOS[6] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]);
// SKK_LOS[7] = SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]);
// SKK_LOS[8] = SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]);
// SKK_LOS[9] = SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]);
// SKK_LOS[10] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]);
// SKK_LOS[11] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]);
// SKK_LOS[12] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]);
// SKK_LOS[13] = SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]);
// SKK_LOS[14] = SH_LOS[0];
// SK_LOS[0] = 1/(R_LOS + SKK_LOS[8]*(P[0][0]*SKK_LOS[8] + P[1][0]*SKK_LOS[7] + P[2][0]*SKK_LOS[9] - P[3][0]*SKK_LOS[6] - P[9][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][0]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][0]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[7]*(P[0][1]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] + P[2][1]*SKK_LOS[9] - P[3][1]*SKK_LOS[6] - P[9][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][1]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][1]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[9]*(P[0][2]*SKK_LOS[8] + P[1][2]*SKK_LOS[7] + P[2][2]*SKK_LOS[9] - P[3][2]*SKK_LOS[6] - P[9][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][2]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][2]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SKK_LOS[6]*(P[0][3]*SKK_LOS[8] + P[1][3]*SKK_LOS[7] + P[2][3]*SKK_LOS[9] - P[3][3]*SKK_LOS[6] - P[9][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][3]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][3]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[8] + P[1][9]*SKK_LOS[7] + P[2][9]*SKK_LOS[9] - P[3][9]*SKK_LOS[6] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][9]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][9]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[8] + P[1][22]*SKK_LOS[7] + P[2][22]*SKK_LOS[9] - P[3][22]*SKK_LOS[6] - P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][22]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][22]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14]*(P[0][4]*SKK_LOS[8] + P[1][4]*SKK_LOS[7] + P[2][4]*SKK_LOS[9] - P[3][4]*SKK_LOS[6] - P[9][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][4]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14]*(P[0][5]*SKK_LOS[8] + P[1][5]*SKK_LOS[7] + P[2][5]*SKK_LOS[9] - P[3][5]*SKK_LOS[6] - P[9][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]*(P[0][6]*SKK_LOS[8] + P[1][6]*SKK_LOS[7] + P[2][6]*SKK_LOS[9] - P[3][6]*SKK_LOS[6] - P[9][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]));
// Kfusion[0] = -SK_LOS[0]*(P[0][0]*SKK_LOS[8] + P[0][1]*SKK_LOS[7] - P[0][3]*SKK_LOS[6] + P[0][2]*SKK_LOS[9] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[0][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[0][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
// Kfusion[1] = -SK_LOS[0]*(P[1][0]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] - P[1][3]*SKK_LOS[6] + P[1][2]*SKK_LOS[9] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[1][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[1][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
// Kfusion[2] = -SK_LOS[0]*(P[2][0]*SKK_LOS[8] + P[2][1]*SKK_LOS[7] - P[2][3]*SKK_LOS[6] + P[2][2]*SKK_LOS[9] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[2][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[2][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
// Kfusion[3] = -SK_LOS[0]*(P[3][0]*SKK_LOS[8] + P[3][1]*SKK_LOS[7] - P[3][3]*SKK_LOS[6] + P[3][2]*SKK_LOS[9] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[3][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[3][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
// Kfusion[4] = -SK_LOS[0]*(P[4][0]*SKK_LOS[8] + P[4][1]*SKK_LOS[7] - P[4][3]*SKK_LOS[6] + P[4][2]*SKK_LOS[9] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[4][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
// Kfusion[5] = -SK_LOS[0]*(P[5][0]*SKK_LOS[8] + P[5][1]*SKK_LOS[7] - P[5][3]*SKK_LOS[6] + P[5][2]*SKK_LOS[9] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
// Kfusion[6] = -SK_LOS[0]*(P[6][0]*SKK_LOS[8] + P[6][1]*SKK_LOS[7] - P[6][3]*SKK_LOS[6] + P[6][2]*SKK_LOS[9] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
// Kfusion[7] = -SK_LOS[0]*(P[7][0]*SKK_LOS[8] + P[7][1]*SKK_LOS[7] - P[7][3]*SKK_LOS[6] + P[7][2]*SKK_LOS[9] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[7][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[7][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
// Kfusion[8] = -SK_LOS[0]*(P[8][0]*SKK_LOS[8] + P[8][1]*SKK_LOS[7] - P[8][3]*SKK_LOS[6] + P[8][2]*SKK_LOS[9] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[8][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[8][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
// Kfusion[9] = -SK_LOS[0]*(P[9][0]*SKK_LOS[8] + P[9][1]*SKK_LOS[7] - P[9][3]*SKK_LOS[6] + P[9][2]*SKK_LOS[9] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[9][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[9][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
// Kfusion[10] = -SK_LOS[0]*(P[10][0]*SKK_LOS[8] + P[10][1]*SKK_LOS[7] - P[10][3]*SKK_LOS[6] + P[10][2]*SKK_LOS[9] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[10][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[10][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
// Kfusion[11] = -SK_LOS[0]*(P[11][0]*SKK_LOS[8] + P[11][1]*SKK_LOS[7] - P[11][3]*SKK_LOS[6] + P[11][2]*SKK_LOS[9] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[11][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[11][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
// Kfusion[12] = -SK_LOS[0]*(P[12][0]*SKK_LOS[8] + P[12][1]*SKK_LOS[7] - P[12][3]*SKK_LOS[6] + P[12][2]*SKK_LOS[9] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[12][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[12][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
// Kfusion[13] = 0.0f;//-SK_LOS[0]*(P[13][0]*SKK_LOS[8] + P[13][1]*SKK_LOS[7] - P[13][3]*SKK_LOS[6] + P[13][2]*SKK_LOS[9] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[13][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[13][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
// Kfusion[14] = -SK_LOS[0]*(P[14][0]*SKK_LOS[8] + P[14][1]*SKK_LOS[7] - P[14][3]*SKK_LOS[6] + P[14][2]*SKK_LOS[9] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[14][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[14][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
// Kfusion[15] = -SK_LOS[0]*(P[15][0]*SKK_LOS[8] + P[15][1]*SKK_LOS[7] - P[15][3]*SKK_LOS[6] + P[15][2]*SKK_LOS[9] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[15][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[15][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
// Kfusion[16] = -SK_LOS[0]*(P[16][0]*SKK_LOS[8] + P[16][1]*SKK_LOS[7] - P[16][3]*SKK_LOS[6] + P[16][2]*SKK_LOS[9] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[16][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[16][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
// Kfusion[17] = -SK_LOS[0]*(P[17][0]*SKK_LOS[8] + P[17][1]*SKK_LOS[7] - P[17][3]*SKK_LOS[6] + P[17][2]*SKK_LOS[9] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[17][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[17][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
// Kfusion[18] = -SK_LOS[0]*(P[18][0]*SKK_LOS[8] + P[18][1]*SKK_LOS[7] - P[18][3]*SKK_LOS[6] + P[18][2]*SKK_LOS[9] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[18][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[18][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
// Kfusion[19] = -SK_LOS[0]*(P[19][0]*SKK_LOS[8] + P[19][1]*SKK_LOS[7] - P[19][3]*SKK_LOS[6] + P[19][2]*SKK_LOS[9] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[19][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[19][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
// Kfusion[20] = -SK_LOS[0]*(P[20][0]*SKK_LOS[8] + P[20][1]*SKK_LOS[7] - P[20][3]*SKK_LOS[6] + P[20][2]*SKK_LOS[9] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[20][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[20][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
// Kfusion[21] = -SK_LOS[0]*(P[21][0]*SKK_LOS[8] + P[21][1]*SKK_LOS[7] - P[21][3]*SKK_LOS[6] + P[21][2]*SKK_LOS[9] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[21][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[21][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
// Kfusion[22] = -SK_LOS[0]*(P[22][0]*SKK_LOS[8] + P[22][1]*SKK_LOS[7] - P[22][3]*SKK_LOS[6] + P[22][2]*SKK_LOS[9] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[22][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[22][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
// varInnovOptFlow[0] = 1.0f/SK_LOS[0];
// innovOptFlow[0] = losPred[0] - losData[0];
// // set the observation index to 1 to fuse the y component next time round and reset the commence fusion flag
// obsIndex = 1;
// fuseOptFlowData = false;
// }
// else if (obsIndex == 1) // we are now fusing the Y measurement
// {
// // Calculate observation jacobians
// for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0;
// H_LOS[0] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]);
// H_LOS[1] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]);
// H_LOS[2] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]);
// H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]) - SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1);
// H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3));
// H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3));
// H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3));
// H_LOS[9] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[7];
// H_LOS[22] = SH_LOS[0]*SH_LOS[1]*SH_LOS[7];
// // Calculate Kalman gains
// SK_LOS[1] = 1/(R_LOS + SKK_LOS[12]*(P[0][2]*SKK_LOS[10] + P[1][2]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[3][2]*SKK_LOS[13] - P[9][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][2]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][2]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SKK_LOS[13]*(P[0][3]*SKK_LOS[10] + P[1][3]*SKK_LOS[11] + P[2][3]*SKK_LOS[12] - P[3][3]*SKK_LOS[13] - P[9][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][3]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][3]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[10]*(P[0][0]*SKK_LOS[10] + P[1][0]*SKK_LOS[11] + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][0]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][0]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][0]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[11]*(P[0][1]*SKK_LOS[10] + P[1][1]*SKK_LOS[11] + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][1]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][1]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][1]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[10] + P[1][9]*SKK_LOS[11] + P[2][9]*SKK_LOS[12] - P[3][9]*SKK_LOS[13] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][9]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][9]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[10] + P[1][22]*SKK_LOS[11] + P[2][22]*SKK_LOS[12] - P[3][22]*SKK_LOS[13] - P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][22]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][22]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14]*(P[0][4]*SKK_LOS[10] + P[1][4]*SKK_LOS[11] + P[2][4]*SKK_LOS[12] - P[3][4]*SKK_LOS[13] - P[9][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][4]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]*(P[0][5]*SKK_LOS[10] + P[1][5]*SKK_LOS[11] + P[2][5]*SKK_LOS[12] - P[3][5]*SKK_LOS[13] - P[9][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]*(P[0][6]*SKK_LOS[10] + P[1][6]*SKK_LOS[11] + P[2][6]*SKK_LOS[12] - P[3][6]*SKK_LOS[13] - P[9][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]));
// Kfusion[0] = SK_LOS[1]*(P[0][0]*SKK_LOS[10] - P[0][3]*SKK_LOS[13] + P[0][1]*SKK_LOS[11] + P[0][2]*SKK_LOS[12] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[0][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
// Kfusion[1] = SK_LOS[1]*(P[1][0]*SKK_LOS[10] - P[1][3]*SKK_LOS[13] + P[1][1]*SKK_LOS[11] + P[1][2]*SKK_LOS[12] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[1][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
// Kfusion[2] = SK_LOS[1]*(P[2][0]*SKK_LOS[10] - P[2][3]*SKK_LOS[13] + P[2][1]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[2][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
// Kfusion[3] = SK_LOS[1]*(P[3][0]*SKK_LOS[10] - P[3][3]*SKK_LOS[13] + P[3][1]*SKK_LOS[11] + P[3][2]*SKK_LOS[12] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[3][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
// Kfusion[4] = SK_LOS[1]*(P[4][0]*SKK_LOS[10] - P[4][3]*SKK_LOS[13] + P[4][1]*SKK_LOS[11] + P[4][2]*SKK_LOS[12] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[4][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
// Kfusion[5] = SK_LOS[1]*(P[5][0]*SKK_LOS[10] - P[5][3]*SKK_LOS[13] + P[5][1]*SKK_LOS[11] + P[5][2]*SKK_LOS[12] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
// Kfusion[6] = SK_LOS[1]*(P[6][0]*SKK_LOS[10] - P[6][3]*SKK_LOS[13] + P[6][1]*SKK_LOS[11] + P[6][2]*SKK_LOS[12] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
// Kfusion[7] = SK_LOS[1]*(P[7][0]*SKK_LOS[10] - P[7][3]*SKK_LOS[13] + P[7][1]*SKK_LOS[11] + P[7][2]*SKK_LOS[12] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[7][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
// Kfusion[8] = SK_LOS[1]*(P[8][0]*SKK_LOS[10] - P[8][3]*SKK_LOS[13] + P[8][1]*SKK_LOS[11] + P[8][2]*SKK_LOS[12] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[8][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
// Kfusion[9] = SK_LOS[1]*(P[9][0]*SKK_LOS[10] - P[9][3]*SKK_LOS[13] + P[9][1]*SKK_LOS[11] + P[9][2]*SKK_LOS[12] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[9][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
// Kfusion[10] = SK_LOS[1]*(P[10][0]*SKK_LOS[10] - P[10][3]*SKK_LOS[13] + P[10][1]*SKK_LOS[11] + P[10][2]*SKK_LOS[12] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[10][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
// Kfusion[11] = SK_LOS[1]*(P[11][0]*SKK_LOS[10] - P[11][3]*SKK_LOS[13] + P[11][1]*SKK_LOS[11] + P[11][2]*SKK_LOS[12] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[11][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
// Kfusion[12] = SK_LOS[1]*(P[12][0]*SKK_LOS[10] - P[12][3]*SKK_LOS[13] + P[12][1]*SKK_LOS[11] + P[12][2]*SKK_LOS[12] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[12][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
// Kfusion[13] = 0.0f;//SK_LOS[1]*(P[13][0]*SKK_LOS[10] - P[13][3]*SKK_LOS[13] + P[13][1]*SKK_LOS[11] + P[13][2]*SKK_LOS[12] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[13][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
// Kfusion[14] = SK_LOS[1]*(P[14][0]*SKK_LOS[10] - P[14][3]*SKK_LOS[13] + P[14][1]*SKK_LOS[11] + P[14][2]*SKK_LOS[12] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[14][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
// Kfusion[15] = SK_LOS[1]*(P[15][0]*SKK_LOS[10] - P[15][3]*SKK_LOS[13] + P[15][1]*SKK_LOS[11] + P[15][2]*SKK_LOS[12] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[15][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
// Kfusion[16] = SK_LOS[1]*(P[16][0]*SKK_LOS[10] - P[16][3]*SKK_LOS[13] + P[16][1]*SKK_LOS[11] + P[16][2]*SKK_LOS[12] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[16][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
// Kfusion[17] = SK_LOS[1]*(P[17][0]*SKK_LOS[10] - P[17][3]*SKK_LOS[13] + P[17][1]*SKK_LOS[11] + P[17][2]*SKK_LOS[12] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[17][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
// Kfusion[18] = SK_LOS[1]*(P[18][0]*SKK_LOS[10] - P[18][3]*SKK_LOS[13] + P[18][1]*SKK_LOS[11] + P[18][2]*SKK_LOS[12] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[18][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
// Kfusion[19] = SK_LOS[1]*(P[19][0]*SKK_LOS[10] - P[19][3]*SKK_LOS[13] + P[19][1]*SKK_LOS[11] + P[19][2]*SKK_LOS[12] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[19][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
// Kfusion[20] = SK_LOS[1]*(P[20][0]*SKK_LOS[10] - P[20][3]*SKK_LOS[13] + P[20][1]*SKK_LOS[11] + P[20][2]*SKK_LOS[12] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[20][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
// Kfusion[21] = SK_LOS[1]*(P[21][0]*SKK_LOS[10] - P[21][3]*SKK_LOS[13] + P[21][1]*SKK_LOS[11] + P[21][2]*SKK_LOS[12] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[21][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
// Kfusion[22] = SK_LOS[1]*(P[22][0]*SKK_LOS[10] - P[22][3]*SKK_LOS[13] + P[22][1]*SKK_LOS[11] + P[22][2]*SKK_LOS[12] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[22][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
// varInnovOptFlow[1] = 1.0f/SK_LOS[1];
// innovOptFlow[1] = losPred[1] - losData[1];
// // reset the observation index
// obsIndex = 0;
// fuseOptFlowData = false;
// }
// // Check the innovation for consistency and don't fuse if > 3Sigma
// if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 9.0f)
// {
// // correct the state vector
// for (uint8_t j = 0; j < n_states; j++)
// {
// states[j] = states[j] - Kfusion[j] * innovOptFlow[obsIndex];
// }
// // normalise the quaternion states
// float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
// if (quatMag > 1e-12f)
// {
// for (uint8_t j= 0; j<=3; j++)
// {
// float quatMagInv = 1.0f/quatMag;
// states[j] = states[j] * quatMagInv;
// }
// }
// // correct the covariance P = (I - K*H)*P
// // take advantage of the empty columns in KH to reduce the
// // number of operations
// for (uint8_t i = 0; i < n_states; i++)
// {
// for (uint8_t j = 0; j <= 6; j++)
// {
// KH[i][j] = Kfusion[i] * H_LOS[j];
// }
// for (uint8_t j = 7; j <= 8; j++)
// {
// KH[i][j] = 0.0f;
// }
// KH[i][9] = Kfusion[i] * H_LOS[9];
// for (uint8_t j = 10; j <= 21; j++)
// {
// KH[i][j] = 0.0f;
// }
// KH[i][22] = Kfusion[i] * H_LOS[22];
// }
// for (uint8_t i = 0; i < n_states; i++)
// {
// for (uint8_t j = 0; j < n_states; j++)
// {
// KHP[i][j] = 0.0f;
// for (uint8_t k = 0; k <= 6; k++)
// {
// KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
// }
// KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j];
// KHP[i][j] = KHP[i][j] + KH[i][22] * P[2][j];
// }
// }
// }
// for (uint8_t i = 0; i < n_states; i++)
// {
// for (uint8_t j = 0; j < n_states; j++)
// {
// P[i][j] = P[i][j] - KHP[i][j];
// }
// }
// ForceSymmetry();
// ConstrainVariances();
// }
}
/*
Estimation of optical flow sensor focal length scale factor and terrain height using a two state EKF
This fiter requires optical flow rates that are not motion compensated
Range to ground measurement is assumed to be via a narrow beam type sensor - eg laser
*/
void AttPosEKF::GroundEKF()
{
// propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption
// limit distance to prevent intialisation afer bad gps causing bad numerical conditioning
if (!inhibitGndState) {
float distanceTravelledSq;
distanceTravelledSq = sq(statesAtRngTime[7] - prevPosN) + sq(statesAtRngTime[8] - prevPosE);
prevPosN = statesAtRngTime[7];
prevPosE = statesAtRngTime[8];
distanceTravelledSq = min(distanceTravelledSq, 100.0f);
Popt[1][1] += (distanceTravelledSq * sq(gndHgtSigma));
}
Vector3f velNED_local;
Vector3f relVelSensor;
// we aren't using optical flow measurements in this hacked implementation so set the covariances for this state to zero to avoid numerical problems
Popt[0][0] = 0.0f;
Popt[0][1] = 0.0f;
Popt[1][0] = 0.0f;
// Perform sequential fusion of optical flow measurements only when in the air and tilt is less than 30 deg.
if (useOpticalFlow && (fuseOptFlowData || obsIndex == 1) && !onGround && Tbs.z.z > 0.866f && rngMea > 5.0f && rngMea < 39.0f)
{
// Sequential fusion of XY components to spread processing load across
// two prediction time steps.
// Fuse range finder data
// Need to check that our range finder tilt angle is less than 30 degrees
float cosRngTilt = - Tbn.z.x * sinf(rngFinderPitch) + Tbn.z.z * cosf(rngFinderPitch);
if (useRangeFinder && fuseRngData && cosRngTilt > 0.87f) {
float range; // range from camera to centre of image
float q0; // quaternion at optical flow measurement time
float q1; // quaternion at optical flow measurement time
float q2; // quaternion at optical flow measurement time
float q3; // quaternion at optical flow measurement time
float R_RNG = 0.5; // range measurement variance (m^2) TODO make this a function of range and tilt to allow for sensor, alignment and AHRS errors
// Calculate observation jacobians and Kalman gains
if (fuseOptFlowData)
{
// Copy required states to local variable names
q0 = statesAtOptFlowTime[0];
q1 = statesAtOptFlowTime[1];
q2 = statesAtOptFlowTime[2];
q3 = statesAtOptFlowTime[3];
vn = statesAtOptFlowTime[4];
ve = statesAtOptFlowTime[5];
vd = statesAtOptFlowTime[6];
pd = statesAtOptFlowTime[9];
ptd = statesAtOptFlowTime[22];
velNED_local.x = vn;
velNED_local.y = ve;
velNED_local.z = vd;
// Copy required states to local variable names
q0 = statesAtRngTime[0];
q1 = statesAtRngTime[1];
q2 = statesAtRngTime[2];
q3 = statesAtRngTime[3];
// calculate rotation from NED to body axes
float q00 = sq(q0);
float q11 = sq(q1);
float q22 = sq(q2);
float q33 = sq(q3);
float q01 = q0 * q1;
float q02 = q0 * q2;
float q03 = q0 * q3;
float q12 = q1 * q2;
float q13 = q1 * q3;
float q23 = q2 * q3;
Tnb_local.x.x = q00 + q11 - q22 - q33;
Tnb_local.y.y = q00 - q11 + q22 - q33;
Tnb_local.z.z = q00 - q11 - q22 + q33;
Tnb_local.y.x = 2*(q12 - q03);
Tnb_local.z.x = 2*(q13 + q02);
Tnb_local.x.y = 2*(q12 + q03);
Tnb_local.z.y = 2*(q23 - q01);
Tnb_local.x.z = 2*(q13 - q02);
Tnb_local.y.z = 2*(q23 + q01);
// calculate transformation from NED to sensor axes
Tns = Tbs*Tnb_local;
// calculate range from ground plain to centre of sensor fov assuming flat earth
float range = ConstrainFloat(((ptd - pd)/Tns.z.z),0.5f,100.0f);
// calculate relative velocity in sensor frame
relVelSensor = Tns*velNED_local;
// divide velocity by range and include angular rate effects to get predicted angular LOS rates relative to X and Y axes
losPred[0] = relVelSensor.y/range;
losPred[1] = -relVelSensor.x/range;
//printf("relVelSensor.x=%5.1f, relVelSensor.y=%5.1f\n", relVelSensor.x, relVelSensor.y);
//printf("Xpred=%5.2f, Xmea=%5.2f, Ypred=%5.2f, Ymea=%5.2f, delAng.x=%4.4f, delAng.y=%4.4f\n", losPred[0], losData[0], losPred[1], losData[1], delAng.x, delAng.y);
//printf("omegaX=%5.2f, omegaY=%5.2f, velY=%5.1f velX=%5.1f\n, range=%5.1f\n", delAngRel.x/dt, delAngRel.y/dt, relVelSensor.y, relVelSensor.x, range);
// Calculate observation jacobians
SH_LOS[0] = a1*(2*q0*q1 + 2*q2*q3) + a2*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3);
SH_LOS[1] = vd*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)) - ve*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + vn*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3));
SH_LOS[2] = ve*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - vd*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)) + vn*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3));
SH_LOS[3] = 1/(pd - ptd);
SH_LOS[4] = 2*q1 - 2*a2*q3 + 2*a3*q2;
SH_LOS[5] = 2*a2*q2 - 2*q0 + 2*a3*q3;
SH_LOS[6] = 2*q2 + 2*a2*q0 - 2*a3*q1;
SH_LOS[7] = 1/sq(pd - ptd);
SH_LOS[8] = 2*q2 + 2*a1*q3 - 2*a3*q1;
SH_LOS[9] = 2*q3 - 2*a1*q2 + 2*a3*q0;
SH_LOS[10] = 2*a1*q1 - 2*q0 + 2*a3*q3;
SH_LOS[11] = 2*q3 + 2*a2*q1 + 2*a3*q0;
SH_LOS[12] = 2*q1 + 2*a1*q0 + 2*a3*q2;
for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0;
H_LOS[0] = - SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]);
H_LOS[1] = - SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]);
H_LOS[2] = SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]) - SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3);
H_LOS[3] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]);
H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3));
H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3));
H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3));
H_LOS[9] = SH_LOS[0]*SH_LOS[2]*SH_LOS[7];
H_LOS[22] = -SH_LOS[0]*SH_LOS[2]*SH_LOS[7];
// Calculate Kalman gain
SKK_LOS[0] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3);
SKK_LOS[1] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3);
SKK_LOS[2] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3);
SKK_LOS[3] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3);
SKK_LOS[4] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3);
SKK_LOS[5] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3);
SKK_LOS[6] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]);
SKK_LOS[7] = SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]);
SKK_LOS[8] = SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]);
SKK_LOS[9] = SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]);
SKK_LOS[10] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]);
SKK_LOS[11] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]);
SKK_LOS[12] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]);
SKK_LOS[13] = SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]);
SKK_LOS[14] = SH_LOS[0];
SK_LOS[0] = 1/(R_LOS + SKK_LOS[8]*(P[0][0]*SKK_LOS[8] + P[1][0]*SKK_LOS[7] + P[2][0]*SKK_LOS[9] - P[3][0]*SKK_LOS[6] - P[9][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][0]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][0]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[7]*(P[0][1]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] + P[2][1]*SKK_LOS[9] - P[3][1]*SKK_LOS[6] - P[9][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][1]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][1]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[9]*(P[0][2]*SKK_LOS[8] + P[1][2]*SKK_LOS[7] + P[2][2]*SKK_LOS[9] - P[3][2]*SKK_LOS[6] - P[9][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][2]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][2]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SKK_LOS[6]*(P[0][3]*SKK_LOS[8] + P[1][3]*SKK_LOS[7] + P[2][3]*SKK_LOS[9] - P[3][3]*SKK_LOS[6] - P[9][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][3]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][3]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[8] + P[1][9]*SKK_LOS[7] + P[2][9]*SKK_LOS[9] - P[3][9]*SKK_LOS[6] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][9]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][9]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[8] + P[1][22]*SKK_LOS[7] + P[2][22]*SKK_LOS[9] - P[3][22]*SKK_LOS[6] - P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][22]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][22]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14]*(P[0][4]*SKK_LOS[8] + P[1][4]*SKK_LOS[7] + P[2][4]*SKK_LOS[9] - P[3][4]*SKK_LOS[6] - P[9][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][4]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14]*(P[0][5]*SKK_LOS[8] + P[1][5]*SKK_LOS[7] + P[2][5]*SKK_LOS[9] - P[3][5]*SKK_LOS[6] - P[9][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]*(P[0][6]*SKK_LOS[8] + P[1][6]*SKK_LOS[7] + P[2][6]*SKK_LOS[9] - P[3][6]*SKK_LOS[6] - P[9][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]));
Kfusion[0] = -SK_LOS[0]*(P[0][0]*SKK_LOS[8] + P[0][1]*SKK_LOS[7] - P[0][3]*SKK_LOS[6] + P[0][2]*SKK_LOS[9] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[0][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[0][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
Kfusion[1] = -SK_LOS[0]*(P[1][0]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] - P[1][3]*SKK_LOS[6] + P[1][2]*SKK_LOS[9] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[1][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[1][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
Kfusion[2] = -SK_LOS[0]*(P[2][0]*SKK_LOS[8] + P[2][1]*SKK_LOS[7] - P[2][3]*SKK_LOS[6] + P[2][2]*SKK_LOS[9] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[2][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[2][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
Kfusion[3] = -SK_LOS[0]*(P[3][0]*SKK_LOS[8] + P[3][1]*SKK_LOS[7] - P[3][3]*SKK_LOS[6] + P[3][2]*SKK_LOS[9] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[3][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[3][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
Kfusion[4] = -SK_LOS[0]*(P[4][0]*SKK_LOS[8] + P[4][1]*SKK_LOS[7] - P[4][3]*SKK_LOS[6] + P[4][2]*SKK_LOS[9] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[4][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
Kfusion[5] = -SK_LOS[0]*(P[5][0]*SKK_LOS[8] + P[5][1]*SKK_LOS[7] - P[5][3]*SKK_LOS[6] + P[5][2]*SKK_LOS[9] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
Kfusion[6] = -SK_LOS[0]*(P[6][0]*SKK_LOS[8] + P[6][1]*SKK_LOS[7] - P[6][3]*SKK_LOS[6] + P[6][2]*SKK_LOS[9] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
Kfusion[7] = -SK_LOS[0]*(P[7][0]*SKK_LOS[8] + P[7][1]*SKK_LOS[7] - P[7][3]*SKK_LOS[6] + P[7][2]*SKK_LOS[9] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[7][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[7][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
Kfusion[8] = -SK_LOS[0]*(P[8][0]*SKK_LOS[8] + P[8][1]*SKK_LOS[7] - P[8][3]*SKK_LOS[6] + P[8][2]*SKK_LOS[9] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[8][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[8][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
Kfusion[9] = -SK_LOS[0]*(P[9][0]*SKK_LOS[8] + P[9][1]*SKK_LOS[7] - P[9][3]*SKK_LOS[6] + P[9][2]*SKK_LOS[9] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[9][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[9][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
Kfusion[10] = -SK_LOS[0]*(P[10][0]*SKK_LOS[8] + P[10][1]*SKK_LOS[7] - P[10][3]*SKK_LOS[6] + P[10][2]*SKK_LOS[9] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[10][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[10][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
Kfusion[11] = -SK_LOS[0]*(P[11][0]*SKK_LOS[8] + P[11][1]*SKK_LOS[7] - P[11][3]*SKK_LOS[6] + P[11][2]*SKK_LOS[9] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[11][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[11][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
Kfusion[12] = -SK_LOS[0]*(P[12][0]*SKK_LOS[8] + P[12][1]*SKK_LOS[7] - P[12][3]*SKK_LOS[6] + P[12][2]*SKK_LOS[9] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[12][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[12][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
Kfusion[13] = 0.0f;//-SK_LOS[0]*(P[13][0]*SKK_LOS[8] + P[13][1]*SKK_LOS[7] - P[13][3]*SKK_LOS[6] + P[13][2]*SKK_LOS[9] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[13][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[13][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
Kfusion[14] = -SK_LOS[0]*(P[14][0]*SKK_LOS[8] + P[14][1]*SKK_LOS[7] - P[14][3]*SKK_LOS[6] + P[14][2]*SKK_LOS[9] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[14][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[14][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
Kfusion[15] = -SK_LOS[0]*(P[15][0]*SKK_LOS[8] + P[15][1]*SKK_LOS[7] - P[15][3]*SKK_LOS[6] + P[15][2]*SKK_LOS[9] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[15][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[15][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
Kfusion[16] = -SK_LOS[0]*(P[16][0]*SKK_LOS[8] + P[16][1]*SKK_LOS[7] - P[16][3]*SKK_LOS[6] + P[16][2]*SKK_LOS[9] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[16][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[16][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
Kfusion[17] = -SK_LOS[0]*(P[17][0]*SKK_LOS[8] + P[17][1]*SKK_LOS[7] - P[17][3]*SKK_LOS[6] + P[17][2]*SKK_LOS[9] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[17][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[17][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
Kfusion[18] = -SK_LOS[0]*(P[18][0]*SKK_LOS[8] + P[18][1]*SKK_LOS[7] - P[18][3]*SKK_LOS[6] + P[18][2]*SKK_LOS[9] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[18][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[18][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
Kfusion[19] = -SK_LOS[0]*(P[19][0]*SKK_LOS[8] + P[19][1]*SKK_LOS[7] - P[19][3]*SKK_LOS[6] + P[19][2]*SKK_LOS[9] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[19][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[19][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
Kfusion[20] = -SK_LOS[0]*(P[20][0]*SKK_LOS[8] + P[20][1]*SKK_LOS[7] - P[20][3]*SKK_LOS[6] + P[20][2]*SKK_LOS[9] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[20][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[20][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
Kfusion[21] = -SK_LOS[0]*(P[21][0]*SKK_LOS[8] + P[21][1]*SKK_LOS[7] - P[21][3]*SKK_LOS[6] + P[21][2]*SKK_LOS[9] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[21][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[21][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
Kfusion[22] = -SK_LOS[0]*(P[22][0]*SKK_LOS[8] + P[22][1]*SKK_LOS[7] - P[22][3]*SKK_LOS[6] + P[22][2]*SKK_LOS[9] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[22][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[22][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
varInnovOptFlow[0] = 1.0f/SK_LOS[0];
innovOptFlow[0] = losPred[0] - losData[0];
// reset the observation index to 0 (we start by fusing the X
// measurement)
obsIndex = 0;
fuseOptFlowData = false;
// calculate Kalman gains
float SK_RNG[3];
SK_RNG[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
SK_RNG[1] = 1/(R_RNG + Popt[1][1]/sq(SK_RNG[0]));
SK_RNG[2] = 1/SK_RNG[0];
float K_RNG[2];
if (!inhibitScaleState) {
K_RNG[0] = Popt[0][1]*SK_RNG[1]*SK_RNG[2];
} else {
K_RNG[0] = 0.0f;
}
else if (obsIndex == 1) // we are now fusing the Y measurement
{
// Calculate observation jacobians
for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0;
H_LOS[0] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]);
H_LOS[1] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]);
H_LOS[2] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]);
H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]) - SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1);
H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3));
H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3));
H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3));
H_LOS[9] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[7];
H_LOS[22] = SH_LOS[0]*SH_LOS[1]*SH_LOS[7];
// Calculate Kalman gains
SK_LOS[1] = 1/(R_LOS + SKK_LOS[12]*(P[0][2]*SKK_LOS[10] + P[1][2]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[3][2]*SKK_LOS[13] - P[9][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][2]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][2]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SKK_LOS[13]*(P[0][3]*SKK_LOS[10] + P[1][3]*SKK_LOS[11] + P[2][3]*SKK_LOS[12] - P[3][3]*SKK_LOS[13] - P[9][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][3]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][3]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[10]*(P[0][0]*SKK_LOS[10] + P[1][0]*SKK_LOS[11] + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][0]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][0]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][0]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[11]*(P[0][1]*SKK_LOS[10] + P[1][1]*SKK_LOS[11] + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][1]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][1]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][1]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[10] + P[1][9]*SKK_LOS[11] + P[2][9]*SKK_LOS[12] - P[3][9]*SKK_LOS[13] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][9]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][9]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[10] + P[1][22]*SKK_LOS[11] + P[2][22]*SKK_LOS[12] - P[3][22]*SKK_LOS[13] - P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][22]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][22]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14]*(P[0][4]*SKK_LOS[10] + P[1][4]*SKK_LOS[11] + P[2][4]*SKK_LOS[12] - P[3][4]*SKK_LOS[13] - P[9][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][4]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]*(P[0][5]*SKK_LOS[10] + P[1][5]*SKK_LOS[11] + P[2][5]*SKK_LOS[12] - P[3][5]*SKK_LOS[13] - P[9][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]*(P[0][6]*SKK_LOS[10] + P[1][6]*SKK_LOS[11] + P[2][6]*SKK_LOS[12] - P[3][6]*SKK_LOS[13] - P[9][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]));
Kfusion[0] = SK_LOS[1]*(P[0][0]*SKK_LOS[10] - P[0][3]*SKK_LOS[13] + P[0][1]*SKK_LOS[11] + P[0][2]*SKK_LOS[12] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[0][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
Kfusion[1] = SK_LOS[1]*(P[1][0]*SKK_LOS[10] - P[1][3]*SKK_LOS[13] + P[1][1]*SKK_LOS[11] + P[1][2]*SKK_LOS[12] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[1][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
Kfusion[2] = SK_LOS[1]*(P[2][0]*SKK_LOS[10] - P[2][3]*SKK_LOS[13] + P[2][1]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[2][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
Kfusion[3] = SK_LOS[1]*(P[3][0]*SKK_LOS[10] - P[3][3]*SKK_LOS[13] + P[3][1]*SKK_LOS[11] + P[3][2]*SKK_LOS[12] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[3][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
Kfusion[4] = SK_LOS[1]*(P[4][0]*SKK_LOS[10] - P[4][3]*SKK_LOS[13] + P[4][1]*SKK_LOS[11] + P[4][2]*SKK_LOS[12] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[4][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
Kfusion[5] = SK_LOS[1]*(P[5][0]*SKK_LOS[10] - P[5][3]*SKK_LOS[13] + P[5][1]*SKK_LOS[11] + P[5][2]*SKK_LOS[12] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
Kfusion[6] = SK_LOS[1]*(P[6][0]*SKK_LOS[10] - P[6][3]*SKK_LOS[13] + P[6][1]*SKK_LOS[11] + P[6][2]*SKK_LOS[12] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
Kfusion[7] = SK_LOS[1]*(P[7][0]*SKK_LOS[10] - P[7][3]*SKK_LOS[13] + P[7][1]*SKK_LOS[11] + P[7][2]*SKK_LOS[12] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[7][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
Kfusion[8] = SK_LOS[1]*(P[8][0]*SKK_LOS[10] - P[8][3]*SKK_LOS[13] + P[8][1]*SKK_LOS[11] + P[8][2]*SKK_LOS[12] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[8][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
Kfusion[9] = SK_LOS[1]*(P[9][0]*SKK_LOS[10] - P[9][3]*SKK_LOS[13] + P[9][1]*SKK_LOS[11] + P[9][2]*SKK_LOS[12] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[9][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
Kfusion[10] = SK_LOS[1]*(P[10][0]*SKK_LOS[10] - P[10][3]*SKK_LOS[13] + P[10][1]*SKK_LOS[11] + P[10][2]*SKK_LOS[12] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[10][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
Kfusion[11] = SK_LOS[1]*(P[11][0]*SKK_LOS[10] - P[11][3]*SKK_LOS[13] + P[11][1]*SKK_LOS[11] + P[11][2]*SKK_LOS[12] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[11][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
Kfusion[12] = SK_LOS[1]*(P[12][0]*SKK_LOS[10] - P[12][3]*SKK_LOS[13] + P[12][1]*SKK_LOS[11] + P[12][2]*SKK_LOS[12] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[12][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
Kfusion[13] = 0.0f;//SK_LOS[1]*(P[13][0]*SKK_LOS[10] - P[13][3]*SKK_LOS[13] + P[13][1]*SKK_LOS[11] + P[13][2]*SKK_LOS[12] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[13][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
Kfusion[14] = SK_LOS[1]*(P[14][0]*SKK_LOS[10] - P[14][3]*SKK_LOS[13] + P[14][1]*SKK_LOS[11] + P[14][2]*SKK_LOS[12] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[14][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
Kfusion[15] = SK_LOS[1]*(P[15][0]*SKK_LOS[10] - P[15][3]*SKK_LOS[13] + P[15][1]*SKK_LOS[11] + P[15][2]*SKK_LOS[12] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[15][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
Kfusion[16] = SK_LOS[1]*(P[16][0]*SKK_LOS[10] - P[16][3]*SKK_LOS[13] + P[16][1]*SKK_LOS[11] + P[16][2]*SKK_LOS[12] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[16][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
Kfusion[17] = SK_LOS[1]*(P[17][0]*SKK_LOS[10] - P[17][3]*SKK_LOS[13] + P[17][1]*SKK_LOS[11] + P[17][2]*SKK_LOS[12] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[17][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
Kfusion[18] = SK_LOS[1]*(P[18][0]*SKK_LOS[10] - P[18][3]*SKK_LOS[13] + P[18][1]*SKK_LOS[11] + P[18][2]*SKK_LOS[12] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[18][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
Kfusion[19] = SK_LOS[1]*(P[19][0]*SKK_LOS[10] - P[19][3]*SKK_LOS[13] + P[19][1]*SKK_LOS[11] + P[19][2]*SKK_LOS[12] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[19][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
Kfusion[20] = SK_LOS[1]*(P[20][0]*SKK_LOS[10] - P[20][3]*SKK_LOS[13] + P[20][1]*SKK_LOS[11] + P[20][2]*SKK_LOS[12] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[20][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
Kfusion[21] = SK_LOS[1]*(P[21][0]*SKK_LOS[10] - P[21][3]*SKK_LOS[13] + P[21][1]*SKK_LOS[11] + P[21][2]*SKK_LOS[12] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[21][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
Kfusion[22] = SK_LOS[1]*(P[22][0]*SKK_LOS[10] - P[22][3]*SKK_LOS[13] + P[22][1]*SKK_LOS[11] + P[22][2]*SKK_LOS[12] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[22][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
varInnovOptFlow[1] = 1.0f/SK_LOS[1];
innovOptFlow[1] = losPred[1] - losData[1];
if (!inhibitGndState) {
K_RNG[1] = Popt[1][1]*SK_RNG[1]*SK_RNG[2];
} else {
K_RNG[1] = 0.0f;
}
// Check the innovation for consistency and don't fuse if > 3Sigma
if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 9.0f)
// Calculate the innovation variance for data logging
varInnovRng = 1.0f/SK_RNG[1];
// constrain terrain height to be below the vehicle
flowStates[1] = maxf(flowStates[1], statesAtRngTime[9] + minFlowRng);
// estimate range to centre of image
range = (flowStates[1] - statesAtRngTime[9]) * SK_RNG[2];
// Calculate the measurement innovation
innovRng = range - rngMea;
// calculate the innovation consistency test ratio
auxRngTestRatio = sq(innovRng) / (sq(rngInnovGate) * varInnovRng);
// Check the innovation for consistency and don't fuse if out of bounds
if (auxRngTestRatio < 1.0f)
{
// correct the state vector
for (uint8_t j = 0; j < n_states; j++)
{
states[j] = states[j] - Kfusion[j] * innovOptFlow[obsIndex];
}
// normalise the quaternion states
float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
if (quatMag > 1e-12f)
{
for (uint8_t j= 0; j<=3; j++)
{
float quatMagInv = 1.0f/quatMag;
states[j] = states[j] * quatMagInv;
}
}
// correct the covariance P = (I - K*H)*P
// take advantage of the empty columns in KH to reduce the
// number of operations
for (uint8_t i = 0; i < n_states; i++)
{
for (uint8_t j = 0; j <= 6; j++)
{
KH[i][j] = Kfusion[i] * H_LOS[j];
}
for (uint8_t j = 7; j <= 8; j++)
{
KH[i][j] = 0.0f;
}
KH[i][9] = Kfusion[i] * H_LOS[9];
for (uint8_t j = 10; j <= 21; j++)
{
KH[i][j] = 0.0f;
}
KH[i][22] = Kfusion[i] * H_LOS[22];
}
for (uint8_t i = 0; i < n_states; i++)
{
for (uint8_t j = 0; j < n_states; j++)
{
KHP[i][j] = 0.0f;
for (uint8_t k = 0; k <= 6; k++)
{
KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
}
KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j];
KHP[i][j] = KHP[i][j] + KH[i][22] * P[2][j];
}
}
}
for (uint8_t i = 0; i < n_states; i++)
{
for (uint8_t j = 0; j < n_states; j++)
{
P[i][j] = P[i][j] - KHP[i][j];
// correct the state
for (uint8_t i = 0; i < 2 ; i++) {
flowStates[i] -= K_RNG[i] * innovRng;
}
// constrain the states
// constrain focal length to 0.1 to 10 mm
flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f);
// constrain altitude
flowStates[1] = maxf(flowStates[1], statesAtRngTime[9] + minFlowRng);
// correct the covariance matrix
float nextPopt[2][2];
nextPopt[0][0] = Popt[0][0] - (Popt[0][1]*Popt[1][0]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2];
nextPopt[0][1] = Popt[0][1] - (Popt[0][1]*Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2];
nextPopt[1][0] = -Popt[1][0]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f);
nextPopt[1][1] = -Popt[1][1]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f);
// prevent the state variances from becoming negative and maintain symmetry
Popt[0][0] = maxf(nextPopt[0][0],0.0f);
Popt[1][1] = maxf(nextPopt[1][1],0.0f);
Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]);
Popt[1][0] = Popt[0][1];
}
}
obsIndex = obsIndex + 1;
ForceSymmetry();
ConstrainVariances();
}
void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
@@ -2126,6 +2305,24 @@ float AttPosEKF::sq(float valIn)
return valIn*valIn;
}
float AttPosEKF::maxf(float valIn1, float valIn2)
{
if (valIn1 >= valIn2) {
return valIn1;
} else {
return valIn2;
}
}
float AttPosEKF::min(float valIn1, float valIn2)
{
if (valIn1 <= valIn2) {
return valIn1;
} else {
return valIn2;
}
}
// Store states in a history array along with time stamp
void AttPosEKF::StoreStates(uint64_t timestamp_ms)
{
@@ -2322,9 +2519,9 @@ void AttPosEKF::OnGroundCheck()
}
// don't update terrain offset state if on ground
if (onGround) {
inhibitGndHgtState = true;
inhibitGndState = true;
} else {
inhibitGndHgtState = false;
inhibitGndState = false;
}
}
@@ -2364,15 +2561,15 @@ void AttPosEKF::CovarianceInit()
P[22][22] = sq(0.5f);
}
float AttPosEKF::ConstrainFloat(float val, float min, float max)
float AttPosEKF::ConstrainFloat(float val, float min_val, float max_val)
{
float ret;
if (val > max) {
ret = max;
ekf_debug("> max: %8.4f, val: %8.4f", (double)max, (double)val);
} else if (val < min) {
ret = min;
ekf_debug("< min: %8.4f, val: %8.4f", (double)min, (double)val);
if (val > max_val) {
ret = max_val;
ekf_debug("> max: %8.4f, val: %8.4f", (double)max_val, (double)val);
} else if (val < min_val) {
ret = min_val;
ekf_debug("< min: %8.4f, val: %8.4f", (double)min_val, (double)val);
} else {
ret = val;
}
@@ -3006,9 +3203,14 @@ void AttPosEKF::ZeroVariables()
{
// Initialize on-init initialized variables
dtIMUfilt = ConstrainFloat(dtIMU, 0.001f, 0.02f);
dtVelPosFilt = ConstrainFloat(dtVelPos, 0.04f, 0.5f);
dtGpsFilt = 1.0f / 5.0f;
dtHgtFilt = 1.0f / 100.0f;
storeIndex = 0;
lastVelPosFusion = millis();
// Do the data structure init
for (unsigned i = 0; i < n_states; i++) {
for (unsigned j = 0; j < n_states; j++) {
@@ -3028,6 +3230,13 @@ void AttPosEKF::ZeroVariables()
dVelIMU.zero();
lastGyroOffset.zero();
windSpdFiltNorth = 0.0f;
windSpdFiltEast = 0.0f;
// setting the altitude to zero will give us a higher
// gain to adjust faster in the first step
windSpdFiltAltitude = 0.0f;
windSpdFiltClimb = 0.0f;
for (unsigned i = 0; i < data_buffer_size; i++) {
for (unsigned j = 0; j < n_states; j++) {
src/modules/ekf_att_pos_estimator/estimator_23states.h
+58 -3
View File
@@ -80,6 +80,14 @@ public:
airspeedMeasurementSigma = 1.4f;
gyroProcessNoise = 1.4544411e-2f;
accelProcessNoise = 0.5f;
gndHgtSigma = 0.1f; // terrain gradient 1-sigma
R_LOS = 0.03f; // 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
}
struct mag_state_struct {
@@ -116,13 +124,16 @@ public:
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
// Times
uint64_t lastVelPosFusion; // the time of the last velocity fusion, in the standard time unit of the filter
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
float statesAtRngTime[n_states]; // filter states at the effective measurement time
float statesAtOptFlowTime[n_states]; // States at the effective optical flow measurement time
float statesAtFlowTime[n_states]; // States 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)
@@ -140,7 +151,16 @@ public:
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)
float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec), this may have significant jitter
float dtIMUfilt; // average time between IMU measurements (sec)
float dtVelPos; // time lapsed since the last position / velocity fusion (seconds), this may have significant jitter
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
@@ -192,7 +212,8 @@ public:
bool inhibitWindStates; // true when wind states and covariances are to remain constant
bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant
bool inhibitGndHgtState; // true when the terrain ground height offset state and covariances are to remain constant
bool inhibitGndState; // true when the terrain ground height offset state and covariances are to remain constant
bool inhibitScaleState; // true when the focal length scale factor state and covariances are to remain constant
bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying)
bool staticMode; ///< boolean true if no position feedback is fused
@@ -211,6 +232,30 @@ public:
unsigned storeIndex;
// Optical Flow error estimation
float storedOmega[3][data_buffer_size]; // angular rate vector stored for the last 50 time steps used by optical flow eror estimators
// Two state EKF used to estimate focal length scale factor and terrain position
float Popt[2][2]; // state covariance matrix
float flowStates[2]; // flow states [scale factor, terrain position]
float prevPosN; // north position at last measurement
float prevPosE; // east position at last measurement
float auxFlowObsInnov[2]; // optical flow observation innovations from focal length scale factor estimator
float auxFlowObsInnovVar[2]; // innovation variance for optical flow observations from focal length scale factor estimator
float fScaleFactorVar; // optical flow sensor focal length scale factor variance
Mat3f Tnb_flow; // Transformation matrix from nav to body at the time fo the optical flow measurement
float R_LOS; // Optical flow observation noise variance (rad/sec)^2
float auxFlowTestRatio[2]; // ratio of X and Y flow observation innovations to fault threshold
float auxRngTestRatio; // ratio of range observation innovations to fault threshold
float flowInnovGate; // number of standard deviations used for the innovation consistency check
float auxFlowInnovGate; // number of standard deviations applied to the optical flow innovation consistency check
float rngInnovGate; // number of standard deviations used for the innovation consistency check
float minFlowRng; // minimum range over which to fuse optical flow measurements
float moCompR_LOS; // scaler from sensor gyro rate to uncertainty in LOS rate
void updateDtGpsFilt(float dt);
void updateDtHgtFilt(float dt);
void UpdateStrapdownEquationsNED();
@@ -226,6 +271,8 @@ void FuseRangeFinder();
void FuseOptFlow();
void GroundEKF();
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);
@@ -268,6 +315,10 @@ static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
static float sq(float valIn);
static float maxf(float valIn1, float valIn2);
static float min(float valIn1, float valIn2);
void OnGroundCheck();
void CovarianceInit();
@@ -300,6 +351,8 @@ void InitializeDynamic(float (&initvelNED)[3], float declination);
protected:
void updateDtVelPosFilt(float dt);
bool FilterHealthy();
bool GyroOffsetsDiverged();
@@ -314,3 +367,5 @@ void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, fl
uint32_t millis();
uint64_t getMicros();
src/modules/fw_att_control/fw_att_control_main.cpp
+58 -11
View File
@@ -63,6 +63,7 @@
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_status.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/pid/pid.h>
@@ -124,6 +125,7 @@ private:
int _params_sub; /**< notification of parameter updates */
int _manual_sub; /**< notification of manual control updates */
int _global_pos_sub; /**< global position subscription */
int _vehicle_status_sub; /**< vehicle status subscription */
orb_advert_t _rate_sp_pub; /**< rate setpoint publication */
orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */
@@ -139,12 +141,14 @@ private:
struct actuator_controls_s _actuators; /**< actuator control inputs */
struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */
struct vehicle_global_position_s _global_pos; /**< global position */
struct vehicle_status_s _vehicle_status; /**< vehicle status */
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */
perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */
bool _setpoint_valid; /**< flag if the position control setpoint is valid */
bool _debug; /**< if set to true, print debug output */
struct {
float tconst;
@@ -274,6 +278,11 @@ private:
*/
void global_pos_poll();
/**
* Check for vehicle status updates.
*/
void vehicle_status_poll();
/**
* Shim for calling task_main from task_create.
*/
@@ -312,6 +321,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_params_sub(-1),
_manual_sub(-1),
_global_pos_sub(-1),
_vehicle_status_sub(-1),
/* publications */
_rate_sp_pub(-1),
@@ -324,7 +334,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")),
_nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")),
/* states */
_setpoint_valid(false)
_setpoint_valid(false),
_debug(false)
{
/* safely initialize structs */
_att = {};
@@ -336,6 +347,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_actuators = {};
_actuators_airframe = {};
_global_pos = {};
_vehicle_status = {};
_parameter_handles.tconst = param_find("FW_ATT_TC");
@@ -558,6 +570,18 @@ FixedwingAttitudeControl::global_pos_poll()
}
}
void
FixedwingAttitudeControl::vehicle_status_poll()
{
/* check if there is new status information */
bool vehicle_status_updated;
orb_check(_vehicle_status_sub, &vehicle_status_updated);
if (vehicle_status_updated) {
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
}
}
void
FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[])
{
@@ -583,6 +607,7 @@ FixedwingAttitudeControl::task_main()
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vcontrol_mode_sub, 200);
@@ -597,6 +622,7 @@ FixedwingAttitudeControl::task_main()
vehicle_accel_poll();
vehicle_control_mode_poll();
vehicle_manual_poll();
vehicle_status_poll();
/* wakeup source(s) */
struct pollfd fds[2];
@@ -665,6 +691,8 @@ FixedwingAttitudeControl::task_main()
global_pos_poll();
vehicle_status_poll();
/* lock integrator until control is started */
bool lock_integrator;
@@ -700,7 +728,8 @@ FixedwingAttitudeControl::task_main()
perf_count(_nonfinite_input_perf);
}
} else {
airspeed = _airspeed.true_airspeed_m_s;
/* prevent numerical drama by requiring 0.5 m/s minimal speed */
airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s);
}
/*
@@ -717,7 +746,14 @@ FixedwingAttitudeControl::task_main()
float pitch_sp = _parameters.pitchsp_offset_rad;
float throttle_sp = 0.0f;
if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) {
/* Read attitude setpoint from uorb if
* - velocity control or position control is enabled (pos controller is running)
* - manual control is disabled (another app may send the setpoint, but it should
* for sure not be set from the remote control values)
*/
if (_vcontrol_mode.flag_control_velocity_enabled ||
_vcontrol_mode.flag_control_position_enabled ||
!_vcontrol_mode.flag_control_manual_enabled) {
/* read in attitude setpoint from attitude setpoint uorb topic */
roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
@@ -776,6 +812,13 @@ FixedwingAttitudeControl::task_main()
}
}
/* If the aircraft is on ground reset the integrators */
if (_vehicle_status.condition_landed) {
_roll_ctrl.reset_integrator();
_pitch_ctrl.reset_integrator();
_yaw_ctrl.reset_integrator();
}
/* Prepare speed_body_u and speed_body_w */
float speed_body_u = 0.0f;
float speed_body_v = 0.0f;
@@ -785,7 +828,7 @@ FixedwingAttitudeControl::task_main()
speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d;
speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d;
} else {
if (loop_counter % 10 == 0) {
if (_debug && loop_counter % 10 == 0) {
warnx("Did not get a valid R\n");
}
}
@@ -808,7 +851,7 @@ FixedwingAttitudeControl::task_main()
_roll_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (loop_counter % 10 == 0) {
if (_debug && loop_counter % 10 == 0) {
warnx("roll_u %.4f", (double)roll_u);
}
}
@@ -821,7 +864,7 @@ FixedwingAttitudeControl::task_main()
if (!isfinite(pitch_u)) {
_pitch_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (loop_counter % 10 == 0) {
if (_debug && loop_counter % 10 == 0) {
warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
" airspeed %.4f, airspeed_scaling %.4f,"
" roll_sp %.4f, pitch_sp %.4f,"
@@ -845,21 +888,25 @@ FixedwingAttitudeControl::task_main()
if (!isfinite(yaw_u)) {
_yaw_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (loop_counter % 10 == 0) {
if (_debug && loop_counter % 10 == 0) {
warnx("yaw_u %.4f", (double)yaw_u);
}
}
/* throttle passed through */
_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
/* throttle passed through if it is finite and if no engine failure was
* detected */
_actuators.control[3] = (isfinite(throttle_sp) &&
!(_vehicle_status.engine_failure ||
_vehicle_status.engine_failure_cmd)) ?
throttle_sp : 0.0f;
if (!isfinite(throttle_sp)) {
if (loop_counter % 10 == 0) {
if (_debug && loop_counter % 10 == 0) {
warnx("throttle_sp %.4f", (double)throttle_sp);
}
}
} else {
perf_count(_nonfinite_input_perf);
if (loop_counter % 10 == 0) {
if (_debug && loop_counter % 10 == 0) {
warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
}
}
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+211 -129
View File
@@ -88,7 +88,6 @@
#include <launchdetection/LaunchDetector.h>
#include <ecl/l1/ecl_l1_pos_controller.h>
#include <external_lgpl/tecs/tecs.h>
#include <drivers/drv_range_finder.h>
#include "landingslope.h"
#include "mtecs/mTecs.h"
@@ -102,6 +101,8 @@ static int _control_task = -1; /**< task handle for sensor task */
*/
extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]);
using namespace launchdetection;
class FixedwingPositionControl
{
public:
@@ -139,11 +140,11 @@ private:
int _pos_sp_triplet_sub;
int _att_sub; /**< vehicle attitude subscription */
int _airspeed_sub; /**< airspeed subscription */
int _control_mode_sub; /**< vehicle status subscription */
int _control_mode_sub; /**< control mode subscription */
int _vehicle_status_sub; /**< vehicle status subscription */
int _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control updates */
int _sensor_combined_sub; /**< for body frame accelerations */
int _range_finder_sub; /**< range finder subscription */
orb_advert_t _attitude_sp_pub; /**< attitude setpoint */
orb_advert_t _tecs_status_pub; /**< TECS status publication */
@@ -154,25 +155,24 @@ private:
struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct airspeed_s _airspeed; /**< airspeed */
struct vehicle_control_mode_s _control_mode; /**< vehicle status */
struct vehicle_control_mode_s _control_mode; /**< control mode */
struct vehicle_status_s _vehicle_status; /**< vehicle status */
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */
struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */
struct range_finder_report _range_finder; /**< range finder report */
perf_counter_t _loop_perf; /**< loop performance counter */
/* land states */
/* not in non-abort mode for landing yet */
bool land_noreturn_horizontal;
bool land_noreturn_vertical;
bool land_stayonground;
bool land_motor_lim;
bool land_onslope;
bool land_useterrain;
/* takeoff/launch states */
bool launch_detected;
bool usePreTakeoffThrust;
LaunchDetectionResult launch_detection_state;
bool last_manual; ///< true if the last iteration was in manual mode (used to determine when a reset is needed)
@@ -210,6 +210,7 @@ private:
float max_climb_rate;
float climbout_diff;
float heightrate_p;
float heightrate_ff;
float speedrate_p;
float throttle_damp;
float integrator_gain;
@@ -239,7 +240,9 @@ private:
float land_flare_alt_relative;
float land_thrust_lim_alt_relative;
float land_heading_hold_horizontal_distance;
float range_finder_rel_alt;
float land_flare_pitch_min_deg;
float land_flare_pitch_max_deg;
int land_use_terrain_estimate;
} _parameters; /**< local copies of interesting parameters */
@@ -255,6 +258,7 @@ private:
param_t max_climb_rate;
param_t climbout_diff;
param_t heightrate_p;
param_t heightrate_ff;
param_t speedrate_p;
param_t throttle_damp;
param_t integrator_gain;
@@ -284,7 +288,9 @@ private:
param_t land_flare_alt_relative;
param_t land_thrust_lim_alt_relative;
param_t land_heading_hold_horizontal_distance;
param_t range_finder_rel_alt;
param_t land_flare_pitch_min_deg;
param_t land_flare_pitch_max_deg;
param_t land_use_terrain_estimate;
} _parameter_handles; /**< handles for interesting parameters */
@@ -301,21 +307,20 @@ private:
void control_update();
/**
* Check for changes in vehicle status.
* Check for changes in control mode
*/
void vehicle_control_mode_poll();
/**
* Check for changes in vehicle status.
*/
void vehicle_status_poll();
/**
* Check for airspeed updates.
*/
bool vehicle_airspeed_poll();
/**
* Check for range finder updates.
*/
bool range_finder_poll();
/**
* Check for position updates.
*/
@@ -337,9 +342,9 @@ private:
void navigation_capabilities_publish();
/**
* Get the relative alt either from the difference between estimate and waypoint or from the laser range finder
* Return the terrain estimate during landing: uses the wp altitude value or the terrain estimate if available
*/
float get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt);
float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos);
/**
* Control position.
@@ -380,7 +385,8 @@ private:
bool climbout_mode, float climbout_pitch_min_rad,
float altitude,
const math::Vector<3> &ground_speed,
tecs_mode mode = TECS_MODE_NORMAL);
tecs_mode mode = TECS_MODE_NORMAL,
bool pitch_max_special = false);
};
@@ -408,10 +414,10 @@ FixedwingPositionControl::FixedwingPositionControl() :
_att_sub(-1),
_airspeed_sub(-1),
_control_mode_sub(-1),
_vehicle_status_sub(-1),
_params_sub(-1),
_manual_control_sub(-1),
_sensor_combined_sub(-1),
_range_finder_sub(-1),
/* publications */
_attitude_sp_pub(-1),
@@ -425,10 +431,10 @@ FixedwingPositionControl::FixedwingPositionControl() :
_manual(),
_airspeed(),
_control_mode(),
_vehicle_status(),
_global_pos(),
_pos_sp_triplet(),
_sensor_combined(),
_range_finder(),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
@@ -438,8 +444,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
land_stayonground(false),
land_motor_lim(false),
land_onslope(false),
launch_detected(false),
usePreTakeoffThrust(false),
land_useterrain(false),
launch_detection_state(LAUNCHDETECTION_RES_NONE),
last_manual(false),
landingslope(),
flare_curve_alt_rel_last(0.0f),
@@ -477,7 +483,9 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT");
_parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT");
_parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
_parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT");
_parameter_handles.land_flare_pitch_min_deg = param_find("FW_FLARE_PMIN");
_parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX");
_parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER");
_parameter_handles.time_const = param_find("FW_T_TIME_CONST");
_parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST");
@@ -494,6 +502,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT");
_parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP");
_parameter_handles.heightrate_p = param_find("FW_T_HRATE_P");
_parameter_handles.heightrate_ff = param_find("FW_T_HRATE_FF");
_parameter_handles.speedrate_p = param_find("FW_T_SRATE_P");
/* fetch initial parameter values */
@@ -563,15 +572,23 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.climbout_diff, &(_parameters.climbout_diff));
param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p));
param_get(_parameter_handles.heightrate_ff, &(_parameters.heightrate_ff));
param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p));
param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle));
param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt));
param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative));
param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative));
param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance));
param_get(_parameter_handles.range_finder_rel_alt, &(_parameters.range_finder_rel_alt));
/* check if negative value for 2/3 of flare altitude is set for throttle cut */
if (_parameters.land_thrust_lim_alt_relative < 0.0f) {
_parameters.land_thrust_lim_alt_relative = 0.66f * _parameters.land_flare_alt_relative;
}
param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance));
param_get(_parameter_handles.land_flare_pitch_min_deg, &(_parameters.land_flare_pitch_min_deg));
param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg));
param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate));
_l1_control.set_l1_damping(_parameters.l1_damping);
_l1_control.set_l1_period(_parameters.l1_period);
@@ -594,6 +611,7 @@ FixedwingPositionControl::parameters_update()
_tecs.set_indicated_airspeed_max(_parameters.airspeed_max);
_tecs.set_max_climb_rate(_parameters.max_climb_rate);
_tecs.set_heightrate_p(_parameters.heightrate_p);
_tecs.set_heightrate_ff(_parameters.heightrate_ff);
_tecs.set_speedrate_p(_parameters.speedrate_p);
/* sanity check parameters */
@@ -627,16 +645,27 @@ FixedwingPositionControl::parameters_update()
void
FixedwingPositionControl::vehicle_control_mode_poll()
{
bool vstatus_updated;
bool updated;
/* Check HIL state if vehicle status has changed */
orb_check(_control_mode_sub, &vstatus_updated);
orb_check(_control_mode_sub, &updated);
if (vstatus_updated) {
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
}
}
void
FixedwingPositionControl::vehicle_status_poll()
{
bool updated;
orb_check(_vehicle_status_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
}
}
bool
FixedwingPositionControl::vehicle_airspeed_poll()
{
@@ -663,20 +692,6 @@ FixedwingPositionControl::vehicle_airspeed_poll()
return airspeed_updated;
}
bool
FixedwingPositionControl::range_finder_poll()
{
/* check if there is a range finder measurement */
bool range_finder_updated;
orb_check(_range_finder_sub, &range_finder_updated);
if (range_finder_updated) {
orb_copy(ORB_ID(sensor_range_finder), _range_finder_sub, &_range_finder);
}
return range_finder_updated;
}
void
FixedwingPositionControl::vehicle_attitude_poll()
{
@@ -814,21 +829,23 @@ void FixedwingPositionControl::navigation_capabilities_publish()
}
}
float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt)
float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos)
{
float rel_alt_estimated = current_alt - land_setpoint_alt;
/* only use range finder if:
* parameter (range_finder_use_relative_alt) > 0
* the measurement is valid
* the estimated relative altitude (from global altitude estimate and landing waypoint) <= range_finder_use_relative_alt
*/
if (range_finder_use_relative_alt < 0 || !range_finder.valid || rel_alt_estimated > range_finder_use_relative_alt ) {
return rel_alt_estimated;
if (!isfinite(global_pos.terrain_alt)) {
return land_setpoint_alt;
}
return range_finder.distance;
/* Decide if the terrain estimation can be used, once we switched to using the terrain we stick with it
* for the whole landing */
if (_parameters.land_use_terrain_estimate && (global_pos.terrain_alt_valid || land_useterrain)) {
if(!land_useterrain) {
mavlink_log_info(_mavlink_fd, "#audio: Landing, using terrain estimate");
land_useterrain = true;
}
return global_pos.terrain_alt;
} else {
return land_setpoint_alt;
}
}
bool
@@ -933,10 +950,17 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
/* Horizontal landing control */
/* switch to heading hold for the last meters, continue heading hold after */
float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
/* calculate a waypoint distance value which is 0 when the aircraft is behind the waypoint */
float wp_distance_save = wp_distance;
if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) >= math::radians(90.0f)) {
wp_distance_save = 0.0f;
}
//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) {
@@ -972,29 +996,30 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* Vertical landing control */
//xxx: using the tecs altitude controller for slope control for now
// /* do not go down too early */
// if (wp_distance > 50.0f) {
// altitude_error = (_global_triplet.current.alt + 25.0f) - _global_pos.alt;
// }
/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
// XXX this could make a great param
float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(pos_sp_triplet.current.param1)
float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
float airspeed_land = 1.3f * _parameters.airspeed_min;
float airspeed_approach = 1.3f * _parameters.airspeed_min;
/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f;
/* Get an estimate of the terrain altitude if available, otherwise terrain_alt will be
* equal to _pos_sp_triplet.current.alt */
float terrain_alt = get_terrain_altitude_landing(_pos_sp_triplet.current.alt, _global_pos);
/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
float L_altitude_rel = _pos_sp_triplet.previous.valid ?
_pos_sp_triplet.previous.alt - terrain_alt : 0.0f;
float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp);
float relative_alt = get_relative_landingalt(_pos_sp_triplet.current.alt, _global_pos.alt, _range_finder, _parameters.range_finder_rel_alt);
if ( (relative_alt < landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out
/* Check if we should start flaring with a vertical and a
* horizontal limit (with some tolerance)
* The horizontal limit is only applied when we are in front of the wp
*/
if (((_global_pos.alt < terrain_alt + landingslope.flare_relative_alt()) &&
(wp_distance_save < landingslope.flare_length() + 5.0f)) ||
land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out
/* land with minimal speed */
// /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */
@@ -1003,7 +1028,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* kill the throttle if param requests it */
throttle_max = _parameters.throttle_max;
if (relative_alt < landingslope.motor_lim_relative_alt() || land_motor_lim) {
if (_global_pos.alt < terrain_alt + landingslope.motor_lim_relative_alt() || land_motor_lim) {
throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
if (!land_motor_lim) {
land_motor_lim = true;
@@ -1021,12 +1046,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
land_stayonground = true;
}
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + flare_curve_alt_rel,
tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel,
calculate_target_airspeed(airspeed_land), eas2tas,
flare_pitch_angle_rad, math::radians(15.0f),
math::radians(_parameters.land_flare_pitch_min_deg),
math::radians(_parameters.land_flare_pitch_max_deg),
0.0f, throttle_max, throttle_land,
false, flare_pitch_angle_rad,
_pos_sp_triplet.current.alt + relative_alt, ground_speed,
false, land_motor_lim ? math::radians(_parameters.land_flare_pitch_min_deg) : math::radians(_parameters.pitch_limit_min),
_global_pos.alt, ground_speed,
land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND);
if (!land_noreturn_vertical) {
@@ -1047,8 +1073,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
* if current position is below the slope continue at previous wp altitude
* until the intersection with slope
* */
float altitude_desired_rel = relative_alt;
if (relative_alt > landing_slope_alt_rel_desired || land_onslope) {
float altitude_desired_rel;
if (_global_pos.alt > terrain_alt + landing_slope_alt_rel_desired || land_onslope) {
/* stay on slope */
altitude_desired_rel = landing_slope_alt_rel_desired;
if (!land_onslope) {
@@ -1057,10 +1083,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
} else {
/* continue horizontally */
altitude_desired_rel = _pos_sp_triplet.previous.valid ? L_altitude_rel : relative_alt;
altitude_desired_rel = _pos_sp_triplet.previous.valid ? L_altitude_rel :
_global_pos.alt - terrain_alt;
}
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + altitude_desired_rel,
tecs_update_pitch_throttle(terrain_alt + altitude_desired_rel,
calculate_target_airspeed(airspeed_approach), eas2tas,
math::radians(_parameters.pitch_limit_min),
math::radians(_parameters.pitch_limit_max),
@@ -1069,48 +1096,53 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_parameters.throttle_cruise,
false,
math::radians(_parameters.pitch_limit_min),
_pos_sp_triplet.current.alt + relative_alt,
_global_pos.alt,
ground_speed);
}
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) {
/* Perform launch detection */
if(!launch_detected) { //do not do further checks once a launch was detected
if (launchDetector.launchDetectionEnabled()) {
static hrt_abstime last_sent = 0;
if(hrt_absolute_time() - last_sent > 4e6) {
mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running");
last_sent = hrt_absolute_time();
}
/* Tell the attitude controller to stop integrating while we are waiting
* for the launch */
_att_sp.roll_reset_integral = true;
_att_sp.pitch_reset_integral = true;
_att_sp.yaw_reset_integral = true;
/* Detect launch */
launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
if (launchDetector.getLaunchDetected()) {
launch_detected = true;
mavlink_log_info(_mavlink_fd, "#audio: Takeoff");
}
} else {
/* no takeoff detection --> fly */
launch_detected = true;
warnx("launchdetection off");
if (launchDetector.launchDetectionEnabled() &&
launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
/* Inform user that launchdetection is running */
static hrt_abstime last_sent = 0;
if(hrt_absolute_time() - last_sent > 4e6) {
mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running");
last_sent = hrt_absolute_time();
}
/* Detect launch */
launchDetector.update(_sensor_combined.accelerometer_m_s2[0]);
/* update our copy of the laucn detection state */
launch_detection_state = launchDetector.getLaunchDetected();
} else {
/* no takeoff detection --> fly */
launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
}
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
/* Set control values depending on the detection state */
if (launch_detection_state != LAUNCHDETECTION_RES_NONE) {
/* Launch has been detected, hence we have to control the plane. */
if (launch_detected) {
usePreTakeoffThrust = false;
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
/* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use
* full throttle, otherwise we use the preTakeOff Throttle */
float takeoff_throttle = launch_detection_state !=
LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS ?
launchDetector.getThrottlePreTakeoff() : _parameters.throttle_max;
/* select maximum pitch: the launchdetector may impose another limit for the pitch
* depending on the state of the launch */
float takeoff_pitch_max_deg = launchDetector.getPitchMax(_parameters.pitch_limit_max);
float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg);
/* apply minimum pitch and limit roll if target altitude is not within climbout_diff
* meters */
if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) {
/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
@@ -1118,18 +1150,20 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
calculate_target_airspeed(1.3f * _parameters.airspeed_min),
eas2tas,
math::radians(_parameters.pitch_limit_min),
math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max,
takeoff_pitch_max_rad,
_parameters.throttle_min, takeoff_throttle,
_parameters.throttle_cruise,
true,
math::max(math::radians(pos_sp_triplet.current.pitch_min),
math::radians(10.0f)),
_global_pos.alt,
ground_speed,
TECS_MODE_TAKEOFF);
TECS_MODE_TAKEOFF,
takeoff_pitch_max_deg != _parameters.pitch_limit_max);
/* limit roll motion to ensure enough lift */
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f));
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f),
math::radians(15.0f));
} else {
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
@@ -1138,17 +1172,26 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
math::radians(_parameters.pitch_limit_min),
math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min,
_parameters.throttle_max,
takeoff_throttle,
_parameters.throttle_cruise,
false,
math::radians(_parameters.pitch_limit_min),
_global_pos.alt,
ground_speed);
}
} else {
usePreTakeoffThrust = true;
/* Tell the attitude controller to stop integrating while we are waiting
* for the launch */
_att_sp.roll_reset_integral = true;
_att_sp.pitch_reset_integral = true;
_att_sp.yaw_reset_integral = true;
/* Set default roll and pitch setpoints during detection phase */
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = math::max(math::radians(pos_sp_triplet.current.pitch_min),
math::radians(10.0f));
}
}
/* reset landing state */
@@ -1182,13 +1225,26 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
}
if (usePreTakeoffThrust) {
if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) {
/* Set thrust to 0 to minimize damage */
_att_sp.thrust = 0.0f;
} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
/* making sure again that the correct thrust is used,
* without depending on library calls for safety reasons */
_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
} else {
/* Copy thrust and pitch values from tecs */
_att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :
_tecs.get_throttle_demand(), throttle_max);
}
else {
_att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : _tecs.get_throttle_demand(), throttle_max);
/* During a takeoff waypoint while waiting for launch the pitch sp is set
* already (not by tecs) */
if (!(pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
launch_detection_state == LAUNCHDETECTION_RES_NONE)) {
_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
}
_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
if (_control_mode.flag_control_position_enabled) {
last_manual = false;
@@ -1212,13 +1268,15 @@ FixedwingPositionControl::task_main()
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
/* rate limit vehicle status updates to 5Hz */
/* rate limit control mode updates to 5Hz */
orb_set_interval(_control_mode_sub, 200);
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vehicle_status_sub, 200);
/* rate limit position updates to 50 Hz */
orb_set_interval(_global_pos_sub, 20);
@@ -1257,9 +1315,12 @@ FixedwingPositionControl::task_main()
perf_begin(_loop_perf);
/* check vehicle status for changes to publication state */
/* check vehicle control mode for changes to publication state */
vehicle_control_mode_poll();
/* check vehicle status for changes to publication state */
vehicle_status_poll();
/* only update parameters if they changed */
if (fds[0].revents & POLLIN) {
/* read from param to clear updated flag */
@@ -1289,7 +1350,6 @@ FixedwingPositionControl::task_main()
vehicle_setpoint_poll();
vehicle_sensor_combined_poll();
vehicle_airspeed_poll();
range_finder_poll();
// vehicle_baro_poll();
math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e, _global_pos.vel_d);
@@ -1342,8 +1402,7 @@ FixedwingPositionControl::task_main()
void FixedwingPositionControl::reset_takeoff_state()
{
launch_detected = false;
usePreTakeoffThrust = false;
launch_detection_state = LAUNCHDETECTION_RES_NONE;
launchDetector.reset();
}
@@ -1354,6 +1413,7 @@ void FixedwingPositionControl::reset_landing_state()
land_stayonground = false;
land_motor_lim = false;
land_onslope = false;
land_useterrain = false;
}
void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas,
@@ -1362,7 +1422,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
bool climbout_mode, float climbout_pitch_min_rad,
float altitude,
const math::Vector<3> &ground_speed,
tecs_mode mode)
tecs_mode mode, bool pitch_max_special)
{
if (_mTecs.getEnabled()) {
/* Using mtecs library: prepare arguments for mtecs call */
@@ -1372,14 +1432,36 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
}
fwPosctrl::LimitOverride limitOverride;
if (climbout_mode) {
if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) {
/* Force the slow downwards spiral */
limitOverride.enablePitchMinOverride(-1.0f);
limitOverride.enablePitchMaxOverride(5.0f);
} else if (climbout_mode) {
limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad);
} else {
limitOverride.disablePitchMinOverride();
}
if (pitch_max_special) {
/* Use the maximum pitch from the argument */
limitOverride.enablePitchMaxOverride(M_RAD_TO_DEG_F * pitch_max_rad);
} else {
/* use pitch max set by MT param */
limitOverride.disablePitchMaxOverride();
}
_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode,
limitOverride);
} else {
if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) {
/* Force the slow downwards spiral */
pitch_min_rad = M_DEG_TO_RAD_F * -1.0f;
pitch_max_rad = M_DEG_TO_RAD_F * 5.0f;
}
/* No underspeed protection in landing mode */
_tecs.set_detect_underspeed_enabled(!(mode == TECS_MODE_LAND || mode == TECS_MODE_LAND_THROTTLELIM));
/* Using tecs library */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp,
_airspeed.indicated_airspeed_m_s, eas2tas,
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+96 -68
View File
@@ -131,8 +131,8 @@ PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f);
/**
* Throttle limit max
*
* This is the maximum throttle % that can be used by the controller.
* For overpowered aircraft, this should be reduced to a value that
* This is the maximum throttle % that can be used by the controller.
* For overpowered aircraft, this should be reduced to a value that
* provides sufficient thrust to climb at the maximum pitch angle PTCH_MAX.
*
* @group L1 Control
@@ -142,10 +142,10 @@ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f);
/**
* Throttle limit min
*
* This is the minimum throttle % that can be used by the controller.
* For electric aircraft this will normally be set to zero, but can be set
* to a small non-zero value if a folding prop is fitted to prevent the
* prop from folding and unfolding repeatedly in-flight or to provide
* This is the minimum throttle % that can be used by the controller.
* For electric aircraft this will normally be set to zero, but can be set
* to a small non-zero value if a folding prop is fitted to prevent the
* prop from folding and unfolding repeatedly in-flight or to provide
* some aerodynamic drag from a turning prop to improve the descent rate.
*
* For aircraft with internal combustion engine this parameter should be set
@@ -158,7 +158,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
/**
* Throttle limit value before flare
*
* This throttle value will be set as throttle limit at FW_LND_TLALT,
* This throttle value will be set as throttle limit at FW_LND_TLALT,
* before arcraft will flare.
*
* @group L1 Control
@@ -180,17 +180,17 @@ PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f);
/**
* Maximum climb rate
*
* This is the best climb rate that the aircraft can achieve with
* the throttle set to THR_MAX and the airspeed set to the
* default value. For electric aircraft make sure this number can be
* achieved towards the end of flight when the battery voltage has reduced.
* The setting of this parameter can be checked by commanding a positive
* altitude change of 100m in loiter, RTL or guided mode. If the throttle
* required to climb is close to THR_MAX and the aircraft is maintaining
* airspeed, then this parameter is set correctly. If the airspeed starts
* to reduce, then the parameter is set to high, and if the throttle
* demand required to climb and maintain speed is noticeably less than
* FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or
* This is the best climb rate that the aircraft can achieve with
* the throttle set to THR_MAX and the airspeed set to the
* default value. For electric aircraft make sure this number can be
* achieved towards the end of flight when the battery voltage has reduced.
* The setting of this parameter can be checked by commanding a positive
* altitude change of 100m in loiter, RTL or guided mode. If the throttle
* required to climb is close to THR_MAX and the aircraft is maintaining
* airspeed, then this parameter is set correctly. If the airspeed starts
* to reduce, then the parameter is set to high, and if the throttle
* demand required to climb and maintain speed is noticeably less than
* FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or
* FW_THR_MAX reduced.
*
* @group L1 Control
@@ -200,8 +200,8 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
/**
* Minimum descent rate
*
* This is the sink rate of the aircraft with the throttle
* set to THR_MIN and flown at the same airspeed as used
* This is the sink rate of the aircraft with the throttle
* set to THR_MIN and flown at the same airspeed as used
* to measure FW_T_CLMB_MAX.
*
* @group Fixed Wing TECS
@@ -211,10 +211,10 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
/**
* Maximum descent rate
*
* This sets the maximum descent rate that the controller will use.
* If this value is too large, the aircraft can over-speed on descent.
* This should be set to a value that can be achieved without
* exceeding the lower pitch angle limit and without over-speeding
* This sets the maximum descent rate that the controller will use.
* If this value is too large, the aircraft can over-speed on descent.
* This should be set to a value that can be achieved without
* exceeding the lower pitch angle limit and without over-speeding
* the aircraft.
*
* @group Fixed Wing TECS
@@ -224,7 +224,7 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
/**
* TECS time constant
*
* This is the time constant of the TECS control algorithm (in seconds).
* This is the time constant of the TECS control algorithm (in seconds).
* Smaller values make it faster to respond, larger values make it slower
* to respond.
*
@@ -235,7 +235,7 @@ PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f);
/**
* TECS Throttle time constant
*
* This is the time constant of the TECS throttle control algorithm (in seconds).
* This is the time constant of the TECS throttle control algorithm (in seconds).
* Smaller values make it faster to respond, larger values make it slower
* to respond.
*
@@ -246,7 +246,7 @@ PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f);
/**
* Throttle damping factor
*
* This is the damping gain for the throttle demand loop.
* This is the damping gain for the throttle demand loop.
* Increase to add damping to correct for oscillations in speed and height.
*
* @group Fixed Wing TECS
@@ -256,9 +256,9 @@ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
/**
* Integrator gain
*
* This is the integrator gain on the control loop.
* Increasing this gain increases the speed at which speed
* and height offsets are trimmed out, but reduces damping and
* This is the integrator gain on the control loop.
* Increasing this gain increases the speed at which speed
* and height offsets are trimmed out, but reduces damping and
* increases overshoot.
*
* @group Fixed Wing TECS
@@ -269,9 +269,9 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
* Maximum vertical acceleration
*
* This is the maximum vertical acceleration (in metres/second square)
* either up or down that the controller will use to correct speed
* or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g)
* allows for reasonably aggressive pitch changes if required to recover
* either up or down that the controller will use to correct speed
* or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g)
* allows for reasonably aggressive pitch changes if required to recover
* from under-speed conditions.
*
* @group Fixed Wing TECS
@@ -281,10 +281,10 @@ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
/**
* Complementary filter "omega" parameter for height
*
* This is the cross-over frequency (in radians/second) of the complementary
* filter used to fuse vertical acceleration and barometric height to obtain
* an estimate of height rate and height. Increasing this frequency weights
* the solution more towards use of the barometer, whilst reducing it weights
* This is the cross-over frequency (in radians/second) of the complementary
* filter used to fuse vertical acceleration and barometric height to obtain
* an estimate of height rate and height. Increasing this frequency weights
* the solution more towards use of the barometer, whilst reducing it weights
* the solution more towards use of the accelerometer data.
*
* @group Fixed Wing TECS
@@ -294,10 +294,10 @@ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
/**
* Complementary filter "omega" parameter for speed
*
* This is the cross-over frequency (in radians/second) of the complementary
* filter used to fuse longitudinal acceleration and airspeed to obtain an
* This is the cross-over frequency (in radians/second) of the complementary
* filter used to fuse longitudinal acceleration and airspeed to obtain an
* improved airspeed estimate. Increasing this frequency weights the solution
* more towards use of the arispeed sensor, whilst reducing it weights the
* more towards use of the arispeed sensor, whilst reducing it weights the
* solution more towards use of the accelerometer data.
*
* @group Fixed Wing TECS
@@ -307,13 +307,13 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
/**
* Roll -> Throttle feedforward
*
* Increasing this gain turn increases the amount of throttle that will
* be used to compensate for the additional drag created by turning.
* Ideally this should be set to approximately 10 x the extra sink rate
* in m/s created by a 45 degree bank turn. Increase this gain if
* the aircraft initially loses energy in turns and reduce if the
* aircraft initially gains energy in turns. Efficient high aspect-ratio
* aircraft (eg powered sailplanes) can use a lower value, whereas
* Increasing this gain turn increases the amount of throttle that will
* be used to compensate for the additional drag created by turning.
* Ideally this should be set to approximately 10 x the extra sink rate
* in m/s created by a 45 degree bank turn. Increase this gain if
* the aircraft initially loses energy in turns and reduce if the
* aircraft initially gains energy in turns. Efficient high aspect-ratio
* aircraft (eg powered sailplanes) can use a lower value, whereas
* inefficient low aspect-ratio models (eg delta wings) can use a higher value.
*
* @group Fixed Wing TECS
@@ -323,15 +323,15 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
/**
* Speed <--> Altitude priority
*
* This parameter adjusts the amount of weighting that the pitch control
* applies to speed vs height errors. Setting it to 0.0 will cause the
* pitch control to control height and ignore speed errors. This will
* normally improve height accuracy but give larger airspeed errors.
* Setting it to 2.0 will cause the pitch control loop to control speed
* and ignore height errors. This will normally reduce airspeed errors,
* but give larger height errors. The default value of 1.0 allows the pitch
* control to simultaneously control height and speed.
* Note to Glider Pilots - set this parameter to 2.0 (The glider will
* This parameter adjusts the amount of weighting that the pitch control
* applies to speed vs height errors. Setting it to 0.0 will cause the
* pitch control to control height and ignore speed errors. This will
* normally improve height accuracy but give larger airspeed errors.
* Setting it to 2.0 will cause the pitch control loop to control speed
* and ignore height errors. This will normally reduce airspeed errors,
* but give larger height errors. The default value of 1.0 allows the pitch
* control to simultaneously control height and speed.
* Note to Glider Pilots - set this parameter to 2.0 (The glider will
* adjust its pitch angle to maintain airspeed, ignoring changes in height).
*
* @group Fixed Wing TECS
@@ -341,9 +341,9 @@ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
/**
* Pitch damping factor
*
* This is the damping gain for the pitch demand loop. Increase to add
* damping to correct for oscillations in height. The default value of 0.0
* will work well provided the pitch to servo controller has been tuned
* This is the damping gain for the pitch demand loop. Increase to add
* damping to correct for oscillations in height. The default value of 0.0
* will work well provided the pitch to servo controller has been tuned
* properly.
*
* @group Fixed Wing TECS
@@ -357,6 +357,13 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
*/
PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
/**
* Height rate FF factor
*
* @group Fixed Wing TECS
*/
PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.0f);
/**
* Speed rate P factor
*
@@ -379,18 +386,23 @@ PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f);
PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f);
/**
* Landing flare altitude (relative)
* Landing flare altitude (relative to landing altitude)
*
* @unit meter
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f);
PARAM_DEFINE_FLOAT(FW_LND_FLALT, 8.0f);
/**
* Landing throttle limit altitude (relative)
* Landing throttle limit altitude (relative landing altitude)
*
* Default of -1.0f lets the system default to applying throttle
* limiting at 2/3 of the flare altitude.
*
* @unit meter
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f);
PARAM_DEFINE_FLOAT(FW_LND_TLALT, -1.0f);
/**
* Landing heading hold horizontal distance
@@ -400,12 +412,28 @@ PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f);
PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);
/**
* Relative altitude threshold for range finder measurements for use during landing
* Enable or disable usage of terrain estimate during landing
*
* range finder measurements will only be used if the estimated relative altitude (gobal_pos.alt - landing_waypoint.alt) is < FW_LND_RFRALT
* set to < 0 to disable
* the correct value of this parameter depends on your range measuring device as well as on the terrain at the landing location
* 0: disabled, 1: enabled
*
* @group L1 Control
*/
PARAM_DEFINE_FLOAT(FW_LND_RFRALT, -1.0f);
PARAM_DEFINE_INT32(FW_LND_USETER, 0);
/**
* Flare, minimum pitch
*
* Minimum pitch during flare, a positive sign means nose up
* Applied once FW_LND_TLALT is reached
*
*/
PARAM_DEFINE_FLOAT(FW_FLARE_PMIN, 2.5f);
/**
* Flare, maximum pitch
*
* Maximum pitch during flare, a positive sign means nose up
* Applied once FW_LND_TLALT is reached
*
*/
PARAM_DEFINE_FLOAT(FW_FLARE_PMAX, 15.0f);
src/modules/mavlink/mavlink.c
+8 -5
View File
@@ -68,14 +68,17 @@ PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
* @group MAVLink
*/
PARAM_DEFINE_INT32(MAV_USEHILGPS, 0);
/**
* Forward external setpoint messages
* If set to 1 incomming external setpoint messages will be directly forwarded to the controllers if in offboard
* control mode
* @group MAVLink
*/
PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1);
mavlink_system_t mavlink_system = {
100,
50,
MAV_TYPE_FIXED_WING,
0,
0,
0
50
}; // System ID, 1-255, Component/Subsystem ID, 1-255
/*

Some files were not shown because too many files have changed in this diff Show More