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

Added TeraRanger one sensor

Browse Source
This commit is contained in:
Lorenz Meier
2014-11-26 07:56:54 +01:00
parent 034d0a6a61 cbcd1ea1d1
commit 6200a3e3a5
336 changed files with 18008 additions and 4788 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/airspeed/airspeed.cpp
+10 -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,12 +159,14 @@ 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();
// drop back to 2 retries once initialised
_retries = 2;
return ret;
}
@@ -381,7 +383,10 @@ Airspeed::cycle_trampoline(void *arg)
Airspeed *dev = (Airspeed *)arg;
dev->cycle();
dev->update_status();
// XXX we do not know if this is
// really helping - do not update the
// subsys state right now
//dev->update_status();
}
void
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/drv_tone_alarm.h
+2
View File
@@ -149,6 +149,8 @@ enum {
TONE_GPS_WARNING_TUNE,
TONE_ARMING_FAILURE_TUNE,
TONE_PARACHUTE_RELEASE_TUNE,
TONE_EKF_WARNING_TUNE,
TONE_BARO_WARNING_TUNE,
TONE_NUMBER_OF_TUNES
};
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
+16 -36
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;
@@ -1283,14 +1264,13 @@ int HMC5883::set_excitement(unsigned enable)
if (OK != ret)
perf_count(_comms_errors);
_conf_reg &= ~0x03; // reset previous excitement mode
if (((int)enable) < 0) {
_conf_reg |= 0x01;
} else if (enable > 0) {
_conf_reg |= 0x02;
} else {
_conf_reg &= ~0x03;
}
// ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)_conf_reg);
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
+13 -34
View File
@@ -92,7 +92,7 @@
#include <drivers/airspeed/airspeed.h>
/* I2C bus address is 1010001x */
#define I2C_ADDRESS_MS4525DO 0x28 //0x51 /* 7-bit address. */
#define I2C_ADDRESS_MS4525DO 0x28 /**< 7-bit address. Depends on the order code (this is for code "I") */
#define PATH_MS4525 "/dev/ms4525"
/* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */
#define I2C_ADDRESS_MS5525DSO 0x77 //0x77/* 7-bit address, addr. pin pulled low */
@@ -102,9 +102,9 @@
#define ADDR_READ_MR 0x00 /* write to this address to start conversion */
/* Measurement rate is 100Hz */
#define MEAS_RATE 100.0f
#define MEAS_DRIVER_FILTER_FREQ 3.0f
#define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */
#define MEAS_RATE 100
#define MEAS_DRIVER_FILTER_FREQ 1.2f
#define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */
class MEASAirspeed : public Airspeed
{
@@ -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/ms5611/ms5611.cpp
+2 -2
View File
@@ -130,7 +130,7 @@ protected:
float _T;
/* altitude conversion calibration */
unsigned _msl_pressure; /* in kPa */
unsigned _msl_pressure; /* in Pa */
orb_advert_t _baro_topic;
@@ -466,7 +466,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
irqrestore(flags);
return OK;
}
src/drivers/pca9685/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt
+29
View File
@@ -0,0 +1,29 @@
The following license agreement covers re-used code from the arduino driver
for the Adafruit I2C to PWM converter.
Software License Agreement (BSD License)
Copyright (c) 2012, Adafruit Industries
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 of the copyright holders 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 ''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 HOLDER 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.
src/drivers/pca9685/module.mk
+42
View File
@@ -0,0 +1,42 @@
############################################################################
#
# Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Driver for the PCA9685 I2C PWM controller
# The chip is used on the adafruit I2C PWM converter,
# which allows to control servos via I2C.
# https://www.adafruit.com/product/815
MODULE_COMMAND = pca9685
SRCS = pca9685.cpp
src/drivers/pca9685/pca9685.cpp
+651
View File
@@ -0,0 +1,651 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file pca9685.cpp
*
* Driver for the PCA9685 I2C PWM module
* The chip is used on the Adafruit I2C/PWM converter https://www.adafruit.com/product/815
*
* Parts of the code are adapted from the arduino library for the board
* https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library
* for the license of these parts see the
* arduino_Adafruit_PWM_Servo_Driver_Library_license.txt file
* see https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library for contributors
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <nuttx/config.h>
#include <drivers/device/i2c.h>
#include <sys/types.h>
#include <stdint.h>
#include <string.h>
#include <stdlib.h>
#include <stdbool.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdio.h>
#include <ctype.h>
#include <math.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <uORB/uORB.h>
#include <uORB/topics/actuator_controls.h>
#include <board_config.h>
#include <drivers/drv_io_expander.h>
#define PCA9685_SUBADR1 0x2
#define PCA9685_SUBADR2 0x3
#define PCA9685_SUBADR3 0x4
#define PCA9685_MODE1 0x0
#define PCA9685_PRESCALE 0xFE
#define LED0_ON_L 0x6
#define LED0_ON_H 0x7
#define LED0_OFF_L 0x8
#define LED0_OFF_H 0x9
#define ALLLED_ON_L 0xFA
#define ALLLED_ON_H 0xFB
#define ALLLED_OFF_L 0xFC
#define ALLLED_OF
#define ADDR 0x40 // I2C adress
#define PCA9685_DEVICE_PATH "/dev/pca9685"
#define PCA9685_BUS PX4_I2C_BUS_EXPANSION
#define PCA9685_PWMFREQ 60.0f
#define PCA9685_NCHANS 16 // total amount of pwm outputs
#define PCA9685_PWMMIN 150 // this is the 'minimum' pulse length count (out of 4096)
#define PCA9685_PWMMAX 600 // this is the 'maximum' pulse length count (out of 4096)_PWMFREQ 60.0f
#define PCA9685_PWMCENTER ((PCA9685_PWMMAX + PCA9685_PWMMIN)/2)
#define PCA9685_MAXSERVODEG 90.0f /* maximal servo deflection in degrees
PCA9685_PWMMIN <--> -PCA9685_MAXSERVODEG
PCA9685_PWMMAX <--> PCA9685_MAXSERVODEG
*/
#define PCA9685_SCALE ((PCA9685_PWMMAX - PCA9685_PWMCENTER)/(M_DEG_TO_RAD_F * PCA9685_MAXSERVODEG)) // scales from rad to PWM
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
class PCA9685 : public device::I2C
{
public:
PCA9685(int bus=PCA9685_BUS, uint8_t address=ADDR);
virtual ~PCA9685();
virtual int init();
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
virtual int info();
virtual int reset();
bool is_running() { return _running; }
private:
work_s _work;
enum IOX_MODE _mode;
bool _running;
int _i2cpwm_interval;
bool _should_run;
perf_counter_t _comms_errors;
uint8_t _msg[6];
int _actuator_controls_sub;
struct actuator_controls_s _actuator_controls;
uint16_t _current_values[NUM_ACTUATOR_CONTROLS]; /**< stores the current pwm output
values as sent to the setPin() */
bool _mode_on_initialized; /** Set to true after the first call of i2cpwm in mode IOX_MODE_ON */
static void i2cpwm_trampoline(void *arg);
void i2cpwm();
/**
* Helper function to set the pwm frequency
*/
int setPWMFreq(float freq);
/**
* Helper function to set the demanded pwm value
* @param num pwm output number
*/
int setPWM(uint8_t num, uint16_t on, uint16_t off);
/**
* Sets pin without having to deal with on/off tick placement and properly handles
* a zero value as completely off. Optional invert parameter supports inverting
* the pulse for sinking to ground.
* @param num pwm output number
* @param val should be a value from 0 to 4095 inclusive.
*/
int setPin(uint8_t num, uint16_t val, bool invert = false);
/* Wrapper to read a byte from addr */
int read8(uint8_t addr, uint8_t &value);
/* Wrapper to wite a byte to addr */
int write8(uint8_t addr, uint8_t value);
};
/* for now, we only support one board */
namespace
{
PCA9685 *g_pca9685;
}
void pca9685_usage();
extern "C" __EXPORT int pca9685_main(int argc, char *argv[]);
PCA9685::PCA9685(int bus, uint8_t address) :
I2C("pca9685", PCA9685_DEVICE_PATH, bus, address, 100000),
_mode(IOX_MODE_OFF),
_running(false),
_i2cpwm_interval(SEC2TICK(1.0f/60.0f)),
_should_run(false),
_comms_errors(perf_alloc(PC_COUNT, "actuator_controls_2_comms_errors")),
_actuator_controls_sub(-1),
_actuator_controls(),
_mode_on_initialized(false)
{
memset(&_work, 0, sizeof(_work));
memset(_msg, 0, sizeof(_msg));
memset(_current_values, 0, sizeof(_current_values));
}
PCA9685::~PCA9685()
{
}
int
PCA9685::init()
{
int ret;
ret = I2C::init();
if (ret != OK) {
return ret;
}
ret = reset();
if (ret != OK) {
return ret;
}
ret = setPWMFreq(PCA9685_PWMFREQ);
return ret;
}
int
PCA9685::ioctl(struct file *filp, int cmd, unsigned long arg)
{
int ret = -EINVAL;
switch (cmd) {
case IOX_SET_MODE:
if (_mode != (IOX_MODE)arg) {
switch ((IOX_MODE)arg) {
case IOX_MODE_OFF:
warnx("shutting down");
break;
case IOX_MODE_ON:
warnx("starting");
break;
case IOX_MODE_TEST_OUT:
warnx("test starting");
break;
default:
return -1;
}
_mode = (IOX_MODE)arg;
}
// if not active, kick it
if (!_running) {
_running = true;
work_queue(LPWORK, &_work, (worker_t)&PCA9685::i2cpwm_trampoline, this, 1);
}
return OK;
default:
// see if the parent class can make any use of it
ret = CDev::ioctl(filp, cmd, arg);
break;
}
return ret;
}
int
PCA9685::info()
{
int ret = OK;
if (is_running()) {
warnx("Driver is running, mode: %u", _mode);
} else {
warnx("Driver started but not running");
}
return ret;
}
void
PCA9685::i2cpwm_trampoline(void *arg)
{
PCA9685 *i2cpwm = reinterpret_cast<PCA9685 *>(arg);
i2cpwm->i2cpwm();
}
/**
* Main loop function
*/
void
PCA9685::i2cpwm()
{
if (_mode == IOX_MODE_TEST_OUT) {
setPin(0, PCA9685_PWMCENTER);
_should_run = true;
} else if (_mode == IOX_MODE_OFF) {
_should_run = false;
} else {
if (!_mode_on_initialized) {
/* Subscribe to actuator control 2 (payload group for gimbal) */
_actuator_controls_sub = orb_subscribe(ORB_ID(actuator_controls_2));
/* set the uorb update interval lower than the driver pwm interval */
orb_set_interval(_actuator_controls_sub, 1000.0f / PCA9685_PWMFREQ - 5);
_mode_on_initialized = true;
}
/* Read the servo setpoints from the actuator control topics (gimbal) */
bool updated;
orb_check(_actuator_controls_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_controls_2), _actuator_controls_sub, &_actuator_controls);
for (int i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
/* Scale the controls to PWM, first multiply by pi to get rad,
* the control[i] values are on the range -1 ... 1 */
uint16_t new_value = PCA9685_PWMCENTER +
(_actuator_controls.control[i] * M_PI_F * PCA9685_SCALE);
debug("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value,
(double)_actuator_controls.control[i]);
if (new_value != _current_values[i] &&
isfinite(new_value) &&
new_value >= PCA9685_PWMMIN &&
new_value <= PCA9685_PWMMAX) {
/* This value was updated, send the command to adjust the PWM value */
setPin(i, new_value);
_current_values[i] = new_value;
}
}
}
_should_run = true;
}
// check if any activity remains, else stop
if (!_should_run) {
_running = false;
return;
}
// re-queue ourselves to run again later
_running = true;
work_queue(LPWORK, &_work, (worker_t)&PCA9685::i2cpwm_trampoline, this, _i2cpwm_interval);
}
int
PCA9685::setPWM(uint8_t num, uint16_t on, uint16_t off)
{
int ret;
/* convert to correct message */
_msg[0] = LED0_ON_L + 4 * num;
_msg[1] = on;
_msg[2] = on >> 8;
_msg[3] = off;
_msg[4] = off >> 8;
/* try i2c transfer */
ret = transfer(_msg, 5, nullptr, 0);
if (OK != ret) {
perf_count(_comms_errors);
log("i2c::transfer returned %d", ret);
}
return ret;
}
int
PCA9685::setPin(uint8_t num, uint16_t val, bool invert)
{
// Clamp value between 0 and 4095 inclusive.
if (val > 4095) {
val = 4095;
}
if (invert) {
if (val == 0) {
// Special value for signal fully on.
return setPWM(num, 4096, 0);
} else if (val == 4095) {
// Special value for signal fully off.
return setPWM(num, 0, 4096);
} else {
return setPWM(num, 0, 4095-val);
}
} else {
if (val == 4095) {
// Special value for signal fully on.
return setPWM(num, 4096, 0);
} else if (val == 0) {
// Special value for signal fully off.
return setPWM(num, 0, 4096);
} else {
return setPWM(num, 0, val);
}
}
return ERROR;
}
int
PCA9685::setPWMFreq(float freq)
{
int ret = OK;
freq *= 0.9f; /* Correct for overshoot in the frequency setting (see issue
https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library/issues/11). */
float prescaleval = 25000000;
prescaleval /= 4096;
prescaleval /= freq;
prescaleval -= 1;
uint8_t prescale = uint8_t(prescaleval + 0.5f); //implicit floor()
uint8_t oldmode;
ret = read8(PCA9685_MODE1, oldmode);
if (ret != OK) {
return ret;
}
uint8_t newmode = (oldmode&0x7F) | 0x10; // sleep
ret = write8(PCA9685_MODE1, newmode); // go to sleep
if (ret != OK) {
return ret;
}
ret = write8(PCA9685_PRESCALE, prescale); // set the prescaler
if (ret != OK) {
return ret;
}
ret = write8(PCA9685_MODE1, oldmode);
if (ret != OK) {
return ret;
}
usleep(5000); //5ms delay (from arduino driver)
ret = write8(PCA9685_MODE1, oldmode | 0xa1); // This sets the MODE1 register to turn on auto increment.
if (ret != OK) {
return ret;
}
return ret;
}
/* Wrapper to read a byte from addr */
int
PCA9685::read8(uint8_t addr, uint8_t &value)
{
int ret = OK;
/* send addr */
ret = transfer(&addr, sizeof(addr), nullptr, 0);
if (ret != OK) {
goto fail_read;
}
/* get value */
ret = transfer(nullptr, 0, &value, 1);
if (ret != OK) {
goto fail_read;
}
return ret;
fail_read:
perf_count(_comms_errors);
log("i2c::transfer returned %d", ret);
return ret;
}
int PCA9685::reset(void) {
warnx("resetting");
return write8(PCA9685_MODE1, 0x0);
}
/* Wrapper to wite a byte to addr */
int
PCA9685::write8(uint8_t addr, uint8_t value) {
int ret = OK;
_msg[0] = addr;
_msg[1] = value;
/* send addr and value */
ret = transfer(_msg, 2, nullptr, 0);
if (ret != OK) {
perf_count(_comms_errors);
log("i2c::transfer returned %d", ret);
}
return ret;
}
void
pca9685_usage()
{
warnx("missing command: try 'start', 'test', 'stop', 'info'");
warnx("options:");
warnx(" -b i2cbus (%d)", PX4_I2C_BUS_EXPANSION);
warnx(" -a addr (0x%x)", ADDR);
}
int
pca9685_main(int argc, char *argv[])
{
int i2cdevice = -1;
int i2caddr = ADDR; // 7bit
int ch;
// jump over start/off/etc and look at options first
while ((ch = getopt(argc, argv, "a:b:")) != EOF) {
switch (ch) {
case 'a':
i2caddr = strtol(optarg, NULL, 0);
break;
case 'b':
i2cdevice = strtol(optarg, NULL, 0);
break;
default:
pca9685_usage();
exit(0);
}
}
if (optind >= argc) {
pca9685_usage();
exit(1);
}
const char *verb = argv[optind];
int fd;
int ret;
if (!strcmp(verb, "start")) {
if (g_pca9685 != nullptr) {
errx(1, "already started");
}
if (i2cdevice == -1) {
// try the external bus first
i2cdevice = PX4_I2C_BUS_EXPANSION;
g_pca9685 = new PCA9685(PX4_I2C_BUS_EXPANSION, i2caddr);
if (g_pca9685 != nullptr && OK != g_pca9685->init()) {
delete g_pca9685;
g_pca9685 = nullptr;
}
if (g_pca9685 == nullptr) {
errx(1, "init failed");
}
}
if (g_pca9685 == nullptr) {
g_pca9685 = new PCA9685(i2cdevice, i2caddr);
if (g_pca9685 == nullptr) {
errx(1, "new failed");
}
if (OK != g_pca9685->init()) {
delete g_pca9685;
g_pca9685 = nullptr;
errx(1, "init failed");
}
}
fd = open(PCA9685_DEVICE_PATH, 0);
if (fd == -1) {
errx(1, "Unable to open " PCA9685_DEVICE_PATH);
}
ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_ON);
close(fd);
exit(0);
}
// need the driver past this point
if (g_pca9685 == nullptr) {
warnx("not started, run pca9685 start");
exit(1);
}
if (!strcmp(verb, "info")) {
g_pca9685->info();
exit(0);
}
if (!strcmp(verb, "reset")) {
g_pca9685->reset();
exit(0);
}
if (!strcmp(verb, "test")) {
fd = open(PCA9685_DEVICE_PATH, 0);
if (fd == -1) {
errx(1, "Unable to open " PCA9685_DEVICE_PATH);
}
ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_TEST_OUT);
close(fd);
exit(ret);
}
if (!strcmp(verb, "stop")) {
fd = open(PCA9685_DEVICE_PATH, 0);
if (fd == -1) {
errx(1, "Unable to open " PCA9685_DEVICE_PATH);
}
ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF);
close(fd);
// wait until we're not running any more
for (unsigned i = 0; i < 15; i++) {
if (!g_pca9685->is_running()) {
break;
}
usleep(50000);
printf(".");
fflush(stdout);
}
printf("\n");
fflush(stdout);
if (!g_pca9685->is_running()) {
delete g_pca9685;
g_pca9685= nullptr;
warnx("stopped, exiting");
exit(0);
} else {
warnx("stop failed.");
exit(1);
}
}
pca9685_usage();
exit(0);
}
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
+188 -61
View File
@@ -135,6 +135,15 @@ public:
*/
virtual int init();
/**
* Initialize the PX4IO class.
*
* Retrieve relevant initial system parameters. Initialize PX4IO registers.
*
* @param disable_rc_handling set to true to forbid override / RC handling on IO
*/
int init(bool disable_rc_handling);
/**
* Detect if a PX4IO is connected.
*
@@ -286,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
@@ -506,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
@@ -579,6 +590,12 @@ PX4IO::detect()
return 0;
}
int
PX4IO::init(bool rc_handling_disabled) {
_rc_handling_disabled = rc_handling_disabled;
return init();
}
int
PX4IO::init()
{
@@ -778,6 +795,11 @@ PX4IO::init()
if (_rc_handling_disabled) {
ret = io_disable_rc_handling();
if (ret != OK) {
log("failed disabling RC handling");
return ret;
}
} else {
/* publish RC config to IO */
ret = io_set_rc_config();
@@ -795,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);
@@ -1031,6 +1058,9 @@ PX4IO::task_main()
}
}
/* Update Circuit breakers */
_cb_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY);
}
}
@@ -1130,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);
@@ -1175,6 +1216,7 @@ PX4IO::io_set_arming_state()
int
PX4IO::disable_rc_handling()
{
_rc_handling_disabled = true;
return io_disable_rc_handling();
}
@@ -1210,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.
@@ -1582,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;
@@ -1606,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();
@@ -1899,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" : ""),
@@ -1947,7 +2003,7 @@ PX4IO::print_status(bool extended_status)
printf("actuators");
for (unsigned i = 0; i < _max_actuators; i++)
printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i));
printf(" %hi", int16_t(io_reg_get(PX4IO_PAGE_ACTUATORS, i)));
printf("\n");
printf("servos");
@@ -2008,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"),
@@ -2017,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",
@@ -2142,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);
@@ -2161,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);
@@ -2180,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);
@@ -2199,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);
@@ -2230,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) {
@@ -2241,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 */
@@ -2418,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;
}
@@ -2499,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);
@@ -2613,24 +2739,25 @@ start(int argc, char *argv[])
errx(1, "driver alloc failed");
}
if (OK != g_dev->init()) {
delete g_dev;
g_dev = nullptr;
errx(1, "driver init failed");
}
bool rc_handling_disabled = false;
/* disable RC handling on request */
if (argc > 1) {
if (!strcmp(argv[1], "norc")) {
if (g_dev->disable_rc_handling())
warnx("Failed disabling RC handling");
rc_handling_disabled = true;
} else {
warnx("unknown argument: %s", argv[1]);
}
}
if (OK != g_dev->init(rc_handling_disabled)) {
delete g_dev;
g_dev = nullptr;
errx(1, "driver init failed");
}
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
int dsm_vcc_ctl;
src/drivers/px4io/px4io_serial.cpp
+7 -1
View File
@@ -157,6 +157,10 @@ private:
perf_counter_t _pc_idle;
perf_counter_t _pc_badidle;
/* do not allow top copying this class */
PX4IO_serial(PX4IO_serial &);
PX4IO_serial& operator = (const PX4IO_serial &);
};
IOPacket PX4IO_serial::_dma_buffer;
@@ -173,7 +177,9 @@ PX4IO_serial::PX4IO_serial() :
_tx_dma(nullptr),
_rx_dma(nullptr),
_rx_dma_status(_dma_status_inactive),
_pc_txns(perf_alloc(PC_ELAPSED, "io_txns ")),
_bus_semaphore(SEM_INITIALIZER(0)),
_completion_semaphore(SEM_INITIALIZER(0)),
_pc_txns(perf_alloc(PC_ELAPSED, "io_txns ")),
_pc_dmasetup(perf_alloc(PC_ELAPSED, "io_dmasetup ")),
_pc_retries(perf_alloc(PC_COUNT, "io_retries ")),
_pc_timeouts(perf_alloc(PC_COUNT, "io_timeouts ")),
src/drivers/px4io/px4io_uploader.cpp
+15 -14
View File
@@ -65,7 +65,8 @@
PX4IO_Uploader::PX4IO_Uploader() :
_io_fd(-1),
_fw_fd(-1)
_fw_fd(-1),
bl_rev(0)
{
}
@@ -245,7 +246,7 @@ PX4IO_Uploader::upload(const char *filenames[])
}
int
PX4IO_Uploader::recv(uint8_t &c, unsigned timeout)
PX4IO_Uploader::recv_byte_with_timeout(uint8_t *c, unsigned timeout)
{
struct pollfd fds[1];
@@ -262,19 +263,19 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout)
return -ETIMEDOUT;
}
read(_io_fd, &c, 1);
read(_io_fd, c, 1);
#ifdef UDEBUG
log("recv 0x%02x", c);
log("recv_bytes 0x%02x", c);
#endif
return OK;
}
int
PX4IO_Uploader::recv(uint8_t *p, unsigned count)
PX4IO_Uploader::recv_bytes(uint8_t *p, unsigned count)
{
int ret;
int ret = OK;
while (count--) {
ret = recv(*p++, 5000);
ret = recv_byte_with_timeout(p++, 5000);
if (ret != OK)
break;
@@ -289,10 +290,10 @@ PX4IO_Uploader::drain()
int ret;
do {
// the small recv timeout here is to allow for fast
// the small recv_bytes timeout here is to allow for fast
// drain when rebooting the io board for a forced
// update of the fw without using the safety switch
ret = recv(c, 40);
ret = recv_byte_with_timeout(&c, 40);
#ifdef UDEBUG
if (ret == OK) {
@@ -331,12 +332,12 @@ PX4IO_Uploader::get_sync(unsigned timeout)
uint8_t c[2];
int ret;
ret = recv(c[0], timeout);
ret = recv_byte_with_timeout(c, timeout);
if (ret != OK)
return ret;
ret = recv(c[1], timeout);
ret = recv_byte_with_timeout(c + 1, timeout);
if (ret != OK)
return ret;
@@ -372,7 +373,7 @@ PX4IO_Uploader::get_info(int param, uint32_t &val)
send(param);
send(PROTO_EOC);
ret = recv((uint8_t *)&val, sizeof(val));
ret = recv_bytes((uint8_t *)&val, sizeof(val));
if (ret != OK)
return ret;
@@ -513,7 +514,7 @@ PX4IO_Uploader::verify_rev2(size_t fw_size)
for (ssize_t i = 0; i < count; i++) {
uint8_t c;
ret = recv(c, 5000);
ret = recv_byte_with_timeout(&c, 5000);
if (ret != OK) {
log("%d: got %d waiting for bytes", sent + i, ret);
@@ -600,7 +601,7 @@ PX4IO_Uploader::verify_rev3(size_t fw_size_local)
send(PROTO_GET_CRC);
send(PROTO_EOC);
ret = recv((uint8_t*)(&crc), sizeof(crc));
ret = recv_bytes((uint8_t*)(&crc), sizeof(crc));
if (ret != OK) {
log("did not receive CRC checksum");
src/drivers/px4io/uploader.h
+4 -4
View File
@@ -74,19 +74,19 @@ private:
INFO_BOARD_REV = 3, /**< board revision */
INFO_FLASH_SIZE = 4, /**< max firmware size in bytes */
PROG_MULTI_MAX = 60, /**< protocol max is 255, must be multiple of 4 */
PROG_MULTI_MAX = 248, /**< protocol max is 255, must be multiple of 4 */
};
int _io_fd;
int _fw_fd;
uint32_t bl_rev; /**< bootloader revision */
uint32_t bl_rev; /**< bootloader revision */
void log(const char *fmt, ...);
int recv(uint8_t &c, unsigned timeout);
int recv(uint8_t *p, unsigned count);
int recv_byte_with_timeout(uint8_t *c, unsigned timeout);
int recv_bytes(uint8_t *p, unsigned count);
void drain();
int send(uint8_t c);
int send(uint8_t *p, unsigned count);
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 -87
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,83 +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));
}
if (_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 */
@@ -708,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/modules/mavlink/mavlink_commands.h → src/drivers/sf0x/sf0x_parser.h
+12 -26
View File
@@ -32,34 +32,20 @@
****************************************************************************/
/**
* @file mavlink_commands.h
* Mavlink commands stream definition.
* @file sf0x_parser.cpp
* @author Lorenz Meier <lm@inf.ethz.ch>
*
* @author Anton Babushkin <anton.babushkin@me.com>
* Declarations of parser for the Lightware SF0x laser rangefinder series
*/
#ifndef MAVLINK_COMMANDS_H_
#define MAVLINK_COMMANDS_H_
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_command.h>
class Mavlink;
class MavlinkCommansStream;
#include "mavlink_main.h"
class MavlinkCommandsStream
{
private:
MavlinkOrbSubscription *_cmd_sub;
struct vehicle_command_s *_cmd;
mavlink_channel_t _channel;
uint64_t _cmd_time;
public:
MavlinkCommandsStream(Mavlink *mavlink, mavlink_channel_t channel);
void update(const hrt_abstime t);
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
};
#endif /* MAVLINK_COMMANDS_H_ */
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/stm32/tone_alarm/tone_alarm.cpp
+4
View File
@@ -336,6 +336,8 @@ ToneAlarm::ToneAlarm() :
_default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#"; //gps warning slow
_default_tunes[TONE_ARMING_FAILURE_TUNE] = "MFT255L4<<<BAP";
_default_tunes[TONE_PARACHUTE_RELEASE_TUNE] = "MFT255L16agagagag"; // parachute release
_default_tunes[TONE_EKF_WARNING_TUNE] = "MFT255L8ddd#d#eeff"; // ekf warning
_default_tunes[TONE_BARO_WARNING_TUNE] = "MFT255L4gf#fed#d"; // baro warning
_tune_names[TONE_STARTUP_TUNE] = "startup"; // startup tune
_tune_names[TONE_ERROR_TUNE] = "error"; // ERROR tone
@@ -348,6 +350,8 @@ ToneAlarm::ToneAlarm() :
_tune_names[TONE_GPS_WARNING_TUNE] = "gps_warning"; // gps warning
_tune_names[TONE_ARMING_FAILURE_TUNE] = "arming_failure"; //fail to arm
_tune_names[TONE_PARACHUTE_RELEASE_TUNE] = "parachute_release"; // parachute release
_tune_names[TONE_EKF_WARNING_TUNE] = "ekf_warning"; // ekf warning
_tune_names[TONE_BARO_WARNING_TUNE] = "baro_warning"; // baro warning
}
ToneAlarm::~ToneAlarm()
src/drivers/trone/module.mk
+5 -1
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2013 PX4 Development Team. All rights reserved.
# 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
@@ -38,3 +38,7 @@
MODULE_COMMAND = trone
SRCS = trone.cpp
MODULE_STACKSIZE = 1200
MAXOPTIMIZATION = -Os
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/ecl/attitude_fw/ecl_pitch_controller.cpp
+4 -2
View File
@@ -169,7 +169,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
float id = _rate_error * dt;
float id = _rate_error * dt * scaler;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
@@ -190,7 +190,9 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
/* Apply PI rate controller and store non-limited output */
_last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed
_last_output = _bodyrate_setpoint * _k_ff * scaler +
_rate_error * _k_p * scaler * scaler
+ integrator_constrained; //scaler is proportional to 1/airspeed
// warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
// warnx("roll: _last_output %.4f", (double)_last_output);
return math::constrain(_last_output, -1.0f, 1.0f);
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+4 -2
View File
@@ -135,7 +135,7 @@ float ECL_RollController::control_bodyrate(float pitch,
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
float id = _rate_error * dt;
float id = _rate_error * dt * scaler;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
@@ -157,7 +157,9 @@ float ECL_RollController::control_bodyrate(float pitch,
//warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max);
/* Apply PI rate controller and store non-limited output */
_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed
_last_output = _bodyrate_setpoint * _k_ff * scaler +
_rate_error * _k_p * scaler * scaler
+ integrator_constrained; //scaler is proportional to 1/airspeed
return math::constrain(_last_output, -1.0f, 1.0f);
}
src/lib/ecl/module.mk
+2
View File
@@ -39,3 +39,5 @@ SRCS = attitude_fw/ecl_pitch_controller.cpp \
attitude_fw/ecl_roll_controller.cpp \
attitude_fw/ecl_yaw_controller.cpp \
l1/ecl_l1_pos_controller.cpp
MAXOPTIMIZATION = -Os
src/lib/external_lgpl/module.mk
+2
View File
@@ -46,3 +46,5 @@
#
SRCS = tecs/tecs.cpp
MAXOPTIMIZATION = -Os
src/lib/external_lgpl/tecs/tecs.cpp
+45 -27
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,11 +299,11 @@ 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
float K_STE2Thr = 1 / (_timeConst * (_STEdot_max - _STEdot_min));
float K_STE2Thr = 1 / (_timeConstThrot * (_STEdot_max - _STEdot_min));
// Calculate feed-forward throttle
float ff_throttle = 0;
@@ -316,28 +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 = _throttle_dem;
_last_throttle_dem - thrRateIncr,
_last_throttle_dem + thrRateIncr);
}
// Ensure _last_throttle_dem is always initialized properly
_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) {
@@ -355,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)
@@ -551,18 +557,30 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
// Calculate pitch demand
_update_pitch();
// // Write internal variables to the log_tuning structure. This
// // structure will be logged in dataflash at 10Hz
// log_tuning.hgt_dem = _hgt_dem_adj;
// log_tuning.hgt = _integ3_state;
// log_tuning.dhgt_dem = _hgt_rate_dem;
// log_tuning.dhgt = _integ2_state;
// log_tuning.spd_dem = _TAS_dem_adj;
// log_tuning.spd = _integ5_state;
// log_tuning.dspd = _vel_dot;
// log_tuning.ithr = _integ6_state;
// log_tuning.iptch = _integ7_state;
// log_tuning.thr = _throttle_dem;
// log_tuning.ptch = _pitch_dem;
// log_tuning.dspd_dem = _TAS_rate_dem;
_tecs_state.timestamp = now;
if (_underspeed) {
_tecs_state.mode = ECL_TECS_MODE_UNDERSPEED;
} else if (_badDescent) {
_tecs_state.mode = ECL_TECS_MODE_BAD_DESCENT;
} else if (_climbOutDem) {
_tecs_state.mode = ECL_TECS_MODE_CLIMBOUT;
} else {
// If no error flag applies, conclude normal
_tecs_state.mode = ECL_TECS_MODE_NORMAL;
}
_tecs_state.hgt_dem = _hgt_dem_adj;
_tecs_state.hgt = _integ3_state;
_tecs_state.dhgt_dem = _hgt_rate_dem;
_tecs_state.dhgt = _integ2_state;
_tecs_state.spd_dem = _TAS_dem_adj;
_tecs_state.spd = _integ5_state;
_tecs_state.dspd = _vel_dot;
_tecs_state.ithr = _integ6_state;
_tecs_state.iptch = _integ7_state;
_tecs_state.thr = _throttle_dem;
_tecs_state.ptch = _pitch_dem;
_tecs_state.dspd_dem = _TAS_rate_dem;
}
src/lib/external_lgpl/tecs/tecs.h
+71 -20
View File
@@ -28,6 +28,28 @@ class __EXPORT TECS
{
public:
TECS() :
_tecs_state {},
_update_50hz_last_usec(0),
_update_speed_last_usec(0),
_update_pitch_throttle_last_usec(0),
// TECS tuning parameters
_hgtCompFiltOmega(0.0f),
_spdCompFiltOmega(0.0f),
_maxClimbRate(2.0f),
_minSinkRate(1.0f),
_maxSinkRate(2.0f),
_timeConst(5.0f),
_timeConstThrot(8.0f),
_ptchDamp(0.0f),
_thrDamp(0.0f),
_integGain(0.0f),
_vertAccLim(0.0f),
_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),
_integ1_state(0.0f),
_integ2_state(0.0f),
@@ -45,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),
@@ -100,29 +125,42 @@ public:
return _spdWeight;
}
// log data on internal state of the controller. Called at 10Hz
// void log_data(DataFlash_Class &dataflash, uint8_t msgid);
enum ECL_TECS_MODE {
ECL_TECS_MODE_NORMAL = 0,
ECL_TECS_MODE_UNDERSPEED,
ECL_TECS_MODE_BAD_DESCENT,
ECL_TECS_MODE_CLIMBOUT
};
// struct PACKED log_TECS_Tuning {
// LOG_PACKET_HEADER;
// float hgt;
// float dhgt;
// float hgt_dem;
// float dhgt_dem;
// float spd_dem;
// float spd;
// float dspd;
// float ithr;
// float iptch;
// float thr;
// float ptch;
// float dspd_dem;
// } log_tuning;
struct tecs_state {
uint64_t timestamp;
float hgt;
float dhgt;
float hgt_dem;
float dhgt_dem;
float spd_dem;
float spd;
float dspd;
float ithr;
float iptch;
float thr;
float ptch;
float dspd_dem;
enum ECL_TECS_MODE mode;
};
void get_tecs_state(struct tecs_state& state) {
state = _tecs_state;
}
void set_time_const(float time_const) {
_timeConst = time_const;
}
void set_time_const_throt(float time_const_throt) {
_timeConstThrot = time_const_throt;
}
void set_min_sink_rate(float rate) {
_minSinkRate = rate;
}
@@ -183,11 +221,22 @@ 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;
// Last time update_50Hz was called
uint64_t _update_50hz_last_usec;
@@ -204,6 +253,7 @@ private:
float _minSinkRate;
float _maxSinkRate;
float _timeConst;
float _timeConstThrot;
float _ptchDamp;
float _thrDamp;
float _integGain;
@@ -211,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
@@ -285,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
+195 -25
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012, 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -49,39 +49,124 @@
#include <stdio.h>
#include <math.h>
#include <stdbool.h>
#include <string.h>
#include <float.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
/*
* Azimuthal Equidistant Projection
* formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html
*/
__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
{
ref->lat = lat_0 / 180.0 * M_PI;
ref->lon = lon_0 / 180.0 * M_PI;
static struct map_projection_reference_s mp_ref = {0.0, 0.0, 0.0, 0.0, false, 0};
static struct globallocal_converter_reference_s gl_ref = {0.0f, false};
ref->sin_lat = sin(ref->lat);
ref->cos_lat = cos(ref->lat);
__EXPORT bool map_projection_global_initialized()
{
return map_projection_initialized(&mp_ref);
}
__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
__EXPORT bool map_projection_initialized(const struct map_projection_reference_s *ref)
{
double lat_rad = lat / 180.0 * M_PI;
double lon_rad = lon / 180.0 * M_PI;
return ref->init_done;
}
__EXPORT uint64_t map_projection_global_timestamp()
{
return map_projection_timestamp(&mp_ref);
}
__EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref)
{
return ref->timestamp;
}
__EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
{
if (strcmp("commander", getprogname()) == 0) {
return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp);
} else {
return -1;
}
}
__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
{
ref->lat_rad = lat_0 * M_DEG_TO_RAD;
ref->lon_rad = lon_0 * M_DEG_TO_RAD;
ref->sin_lat = sin(ref->lat_rad);
ref->cos_lat = cos(ref->lat_rad);
ref->timestamp = timestamp;
ref->init_done = true;
return 0;
}
__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
{
return map_projection_init_timestamped(ref, lat_0, lon_0, hrt_absolute_time());
}
__EXPORT int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad)
{
return map_projection_reference(&mp_ref, ref_lat_rad, ref_lon_rad);
}
__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad)
{
if (!map_projection_initialized(ref)) {
return -1;
}
*ref_lat_rad = ref->lat_rad;
*ref_lon_rad = ref->lon_rad;
return 0;
}
__EXPORT int map_projection_global_project(double lat, double lon, float *x, float *y)
{
return map_projection_project(&mp_ref, lat, lon, x, y);
}
__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
{
if (!map_projection_initialized(ref)) {
return -1;
}
double lat_rad = lat * M_DEG_TO_RAD;
double lon_rad = lon * M_DEG_TO_RAD;
double sin_lat = sin(lat_rad);
double cos_lat = cos(lat_rad);
double cos_d_lon = cos(lon_rad - ref->lon);
double cos_d_lon = cos(lon_rad - ref->lon_rad);
double c = acos(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon);
double k = (c == 0.0) ? 1.0 : (c / sin(c));
double k = (fabs(c) < DBL_EPSILON) ? 1.0 : (c / sin(c));
*x = k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH;
*y = k * cos_lat * sin(lon_rad - ref->lon) * CONSTANTS_RADIUS_OF_EARTH;
*y = k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH;
return 0;
}
__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon)
__EXPORT int map_projection_global_reproject(float x, float y, double *lat, double *lon)
{
return map_projection_reproject(&mp_ref, x, y, lat, lon);
}
__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon)
{
if (!map_projection_initialized(ref)) {
return -1;
}
double x_rad = x / CONSTANTS_RADIUS_OF_EARTH;
double y_rad = y / CONSTANTS_RADIUS_OF_EARTH;
double c = sqrtf(x_rad * x_rad + y_rad * y_rad);
@@ -91,19 +176,101 @@ __EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, f
double lat_rad;
double lon_rad;
if (c != 0.0) {
if (fabs(c) > DBL_EPSILON) {
lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c);
lon_rad = (ref->lon + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));
lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));
} else {
lat_rad = ref->lat;
lon_rad = ref->lon;
lat_rad = ref->lat_rad;
lon_rad = ref->lon_rad;
}
*lat = lat_rad * 180.0 / M_PI;
*lon = lon_rad * 180.0 / M_PI;
return 0;
}
__EXPORT int map_projection_global_getref(double *lat_0, double *lon_0)
{
if (!map_projection_global_initialized()) {
return -1;
}
if (lat_0 != NULL) {
*lat_0 = M_RAD_TO_DEG * mp_ref.lat_rad;
}
if (lon_0 != NULL) {
*lon_0 = M_RAD_TO_DEG * mp_ref.lon_rad;
}
return 0;
}
__EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp)
{
if (strcmp("commander", getprogname()) == 0) {
gl_ref.alt = alt_0;
if (!map_projection_global_init(lat_0, lon_0, timestamp))
{
gl_ref.init_done = true;
return 0;
} else {
gl_ref.init_done = false;
return -1;
}
} else {
return -1;
}
}
__EXPORT bool globallocalconverter_initialized()
{
return gl_ref.init_done && map_projection_global_initialized();
}
__EXPORT int globallocalconverter_tolocal(double lat, double lon, float alt, float *x, float *y, float *z)
{
if (!map_projection_global_initialized()) {
return -1;
}
map_projection_global_project(lat, lon, x, y);
*z = gl_ref.alt - alt;
return 0;
}
__EXPORT int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt)
{
if (!map_projection_global_initialized()) {
return -1;
}
map_projection_global_reproject(x, y, lat, lon);
*alt = gl_ref.alt - z;
return 0;
}
__EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0)
{
if (!map_projection_global_initialized()) {
return -1;
}
if (map_projection_global_getref(lat_0, lon_0))
{
return -1;
}
if (alt_0 != NULL) {
*alt_0 = gl_ref.alt;
}
return 0;
}
__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
{
@@ -195,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);
@@ -210,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) {
@@ -247,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;
@@ -296,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;
@@ -317,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/geo/geo.h
+132 -9
View File
@@ -69,39 +69,162 @@ struct crosstrack_error_s {
/* lat/lon are in radians */
struct map_projection_reference_s {
double lat;
double lon;
double lat_rad;
double lon_rad;
double sin_lat;
double cos_lat;
bool init_done;
uint64_t timestamp;
};
struct globallocal_converter_reference_s {
float alt;
bool init_done;
};
/**
* Initializes the map transformation.
* Checks if global projection was initialized
* @return true if map was initialized before, false else
*/
__EXPORT bool map_projection_global_initialized(void);
/**
* Checks if projection given as argument was initialized
* @return true if map was initialized before, false else
*/
__EXPORT bool map_projection_initialized(const struct map_projection_reference_s *ref);
/**
* Get the timestamp of the global map projection
* @return the timestamp of the map_projection
*/
__EXPORT uint64_t map_projection_global_timestamp(void);
/**
* Get the timestamp of the map projection given by the argument
* @return the timestamp of the map_projection
*/
__EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref);
/**
* Writes the reference values of the global projection to ref_lat and ref_lon
* @return 0 if map_projection_init was called before, -1 else
*/
__EXPORT int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad);
/**
* Writes the reference values of the projection given by the argument to ref_lat and ref_lon
* @return 0 if map_projection_init was called before, -1 else
*/
__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad);
/**
* Initializes the global map transformation.
*
* Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane
* Initializes the transformation between the geographic coordinate system and
* the azimuthal equidistant plane
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
__EXPORT void map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0);
__EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp);
/**
* Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane
* Initializes the map transformation given by the argument.
*
* Initializes the transformation between the geographic coordinate system and
* the azimuthal equidistant plane
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref,
double lat_0, double lon_0, uint64_t timestamp);
/**
* Initializes the map transformation given by the argument and sets the timestamp to now.
*
* Initializes the transformation between the geographic coordinate system and
* the azimuthal equidistant plane
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
*/
__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0);
/**
* Transforms a point in the geographic coordinate system to the local
* azimuthal equidistant plane using the global projection
* @param x north
* @param y east
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
* @return 0 if map_projection_init was called before, -1 else
*/
__EXPORT void map_projection_project(struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y);
__EXPORT int map_projection_global_project(double lat, double lon, float *x, float *y);
/* Transforms a point in the geographic coordinate system to the local
* azimuthal equidistant plane using the projection given by the argument
* @param x north
* @param y east
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
* @return 0 if map_projection_init was called before, -1 else
*/
__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y);
/**
* Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system
* Transforms a point in the local azimuthal equidistant plane to the
* geographic coordinate system using the global projection
*
* @param x north
* @param y east
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
* @return 0 if map_projection_init was called before, -1 else
*/
__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon);
__EXPORT int map_projection_global_reproject(float x, float y, double *lat, double *lon);
/**
* Transforms a point in the local azimuthal equidistant plane to the
* geographic coordinate system using the projection given by the argument
*
* @param x north
* @param y east
* @param lat in degrees (47.1234567°, not 471234567°)
* @param lon in degrees (8.1234567°, not 81234567°)
* @return 0 if map_projection_init was called before, -1 else
*/
__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon);
/**
* Get reference position of the global map projection
*/
__EXPORT int map_projection_global_getref(double *lat_0, double *lon_0);
/**
* Initialize the global mapping between global position (spherical) and local position (NED).
*/
__EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp);
/**
* Checks if globallocalconverter was initialized
* @return true if map was initialized before, false else
*/
__EXPORT bool globallocalconverter_initialized(void);
/**
* Convert from global position coordinates to local position coordinates using the global reference
*/
__EXPORT int globallocalconverter_tolocal(double lat, double lon, float alt, float *x, float *y, float *z);
/**
* Convert from local position coordinates to global position coordinates using the global reference
*/
__EXPORT int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt);
/**
* Get reference position of the global to local converter
*/
__EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0);
/**
* Returns the distance to the next waypoint in meters.
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
@@ -206,10 +206,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
0, 0, 1.f
}; /**< init: identity matrix */
// 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] = { "front", "back", "left", "right", "top", "bottom" };
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[0]) ? "front " : "",
(!data_collected[1]) ? "back " : "",
(!data_collected[2]) ? "left " : "",
(!data_collected[3]) ? "right " : "",
(!data_collected[4]) ? "up " : "",
(!data_collected[5]) ? "down " : "");
/* 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
+510 -152
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
+10 -13
View File
@@ -49,7 +49,7 @@ public:
StateMachineHelperTest();
virtual ~StateMachineHelperTest();
virtual void runTests(void);
virtual bool run_tests(void);
private:
bool armingStateTransitionTest();
@@ -286,7 +286,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
armed.ready_to_arm = test->current_state.ready_to_arm;
// Attempt transition
transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, 0 /* no mavlink_fd */);
transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, 0 /* no mavlink_fd */);
// Validate result of transition
ut_assert(test->assertMsg, test->expected_transition_result == result);
@@ -335,12 +335,12 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
MTT_ALL_NOT_VALID,
MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
{ "transition: MANUAL to AUTO_MISSION - global position valid",
MTT_GLOBAL_POS_VALID,
{ "transition: MANUAL to AUTO_MISSION - global position valid, home position valid",
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
{ "transition: AUTO_MISSION to MANUAL - global position valid",
MTT_GLOBAL_POS_VALID,
{ "transition: AUTO_MISSION to MANUAL - global position valid, home position valid",
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
{ "transition: MANUAL to AUTO_LOITER - global position valid",
@@ -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;

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