fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-21 09:47:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:PX4/Firmware into simon_test

Browse Source
This commit is contained in:
Lorenz Meier
2014-07-03 13:26:59 +02:00
parent 628477ee2c 2a7954b6cf
commit 73d7c8e383
101 changed files with 2073 additions and 1258 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/10015_tbs_discovery
+1 -1
View File
@@ -20,7 +20,7 @@ then
param set MC_PITCHRATE_D 0.0025
param set MC_YAW_P 2.8
param set MC_YAWRATE_P 0.28
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_I 0.1
param set MC_YAWRATE_D 0.0
fi
ROMFS/px4fmu_common/init.d/4010_dji_f330
+1 -1
View File
@@ -19,7 +19,7 @@ then
param set MC_PITCHRATE_D 0.003
param set MC_YAW_P 2.8
param set MC_YAWRATE_P 0.2
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_I 0.1
param set MC_YAWRATE_D 0.0
fi
ROMFS/px4fmu_common/init.d/4011_dji_f450
+1 -1
View File
@@ -20,7 +20,7 @@ then
param set MC_PITCHRATE_D 0.003
param set MC_YAW_P 2.8
param set MC_YAWRATE_P 0.2
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_I 0.1
param set MC_YAWRATE_D 0.0
fi
ROMFS/px4fmu_common/init.d/rc.mc_defaults
+1 -1
View File
@@ -14,7 +14,7 @@ then
param set MC_PITCHRATE_D 0.003
param set MC_YAW_P 2.8
param set MC_YAWRATE_P 0.2
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_I 0.1
param set MC_YAWRATE_D 0.0
param set MC_YAW_FF 0.5
ROMFS/px4fmu_common/init.d/rcS
+4
View File
@@ -132,6 +132,10 @@ then
#
if param compare SYS_AUTOCONFIG 1
then
# We can't be sure the defaults haven't changed, so
# if someone requests a re-configuration, we do it
# cleanly from scratch (except autostart / autoconfig)
param reset_nostart
set DO_AUTOCONFIG yes
else
set DO_AUTOCONFIG no
Tools/px_generate_xml.sh
+2
View File
@@ -0,0 +1,2 @@
#!/bin/sh
python px_process_params.py --xml
src/drivers/blinkm/blinkm.cpp
+1 -7
View File
@@ -559,13 +559,7 @@ BlinkM::led()
}
/* get number of used satellites in navigation */
num_of_used_sats = 0;
for(unsigned satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) {
if(vehicle_gps_position_raw.satellite_used[satloop] == 1) {
num_of_used_sats++;
}
}
num_of_used_sats = vehicle_gps_position_raw.satellites_used;
if (new_data_vehicle_status || no_data_vehicle_status < 3) {
if (num_of_cells == 0) {
src/drivers/boards/px4fmu-v1/px4fmu_init.c
+1 -1
View File
@@ -99,7 +99,7 @@
* CONFIG_ARCH_LEDS configuration switch.
*/
__BEGIN_DECLS
extern void led_init();
extern void led_init(void);
extern void led_on(int led);
extern void led_off(int led);
__END_DECLS
src/drivers/boards/px4fmu-v1/px4fmu_led.c
+2 -2
View File
@@ -54,13 +54,13 @@
* CONFIG_ARCH_LEDS configuration switch.
*/
__BEGIN_DECLS
extern void led_init();
extern void led_init(void);
extern void led_on(int led);
extern void led_off(int led);
extern void led_toggle(int led);
__END_DECLS
__EXPORT void led_init()
__EXPORT void led_init(void)
{
/* Configure LED1-2 GPIOs for output */
src/drivers/boards/px4fmu-v2/px4fmu2_led.c
+1 -1
View File
@@ -54,7 +54,7 @@
* CONFIG_ARCH_LEDS configuration switch.
*/
__BEGIN_DECLS
extern void led_init();
extern void led_init(void);
extern void led_on(int led);
extern void led_off(int led);
extern void led_toggle(int led);
src/drivers/ets_airspeed/ets_airspeed.cpp
+2 -2
View File
@@ -364,7 +364,7 @@ test()
err(1, "immediate read failed");
warnx("single read");
warnx("diff pressure: %d pa", report.differential_pressure_pa);
warnx("diff pressure: %f pa", (double)report.differential_pressure_pa);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
@@ -389,7 +389,7 @@ test()
err(1, "periodic read failed");
warnx("periodic read %u", i);
warnx("diff pressure: %d pa", report.differential_pressure_pa);
warnx("diff pressure: %f pa", (double)report.differential_pressure_pa);
}
/* reset the sensor polling to its default rate */
src/drivers/gps/gps.cpp
+93 -50
View File
@@ -63,6 +63,7 @@
#include <drivers/drv_gps.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/satellite_info.h>
#include <board_config.h>
@@ -79,10 +80,18 @@
#endif
static const int ERROR = -1;
/* class for dynamic allocation of satellite info data */
class GPS_Sat_Info
{
public:
struct satellite_info_s _data;
};
class GPS : public device::CDev
{
public:
GPS(const char *uart_path, bool fake_gps);
GPS(const char *uart_path, bool fake_gps, bool enable_sat_info);
virtual ~GPS();
virtual int init();
@@ -100,14 +109,17 @@ private:
int _serial_fd; ///< serial interface to GPS
unsigned _baudrate; ///< current baudrate
char _port[20]; ///< device / serial port path
volatile int _task; //< worker task
volatile int _task; ///< worker task
bool _healthy; ///< flag to signal if the GPS is ok
bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed
bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed
bool _mode_changed; ///< flag that the GPS mode has changed
gps_driver_mode_t _mode; ///< current mode
GPS_Helper *_Helper; ///< instance of GPS parser
struct vehicle_gps_position_s _report; ///< uORB topic for gps position
orb_advert_t _report_pub; ///< uORB pub for gps position
GPS_Sat_Info *_Sat_Info; ///< instance of GPS sat info data object
struct vehicle_gps_position_s _report_gps_pos; ///< uORB topic for gps position
orb_advert_t _report_gps_pos_pub; ///< uORB pub for gps position
struct satellite_info_s *_p_report_sat_info; ///< pointer to uORB topic for satellite info
orb_advert_t _report_sat_info_pub; ///< uORB pub for satellite info
float _rate; ///< position update rate
bool _fake_gps; ///< fake gps output
@@ -154,14 +166,17 @@ GPS *g_dev;
}
GPS::GPS(const char *uart_path, bool fake_gps) :
GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info) :
CDev("gps", GPS_DEVICE_PATH),
_task_should_exit(false),
_healthy(false),
_mode_changed(false),
_mode(GPS_DRIVER_MODE_UBX),
_Helper(nullptr),
_report_pub(-1),
_Sat_Info(nullptr),
_report_gps_pos_pub(-1),
_p_report_sat_info(nullptr),
_report_sat_info_pub(-1),
_rate(0.0f),
_fake_gps(fake_gps)
{
@@ -172,7 +187,14 @@ GPS::GPS(const char *uart_path, bool fake_gps) :
/* we need this potentially before it could be set in task_main */
g_dev = this;
memset(&_report, 0, sizeof(_report));
memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
/* create satellite info data object if requested */
if (enable_sat_info) {
_Sat_Info = new(GPS_Sat_Info);
_p_report_sat_info = &_Sat_Info->_data;
memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info));
}
_debug_enabled = true;
}
@@ -207,7 +229,7 @@ GPS::init()
/* start the GPS driver worker task */
_task = task_spawn_cmd("gps", SCHED_DEFAULT,
SCHED_PRIORITY_SLOW_DRIVER, 2000, (main_t)&GPS::task_main_trampoline, nullptr);
SCHED_PRIORITY_SLOW_DRIVER, 1500, (main_t)&GPS::task_main_trampoline, nullptr);
if (_task < 0) {
warnx("task start failed: %d", errno);
@@ -271,33 +293,33 @@ GPS::task_main()
if (_fake_gps) {
_report.timestamp_position = hrt_absolute_time();
_report.lat = (int32_t)47.378301e7f;
_report.lon = (int32_t)8.538777e7f;
_report.alt = (int32_t)1200e3f;
_report.timestamp_variance = hrt_absolute_time();
_report.s_variance_m_s = 10.0f;
_report.p_variance_m = 10.0f;
_report.c_variance_rad = 0.1f;
_report.fix_type = 3;
_report.eph = 0.9f;
_report.epv = 1.8f;
_report.timestamp_velocity = hrt_absolute_time();
_report.vel_n_m_s = 0.0f;
_report.vel_e_m_s = 0.0f;
_report.vel_d_m_s = 0.0f;
_report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s);
_report.cog_rad = 0.0f;
_report.vel_ned_valid = true;
_report_gps_pos.timestamp_position = hrt_absolute_time();
_report_gps_pos.lat = (int32_t)47.378301e7f;
_report_gps_pos.lon = (int32_t)8.538777e7f;
_report_gps_pos.alt = (int32_t)1200e3f;
_report_gps_pos.timestamp_variance = hrt_absolute_time();
_report_gps_pos.s_variance_m_s = 10.0f;
_report_gps_pos.p_variance_m = 10.0f;
_report_gps_pos.c_variance_rad = 0.1f;
_report_gps_pos.fix_type = 3;
_report_gps_pos.eph = 0.9f;
_report_gps_pos.epv = 1.8f;
_report_gps_pos.timestamp_velocity = hrt_absolute_time();
_report_gps_pos.vel_n_m_s = 0.0f;
_report_gps_pos.vel_e_m_s = 0.0f;
_report_gps_pos.vel_d_m_s = 0.0f;
_report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s);
_report_gps_pos.cog_rad = 0.0f;
_report_gps_pos.vel_ned_valid = true;
//no time and satellite information simulated
if (!(_pub_blocked)) {
if (_report_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
if (_report_gps_pos_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
} else {
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
}
}
@@ -313,11 +335,11 @@ GPS::task_main()
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
_Helper = new UBX(_serial_fd, &_report);
_Helper = new UBX(_serial_fd, &_report_gps_pos, _p_report_sat_info);
break;
case GPS_DRIVER_MODE_MTK:
_Helper = new MTK(_serial_fd, &_report);
_Helper = new MTK(_serial_fd, &_report_gps_pos);
break;
default:
@@ -332,20 +354,33 @@ GPS::task_main()
// GPS is obviously detected successfully, reset statistics
_Helper->reset_update_rates();
while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
int helper_ret;
while ((helper_ret = _Helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) {
// lock();
/* opportunistic publishing - else invalid data would end up on the bus */
if (!(_pub_blocked)) {
if (_report_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
if (helper_ret & 1) {
if (_report_gps_pos_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
} else {
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
} else {
_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
}
}
if (_p_report_sat_info && (helper_ret & 2)) {
if (_report_sat_info_pub > 0) {
orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info);
} else {
_report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info);
}
}
}
last_rate_count++;
if (helper_ret & 1) { // consider only pos info updates for rate calculation */
last_rate_count++;
}
/* measure update rate every 5 seconds */
if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
@@ -357,7 +392,7 @@ GPS::task_main()
}
if (!_healthy) {
char *mode_str = "unknown";
const char *mode_str = "unknown";
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
@@ -446,12 +481,13 @@ GPS::print_info()
}
warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
warnx("sat info: %s", (_p_report_sat_info != nullptr) ? "enabled" : "disabled");
if (_report.timestamp_position != 0) {
warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report.fix_type,
_report.satellites_visible, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0f);
warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph, (double)_report.epv);
if (_report_gps_pos.timestamp_position != 0) {
warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type,
_report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0);
warnx("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt);
warnx("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.epv);
warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
warnx("rate publication:\t%6.2f Hz", (double)_rate);
@@ -469,7 +505,7 @@ namespace gps
GPS *g_dev;
void start(const char *uart_path, bool fake_gps);
void start(const char *uart_path, bool fake_gps, bool enable_sat_info);
void stop();
void test();
void reset();
@@ -479,7 +515,7 @@ void info();
* Start the driver.
*/
void
start(const char *uart_path, bool fake_gps)
start(const char *uart_path, bool fake_gps, bool enable_sat_info)
{
int fd;
@@ -487,7 +523,7 @@ start(const char *uart_path, bool fake_gps)
errx(1, "already started");
/* create the driver */
g_dev = new GPS(uart_path, fake_gps);
g_dev = new GPS(uart_path, fake_gps, enable_sat_info);
if (g_dev == nullptr)
goto fail;
@@ -578,8 +614,9 @@ gps_main(int argc, char *argv[])
{
/* set to default */
char *device_name = GPS_DEFAULT_UART_PORT;
const char *device_name = GPS_DEFAULT_UART_PORT;
bool fake_gps = false;
bool enable_sat_info = false;
/*
* Start/load the driver.
@@ -601,7 +638,13 @@ gps_main(int argc, char *argv[])
fake_gps = true;
}
gps::start(device_name, fake_gps);
/* Detect sat info option */
for (int i = 2; i < argc; i++) {
if (!strcmp(argv[i], "-s"))
enable_sat_info = true;
}
gps::start(device_name, fake_gps, enable_sat_info);
}
if (!strcmp(argv[1], "stop"))
@@ -626,5 +669,5 @@ gps_main(int argc, char *argv[])
gps::info();
out:
errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f]");
errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f][-s]");
}
src/drivers/gps/gps_helper.h
+2 -2
View File
@@ -62,8 +62,8 @@ protected:
uint8_t _rate_count_lat_lon;
uint8_t _rate_count_vel;
float _rate_lat_lon;
float _rate_vel;
float _rate_lat_lon = 0.0f;
float _rate_vel = 0.0f;
uint64_t _interval_rate_start;
};
src/drivers/gps/mtk.cpp
+1 -1
View File
@@ -263,7 +263,7 @@ MTK::handle_message(gps_mtk_packet_t &packet)
_gps_position->epv = _gps_position->eph; // unknown in mtk custom mode, so we cheat with eph
_gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s
_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
_gps_position->satellites_visible = packet.satellites;
_gps_position->satellites_used = packet.satellites;
/* convert time and date information to unix timestamp */
struct tm timeinfo; //TODO: test this conversion
src/drivers/gps/ubx.cpp
+604 -448
View File
File diff suppressed because it is too large Load Diff
src/drivers/gps/ubx.h
+394 -279
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
* Copyright (c) 2012, 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
@@ -34,13 +34,16 @@
/**
* @file ubx.h
*
* U-Blox protocol definition. Following u-blox 6/7 Receiver Description
* U-Blox protocol definition. Following u-blox 6/7/8 Receiver Description
* including Prototol Specification.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
*
* @author Hannes Delago
* (rework, add ubx7+ compatibility)
*
*/
#ifndef UBX_H_
@@ -51,319 +54,423 @@
#define UBX_SYNC1 0xB5
#define UBX_SYNC2 0x62
/* ClassIDs (the ones that are used) */
#define UBX_CLASS_NAV 0x01
//#define UBX_CLASS_RXM 0x02
#define UBX_CLASS_ACK 0x05
#define UBX_CLASS_CFG 0x06
#define UBX_CLASS_MON 0x0A
/* Message Classes */
#define UBX_CLASS_NAV 0x01
#define UBX_CLASS_ACK 0x05
#define UBX_CLASS_CFG 0x06
#define UBX_CLASS_MON 0x0A
/* MessageIDs (the ones that are used) */
#define UBX_MESSAGE_NAV_POSLLH 0x02
//#define UBX_MESSAGE_NAV_DOP 0x04
#define UBX_MESSAGE_NAV_SOL 0x06
#define UBX_MESSAGE_NAV_VELNED 0x12
//#define UBX_MESSAGE_RXM_SVSI 0x20
#define UBX_MESSAGE_NAV_TIMEUTC 0x21
#define UBX_MESSAGE_NAV_SVINFO 0x30
#define UBX_MESSAGE_ACK_NAK 0x00
#define UBX_MESSAGE_ACK_ACK 0x01
#define UBX_MESSAGE_CFG_PRT 0x00
#define UBX_MESSAGE_CFG_MSG 0x01
#define UBX_MESSAGE_CFG_RATE 0x08
#define UBX_MESSAGE_CFG_NAV5 0x24
/* Message IDs */
#define UBX_ID_NAV_POSLLH 0x02
#define UBX_ID_NAV_SOL 0x06
#define UBX_ID_NAV_PVT 0x07
#define UBX_ID_NAV_VELNED 0x12
#define UBX_ID_NAV_TIMEUTC 0x21
#define UBX_ID_NAV_SVINFO 0x30
#define UBX_ID_ACK_NAK 0x00
#define UBX_ID_ACK_ACK 0x01
#define UBX_ID_CFG_PRT 0x00
#define UBX_ID_CFG_MSG 0x01
#define UBX_ID_CFG_RATE 0x08
#define UBX_ID_CFG_NAV5 0x24
#define UBX_ID_MON_VER 0x04
#define UBX_ID_MON_HW 0x09
#define UBX_MESSAGE_MON_HW 0x09
/* Message Classes & IDs */
#define UBX_MSG_NAV_POSLLH ((UBX_CLASS_NAV) | UBX_ID_NAV_POSLLH << 8)
#define UBX_MSG_NAV_SOL ((UBX_CLASS_NAV) | UBX_ID_NAV_SOL << 8)
#define UBX_MSG_NAV_PVT ((UBX_CLASS_NAV) | UBX_ID_NAV_PVT << 8)
#define UBX_MSG_NAV_VELNED ((UBX_CLASS_NAV) | UBX_ID_NAV_VELNED << 8)
#define UBX_MSG_NAV_TIMEUTC ((UBX_CLASS_NAV) | UBX_ID_NAV_TIMEUTC << 8)
#define UBX_MSG_NAV_SVINFO ((UBX_CLASS_NAV) | UBX_ID_NAV_SVINFO << 8)
#define UBX_MSG_ACK_NAK ((UBX_CLASS_ACK) | UBX_ID_ACK_NAK << 8)
#define UBX_MSG_ACK_ACK ((UBX_CLASS_ACK) | UBX_ID_ACK_ACK << 8)
#define UBX_MSG_CFG_PRT ((UBX_CLASS_CFG) | UBX_ID_CFG_PRT << 8)
#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_MON_HW ((UBX_CLASS_MON) | UBX_ID_MON_HW << 8)
#define UBX_MSG_MON_VER ((UBX_CLASS_MON) | UBX_ID_MON_VER << 8)
#define UBX_CFG_PRT_LENGTH 20
#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */
#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */
#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< choose 38400 as GPS baudrate */
#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */
#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */
/* RX NAV-PVT message content details */
/* Bitfield "valid" masks */
#define UBX_RX_NAV_PVT_VALID_VALIDDATE 0x01 /**< validDate (Valid UTC Date) */
#define UBX_RX_NAV_PVT_VALID_VALIDTIME 0x02 /**< validTime (Valid UTC Time) */
#define UBX_RX_NAV_PVT_VALID_FULLYRESOLVED 0x04 /**< fullyResolved (1 = UTC Time of Day has been fully resolved (no seconds uncertainty)) */
#define UBX_CFG_RATE_LENGTH 6
#define UBX_CFG_RATE_PAYLOAD_MEASINTERVAL 200 /**< 200ms for 5Hz */
#define UBX_CFG_RATE_PAYLOAD_NAVRATE 1 /**< cannot be changed */
#define UBX_CFG_RATE_PAYLOAD_TIMEREF 0 /**< 0: UTC, 1: GPS time */
/* Bitfield "flags" masks */
#define UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK 0x01 /**< gnssFixOK (A valid fix (i.e within DOP & accuracy masks)) */
#define UBX_RX_NAV_PVT_FLAGS_DIFFSOLN 0x02 /**< diffSoln (1 if differential corrections were applied) */
#define UBX_RX_NAV_PVT_FLAGS_PSMSTATE 0x1C /**< psmState (Power Save Mode state (see Power Management)) */
#define UBX_RX_NAV_PVT_FLAGS_HEADVEHVALID 0x20 /**< headVehValid (Heading of vehicle is valid) */
/* TX CFG-PRT message contents */
#define UBX_TX_CFG_PRT_PORTID 0x01 /**< UART1 */
#define UBX_TX_CFG_PRT_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */
#define UBX_TX_CFG_PRT_BAUDRATE 38400 /**< choose 38400 as GPS baudrate */
#define UBX_TX_CFG_PRT_INPROTOMASK 0x01 /**< UBX in */
#define UBX_TX_CFG_PRT_OUTPROTOMASK 0x01 /**< UBX out */
/* TX CFG-RATE message contents */
#define UBX_TX_CFG_RATE_MEASINTERVAL 200 /**< 200ms for 5Hz */
#define UBX_TX_CFG_RATE_NAVRATE 1 /**< cannot be changed */
#define UBX_TX_CFG_RATE_TIMEREF 0 /**< 0: UTC, 1: GPS time */
/* TX CFG-NAV5 message contents */
#define UBX_TX_CFG_NAV5_MASK 0x0005 /**< Only update dynamic model and fix mode */
#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-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 */
#define UBX_TX_CFG_MSG_RATE1_05HZ 10
#define UBX_CFG_NAV5_LENGTH 36
#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0005 /**< XXX only update dynamic model and fix mode */
#define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7 /**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */
#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2 /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */
#define UBX_CFG_MSG_LENGTH 8
#define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
#define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
#define UBX_CFG_MSG_PAYLOAD_RATE1_05HZ 10
#define UBX_MAX_PAYLOAD_LENGTH 500
// ************
/** the structures of the binary packets */
/*** u-blox protocol binary message and payload definitions ***/
#pragma pack(push, 1)
struct ubx_header {
uint8_t sync1;
uint8_t sync2;
uint8_t msg_class;
uint8_t msg_id;
uint16_t length;
};
/* General: Header */
typedef struct {
uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */
int32_t lon; /**< Longitude * 1e-7, deg */
int32_t lat; /**< Latitude * 1e-7, deg */
int32_t height; /**< Height above Ellipsoid, mm */
int32_t height_msl; /**< Height above mean sea level, mm */
uint32_t hAcc; /**< Horizontal Accuracy Estimate, mm */
uint32_t vAcc; /**< Vertical Accuracy Estimate, mm */
uint8_t ck_a;
uint8_t ck_b;
} gps_bin_nav_posllh_packet_t;
uint8_t sync1;
uint8_t sync2;
uint16_t msg;
uint16_t length;
} ubx_header_t;
/* General: Checksum */
typedef struct {
uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */
int32_t time_nanoseconds; /**< Fractional Nanoseconds remainder of rounded ms above, range -500000 .. 500000 */
int16_t week; /**< GPS week (GPS time) */
uint8_t gpsFix; /**< GPS Fix: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix */
uint8_t flags;
int32_t ecefX;
int32_t ecefY;
int32_t ecefZ;
uint32_t pAcc;
int32_t ecefVX;
int32_t ecefVY;
int32_t ecefVZ;
uint32_t sAcc;
uint16_t pDOP;
uint8_t reserved1;
uint8_t numSV;
uint32_t reserved2;
uint8_t ck_a;
uint8_t ck_b;
} gps_bin_nav_sol_packet_t;
uint8_t ck_a;
uint8_t ck_b;
} ubx_checksum_t ;
/* Rx NAV-POSLLH */
typedef struct {
uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */
uint32_t time_accuracy; /**< Time Accuracy Estimate, ns */
int32_t time_nanoseconds; /**< Nanoseconds of second, range -1e9 .. 1e9 (UTC) */
uint16_t year; /**< Year, range 1999..2099 (UTC) */
uint8_t month; /**< Month, range 1..12 (UTC) */
uint8_t day; /**< Day of Month, range 1..31 (UTC) */
uint8_t hour; /**< Hour of Day, range 0..23 (UTC) */
uint8_t min; /**< Minute of Hour, range 0..59 (UTC) */
uint8_t sec; /**< Seconds of Minute, range 0..59 (UTC) */
uint8_t valid_flag; /**< Validity Flags (see ubx documentation) */
uint8_t ck_a;
uint8_t ck_b;
} gps_bin_nav_timeutc_packet_t;
//typedef struct {
// uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */
// uint16_t gDOP; /**< Geometric DOP (scaling 0.01) */
// uint16_t pDOP; /**< Position DOP (scaling 0.01) */
// uint16_t tDOP; /**< Time DOP (scaling 0.01) */
// uint16_t vDOP; /**< Vertical DOP (scaling 0.01) */
// uint16_t hDOP; /**< Horizontal DOP (scaling 0.01) */
// uint16_t nDOP; /**< Northing DOP (scaling 0.01) */
// uint16_t eDOP; /**< Easting DOP (scaling 0.01) */
// uint8_t ck_a;
// uint8_t ck_b;
//} gps_bin_nav_dop_packet_t;
uint32_t iTOW; /**< GPS Time of Week [ms] */
int32_t lon; /**< Longitude [1e-7 deg] */
int32_t lat; /**< Latitude [1e-7 deg] */
int32_t height; /**< Height above ellipsoid [mm] */
int32_t hMSL; /**< Height above mean sea level [mm] */
uint32_t hAcc; /**< Horizontal accuracy estimate [mm] */
uint32_t vAcc; /**< Vertical accuracy estimate [mm] */
} ubx_payload_rx_nav_posllh_t;
/* Rx NAV-SOL */
typedef struct {
uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */
uint8_t numCh; /**< Number of channels */
uint8_t globalFlags;
uint16_t reserved2;
} gps_bin_nav_svinfo_part1_packet_t;
uint32_t iTOW; /**< GPS Time of Week [ms] */
int32_t fTOW; /**< Fractional part of iTOW (range: +/-500000) [ns] */
int16_t week; /**< GPS week */
uint8_t gpsFix; /**< GPSfix type: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix */
uint8_t flags;
int32_t ecefX;
int32_t ecefY;
int32_t ecefZ;
uint32_t pAcc;
int32_t ecefVX;
int32_t ecefVY;
int32_t ecefVZ;
uint32_t sAcc;
uint16_t pDOP;
uint8_t reserved1;
uint8_t numSV; /**< Number of SVs used in Nav Solution */
uint32_t reserved2;
} ubx_payload_rx_nav_sol_t;
/* Rx NAV-PVT */
typedef struct {
uint8_t chn; /**< Channel number, 255 for SVs not assigned to a channel */
uint8_t svid; /**< Satellite ID */
uint8_t flags;
uint8_t quality;
uint8_t cno; /**< Carrier to Noise Ratio (Signal Strength), dbHz */
int8_t elev; /**< Elevation in integer degrees */
int16_t azim; /**< Azimuth in integer degrees */
int32_t prRes; /**< Pseudo range residual in centimetres */
} gps_bin_nav_svinfo_part2_packet_t;
uint32_t iTOW; /**< GPS Time of Week [ms] */
uint16_t year; /**< Year (UTC)*/
uint8_t month; /**< Month, range 1..12 (UTC) */
uint8_t day; /**< Day of month, range 1..31 (UTC) */
uint8_t hour; /**< Hour of day, range 0..23 (UTC) */
uint8_t min; /**< Minute of hour, range 0..59 (UTC) */
uint8_t sec; /**< Seconds of minute, range 0..60 (UTC) */
uint8_t valid; /**< Validity flags (see UBX_RX_NAV_PVT_VALID_...) */
uint32_t tAcc; /**< Time accuracy estimate (UTC) [ns] */
int32_t nano; /**< Fraction of second (UTC) [-1e9...1e9 ns] */
uint8_t fixType; /**< GNSSfix type: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GNSS + dead reckoning, 5 = time only fix */
uint8_t flags; /**< Fix Status Flags (see UBX_RX_NAV_PVT_FLAGS_...) */
uint8_t reserved1;
uint8_t numSV; /**< Number of SVs used in Nav Solution */
int32_t lon; /**< Longitude [1e-7 deg] */
int32_t lat; /**< Latitude [1e-7 deg] */
int32_t height; /**< Height above ellipsoid [mm] */
int32_t hMSL; /**< Height above mean sea level [mm] */
uint32_t hAcc; /**< Horizontal accuracy estimate [mm] */
uint32_t vAcc; /**< Vertical accuracy estimate [mm] */
int32_t velN; /**< NED north velocity [mm/s]*/
int32_t velE; /**< NED east velocity [mm/s]*/
int32_t velD; /**< NED down velocity [mm/s]*/
int32_t gSpeed; /**< Ground Speed (2-D) [mm/s] */
int32_t headMot; /**< Heading of motion (2-D) [1e-5 deg] */
uint32_t sAcc; /**< Speed accuracy estimate [mm/s] */
uint32_t headAcc; /**< Heading accuracy estimate (motion and vehicle) [1e-5 deg] */
uint16_t pDOP; /**< Position DOP [0.01] */
uint16_t reserved2;
uint32_t reserved3;
int32_t headVeh; /**< Heading of vehicle (2-D) [1e-5 deg] */
uint32_t reserved4;
} ubx_payload_rx_nav_pvt_t;
/* Rx NAV-TIMEUTC */
typedef struct {
uint8_t ck_a;
uint8_t ck_b;
} gps_bin_nav_svinfo_part3_packet_t;
uint32_t iTOW; /**< GPS Time of Week [ms] */
uint32_t tAcc; /**< Time accuracy estimate (UTC) [ns] */
int32_t nano; /**< Fraction of second, range -1e9 .. 1e9 (UTC) [ns] */
uint16_t year; /**< Year, range 1999..2099 (UTC) */
uint8_t month; /**< Month, range 1..12 (UTC) */
uint8_t day; /**< Day of month, range 1..31 (UTC) */
uint8_t hour; /**< Hour of day, range 0..23 (UTC) */
uint8_t min; /**< Minute of hour, range 0..59 (UTC) */
uint8_t sec; /**< Seconds of minute, range 0..60 (UTC) */
uint8_t valid; /**< Validity Flags (see ubx documentation) */
} ubx_payload_rx_nav_timeutc_t;
/* Rx NAV-SVINFO Part 1 */
typedef struct {
uint32_t time_milliseconds; // GPS Millisecond Time of Week
int32_t velN; //NED north velocity, cm/s
int32_t velE; //NED east velocity, cm/s
int32_t velD; //NED down velocity, cm/s
uint32_t speed; //Speed (3-D), cm/s
uint32_t gSpeed; //Ground Speed (2-D), cm/s
int32_t heading; //Heading of motion 2-D, deg, scaling: 1e-5
uint32_t sAcc; //Speed Accuracy Estimate, cm/s
uint32_t cAcc; //Course / Heading Accuracy Estimate, scaling: 1e-5
uint8_t ck_a;
uint8_t ck_b;
} gps_bin_nav_velned_packet_t;
struct gps_bin_mon_hw_packet {
uint32_t pinSel;
uint32_t pinBank;
uint32_t pinDir;
uint32_t pinVal;
uint16_t noisePerMS;
uint16_t agcCnt;
uint8_t aStatus;
uint8_t aPower;
uint8_t flags;
uint8_t __reserved1;
uint32_t usedMask;
uint8_t VP[25];
uint8_t jamInd;
uint16_t __reserved3;
uint32_t pinIrq;
uint32_t pulLH;
uint32_t pullL;
};
//typedef struct {
// int32_t time_milliseconds; /**< Measurement integer millisecond GPS time of week */
// int16_t week; /**< Measurement GPS week number */
// uint8_t numVis; /**< Number of visible satellites */
//
// //... rest of package is not used in this implementation
//
//} gps_bin_rxm_svsi_packet_t;
uint32_t iTOW; /**< GPS Time of Week [ms] */
uint8_t numCh; /**< Number of channels */
uint8_t globalFlags;
uint16_t reserved2;
} ubx_payload_rx_nav_svinfo_part1_t;
/* Rx NAV-SVINFO Part 2 (repeated) */
typedef struct {
uint8_t clsID;
uint8_t msgID;
uint8_t ck_a;
uint8_t ck_b;
} gps_bin_ack_ack_packet_t;
uint8_t chn; /**< Channel number, 255 for SVs not assigned to a channel */
uint8_t svid; /**< Satellite ID */
uint8_t flags;
uint8_t quality;
uint8_t cno; /**< Carrier to Noise Ratio (Signal Strength) [dbHz] */
int8_t elev; /**< Elevation [deg] */
int16_t azim; /**< Azimuth [deg] */
int32_t prRes; /**< Pseudo range residual [cm] */
} ubx_payload_rx_nav_svinfo_part2_t;
/* Rx NAV-VELNED */
typedef struct {
uint8_t clsID;
uint8_t msgID;
uint8_t ck_a;
uint8_t ck_b;
} gps_bin_ack_nak_packet_t;
uint32_t iTOW; /**< GPS Time of Week [ms] */
int32_t velN; /**< North velocity component [cm/s]*/
int32_t velE; /**< East velocity component [cm/s]*/
int32_t velD; /**< Down velocity component [cm/s]*/
uint32_t speed; /**< Speed (3-D) [cm/s] */
uint32_t gSpeed; /**< Ground speed (2-D) [cm/s] */
int32_t heading; /**< Heading of motion 2-D [1e-5 deg] */
uint32_t sAcc; /**< Speed accuracy estimate [cm/s] */
uint32_t cAcc; /**< Course / Heading accuracy estimate [1e-5 deg] */
} ubx_payload_rx_nav_velned_t;
/* Rx MON-HW (ubx6) */
typedef struct {
uint8_t clsID;
uint8_t msgID;
uint16_t length;
uint8_t portID;
uint8_t res0;
uint16_t res1;
uint32_t mode;
uint32_t baudRate;
uint16_t inProtoMask;
uint16_t outProtoMask;
uint16_t flags;
uint16_t pad;
uint8_t ck_a;
uint8_t ck_b;
} type_gps_bin_cfg_prt_packet_t;
uint32_t pinSel;
uint32_t pinBank;
uint32_t pinDir;
uint32_t pinVal;
uint16_t noisePerMS;
uint16_t agcCnt;
uint8_t aStatus;
uint8_t aPower;
uint8_t flags;
uint8_t reserved1;
uint32_t usedMask;
uint8_t VP[25];
uint8_t jamInd;
uint16_t reserved3;
uint32_t pinIrq;
uint32_t pullH;
uint32_t pullL;
} ubx_payload_rx_mon_hw_ubx6_t;
/* Rx MON-HW (ubx7+) */
typedef struct {
uint8_t clsID;
uint8_t msgID;
uint16_t length;
uint16_t measRate;
uint16_t navRate;
uint16_t timeRef;
uint8_t ck_a;
uint8_t ck_b;
} type_gps_bin_cfg_rate_packet_t;
uint32_t pinSel;
uint32_t pinBank;
uint32_t pinDir;
uint32_t pinVal;
uint16_t noisePerMS;
uint16_t agcCnt;
uint8_t aStatus;
uint8_t aPower;
uint8_t flags;
uint8_t reserved1;
uint32_t usedMask;
uint8_t VP[17];
uint8_t jamInd;
uint16_t reserved3;
uint32_t pinIrq;
uint32_t pullH;
uint32_t pullL;
} ubx_payload_rx_mon_hw_ubx7_t;
/* Rx MON-VER Part 1 */
typedef struct {
uint8_t clsID;
uint8_t msgID;
uint16_t length;
uint16_t mask;
uint8_t dynModel;
uint8_t fixMode;
int32_t fixedAlt;
uint32_t fixedAltVar;
int8_t minElev;
uint8_t drLimit;
uint16_t pDop;
uint16_t tDop;
uint16_t pAcc;
uint16_t tAcc;
uint8_t staticHoldThresh;
uint8_t dgpsTimeOut;
uint32_t reserved2;
uint32_t reserved3;
uint32_t reserved4;
uint8_t ck_a;
uint8_t ck_b;
} type_gps_bin_cfg_nav5_packet_t;
uint8_t swVersion[30];
uint8_t hwVersion[10];
} ubx_payload_rx_mon_ver_part1_t;
/* Rx MON-VER Part 2 (repeated) */
typedef struct {
uint8_t clsID;
uint8_t msgID;
uint16_t length;
uint8_t msgClass_payload;
uint8_t msgID_payload;
uint8_t extension[30];
} ubx_payload_rx_mon_ver_part2_t;
/* Rx ACK-ACK */
typedef union {
uint16_t msg;
struct {
uint8_t clsID;
uint8_t msgID;
};
} ubx_payload_rx_ack_ack_t;
/* Rx ACK-NAK */
typedef union {
uint16_t msg;
struct {
uint8_t clsID;
uint8_t msgID;
};
} ubx_payload_rx_ack_nak_t;
/* Tx CFG-PRT */
typedef struct {
uint8_t portID;
uint8_t reserved0;
uint16_t txReady;
uint32_t mode;
uint32_t baudRate;
uint16_t inProtoMask;
uint16_t outProtoMask;
uint16_t flags;
uint16_t reserved5;
} ubx_payload_tx_cfg_prt_t;
/* Tx CFG-RATE */
typedef struct {
uint16_t measRate; /**< Measurement Rate, GPS measurements are taken every measRate milliseconds */
uint16_t navRate; /**< Navigation Rate, in number of measurement cycles. This parameter cannot be changed, and must be set to 1 */
uint16_t timeRef; /**< Alignment to reference time: 0 = UTC time, 1 = GPS time */
} ubx_payload_tx_cfg_rate_t;
/* Tx CFG-NAV5 */
typedef struct {
uint16_t mask;
uint8_t dynModel; /**< Dynamic Platform model: 0 Portable, 2 Stationary, 3 Pedestrian, 4 Automotive, 5 Sea, 6 Airborne <1g, 7 Airborne <2g, 8 Airborne <4g */
uint8_t fixMode; /**< Position Fixing Mode: 1 2D only, 2 3D only, 3 Auto 2D/3D */
int32_t fixedAlt;
uint32_t fixedAltVar;
int8_t minElev;
uint8_t drLimit;
uint16_t pDop;
uint16_t tDop;
uint16_t pAcc;
uint16_t tAcc;
uint8_t staticHoldThresh;
uint8_t dgpsTimeOut;
uint8_t cnoThreshNumSVs; /**< (ubx7+ only, else 0) */
uint8_t cnoThresh; /**< (ubx7+ only, else 0) */
uint16_t reserved;
uint16_t staticHoldMaxDist; /**< (ubx8+ only, else 0) */
uint8_t utcStandard; /**< (ubx8+ only, else 0) */
uint8_t reserved3;
uint32_t reserved4;
} ubx_payload_tx_cfg_nav5_t;
/* Tx CFG-MSG */
typedef struct {
union {
uint16_t msg;
struct {
uint8_t msgClass;
uint8_t msgID;
};
};
uint8_t rate;
uint8_t ck_a;
uint8_t ck_b;
} type_gps_bin_cfg_msg_packet_t;
} ubx_payload_tx_cfg_msg_t;
struct ubx_cfg_msg_rate {
uint8_t msg_class;
uint8_t msg_id;
uint8_t rate;
};
/* General message and payload buffer union */
typedef union {
ubx_payload_rx_nav_posllh_t payload_rx_nav_posllh;
ubx_payload_rx_nav_sol_t payload_rx_nav_sol;
ubx_payload_rx_nav_timeutc_t payload_rx_nav_timeutc;
ubx_payload_rx_nav_svinfo_part1_t payload_rx_nav_svinfo_part1;
ubx_payload_rx_nav_svinfo_part2_t payload_rx_nav_svinfo_part2;
ubx_payload_rx_nav_velned_t payload_rx_nav_velned;
ubx_payload_rx_mon_hw_ubx6_t payload_rx_mon_hw_ubx6;
ubx_payload_rx_mon_hw_ubx7_t payload_rx_mon_hw_ubx7;
ubx_payload_rx_mon_ver_part1_t payload_rx_mon_ver_part1;
ubx_payload_rx_mon_ver_part2_t payload_rx_mon_ver_part2;
ubx_payload_rx_ack_ack_t payload_rx_ack_ack;
ubx_payload_rx_ack_nak_t payload_rx_ack_nak;
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_msg_t payload_tx_cfg_msg;
uint8_t raw[];
} ubx_buf_t;
#pragma pack(pop)
/*** END OF u-blox protocol binary message and payload definitions ***/
// END the structures of the binary packets
// ************
/* Decoder state */
typedef enum {
UBX_DECODE_UNINIT = 0,
UBX_DECODE_GOT_SYNC1,
UBX_DECODE_GOT_SYNC2,
UBX_DECODE_GOT_CLASS,
UBX_DECODE_GOT_MESSAGEID,
UBX_DECODE_GOT_LENGTH1,
UBX_DECODE_GOT_LENGTH2
UBX_DECODE_SYNC1 = 0,
UBX_DECODE_SYNC2,
UBX_DECODE_CLASS,
UBX_DECODE_ID,
UBX_DECODE_LENGTH1,
UBX_DECODE_LENGTH2,
UBX_DECODE_PAYLOAD,
UBX_DECODE_CHKSUM1,
UBX_DECODE_CHKSUM2
} ubx_decode_state_t;
//typedef type_gps_bin_ubx_state gps_bin_ubx_state_t;
#pragma pack(pop)
/* Rx message state */
typedef enum {
UBX_RXMSG_IGNORE = 0,
UBX_RXMSG_HANDLE,
UBX_RXMSG_DISABLE,
UBX_RXMSG_ERROR_LENGTH
} ubx_rxmsg_state_t;
/* ACK state */
typedef enum {
UBX_ACK_IDLE = 0,
UBX_ACK_WAITING,
UBX_ACK_GOT_ACK,
UBX_ACK_GOT_NAK
} ubx_ack_state_t;
#define RECV_BUFFER_SIZE 300 //The NAV-SOL messages really need such a big buffer
class UBX : public GPS_Helper
{
public:
UBX(const int &fd, struct vehicle_gps_position_s *gps_position);
UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info);
~UBX();
int receive(unsigned timeout);
int receive(const unsigned timeout);
int configure(unsigned &baudrate);
private:
/**
* Parse the binary MTK packet
* Parse the binary UBX packet
*/
int parse_char(uint8_t b);
int parse_char(const uint8_t b);
/**
* Handle the package once it has arrived
* Start payload rx
*/
int handle_message(void);
int payload_rx_init(void);
/**
* Add payload rx byte
*/
int payload_rx_add(const uint8_t b);
int payload_rx_add_nav_svinfo(const uint8_t b);
int payload_rx_add_mon_ver(const uint8_t b);
/**
* Finish payload rx
*/
int payload_rx_done(void);
/**
* Reset the parse state machine for a fresh start
@@ -373,44 +480,52 @@ private:
/**
* While parsing add every byte (except the sync bytes) to the checksum
*/
void add_byte_to_checksum(uint8_t);
void add_byte_to_checksum(const uint8_t);
/**
* Add the two checksum bytes to an outgoing message
* Send a message
*/
void add_checksum_to_message(uint8_t *message, const unsigned length);
void send_message(const uint16_t msg, const uint8_t *payload, const uint16_t length);
/**
* Helper to send a config packet
* Configure message rate
*/
void send_config_packet(const int &fd, uint8_t *packet, const unsigned length);
void configure_message_rate(const uint16_t msg, const uint8_t rate);
void configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
/**
* Calculate & add checksum for given buffer
*/
void calc_checksum(const uint8_t *buffer, const uint16_t length, ubx_checksum_t *checksum);
void send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size);
/**
* Wait for message acknowledge
*/
int wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report);
void add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b);
int wait_for_ack(unsigned timeout);
/**
* Calculate FNV1 hash
*/
uint32_t fnv1_32_str(uint8_t *str, uint32_t hval);
int _fd;
struct vehicle_gps_position_s *_gps_position;
struct satellite_info_s *_satellite_info;
bool _enable_sat_info;
bool _configured;
bool _waiting_for_ack;
ubx_ack_state_t _ack_state;
bool _got_posllh;
bool _got_velned;
bool _got_timeutc;
uint8_t _message_class_needed;
uint8_t _message_id_needed;
ubx_decode_state_t _decode_state;
uint8_t _rx_buffer[RECV_BUFFER_SIZE];
unsigned _rx_count;
uint16_t _rx_msg;
ubx_rxmsg_state_t _rx_state;
uint16_t _rx_payload_length;
uint16_t _rx_payload_index;
uint8_t _rx_ck_a;
uint8_t _rx_ck_b;
uint8_t _message_class;
uint8_t _message_id;
unsigned _payload_size;
hrt_abstime _disable_cmd_last;
uint16_t _ack_waiting_msg;
ubx_buf_t _buf;
uint32_t _ubx_version;
};
#endif /* UBX_H_ */
src/drivers/hmc5883/hmc5883.cpp
+2 -2
View File
@@ -326,9 +326,9 @@ HMC5883::HMC5883(int bus) :
_range_scale(0), /* default range scale from counts to gauss */
_range_ga(1.3f),
_collect_phase(false),
_class_instance(-1),
_mag_topic(-1),
_subsystem_pub(-1),
_class_instance(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")),
@@ -1228,7 +1228,7 @@ HMC5883::print_info()
printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset);
printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n",
(double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale,
(double)1.0/_range_scale, (double)_range_ga);
(double)(1.0f/_range_scale), (double)_range_ga);
_reports->print_info("report queue");
}
src/drivers/hott/messages.cpp
+1 -1
View File
@@ -226,7 +226,7 @@ build_gps_response(uint8_t *buffer, size_t *size)
msg.sensor_id = GPS_SENSOR_ID;
msg.sensor_text_id = GPS_SENSOR_TEXT_ID;
msg.gps_num_sat = gps.satellites_visible;
msg.gps_num_sat = gps.satellites_used;
/* The GPS fix type: 0 = none, 2 = 2D, 3 = 3D */
msg.gps_fix_char = (uint8_t)(gps.fix_type + 48);
src/drivers/mkblctrl/mkblctrl.cpp
+12 -17
View File
@@ -131,8 +131,8 @@ public:
int set_motor_count(unsigned count);
int set_motor_test(bool motortest);
int set_overrideSecurityChecks(bool overrideSecurityChecks);
int set_px4mode(int px4mode);
int set_frametype(int frametype);
void set_px4mode(int px4mode);
void set_frametype(int frametype);
unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C);
private:
@@ -222,15 +222,15 @@ MK::MK(int bus, const char *_device_path) :
_task(-1),
_t_actuators(-1),
_t_actuator_armed(-1),
_t_outputs(0),
_t_esc_status(0),
_num_outputs(0),
_motortest(false),
_overrideSecurityChecks(false),
_motor(-1),
_px4mode(MAPPING_MK),
_frametype(FRAME_PLUS),
_t_outputs(0),
_t_esc_status(0),
_num_outputs(0),
_primary_pwm_device(false),
_motortest(false),
_overrideSecurityChecks(false),
_task_should_exit(false),
_armed(false),
_mixers(nullptr)
@@ -330,13 +330,13 @@ MK::set_update_rate(unsigned rate)
return OK;
}
int
void
MK::set_px4mode(int px4mode)
{
_px4mode = px4mode;
}
int
void
MK::set_frametype(int frametype)
{
_frametype = frametype;
@@ -440,9 +440,6 @@ MK::scaling(float val, float inMin, float inMax, float outMin, float outMax)
void
MK::task_main()
{
long update_rate_in_us = 0;
float tmpVal = 0;
/*
* Subscribe to the appropriate PWM output topic based on whether we are the
* primary PWM output or not.
@@ -483,7 +480,6 @@ MK::task_main()
/* handle update rate changes */
if (_current_update_rate != _update_rate) {
int update_rate_in_ms = int(1000 / _update_rate);
update_rate_in_us = long(1000000 / _update_rate);
/* reject faster than 500 Hz updates */
if (update_rate_in_ms < 2) {
@@ -735,7 +731,6 @@ MK::mk_servo_set(unsigned int chan, short val)
_retries = 0;
uint8_t result[3] = { 0, 0, 0 };
uint8_t msg[2] = { 0, 0 };
uint8_t rod = 0;
uint8_t bytesToSendBL2 = 2;
tmpVal = val;
@@ -824,7 +819,7 @@ MK::mk_servo_set(unsigned int chan, short val)
if (debugCounter == 2000) {
debugCounter = 0;
for (int i = 0; i < _num_outputs; i++) {
for (unsigned int i = 0; i < _num_outputs; i++) {
if (Motor[i].State & MOTOR_STATE_PRESENT_MASK) {
fprintf(stderr, "[mkblctrl] #%i:\tVer: %i\tVal: %i\tCurr: %i\tMaxPWM: %i\tTemp: %i\tState: %i\n", i, Motor[i].Version, Motor[i].SetPoint, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature, Motor[i].State);
}
@@ -1169,7 +1164,7 @@ mk_new_mode(int update_rate, int motorcount, bool motortest, int px4mode, int fr
}
int
mk_start(unsigned motors, char *device_path)
mk_start(unsigned motors, const char *device_path)
{
int ret;
@@ -1228,7 +1223,7 @@ mkblctrl_main(int argc, char *argv[])
bool overrideSecurityChecks = false;
bool showHelp = false;
bool newMode = false;
char *devicepath = "";
const char *devicepath = "";
/*
* optional parameters
src/drivers/px4fmu/fmu.cpp
+7 -7
View File
@@ -240,8 +240,6 @@ PX4FMU::PX4FMU() :
_pwm_alt_rate_channels(0),
_current_update_rate(0),
_task(-1),
_control_subs({-1}),
_poll_fds_num(0),
_armed_sub(-1),
_outputs_pub(-1),
_num_outputs(0),
@@ -252,10 +250,12 @@ PX4FMU::PX4FMU() :
_mixers(nullptr),
_groups_required(0),
_groups_subscribed(0),
_failsafe_pwm({0}),
_disarmed_pwm({0}),
_num_failsafe_set(0),
_num_disarmed_set(0)
_control_subs{-1},
_poll_fds_num(0),
_failsafe_pwm{0},
_disarmed_pwm{0},
_num_failsafe_set(0),
_num_disarmed_set(0)
{
for (unsigned i = 0; i < _max_actuators; i++) {
_min_pwm[i] = PWM_DEFAULT_MIN;
@@ -741,7 +741,7 @@ PX4FMU::task_main()
}
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs > 0) {
if (_control_subs[i] > 0) {
::close(_control_subs[i]);
_control_subs[i] = -1;
}
src/drivers/px4io/px4io.cpp
+14
View File
@@ -72,6 +72,7 @@
#include <systemlib/systemlib.h>
#include <systemlib/scheduling_priorities.h>
#include <systemlib/param/param.h>
#include <systemlib/circuit_breaker.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
@@ -1012,6 +1013,19 @@ PX4IO::task_main()
}
}
int32_t safety_param_val;
param_t safety_param = param_find("RC_FAILS_THR");
if (safety_param != PARAM_INVALID) {
param_get(safety_param, &safety_param_val);
if (safety_param_val == PX4IO_FORCE_SAFETY_MAGIC) {
/* disable IO safety if circuit breaker asked for it */
(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, safety_param_val);
}
}
}
}
src/drivers/px4io/px4io_serial.cpp
+1 -1
View File
@@ -639,7 +639,7 @@ PX4IO_serial::_do_interrupt()
if (_rx_dma_status == _dma_status_waiting) {
/* verify that the received packet is complete */
int length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma);
size_t length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma);
if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) {
perf_count(_pc_badidle);
src/drivers/roboclaw/RoboClaw.cpp
+8 -2
View File
@@ -182,7 +182,10 @@ float RoboClaw::getMotorPosition(e_motor motor)
return _motor1Position;
} else if (motor == MOTOR_2) {
return _motor2Position;
}
} else {
warnx("Unknown motor value passed to RoboClaw::getMotorPosition");
return NAN;
}
}
float RoboClaw::getMotorSpeed(e_motor motor)
@@ -191,7 +194,10 @@ float RoboClaw::getMotorSpeed(e_motor motor)
return _motor1Speed;
} else if (motor == MOTOR_2) {
return _motor2Speed;
}
} else {
warnx("Unknown motor value passed to RoboClaw::getMotorPosition");
return NAN;
}
}
int RoboClaw::setMotorSpeed(e_motor motor, float value)
src/drivers/sf0x/sf0x.cpp
+1 -1
View File
@@ -616,7 +616,7 @@ SF0X::collect()
}
}
debug("val (float): %8.4f, raw: %s, valid: %s\n", si_units, _linebuf, ((valid) ? "OK" : "NO"));
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;
src/examples/px4_simple_app/px4_simple_app.c
+1
View File
@@ -41,6 +41,7 @@
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <string.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
src/lib/external_lgpl/tecs/tecs.cpp
+1 -1
View File
@@ -310,7 +310,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi , 0.1f, 1.0f) - 1.0f);
if (STEdot_dem >= 0) {
ff_throttle = nomThr + STEdot_dem / _STEdot_max * (1.0f - nomThr);
ff_throttle = nomThr + STEdot_dem / _STEdot_max * (_THRmaxf - nomThr);
} else {
ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr;
src/lib/geo/geo.c
+16 -17
View File
@@ -82,8 +82,8 @@ __EXPORT void map_projection_project(struct map_projection_reference_s *ref, dou
__EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon)
{
float x_rad = x / CONSTANTS_RADIUS_OF_EARTH;
float y_rad = y / CONSTANTS_RADIUS_OF_EARTH;
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);
double sin_c = sin(c);
double cos_c = cos(c);
@@ -146,7 +146,6 @@ __EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double
double lat_next_rad = lat_next * M_DEG_TO_RAD;
double lon_next_rad = lon_next * M_DEG_TO_RAD;
double d_lat = lat_next_rad - lat_now_rad;
double d_lon = lon_next_rad - lon_now_rad;
/* conscious mix of double and float trig function to maximize speed and efficiency */
@@ -174,8 +173,8 @@ __EXPORT void add_vector_to_global_position(double lat_now, double lon_now, floa
double lat_now_rad = lat_now * M_DEG_TO_RAD;
double lon_now_rad = lon_now * M_DEG_TO_RAD;
*lat_res = (lat_now_rad + v_n / CONSTANTS_RADIUS_OF_EARTH) * M_RAD_TO_DEG;
*lon_res = (lon_now_rad + v_e / (CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad))) * M_RAD_TO_DEG;
*lat_res = (lat_now_rad + (double)v_n / CONSTANTS_RADIUS_OF_EARTH) * M_RAD_TO_DEG;
*lon_res = (lon_now_rad + (double)v_e / (CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad))) * M_RAD_TO_DEG;
}
// Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
@@ -197,7 +196,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d
crosstrack_error->bearing = 0.0f;
// Return error if arguments are bad
if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) { return return_value; }
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; }
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);
@@ -212,7 +211,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d
}
dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
crosstrack_error->distance = (dist_to_end) * sin(bearing_diff);
crosstrack_error->distance = (dist_to_end) * sinf(bearing_diff);
if (sin(bearing_diff) >= 0) {
crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F);
@@ -248,7 +247,7 @@ __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.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) { return return_value; }
if (lat_now == 0.0 || lon_now == 0.0 || lat_center == 0.0 || lon_center == 0.0 || radius == 0.0f) { return return_value; }
if (arc_sweep >= 0) {
@@ -296,14 +295,14 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
// as this function generally will not be called repeatedly when we are out of the sector.
// TO DO - this is messed up and won't compile
float start_disp_x = radius * sin(arc_start_bearing);
float start_disp_y = radius * cos(arc_start_bearing);
float end_disp_x = radius * sin(_wrapPI(arc_start_bearing + arc_sweep));
float end_disp_y = radius * cos(_wrapPI(arc_start_bearing + arc_sweep));
float lon_start = lon_now + start_disp_x / 111111.0d;
float lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0d;
float lon_end = lon_now + end_disp_x / 111111.0d;
float lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0d;
float start_disp_x = radius * sinf(arc_start_bearing);
float start_disp_y = radius * cosf(arc_start_bearing);
float end_disp_x = radius * sinf(_wrapPI(arc_start_bearing + arc_sweep));
float end_disp_y = radius * cosf(_wrapPI(arc_start_bearing + arc_sweep));
float lon_start = lon_now + start_disp_x / 111111.0f;
float lat_start = lat_now + start_disp_y * cosf(lat_now) / 111111.0f;
float lon_end = lon_now + end_disp_x / 111111.0f;
float lat_end = lat_now + end_disp_y * cosf(lat_now) / 111111.0f;
float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
@@ -337,7 +336,7 @@ __EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now
double d_lat = x_rad - current_x_rad;
double d_lon = y_rad - current_y_rad;
double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0f) * cos(current_x_rad) * cos(x_rad);
double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(current_x_rad) * cos(x_rad);
double c = 2 * atan2(sqrt(a), sqrt(1 - a));
float dxy = CONSTANTS_RADIUS_OF_EARTH * c;
src/lib/geo_lookup/geo_mag_declination.c
+13 -18
View File
@@ -54,24 +54,19 @@
static const int8_t declination_table[13][37] = \
{
46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, \
-66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46, 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, \
-3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, \
29, 30, 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, \
-40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21, 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, \
8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, \
10, 13, 15, 16, 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, \
-10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12, 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, \
-14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10, 9, 9, 9, \
9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, \
7, 8, 9, 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, \
0, 0, 0, 1, 3, 5, 7, 8, 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, \
0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8, 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, \
0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6, 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, \
-16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5, 4, 8, 12, 15, 17, 18, 16, \
12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, \
0, 4, 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, \
13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3
{ 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46 },
{ 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, 29, 30 },
{ 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21 },
{ 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, 10, 13, 15, 16 },
{ 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12 },
{ 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10 },
{ 9, 9, 9, 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, 7, 8, 9 },
{ 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, 0, 0, 0, 1, 3, 5, 7, 8 },
{ 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8 },
{ 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6 },
{ 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5 },
{ 4, 8, 12, 15, 17, 18, 16, 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, 0, 4 },
{ 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 },
};
static float get_lookup_table_val(unsigned lat, unsigned lon);
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+1 -1
View File
@@ -101,7 +101,7 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
param_get(h->r3, &(p->r[3]));
param_get(h->mag_decl, &(p->mag_decl));
p->mag_decl *= M_PI / 180.0f;
p->mag_decl *= M_PI_F / 180.0f;
param_get(h->acc_comp, &(p->acc_comp));
src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
+4 -17
View File
@@ -392,8 +392,6 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl
*/
int attitude_estimator_so3_thread_main(int argc, char *argv[])
{
const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
//! Time constant
float dt = 0.005f;
@@ -438,11 +436,9 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
int loopcounter = 0;
int printcounter = 0;
thread_running = true;
float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
// XXX write this out to perf regs
/* keep track of sensor updates */
@@ -513,7 +509,7 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
gyro_offsets[0] /= offset_count;
gyro_offsets[1] /= offset_count;
gyro_offsets[2] /= offset_count;
warnx("gyro initialized, offsets: %.5f %.5f %.5f", gyro_offsets[0], gyro_offsets[1], gyro_offsets[2]);
warnx("gyro initialized, offsets: %.5f %.5f %.5f", (double)gyro_offsets[0], (double)gyro_offsets[1], (double)gyro_offsets[2]);
}
} else {
@@ -523,12 +519,9 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
/* Calculate data time difference in seconds */
dt = (raw.timestamp - last_measurement) / 1000000.0f;
last_measurement = raw.timestamp;
uint8_t update_vect[3] = {0, 0, 0};
/* Fill in gyro measurements */
if (sensor_last_timestamp[0] != raw.timestamp) {
update_vect[0] = 1;
sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
sensor_last_timestamp[0] = raw.timestamp;
}
@@ -538,8 +531,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
/* update accelerometer measurements */
if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
update_vect[1] = 1;
sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
sensor_last_timestamp[1] = raw.accelerometer_timestamp;
}
@@ -549,8 +540,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
/* update magnetometer measurements */
if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
update_vect[2] = 1;
sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
sensor_last_timestamp[2] = raw.magnetometer_timestamp;
}
@@ -569,8 +558,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
continue;
}
uint64_t timing_start = hrt_absolute_time();
// NOTE : Accelerometer is reversed.
// Because proper mount of PX4 will give you a reversed accelerometer readings.
NonlinearSO3AHRSupdate(gyro[0], gyro[1], gyro[2],
@@ -609,9 +596,9 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
/* due to inputs or numerical failure the output is invalid, skip it */
// Due to inputs or numerical failure the output is invalid
warnx("infinite euler angles, rotation matrix:");
warnx("%.3f %.3f %.3f", Rot_matrix[0], Rot_matrix[1], Rot_matrix[2]);
warnx("%.3f %.3f %.3f", Rot_matrix[3], Rot_matrix[4], Rot_matrix[5]);
warnx("%.3f %.3f %.3f", Rot_matrix[6], Rot_matrix[7], Rot_matrix[8]);
warnx("%.3f %.3f %.3f", (double)Rot_matrix[0], (double)Rot_matrix[1], (double)Rot_matrix[2]);
warnx("%.3f %.3f %.3f", (double)Rot_matrix[3], (double)Rot_matrix[4], (double)Rot_matrix[5]);
warnx("%.3f %.3f %.3f", (double)Rot_matrix[6], (double)Rot_matrix[7], (double)Rot_matrix[8]);
// Don't publish anything
continue;
}
src/modules/commander/accelerometer_calibration.cpp
+6 -3
View File
@@ -131,6 +131,7 @@
#include <fcntl.h>
#include <sys/prctl.h>
#include <math.h>
#include <float.h>
#include <mathlib/mathlib.h>
#include <string.h>
#include <drivers/drv_hrt.h>
@@ -158,6 +159,8 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
int do_accel_calibration(int mavlink_fd)
{
int fd;
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
struct accel_scale accel_scale = {
@@ -172,7 +175,7 @@ int do_accel_calibration(int mavlink_fd)
int res = OK;
/* reset all offsets to zero and all scales to one */
int fd = open(ACCEL_DEVICE_PATH, 0);
fd = open(ACCEL_DEVICE_PATH, 0);
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
close(fd);
@@ -223,7 +226,7 @@ int do_accel_calibration(int mavlink_fd)
if (res == OK) {
/* apply new scaling and offsets */
int fd = open(ACCEL_DEVICE_PATH, 0);
fd = open(ACCEL_DEVICE_PATH, 0);
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
close(fd);
@@ -524,7 +527,7 @@ int mat_invert3(float src[3][3], float dst[3][3])
src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
if (det == 0.0f) {
if (fabsf(det) < FLT_EPSILON) {
return ERROR; // Singular matrix
}
src/modules/commander/calibration_routines.cpp
+5 -4
View File
@@ -40,6 +40,7 @@
*/
#include <math.h>
#include <float.h>
#include "calibration_routines.h"
@@ -170,7 +171,7 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
float aA, aB, aC, nA, nB, nC, dA, dB, dC;
//Iterate N times, ignore stop condition.
int n = 0;
unsigned int n = 0;
while (n < max_iterations) {
n++;
@@ -179,9 +180,9 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2);
aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2);
aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2);
aA = (aA == 0.0f) ? 1.0f : aA;
aB = (aB == 0.0f) ? 1.0f : aB;
aC = (aC == 0.0f) ? 1.0f : aC;
aA = (fabsf(aA) < FLT_EPSILON) ? 1.0f : aA;
aB = (fabsf(aB) < FLT_EPSILON) ? 1.0f : aB;
aC = (fabsf(aC) < FLT_EPSILON) ? 1.0f : aC;
//Compute next iteration
nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA);
src/modules/commander/commander.cpp
+64 -9
View File
@@ -52,6 +52,7 @@
#include <fcntl.h>
#include <errno.h>
#include <systemlib/err.h>
#include <systemlib/circuit_breaker.h>
#include <debug.h>
#include <sys/prctl.h>
#include <sys/stat.h>
@@ -76,6 +77,7 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/system_power.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/telemetry_status.h>
@@ -371,16 +373,16 @@ void print_status()
static orb_advert_t status_pub;
transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy)
transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char *armedBy)
{
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
// output appropriate error messages if the state cannot transition.
arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd);
arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd_local);
if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
} else if (arming_res == TRANSITION_DENIED) {
tune_negative(true);
@@ -507,7 +509,14 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7);
mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f",
(double)cmd->param1,
(double)cmd->param2,
(double)cmd->param3,
(double)cmd->param4,
(double)cmd->param5,
(double)cmd->param6,
(double)cmd->param7);
}
}
break;
@@ -603,6 +612,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
// XXX TODO
}
return true;
}
int commander_thread_main(int argc, char *argv[])
@@ -699,6 +709,12 @@ int commander_thread_main(int argc, char *argv[])
status.counter++;
status.timestamp = hrt_absolute_time();
status.condition_power_input_valid = true;
status.avionics_power_rail_voltage = -1.0f;
// CIRCUIT BREAKERS
status.circuit_breaker_engaged_power_check = false;
/* publish initial state */
status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
@@ -751,7 +767,6 @@ int commander_thread_main(int argc, char *argv[])
hrt_abstime last_idle_time = 0;
hrt_abstime start_time = 0;
hrt_abstime last_auto_state_valid = 0;
bool status_changed = true;
bool param_init_forced = true;
@@ -845,6 +860,11 @@ int commander_thread_main(int argc, char *argv[])
struct position_setpoint_triplet_s pos_sp_triplet;
memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
/* Subscribe to system power */
int system_power_sub = orb_subscribe(ORB_ID(system_power));
struct system_power_s system_power;
memset(&system_power, 0, sizeof(system_power));
control_status_leds(&status, &armed, true);
/* now initialized */
@@ -861,6 +881,7 @@ int commander_thread_main(int argc, char *argv[])
bool arming_state_changed = false;
bool main_state_changed = false;
bool failsafe_old = false;
bool system_checked = false;
while (!thread_should_exit) {
@@ -902,6 +923,9 @@ int commander_thread_main(int argc, char *argv[])
/* check and update system / component ID */
param_get(_param_system_id, &(status.system_id));
param_get(_param_component_id, &(status.component_id));
status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
status_changed = true;
/* re-check RC calibration */
@@ -914,6 +938,15 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_enable_datalink_loss, &datalink_loss_enabled);
}
/* Perform system checks (again) once params are loaded and MAVLink is up. */
if (!system_checked && mavlink_fd &&
(telemetry.heartbeat_time > 0) &&
(hrt_elapsed_time(&telemetry.heartbeat_time) < 1 * 1000 * 1000)) {
(void)rc_calibration_check(mavlink_fd);
system_checked = true;
}
orb_check(sp_man_sub, &updated);
if (updated) {
@@ -944,6 +977,26 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
}
orb_check(system_power_sub, &updated);
if (updated) {
orb_copy(ORB_ID(system_power), system_power_sub, &system_power);
if (hrt_elapsed_time(&system_power.timestamp) < 200000) {
if (system_power.servo_valid &&
!system_power.brick_valid &&
!system_power.usb_connected) {
/* flying only on servo rail, this is unsafe */
status.condition_power_input_valid = false;
} else {
status.condition_power_input_valid = true;
}
/* copy avionics voltage */
status.avionics_power_rail_voltage = system_power.voltage5V_v;
}
}
check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed);
/* update safety topic */
@@ -1253,12 +1306,13 @@ int commander_thread_main(int argc, char *argv[])
if (status.arming_state == ARMING_STATE_STANDBY &&
sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) {
print_reject_arm("#audio: NOT ARMING: Press safety switch first.");
} else if (status.main_state != MAIN_STATE_MANUAL) {
/* we check outside of the transition function here because the requirement
* for being in manual mode only applies to manual arming actions.
* the system can be armed in auto if armed via the GCS.
*/
if (status.main_state != MAIN_STATE_MANUAL) {
print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first.");
} else {
arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd);
if (arming_ret == TRANSITION_CHANGED) {
@@ -1288,6 +1342,7 @@ int commander_thread_main(int argc, char *argv[])
} else if (arming_ret == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied");
tune_negative(true);
}
/* evaluate the main state machine according to mode switches */
src/modules/commander/commander_helper.cpp
+6
View File
@@ -209,12 +209,18 @@ int led_init()
/* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */
(void)ioctl(leds, LED_ON, LED_BLUE);
/* switch blue off */
led_off(LED_BLUE);
/* we consider the amber led mandatory */
if (ioctl(leds, LED_ON, LED_AMBER)) {
warnx("Amber LED: ioctl fail\n");
return ERROR;
}
/* switch amber off */
led_off(LED_AMBER);
/* then try RGB LEDs, this can fail on FMUv1*/
rgbleds = open(RGBLED_DEVICE_PATH, 0);
src/modules/commander/state_machine_helper.cpp
+149 -20
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
@@ -46,20 +46,32 @@
#include <dirent.h>
#include <fcntl.h>
#include <string.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/differential_pressure.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_airspeed.h>
#include <drivers/drv_device.h>
#include <mavlink/mavlink_log.h>
#include "state_machine_helper.h"
#include "commander_helper.h"
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
static int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd);
// This array defines the arming state transitions. The rows are the new state, and the columns
// are the current state. Using new state and current state you can index into the array which
// will be true for a valid transition or false for a invalid transition. In some cases even
@@ -98,18 +110,31 @@ arming_state_transition(struct vehicle_status_s *status, /// current
ASSERT(ARMING_STATE_INIT == 0);
ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
/*
* Perform an atomic state update
*/
irqstate_t flags = irqsave();
transition_result_t ret = TRANSITION_DENIED;
arming_state_t current_arming_state = status->arming_state;
/* only check transition if the new state is actually different from the current one */
if (new_arming_state == status->arming_state) {
if (new_arming_state == current_arming_state) {
ret = TRANSITION_NOT_CHANGED;
} else {
/*
* Get sensing state if necessary
*/
int prearm_ret = OK;
/* only perform the check if we have to */
if (new_arming_state == ARMING_STATE_ARMED) {
prearm_ret = prearm_check(status, mavlink_fd);
}
/*
* Perform an atomic state update
*/
irqstate_t flags = irqsave();
/* enforce lockdown in HIL */
if (status->hil_state == HIL_STATE_ON) {
armed->lockdown = true;
@@ -124,15 +149,44 @@ arming_state_transition(struct vehicle_status_s *status, /// current
if (valid_transition) {
// We have a good transition. Now perform any secondary validation.
if (new_arming_state == ARMING_STATE_ARMED) {
// Fail transition if we need safety switch press
// Allow if coming from in air restore
// Do not perform pre-arm checks if coming from in air restore
// Allow if HIL_STATE_ON
if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) {
if (mavlink_fd) {
mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch first.");
if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE &&
status->hil_state == HIL_STATE_OFF) {
// Fail transition if pre-arm check fails
if (prearm_ret) {
valid_transition = false;
// Fail transition if we need safety switch press
} else if (safety->safety_switch_available && !safety->safety_off) {
mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch!");
valid_transition = false;
}
// Perform power checks only if circuit breaker is not
// engaged for these checks
if (!status->circuit_breaker_engaged_power_check) {
// Fail transition if power is not good
if (!status->condition_power_input_valid) {
mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module.");
valid_transition = false;
}
// Fail transition if power levels on the avionics rail
// are measured but are insufficient
if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) &&
(status->avionics_power_rail_voltage < 4.9f)) {
mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage);
valid_transition = false;
}
}
valid_transition = false;
}
} else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) {
@@ -157,17 +211,15 @@ arming_state_transition(struct vehicle_status_s *status, /// current
ret = TRANSITION_CHANGED;
status->arming_state = new_arming_state;
}
/* end of atomic state update */
irqrestore(flags);
}
/* end of atomic state update */
irqrestore(flags);
if (ret == TRANSITION_DENIED) {
static const char *errMsg = "Invalid arming transition from %s to %s";
static const char *errMsg = "INVAL: %s - %s";
if (mavlink_fd) {
mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
}
mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]);
}
@@ -539,3 +591,80 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
return status->nav_state != nav_state_old;
}
int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
{
int ret;
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL SENSOR MISSING");
ret = fd;
goto system_eval;
}
ret = ioctl(fd, ACCELIOCSELFTEST, 0);
if (ret != OK) {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION");
goto system_eval;
}
/* check measurement result range */
struct accel_report acc;
ret = read(fd, &acc, sizeof(acc));
if (ret == sizeof(acc)) {
/* evaluate values */
float accel_scale = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
if (accel_scale < 9.78f || accel_scale > 9.83f) {
mavlink_log_info(mavlink_fd, "#audio: Accelerometer calibration recommended.");
}
if (accel_scale > 30.0f /* m/s^2 */) {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE");
/* this is frickin' fatal */
ret = ERROR;
goto system_eval;
} else {
ret = OK;
}
} else {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ");
/* this is frickin' fatal */
ret = ERROR;
goto system_eval;
}
if (!status->is_rotary_wing) {
fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING");
ret = fd;
goto system_eval;
}
struct differential_pressure_s diff_pres;
ret = read(fd, &diff_pres, sizeof(diff_pres));
if (ret == sizeof(diff_pres)) {
if (fabsf(diff_pres.differential_pressure_filtered_pa > 5.0f)) {
mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING");
// XXX do not make this fatal yet
ret = OK;
}
} else {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ");
/* this is frickin' fatal */
ret = ERROR;
goto system_eval;
}
}
system_eval:
close(fd);
return ret;
}
src/modules/dataman/dataman.c
+2 -1
View File
@@ -50,6 +50,7 @@
#include <string.h>
#include "dataman.h"
#include <systemlib/param/param.h>
/**
* data manager app start / stop handling function
@@ -187,7 +188,7 @@ create_work_item(void)
if (item) {
item->first = 1;
lock_queue(&g_free_q);
for (int i = 1; i < k_work_item_allocation_chunk_size; i++) {
for (size_t i = 1; i < k_work_item_allocation_chunk_size; i++) {
(item + i)->first = 0;
sq_addfirst(&(item + i)->link, &(g_free_q.q));
}
src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+7 -7
View File
@@ -2201,7 +2201,7 @@ void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt
posNED[2] = -(hgt - hgtReference);
}
void AttPosEKF::calcLLH(float (&posNED)[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef)
void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef)
{
lat = latRef + (double)posNED[0] * earthRadiusInv;
lon = lonRef + (double)posNED[1] * earthRadiusInv / cos(latRef);
@@ -2831,12 +2831,12 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
// Calculate initial Tbn matrix and rotate Mag measurements into NED
// to set initial NED magnetic field states
Mat3f DCM;
quat2Tbn(DCM, initQuat);
quat2Tbn(Tbn, initQuat);
Tnb = Tbn.transpose();
Vector3f initMagNED;
initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z;
initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z;
initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z;
initMagNED.x = Tbn.x.x*initMagXYZ.x + Tbn.x.y*initMagXYZ.y + Tbn.x.z*initMagXYZ.z;
initMagNED.y = Tbn.y.x*initMagXYZ.x + Tbn.y.y*initMagXYZ.y + Tbn.y.z*initMagXYZ.z;
initMagNED.z = Tbn.z.x*initMagXYZ.x + Tbn.z.y*initMagXYZ.y + Tbn.z.z*initMagXYZ.z;
magstate.q0 = initQuat[0];
magstate.q1 = initQuat[1];
@@ -2849,7 +2849,7 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
magstate.magYbias = magBias.y;
magstate.magZbias = magBias.z;
magstate.R_MAG = sq(magMeasurementSigma);
magstate.DCM = DCM;
magstate.DCM = Tbn;
// write to state vector
for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions
src/modules/ekf_att_pos_estimator/estimator_23states.h
+2 -2
View File
@@ -252,7 +252,7 @@ int RecallStates(float *statesForFusion, uint64_t msec);
void ResetStoredStates();
void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]);
void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]);
void calcEarthRateNED(Vector3f &omega, float latitude);
@@ -264,7 +264,7 @@ static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, flo
void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
static void calcLLH(float (&posNED)[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef);
static void calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef);
static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
src/modules/fw_pos_control_l1/landingslope.cpp
+8 -8
View File
@@ -46,16 +46,16 @@
#include <unistd.h>
#include <mathlib/mathlib.h>
void Landingslope::update(float landing_slope_angle_rad,
float flare_relative_alt,
float motor_lim_relative_alt,
float H1_virt)
void Landingslope::update(float landing_slope_angle_rad_new,
float flare_relative_alt_new,
float motor_lim_relative_alt_new,
float H1_virt_new)
{
_landing_slope_angle_rad = landing_slope_angle_rad;
_flare_relative_alt = flare_relative_alt;
_motor_lim_relative_alt = motor_lim_relative_alt;
_H1_virt = H1_virt;
_landing_slope_angle_rad = landing_slope_angle_rad_new;
_flare_relative_alt = flare_relative_alt_new;
_motor_lim_relative_alt = motor_lim_relative_alt_new;
_H1_virt = H1_virt_new;
calculateSlopeValues();
}
src/modules/fw_pos_control_l1/landingslope.h
+4 -4
View File
@@ -123,10 +123,10 @@ public:
float getFlareCurveAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude);
void update(float landing_slope_angle_rad,
float flare_relative_alt,
float motor_lim_relative_alt,
float H1_virt);
void update(float landing_slope_angle_rad_new,
float flare_relative_alt_new,
float motor_lim_relative_alt_new,
float H1_virt_new);
inline float landing_slope_angle_rad() {return _landing_slope_angle_rad;}
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+12 -7
View File
@@ -58,6 +58,7 @@ mTecs::mTecs() :
_controlEnergyDistribution(this, "PIT", true),
_controlAltitude(this, "FPA", true),
_controlAirSpeed(this, "ACC"),
_flightPathAngleLowpass(this, "FPA_LP"),
_airspeedLowpass(this, "A_LP"),
_airspeedDerivative(this, "AD"),
_throttleSp(0.0f),
@@ -123,7 +124,7 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
/* time measurement */
updateTimeMeasurement();
/* Filter arispeed */
/* Filter airspeed */
float airspeedFiltered = _airspeedLowpass.update(airspeed);
/* calculate longitudinal acceleration setpoint from airspeed setpoint*/
@@ -138,8 +139,6 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
}
/* Write part of the status message */
_status.flightPathAngleSp = flightPathAngleSp;
_status.flightPathAngle = flightPathAngle;
_status.airspeedSp = airspeedSp;
_status.airspeed = airspeed;
_status.airspeedFiltered = airspeedFiltered;
@@ -164,8 +163,11 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
/* update parameters first */
updateParams();
/* Filter flightpathangle */
float flightPathAngleFiltered = _flightPathAngleLowpass.update(flightPathAngle);
/* calculate values (energies) */
float flightPathAngleError = flightPathAngleSp - flightPathAngle;
float flightPathAngleError = flightPathAngleSp - flightPathAngleFiltered;
float airspeedDerivative = 0.0f;
if(_airspeedDerivative.getDt() > 0.0f) {
airspeedDerivative = _airspeedDerivative.update(airspeedFiltered);
@@ -175,12 +177,12 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
float airspeedDerivativeNormSp = airspeedDerivativeSp / CONSTANTS_ONE_G;
float airspeedDerivativeNormError = airspeedDerivativeNormSp - airspeedDerivativeNorm;
float totalEnergyRate = flightPathAngle + airspeedDerivativeNorm;
float totalEnergyRate = flightPathAngleFiltered + airspeedDerivativeNorm;
float totalEnergyRateError = flightPathAngleError + airspeedDerivativeNormError;
float totalEnergyRateSp = flightPathAngleSp + airspeedDerivativeNormSp;
float totalEnergyRateError2 = totalEnergyRateSp - totalEnergyRate;
float energyDistributionRate = flightPathAngle - airspeedDerivativeNorm;
float energyDistributionRate = flightPathAngleFiltered - airspeedDerivativeNorm;
float energyDistributionRateError = flightPathAngleError - airspeedDerivativeNormError;
float energyDistributionRateSp = flightPathAngleSp - airspeedDerivativeNormSp;
float energyDistributionRateError2 = energyDistributionRateSp - energyDistributionRate;
@@ -202,7 +204,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter();
BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter();
if (mode == TECS_MODE_TAKEOFF) {
outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector
outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle;
outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch;
} else if (mode == TECS_MODE_LAND) {
// only limit pitch but do not limit throttle
@@ -221,6 +223,9 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
bool limitApplied = limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
/* Write part of the status message */
_status.flightPathAngleSp = flightPathAngleSp;
_status.flightPathAngle = flightPathAngle;
_status.flightPathAngleFiltered = flightPathAngleFiltered;
_status.airspeedDerivativeSp = airspeedDerivativeSp;
_status.airspeedDerivative = airspeedDerivative;
_status.totalEnergyRateSp = totalEnergyRateSp;
src/modules/fw_pos_control_l1/mtecs/mTecs.h
+20 -14
View File
@@ -104,12 +104,17 @@ protected:
uORB::Publication<tecs_status_s> _status; /**< publish internal values for logging */
/* control blocks */
BlockFFPILimitedCustom _controlTotalEnergy; /**< FFPI controller for total energy control: output is throttle */
BlockFFPILimitedCustom _controlEnergyDistribution; /**< FFPI controller for energy distribution control: output is pitch */
BlockPDLimited _controlAltitude; /**< PD controller for altitude: output is the flight path angle setpoint */
BlockPDLimited _controlAirSpeed; /**< PD controller for airspeed: output is acceleration setpoint */
BlockFFPILimitedCustom _controlTotalEnergy; /**< FFPI controller for total energy control: output
is throttle */
BlockFFPILimitedCustom _controlEnergyDistribution; /**< FFPI controller for energy distribution control:
output is pitch */
BlockPDLimited _controlAltitude; /**< PD controller for altitude: output is the flight
path angle setpoint */
BlockPDLimited _controlAirSpeed; /**< PD controller for airspeed: output is acceleration
setpoint */
/* Other calculation Blocks */
control::BlockLowPass _flightPathAngleLowpass; /**< low pass filter for the flight path angle */
control::BlockLowPass _airspeedLowpass; /**< low pass filter for airspeed */
control::BlockDerivative _airspeedDerivative; /**< airspeed derivative calulation */
@@ -118,21 +123,22 @@ protected:
float _pitchSp; /**< Pitch Setpoint from -pi to pi */
/* Output Limits in special modes */
BlockOutputLimiter _BlockOutputLimiterTakeoffThrottle; /**< Throttle Limits during takeoff */
BlockOutputLimiter _BlockOutputLimiterTakeoffPitch; /**< Pitch Limit during takeoff */
BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle; /**< Throttle Limits when underspeed is detected */
BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch; /**< Pitch Limit when underspeed is detected */
BlockOutputLimiter _BlockOutputLimiterLandThrottle; /**< Throttle Limits during landing (only in last phase)*/
BlockOutputLimiter _BlockOutputLimiterLandPitch; /**< Pitch Limit during landing */
BlockOutputLimiter _BlockOutputLimiterTakeoffThrottle; /**< Throttle Limits during takeoff */
BlockOutputLimiter _BlockOutputLimiterTakeoffPitch; /**< Pitch Limit during takeoff */
BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle; /**< Throttle Limits when underspeed is detected */
BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch; /**< Pitch Limit when underspeed is detected */
BlockOutputLimiter _BlockOutputLimiterLandThrottle; /**< Throttle Limits during landing (only in
last phase)*/
BlockOutputLimiter _BlockOutputLimiterLandPitch; /**< Pitch Limit during landing */
/* Time measurements */
hrt_abstime timestampLastIteration; /**< Saves the result of hrt_absolute_time() of the last iteration */
hrt_abstime timestampLastIteration; /**< Saves the result of hrt_absolute_time() of the last iteration */
bool _firstIterationAfterReset; /**< True during the first iteration after a reset */
bool _dtCalculated; /**< True if dt has been calculated in this iteration */
bool _firstIterationAfterReset; /**< True during the first iteration after a reset */
bool _dtCalculated; /**< True if dt has been calculated in this iteration */
int _counter;
bool _debug; ///< Set true to enable debug output
bool _debug; ///< Set true to enable debug output
static void debug_print(const char *fmt, va_list args);
src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
+7
View File
@@ -174,6 +174,13 @@ PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f);
*/
PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f);
/**
* Lowpass (cutoff freq.) for the flight path angle
*
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_FPA_LP, 1.0f);
/**
* P gain for the altitude control
* Maps the altitude error to the flight path angle setpoint
src/modules/gpio_led/gpio_led.c
+2 -5
View File
@@ -181,16 +181,13 @@ int gpio_led_main(int argc, char *argv[])
} else {
gpio_led_started = true;
warnx("start, using pin: %s", pin_name);
exit(0);
}
exit(0);
} else if (!strcmp(argv[1], "stop")) {
if (gpio_led_started) {
gpio_led_started = false;
warnx("stop");
exit(0);
} else {
errx(1, "not running");
}
src/modules/mavlink/mavlink_ftp.cpp
+42 -42
View File
@@ -127,7 +127,7 @@ MavlinkFTP::_worker(Request *req)
break;
case kCmdTerminate:
errorCode = _workTerminate(req);
errorCode = _workTerminate(req);
break;
case kCmdReset:
@@ -222,12 +222,12 @@ MavlinkFTP::_workList(Request *req)
// no more entries?
if (result == nullptr) {
if (hdr->offset != 0 && offset == 0) {
// User is requesting subsequent dir entries but there were none. This means the user asked
// to seek past EOF.
errorCode = kErrEOF;
}
// Otherwise we are just at the last directory entry, so we leave the errorCode at kErrorNone to signal that
if (hdr->offset != 0 && offset == 0) {
// User is requesting subsequent dir entries but there were none. This means the user asked
// to seek past EOF.
errorCode = kErrEOF;
}
// Otherwise we are just at the last directory entry, so we leave the errorCode at kErrorNone to signal that
break;
}
@@ -266,18 +266,18 @@ MavlinkFTP::_workOpen(Request *req, bool create)
{
auto hdr = req->header();
int session_index = _findUnusedSession();
int session_index = _findUnusedSession();
if (session_index < 0) {
return kErrNoSession;
}
int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
int fd = ::open(req->dataAsCString(), oflag);
int fd = ::open(req->dataAsCString(), oflag);
if (fd < 0) {
return create ? kErrPerm : kErrNotFile;
}
_session_fds[session_index] = fd;
_session_fds[session_index] = fd;
hdr->session = session_index;
hdr->size = 0;
@@ -290,28 +290,28 @@ MavlinkFTP::_workRead(Request *req)
{
auto hdr = req->header();
int session_index = hdr->session;
if (!_validSession(session_index)) {
int session_index = hdr->session;
if (!_validSession(session_index)) {
return kErrNoSession;
}
// Seek to the specified position
printf("Seek %d\n", hdr->offset);
}
// Seek to the specified position
printf("Seek %d\n", hdr->offset);
if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) {
// Unable to see to the specified location
// Unable to see to the specified location
return kErrEOF;
}
int bytes_read = ::read(_session_fds[session_index], &hdr->data[0], kMaxDataLength);
if (bytes_read < 0) {
// Negative return indicates error other than eof
// Negative return indicates error other than eof
return kErrIO;
}
printf("Read success %d\n", bytes_read);
printf("Read success %d\n", bytes_read);
hdr->size = bytes_read;
return kErrNone;
}
@@ -357,26 +357,26 @@ MavlinkFTP::_workTerminate(Request *req)
{
auto hdr = req->header();
if (!_validSession(hdr->session)) {
if (!_validSession(hdr->session)) {
return kErrNoSession;
}
}
::close(_session_fds[hdr->session]);
::close(_session_fds[hdr->session]);
return kErrNone;
return kErrNone;
}
MavlinkFTP::ErrorCode
MavlinkFTP::_workReset(void)
{
for (size_t i=0; i<kMaxSession; i++) {
if (_session_fds[i] != -1) {
::close(_session_fds[i]);
_session_fds[i] = -1;
}
}
return kErrNone;
for (size_t i=0; i<kMaxSession; i++) {
if (_session_fds[i] != -1) {
::close(_session_fds[i]);
_session_fds[i] = -1;
}
}
return kErrNone;
}
bool
@@ -391,13 +391,13 @@ MavlinkFTP::_validSession(unsigned index)
int
MavlinkFTP::_findUnusedSession(void)
{
for (size_t i=0; i<kMaxSession; i++) {
if (_session_fds[i] == -1) {
return i;
}
}
return -1;
for (size_t i=0; i<kMaxSession; i++) {
if (_session_fds[i] == -1) {
return i;
}
}
return -1;
}
char *
src/modules/mavlink/mavlink_ftp.h
+2 -2
View File
@@ -199,8 +199,8 @@ private:
ErrorCode _workRead(Request *req);
ErrorCode _workWrite(Request *req);
ErrorCode _workRemove(Request *req);
ErrorCode _workTerminate(Request *req);
ErrorCode _workReset();
ErrorCode _workTerminate(Request *req);
ErrorCode _workReset();
// work freelist
Request _workBufs[kRequestQueueSize];
src/modules/mavlink/mavlink_main.cpp
+5 -5
View File
@@ -474,7 +474,7 @@ Mavlink::get_instance_id()
return _instance_id;
}
const mavlink_channel_t
mavlink_channel_t
Mavlink::get_channel()
{
return _channel;
@@ -2109,7 +2109,7 @@ Mavlink::task_main(int argc, char *argv[])
write_ptr = (uint8_t*)&msg;
// Pull a single message from the buffer
int read_count = available;
size_t read_count = available;
if (read_count > sizeof(mavlink_message_t)) {
read_count = sizeof(mavlink_message_t);
}
@@ -2266,13 +2266,13 @@ Mavlink::start(int argc, char *argv[])
}
void
Mavlink::status()
Mavlink::display_status()
{
warnx("running");
}
int
Mavlink::stream(int argc, char *argv[])
Mavlink::stream_command(int argc, char *argv[])
{
const char *device_name = DEFAULT_DEVICE_NAME;
float rate = -1.0f;
@@ -2360,7 +2360,7 @@ int mavlink_main(int argc, char *argv[])
// mavlink::g_mavlink->status();
} else if (!strcmp(argv[1], "stream")) {
return Mavlink::stream(argc, argv);
return Mavlink::stream_command(argc, argv);
} else {
usage();
src/modules/mavlink/mavlink_main.h
+8 -8
View File
@@ -123,9 +123,9 @@ public:
/**
* Display the mavlink status.
*/
void status();
void display_status();
static int stream(int argc, char *argv[]);
static int stream_command(int argc, char *argv[]);
static int instance_count();
@@ -213,15 +213,15 @@ public:
*/
int enable_flow_control(bool enabled);
const mavlink_channel_t get_channel();
mavlink_channel_t get_channel();
void configure_stream_threadsafe(const char *stream_name, const float rate);
void configure_stream_threadsafe(const char *stream_name, float rate);
bool _task_should_exit; /**< if true, mavlink task should exit */
int get_mavlink_fd() { return _mavlink_fd; }
MavlinkStream * get_streams() { return _streams; } const
MavlinkStream * get_streams() const { return _streams; }
/* Functions for waiting to start transmission until message received. */
@@ -311,15 +311,15 @@ private:
pthread_mutex_t _message_buffer_mutex;
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _txerr_perf; /**< TX error counter */
bool _param_initialized;
param_t _param_system_id;
param_t _param_component_id;
param_t _param_system_type;
param_t _param_use_hil_gps;
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _txerr_perf; /**< TX error counter */
/**
* Send one parameter.
*
src/modules/mavlink/mavlink_messages.cpp
+1 -2
View File
@@ -45,7 +45,6 @@
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_global_position.h>
@@ -678,7 +677,7 @@ protected:
cm_uint16_from_m_float(gps.epv),
gps.vel_m_s * 100.0f,
_wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
gps.satellites_visible);
gps.satellites_used);
}
}
};
src/modules/mavlink/mavlink_orb_subscription.cpp
+3 -3
View File
@@ -47,10 +47,10 @@
#include "mavlink_orb_subscription.h"
MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) :
_fd(orb_subscribe(_topic)),
_published(false),
next(nullptr),
_topic(topic),
next(nullptr)
_fd(orb_subscribe(_topic)),
_published(false)
{
}
src/modules/mavlink/mavlink_receiver.cpp
+1 -2
View File
@@ -751,9 +751,8 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
hil_gps.vel_ned_valid = true;
hil_gps.cog_rad = _wrap_pi(gps.cog * M_DEG_TO_RAD_F * 1e-2f);
hil_gps.timestamp_satellites = timestamp;
hil_gps.fix_type = gps.fix_type;
hil_gps.satellites_visible = gps.satellites_visible;
hil_gps.satellites_used = gps.satellites_visible; //TODO: rename mavlink_hil_gps_t sats visible to used?
if (_gps_pub < 0) {
_gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
src/modules/mavlink/mavlink_stream.cpp
+5 -1
View File
@@ -43,7 +43,11 @@
#include "mavlink_stream.h"
#include "mavlink_main.h"
MavlinkStream::MavlinkStream() : _interval(1000000), _last_sent(0), _channel(MAVLINK_COMM_0), next(nullptr)
MavlinkStream::MavlinkStream() :
next(nullptr),
_channel(MAVLINK_COMM_0),
_interval(1000000),
_last_sent(0)
{
}
src/modules/mc_att_control/mc_att_control_main.cpp
+13 -4
View File
@@ -72,6 +72,7 @@
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <systemlib/circuit_breaker.h>
#include <lib/mathlib/mathlib.h>
#include <lib/geo/geo.h>
@@ -123,6 +124,8 @@ private:
orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */
orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */
bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */
struct vehicle_attitude_s _v_att; /**< vehicle attitude */
struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */
struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */
@@ -267,6 +270,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_v_rates_sp_pub(-1),
_actuators_0_pub(-1),
_actuators_0_circuit_breaker_enabled(false),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
@@ -402,6 +407,8 @@ MulticopterAttitudeControl::parameters_update()
param_get(_params_handles.acro_yaw_max, &v);
_params.acro_rate_max(2) = math::radians(v);
_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
return OK;
}
@@ -840,11 +847,13 @@ MulticopterAttitudeControl::task_main()
_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
_actuators.timestamp = hrt_absolute_time();
if (_actuators_0_pub > 0) {
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
if (!_actuators_0_circuit_breaker_enabled) {
if (_actuators_0_pub > 0) {
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
} else {
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
} else {
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
}
}
}
}
src/modules/mc_pos_control/mc_pos_control_main.cpp
+2 -3
View File
@@ -466,7 +466,7 @@ MulticopterPositionControl::update_ref()
{
if (_local_pos.ref_timestamp != _ref_timestamp) {
double lat_sp, lon_sp;
float alt_sp;
float alt_sp = 0.0f;
if (_ref_timestamp != 0) {
/* calculate current position setpoint in global frame */
@@ -545,7 +545,6 @@ MulticopterPositionControl::task_main()
hrt_abstime t_prev = 0;
const float alt_ctl_dz = 0.2f;
const float pos_ctl_dz = 0.05f;
math::Vector<3> sp_move_rate;
sp_move_rate.zero();
@@ -862,7 +861,7 @@ MulticopterPositionControl::task_main()
if (_control_mode.flag_control_velocity_enabled) {
/* limit max tilt */
if (thr_min >= 0.0f && tilt_max < M_PI / 2 - 0.05f) {
if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) {
/* absolute horizontal thrust */
float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
src/modules/navigator/mission_block.cpp
+3 -1
View File
@@ -42,6 +42,8 @@
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <math.h>
#include <float.h>
#include <systemlib/err.h>
#include <geo/geo.h>
@@ -222,7 +224,7 @@ MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet
}
if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER
|| pos_sp_triplet->current.loiter_radius != _navigator->get_loiter_radius()
|| (fabsf(pos_sp_triplet->current.loiter_radius - _navigator->get_loiter_radius()) > FLT_EPSILON)
|| pos_sp_triplet->current.loiter_direction != 1
|| pos_sp_triplet->previous.valid
|| !pos_sp_triplet->current.valid
src/modules/navigator/rtl.cpp
+1 -1
View File
@@ -227,7 +227,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
_navigator->set_can_loiter_at_sp(true);
if (autoland) {
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", _mission_item.time_inside);
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", (double)_mission_item.time_inside);
} else {
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter");
src/modules/position_estimator_inav/position_estimator_inav_main.c
+37 -31
View File
@@ -49,6 +49,7 @@
#include <sys/prctl.h>
#include <termios.h>
#include <math.h>
#include <float.h>
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/actuator_controls.h>
@@ -179,15 +180,21 @@ int position_estimator_inav_main(int argc, char *argv[])
exit(1);
}
void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v)
static void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v)
{
FILE *f = fopen("/fs/microsd/inav.log", "a");
if (f) {
char *s = malloc(256);
unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], y_est[0], y_est[1], z_est[0], z_est[1], x_est_prev[0], x_est_prev[1], y_est_prev[0], y_est_prev[1], z_est_prev[0], z_est_prev[1]);
unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n",
hrt_absolute_time(), msg, (double)dt,
(double)x_est[0], (double)x_est[1], (double)y_est[0], (double)y_est[1], (double)z_est[0], (double)z_est[1],
(double)x_est_prev[0], (double)x_est_prev[1], (double)y_est_prev[0], (double)y_est_prev[1], (double)z_est_prev[0], (double)z_est_prev[1]);
fwrite(s, 1, n, f);
n = snprintf(s, 256, "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", acc[0], acc[1], acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v);
n = snprintf(s, 256, "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n",
(double)acc[0], (double)acc[1], (double)acc[2],
(double)corr_gps[0][0], (double)corr_gps[1][0], (double)corr_gps[2][0], (double)corr_gps[0][1], (double)corr_gps[1][1], (double)corr_gps[2][1],
(double)w_xy_gps_p, (double)w_xy_gps_v);
fwrite(s, 1, n, f);
free(s);
}
@@ -261,9 +268,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
hrt_abstime t_prev = 0;
/* acceleration in NED frame */
float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G };
/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */
float acc[] = { 0.0f, 0.0f, 0.0f }; // N E D
float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame
@@ -285,7 +289,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
hrt_abstime flow_prev = 0; // time of last flow measurement
hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered)
hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered)
hrt_abstime xy_src_time = 0; // time of last available position data
bool gps_valid = false; // GPS is valid
bool sonar_valid = false; // sonar is valid
@@ -370,8 +373,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} else {
wait_baro = false;
baro_offset /= (float) baro_init_cnt;
warnx("baro offs: %.2f", baro_offset);
mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset);
warnx("baro offs: %.2f", (double)baro_offset);
mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", (double)baro_offset);
local_pos.z_valid = true;
local_pos.v_z_valid = true;
}
@@ -475,7 +478,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f;
flow_prev = flow.flow_timestamp;
if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && flow.ground_distance_m != sonar_prev) {
if ((flow.ground_distance_m > 0.31f) &&
(flow.ground_distance_m < 4.0f) &&
(att.R[2][2] > 0.7f) &&
(fabsf(flow.ground_distance_m - sonar_prev) > FLT_EPSILON)) {
sonar_time = t;
sonar_prev = flow.ground_distance_m;
corr_sonar = flow.ground_distance_m + surface_offset + z_est[0];
@@ -497,7 +504,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
sonar_valid_time = t;
sonar_valid = true;
local_pos.surface_bottom_timestamp = t;
mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", surface_offset);
mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", (double)surface_offset);
}
} else {
@@ -510,7 +517,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float flow_q = flow.quality / 255.0f;
float dist_bottom = - z_est[0] - surface_offset;
if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7) {
if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7f) {
/* distance to surface */
float flow_dist = dist_bottom / att.R[2][2];
/* check if flow if too large for accurate measurements */
@@ -558,7 +565,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
/* under ideal conditions, on 1m distance assume EPH = 10cm */
eph_flow = 0.1 / w_flow;
eph_flow = 0.1f / w_flow;
flow_valid = true;
@@ -644,25 +651,22 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} else if (t > ref_init_start + ref_init_delay) {
ref_inited = true;
/* update baro offset */
baro_offset -= z_est[0];
/* set position estimate to (0, 0, 0), use GPS velocity for XY */
x_est[0] = 0.0f;
x_est[1] = gps.vel_n_m_s;
y_est[0] = 0.0f;
y_est[1] = gps.vel_e_m_s;
z_est[0] = 0.0f;
local_pos.ref_lat = lat;
local_pos.ref_lon = lon;
local_pos.ref_alt = alt;
local_pos.ref_alt = alt + z_est[0];
local_pos.ref_timestamp = t;
/* initialize projection */
map_projection_init(&ref, lat, lon);
warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt);
mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %.2f", (double)lat, (double)lon, (double)alt);
}
}
@@ -746,10 +750,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* increase EPH/EPV on each step */
if (eph < max_eph_epv) {
eph *= 1.0 + dt;
eph *= 1.0f + dt;
}
if (epv < max_eph_epv) {
epv += 0.005 * dt; // add 1m to EPV each 200s (baro drift)
epv += 0.005f * dt; // add 1m to EPV each 200s (baro drift)
}
/* use GPS if it's valid and reference position initialized */
@@ -758,11 +762,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* use flow if it's valid and (accurate or no GPS available) */
bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);
/* try to estimate position during some time after position sources lost */
if (use_gps_xy || use_flow) {
xy_src_time = t;
}
bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow;
bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout);
@@ -916,6 +915,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
memcpy(x_est_prev, x_est, sizeof(x_est));
memcpy(y_est_prev, y_est, sizeof(y_est));
}
} else {
/* gradually reset xy velocity estimates */
inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_res_v);
inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v);
}
/* detect land */
@@ -931,6 +934,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
landed = false;
landed_time = 0;
}
/* reset xy velocity estimates when landed */
x_est[1] = 0.0f;
y_est[1] = 0.0f;
} else {
if (alt_disp2 < land_disp2 && thrust < params.land_thr) {
@@ -955,11 +961,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float updates_dt = (t - updates_counter_start) * 0.000001f;
warnx(
"updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s",
accel_updates / updates_dt,
baro_updates / updates_dt,
gps_updates / updates_dt,
attitude_updates / updates_dt,
flow_updates / updates_dt);
(double)(accel_updates / updates_dt),
(double)(baro_updates / updates_dt),
(double)(gps_updates / updates_dt),
(double)(attitude_updates / updates_dt),
(double)(flow_updates / updates_dt));
updates_counter_start = t;
accel_updates = 0;
baro_updates = 0;
src/modules/position_estimator_inav/position_estimator_inav_params.c
+3
View File
@@ -46,6 +46,7 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f);
PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f);
PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f);
PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f);
@@ -65,6 +66,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P");
h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V");
h->w_xy_flow = param_find("INAV_W_XY_FLOW");
h->w_xy_res_v = param_find("INAV_W_XY_RES_V");
h->w_gps_flow = param_find("INAV_W_GPS_FLOW");
h->w_acc_bias = param_find("INAV_W_ACC_BIAS");
h->flow_k = param_find("INAV_FLOW_K");
@@ -87,6 +89,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
param_get(h->w_xy_gps_p, &(p->w_xy_gps_p));
param_get(h->w_xy_gps_v, &(p->w_xy_gps_v));
param_get(h->w_xy_flow, &(p->w_xy_flow));
param_get(h->w_xy_res_v, &(p->w_xy_res_v));
param_get(h->w_gps_flow, &(p->w_gps_flow));
param_get(h->w_acc_bias, &(p->w_acc_bias));
param_get(h->flow_k, &(p->flow_k));
src/modules/position_estimator_inav/position_estimator_inav_params.h
+2
View File
@@ -47,6 +47,7 @@ struct position_estimator_inav_params {
float w_xy_gps_p;
float w_xy_gps_v;
float w_xy_flow;
float w_xy_res_v;
float w_gps_flow;
float w_acc_bias;
float flow_k;
@@ -66,6 +67,7 @@ struct position_estimator_inav_param_handles {
param_t w_xy_gps_p;
param_t w_xy_gps_v;
param_t w_xy_flow;
param_t w_xy_res_v;
param_t w_gps_flow;
param_t w_acc_bias;
param_t flow_k;
src/modules/px4iofirmware/controls.c
+2 -2
View File
@@ -47,8 +47,8 @@
#include "px4io.h"
#define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */
#define RC_CHANNEL_HIGH_THRESH 5000
#define RC_CHANNEL_LOW_THRESH -5000
#define RC_CHANNEL_HIGH_THRESH 5000 /* 75% threshold */
#define RC_CHANNEL_LOW_THRESH -8000 /* 10% threshold */
static bool ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len);
src/modules/px4iofirmware/i2c.c
+2
View File
@@ -331,6 +331,7 @@ i2c_tx_complete(void)
i2c_tx_setup();
}
#ifdef DEBUG
static void
i2c_dump(void)
{
@@ -339,3 +340,4 @@ i2c_dump(void)
debug("CCR 0x%08x TRISE 0x%08x", rCCR, rTRISE);
debug("SR1 0x%08x SR2 0x%08x", rSR1, rSR2);
}
#endif
src/modules/px4iofirmware/protocol.h
+1 -1
View File
@@ -312,7 +312,7 @@ struct IOPacket {
#define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK)
#define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK)
#define PKT_SIZE(_p) ((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p)))
#define PKT_SIZE(_p) ((size_t)((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))))
static const uint8_t crc8_tab[256] __attribute__((unused)) =
{
src/modules/px4iofirmware/px4io.c
+2 -3
View File
@@ -37,6 +37,7 @@
*/
#include <nuttx/config.h>
#include <nuttx/arch.h>
#include <stdio.h> // required for task_create
#include <stdbool.h>
@@ -303,14 +304,12 @@ user_start(int argc, char *argv[])
*/
if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {
struct mallinfo minfo = mallinfo();
isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u",
(unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG],
(unsigned)r_status_flags,
(unsigned)r_setup_arming,
(unsigned)r_setup_features,
(unsigned)minfo.mxordblk);
(unsigned)mallinfo().mxordblk);
last_debug_time = hrt_absolute_time();
}
}
src/modules/px4iofirmware/registers.c
+1 -2
View File
@@ -119,7 +119,6 @@ uint16_t r_page_raw_rc_input[] =
[PX4IO_P_RAW_RC_DATA] = 0,
[PX4IO_P_RAW_FRAME_COUNT] = 0,
[PX4IO_P_RAW_LOST_FRAME_COUNT] = 0,
[PX4IO_P_RAW_RC_DATA] = 0,
[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0
};
@@ -670,7 +669,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) {
disabled = true;
} else if ((int)(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
} else if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) {
count++;
}
src/modules/px4iofirmware/sbus.c
+4 -2
View File
@@ -119,13 +119,15 @@ sbus_init(const char *device)
bool
sbus1_output(uint16_t *values, uint16_t num_values)
{
write(sbus_fd, 'A', 1);
char a = 'A';
write(sbus_fd, &a, 1);
}
bool
sbus2_output(uint16_t *values, uint16_t num_values)
{
write(sbus_fd, 'B', 1);
char b = 'B';
write(sbus_fd, &b, 1);
}
bool
src/modules/sdlog2/sdlog2.c
+34 -21
View File
@@ -74,6 +74,7 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/satellite_info.h>
#include <uORB/topics/vehicle_vicon_position.h>
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <uORB/topics/optical_flow.h>
@@ -141,6 +142,8 @@ PARAM_DEFINE_INT32(SDLOG_EXT, -1);
fds[fdsc_count].events = POLLIN; \
fdsc_count++;
#define MIN(X,Y) ((X) < (Y) ? (X) : (Y))
static bool main_thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
@@ -944,6 +947,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct tecs_status_s tecs_status;
struct system_power_s system_power;
struct servorail_status_s servorail_status;
struct satellite_info_s sat_info;
struct wind_estimate_s wind_estimate;
} buf;
@@ -1007,6 +1011,7 @@ int sdlog2_thread_main(int argc, char *argv[])
int global_pos_sub;
int triplet_sub;
int gps_pos_sub;
int sat_info_sub;
int vicon_pos_sub;
int flow_sub;
int rc_sub;
@@ -1026,6 +1031,7 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
subs.status_sub = orb_subscribe(ORB_ID(vehicle_status));
subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info));
subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
@@ -1066,11 +1072,14 @@ int sdlog2_thread_main(int argc, char *argv[])
hrt_abstime barometer_timestamp = 0;
hrt_abstime differential_pressure_timestamp = 0;
/* initialize calculated mean SNR */
float snr_mean = 0.0f;
/* enable logging on start if needed */
if (log_on_start) {
/* check GPS topic to get GPS time */
if (log_name_timestamp) {
if (copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) {
if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) {
gps_time = buf_gps_pos.time_gps_usec;
}
}
@@ -1128,14 +1137,6 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- GPS POSITION - UNIT #1 --- */
if (gps_pos_updated) {
float snr_mean = 0.0f;
for (unsigned i = 0; i < buf_gps_pos.satellites_visible; i++) {
snr_mean += buf_gps_pos.satellite_snr[i];
}
snr_mean /= buf_gps_pos.satellites_visible;
log_msg.msg_type = LOG_GPS_MSG;
log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec;
log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type;
@@ -1148,44 +1149,55 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_GPS.vel_e = buf_gps_pos.vel_e_m_s;
log_msg.body.log_GPS.vel_d = buf_gps_pos.vel_d_m_s;
log_msg.body.log_GPS.cog = buf_gps_pos.cog_rad;
log_msg.body.log_GPS.sats = buf_gps_pos.satellites_visible;
log_msg.body.log_GPS.sats = buf_gps_pos.satellites_used;
log_msg.body.log_GPS.snr_mean = snr_mean;
log_msg.body.log_GPS.noise_per_ms = buf_gps_pos.noise_per_ms;
log_msg.body.log_GPS.jamming_indicator = buf_gps_pos.jamming_indicator;
LOGBUFFER_WRITE_AND_COUNT(GPS);
}
/* --- SATELLITE INFO - UNIT #1 --- */
if (_extended_logging) {
if (copy_if_updated(ORB_ID(satellite_info), subs.sat_info_sub, &buf.sat_info)) {
if (_extended_logging) {
/* log the SNR of each satellite for a detailed view of signal quality */
unsigned gps_msg_max_snr = sizeof(buf_gps_pos.satellite_snr) / sizeof(buf_gps_pos.satellite_snr[0]);
unsigned sat_info_count = MIN(buf.sat_info.count, sizeof(buf.sat_info.snr) / sizeof(buf.sat_info.snr[0]));
unsigned log_max_snr = sizeof(log_msg.body.log_GS0A.satellite_snr) / sizeof(log_msg.body.log_GS0A.satellite_snr[0]);
log_msg.msg_type = LOG_GS0A_MSG;
memset(&log_msg.body.log_GS0A, 0, sizeof(log_msg.body.log_GS0A));
/* fill set A */
for (unsigned i = 0; i < gps_msg_max_snr; i++) {
snr_mean = 0.0f;
int satindex = buf_gps_pos.satellite_prn[i] - 1;
/* fill set A and calculate mean SNR */
for (unsigned i = 0; i < sat_info_count; i++) {
snr_mean += buf.sat_info.snr[i];
int satindex = buf.sat_info.svid[i] - 1;
/* handles index exceeding and wraps to to arithmetic errors */
if ((satindex >= 0) && (satindex < (int)log_max_snr)) {
/* map satellites by their ID so that logs from two receivers can be compared */
log_msg.body.log_GS0A.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i];
log_msg.body.log_GS0A.satellite_snr[satindex] = buf.sat_info.snr[i];
}
}
LOGBUFFER_WRITE_AND_COUNT(GS0A);
snr_mean /= sat_info_count;
log_msg.msg_type = LOG_GS0B_MSG;
memset(&log_msg.body.log_GS0B, 0, sizeof(log_msg.body.log_GS0B));
/* fill set B */
for (unsigned i = 0; i < gps_msg_max_snr; i++) {
for (unsigned i = 0; i < sat_info_count; i++) {
/* get second bank of satellites, thus deduct bank size from index */
int satindex = buf_gps_pos.satellite_prn[i] - 1 - log_max_snr;
int satindex = buf.sat_info.svid[i] - 1 - log_max_snr;
/* handles index exceeding and wraps to to arithmetic errors */
if ((satindex >= 0) && (satindex < (int)log_max_snr)) {
/* map satellites by their ID so that logs from two receivers can be compared */
log_msg.body.log_GS0B.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i];
log_msg.body.log_GS0B.satellite_snr[satindex] = buf.sat_info.snr[i];
}
}
LOGBUFFER_WRITE_AND_COUNT(GS0B);
@@ -1394,8 +1406,8 @@ int sdlog2_thread_main(int argc, char *argv[])
if (copy_if_updated(ORB_ID(rc_channels), subs.rc_sub, &buf.rc)) {
log_msg.msg_type = LOG_RC_MSG;
/* Copy only the first 8 channels of 14 */
memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel));
log_msg.body.log_RC.channel_count = buf.rc.chan_count;
memcpy(log_msg.body.log_RC.channel, buf.rc.channels, sizeof(log_msg.body.log_RC.channel));
log_msg.body.log_RC.channel_count = buf.rc.channel_count;
log_msg.body.log_RC.signal_lost = buf.rc.signal_lost;
LOGBUFFER_WRITE_AND_COUNT(RC);
}
@@ -1514,6 +1526,7 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_TECS.altitude = buf.tecs_status.altitude;
log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp;
log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle;
log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered;
log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp;
log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed;
log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered;
src/modules/sdlog2/sdlog2_messages.h
+2 -1
View File
@@ -345,6 +345,7 @@ struct log_TECS_s {
float altitude;
float flightPathAngleSp;
float flightPathAngle;
float flightPathAngleFiltered;
float airspeedSp;
float airspeed;
float airspeedFiltered;
@@ -440,7 +441,7 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
LOG_FORMAT(TECS, "fffffffffffffB", "AltSP,Alt,FpaSP,Fpa,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
LOG_FORMAT(TECS, "ffffffffffffffB", "AltSP,Alt,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"),
/* system-level messages, ID >= 0x80 */
src/modules/sensors/sensors.cpp
+15 -15
View File
@@ -649,7 +649,7 @@ Sensors::parameters_update()
if (!isfinite(tmpScaleFactor) ||
(tmpRevFactor < 0.000001f) ||
(tmpRevFactor > 0.2f)) {
warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, tmpScaleFactor, (int)(_parameters.rev[i]));
warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, (double)tmpScaleFactor, (int)(_parameters.rev[i]));
/* scaling factors do not make sense, lock them down */
_parameters.scaling_factor[i] = 0.0f;
rc_valid = false;
@@ -1223,9 +1223,9 @@ Sensors::parameter_update_poll(bool forced)
}
#if 0
printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], (int)(_rc.chan[0].scaling_factor * 10000), (int)(_rc.chan[0].mid), (int)_rc.function[0]);
printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], (int)(_rc.chan[1].scaling_factor * 10000), (int)(_rc.chan[1].mid), (int)_rc.function[1]);
printf("MAN: %d %d\n", (int)(_rc.chan[0].scaled * 100), (int)(_rc.chan[1].scaled * 100));
printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], (int)(_rc.channels[0].scaling_factor * 10000), (int)(_rc.channels[0].mid), (int)_rc.function[0]);
printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], (int)(_rc.channels[1].scaling_factor * 10000), (int)(_rc.channels[1].mid), (int)_rc.function[1]);
printf("MAN: %d %d\n", (int)(_rc.channels[0] * 100), (int)(_rc.channels[1] * 100));
fflush(stdout);
usleep(5000);
#endif
@@ -1315,7 +1315,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
* a valid voltage from a connected sensor. Also assume a non-
* zero offset from the sensor if its connected.
*/
if (voltage > 0.4f && _parameters.diff_pres_analog_enabled) {
if (voltage > 0.4f && (_parameters.diff_pres_analog_enabled > 0)) {
float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor
@@ -1355,7 +1355,7 @@ float
Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value)
{
if (_rc.function[func] >= 0) {
float value = _rc.chan[_rc.function[func]].scaled;
float value = _rc.channels[_rc.function[func]];
if (value < min_value) {
return min_value;
@@ -1376,7 +1376,7 @@ switch_pos_t
Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv)
{
if (_rc.function[func] >= 0) {
float value = 0.5f * _rc.chan[_rc.function[func]].scaled + 0.5f;
float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
if (on_inv ? value < on_th : value > on_th) {
return SWITCH_POS_ON;
@@ -1397,7 +1397,7 @@ switch_pos_t
Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv)
{
if (_rc.function[func] >= 0) {
float value = 0.5f * _rc.chan[_rc.function[func]].scaled + 0.5f;
float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
if (on_inv ? value < on_th : value > on_th) {
return SWITCH_POS_ON;
@@ -1489,25 +1489,25 @@ Sensors::rc_poll()
* DO NOT REMOVE OR ALTER STEP 1!
*/
if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) {
_rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]);
_rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]);
} else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) {
_rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] + _parameters.dz[i]) / (float)(_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]);
_rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] + _parameters.dz[i]) / (float)(_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]);
} else {
/* in the configured dead zone, output zero */
_rc.chan[i].scaled = 0.0f;
_rc.channels[i] = 0.0f;
}
_rc.chan[i].scaled *= _parameters.rev[i];
_rc.channels[i] *= _parameters.rev[i];
/* handle any parameter-induced blowups */
if (!isfinite(_rc.chan[i].scaled)) {
_rc.chan[i].scaled = 0.0f;
if (!isfinite(_rc.channels[i])) {
_rc.channels[i] = 0.0f;
}
}
_rc.chan_count = rc_input.channel_count;
_rc.channel_count = rc_input.channel_count;
_rc.rssi = rc_input.rssi;
_rc.signal_lost = signal_lost;
_rc.timestamp = rc_input.timestamp_last_signal;
src/modules/systemlib/circuit_breaker.c
+93
View File
@@ -0,0 +1,93 @@
/****************************************************************************
*
* 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 circuit_breaker.c
*
* Circuit breaker parameters.
* Analog to real aviation circuit breakers these parameters
* allow to disable subsystems. They are not supported as standard
* operation procedure and are only provided for development purposes.
* To ensure they are not activated accidentally, the associated
* parameter needs to set to the key (magic).
*/
#include <systemlib/param/param.h>
#include <systemlib/circuit_breaker.h>
/**
* Circuit breaker for power supply check
*
* Setting this parameter to 894281 will disable the power valid
* checks in the commander.
* WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
*
* @min 0
* @max 894281
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0);
/**
* Circuit breaker for rate controller output
*
* Setting this parameter to 140253 will disable the rate
* controller uORB publication.
* WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
*
* @min 0
* @max 140253
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0);
/**
* Circuit breaker for IO safety
*
* Setting this parameter to 894281 will disable IO safety.
* WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
*
* @min 0
* @max 22027
* @group Circuit Breaker
*/
PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0);
bool circuit_breaker_enabled(const char* breaker, int32_t magic)
{
int32_t val;
(void)param_get(param_find(breaker), &val);
return (val == magic);
}
src/modules/systemlib/circuit_breaker.h
+64
View File
@@ -0,0 +1,64 @@
/****************************************************************************
*
* 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 circuit_breaker.h
*
* Circuit breaker functionality.
*/
#ifndef CIRCUIT_BREAKER_H_
#define CIRCUIT_BREAKER_H_
/* SAFETY WARNING -- SAFETY WARNING -- SAFETY WARNING
*
* OBEY THE DOCUMENTATION FOR ALL CIRCUIT BREAKERS HERE,
* ENSURE TO READ CAREFULLY ALL SAFETY WARNINGS.
* http://pixhawk.org/dev/circuit_breakers
*
* CIRCUIT BREAKERS ARE NOT PART OF THE STANDARD OPERATION PROCEDURE
* AND MAY DISABLE CHECKS THAT ARE VITAL FOR SAFE FLIGHT.
*/
#define CBRK_SUPPLY_CHK_KEY 894281
#define CBRK_RATE_CTRL_KEY 140253
#define CBRK_IO_SAFETY_KEY 22027
#include <stdbool.h>
__BEGIN_DECLS
__EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic);
__END_DECLS
#endif /* CIRCUIT_BREAKER_H_ */
src/modules/systemlib/err.c
+1 -1
View File
@@ -86,7 +86,7 @@ warnerr_core(int errcode, const char *fmt, va_list args)
fprintf(stderr, "\n");
#elif CONFIG_ARCH_LOWPUTC
lowsyslog("%s: ", getprogname());
lowvyslog(fmt, args);
lowvsyslog(fmt, args);
/* convenience as many parts of NuttX use negative errno */
if (errcode < 0)
src/modules/systemlib/hx_stream.c
+1 -1
View File
@@ -63,7 +63,7 @@ struct hx_stream {
/* TX state */
int fd;
bool tx_error;
uint8_t *tx_buf;
const uint8_t *tx_buf;
unsigned tx_resid;
uint32_t tx_crc;
enum {
src/modules/systemlib/mixer/mixer_multirotor.cpp
+1 -3
View File
@@ -208,7 +208,6 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
char geomname[8];
int s[4];
int used;
const char *end = buf + buflen;
/* enforce that the mixer ends with space or a new line */
for (int i = buflen - 1; i >= 0; i--) {
@@ -302,7 +301,6 @@ MultirotorMixer::mix(float *outputs, unsigned space)
//lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3)));
float min_out = 0.0f;
float max_out = 0.0f;
float scale_yaw = 1.0f;
/* perform initial mix pass yielding unbounded outputs, ignore yaw */
for (unsigned i = 0; i < _rotor_count; i++) {
@@ -327,7 +325,7 @@ MultirotorMixer::mix(float *outputs, unsigned space)
}
/* scale down roll/pitch controls if some outputs are negative, don't add yaw, keep total thrust */
if (min_out < 0.0) {
if (min_out < 0.0f) {
float scale_in = thrust / (thrust - min_out);
/* mix again with adjusted controls */
src/modules/systemlib/module.mk
+3 -2
View File
@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (C) 2012 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
@@ -52,5 +52,6 @@ SRCS = err.c \
rc_check.c \
otp.c \
board_serial.c \
pwm_limit/pwm_limit.c
pwm_limit/pwm_limit.c \
circuit_breaker.c
src/modules/systemlib/otp.c
+1 -1
View File
@@ -133,7 +133,7 @@ int lock_otp(void)
// COMPLETE, BUSY, or other flash error?
int F_GetStatus(void)
static int F_GetStatus(void)
{
int fs = F_COMPLETE;
src/modules/systemlib/param/param.c
-2
View File
@@ -96,8 +96,6 @@ ORB_DEFINE(parameter_update, struct parameter_update_s);
/** parameter update topic handle */
static orb_advert_t param_topic = -1;
static sem_t param_sem = { .semcount = 1 };
/** lock the parameter store */
static void
param_lock(void)
src/modules/systemlib/pwm_limit/pwm_limit.c
-1
View File
@@ -97,7 +97,6 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
}
unsigned progress;
uint16_t temp_pwm;
/* then set effective_pwm based on state */
switch (limit->state) {
src/modules/systemlib/rc_check.c
+7 -6
View File
@@ -42,6 +42,7 @@
#include <stdio.h>
#include <fcntl.h>
#include <systemlib/err.h>
#include <systemlib/rc_check.h>
#include <systemlib/param/param.h>
#include <mavlink/mavlink_log.h>
@@ -98,32 +99,32 @@ int rc_calibration_check(int mavlink_fd) {
/* assert min..center..max ordering */
if (param_min < 500) {
count++;
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1);
mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_MIN < 500", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_max > 2500) {
count++;
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1);
mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_MAX > 2500", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_trim < param_min) {
count++;
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN (%d/%d)", i+1, (int)param_trim, (int)param_min);
mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_TRIM < MIN (%d/%d)", i+1, (int)param_trim, (int)param_min);
/* give system time to flush error message in case there are more */
usleep(100000);
}
if (param_trim > param_max) {
count++;
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX (%d/%d)", i+1, (int)param_trim, (int)param_max);
mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_TRIM > MAX (%d/%d)", i+1, (int)param_trim, (int)param_max);
/* give system time to flush error message in case there are more */
usleep(100000);
}
/* assert deadzone is sane */
if (param_dz > 500) {
mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1);
mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_DZ > 500", i+1);
/* give system time to flush error message in case there are more */
usleep(100000);
count++;
@@ -139,7 +140,7 @@ int rc_calibration_check(int mavlink_fd) {
/* sanity checks pass, enable channel */
if (count) {
mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
mavlink_log_critical(mavlink_fd, "#audio ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
warnx("ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
usleep(100000);
}
src/modules/systemlib/systemlib.c
+3
View File
@@ -64,6 +64,9 @@ systemreset(bool to_bootloader)
*(uint32_t *)0x40002850 = 0xb007b007;
}
up_systemreset();
/* lock up here */
while(true);
}
static void kill_task(FAR struct tcb_s *tcb, FAR void *arg);
src/modules/uORB/Subscription.cpp
+2
View File
@@ -40,6 +40,7 @@
#include "topics/parameter_update.h"
#include "topics/actuator_controls.h"
#include "topics/vehicle_gps_position.h"
#include "topics/satellite_info.h"
#include "topics/sensor_combined.h"
#include "topics/vehicle_attitude.h"
#include "topics/vehicle_global_position.h"
@@ -88,6 +89,7 @@ T Subscription<T>::getData() {
template class __EXPORT Subscription<parameter_update_s>;
template class __EXPORT Subscription<actuator_controls_s>;
template class __EXPORT Subscription<vehicle_gps_position_s>;
template class __EXPORT Subscription<satellite_info_s>;
template class __EXPORT Subscription<sensor_combined_s>;
template class __EXPORT Subscription<vehicle_attitude_s>;
template class __EXPORT Subscription<vehicle_global_position_s>;
src/modules/uORB/objects_common.cpp
+3
View File
@@ -75,6 +75,9 @@ ORB_DEFINE(sensor_combined, struct sensor_combined_s);
#include "topics/vehicle_gps_position.h"
ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s);
#include "topics/satellite_info.h"
ORB_DEFINE(satellite_info, struct satellite_info_s);
#include "topics/home_position.h"
ORB_DEFINE(home_position, struct home_position_s);
src/modules/uORB/topics/rc_channels.h
+26 -42
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012, 2013 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
@@ -42,60 +42,44 @@
#include <stdint.h>
#include "../uORB.h"
/**
* The number of RC channel inputs supported.
* Current (Q4/2013) radios support up to 18 channels,
* leaving at a sane value of 16.
* This number can be greater then number of RC channels,
* because single RC channel can be mapped to multiple
* functions, e.g. for various mode switches.
*/
#define RC_CHANNELS_MAPPED_MAX 16
/**
* This defines the mapping of the RC functions.
* The value assigned to the specific function corresponds to the entry of
* the channel array chan[].
* the channel array channels[].
*/
enum RC_CHANNELS_FUNCTION {
THROTTLE = 0,
ROLL = 1,
PITCH = 2,
YAW = 3,
MODE = 4,
RETURN = 5,
POSCTL = 6,
LOITER = 7,
OFFBOARD_MODE = 8,
ACRO = 9,
FLAPS = 10,
AUX_1 = 11,
AUX_2 = 12,
AUX_3 = 13,
AUX_4 = 14,
AUX_5 = 15,
RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */
ROLL,
PITCH,
YAW,
MODE,
RETURN,
POSCTL,
LOITER,
OFFBOARD_MODE,
ACRO,
FLAPS,
AUX_1,
AUX_2,
AUX_3,
AUX_4,
AUX_5,
RC_CHANNELS_FUNCTION_MAX /**< Indicates the number of functions. There can be more functions than RC channels. */
};
/**
* @addtogroup topics
* @{
*/
struct rc_channels_s {
uint64_t timestamp; /**< In microseconds since boot time. */
uint64_t timestamp_last_valid; /**< timestamp of last valid RC signal. */
struct {
float scaled; /**< Scaled to -1..1 (throttle: 0..1) */
} chan[RC_CHANNELS_MAPPED_MAX];
uint8_t chan_count; /**< number of valid channels */
/*String array to store the names of the functions*/
char function_name[RC_CHANNELS_FUNCTION_MAX][20];
int8_t function[RC_CHANNELS_FUNCTION_MAX];
uint8_t rssi; /**< Overall receive signal strength */
bool signal_lost; /**< control signal lost, should be checked together with topic timeout */
uint64_t timestamp; /**< Timestamp in microseconds since boot time */
uint64_t timestamp_last_valid; /**< Timestamp of last valid RC signal */
float channels[RC_CHANNELS_FUNCTION_MAX]; /**< Scaled to -1..1 (throttle: 0..1) */
uint8_t channel_count; /**< Number of valid channels */
char function_name[RC_CHANNELS_FUNCTION_MAX][20]; /**< String array to store the names of the functions */
int8_t function[RC_CHANNELS_FUNCTION_MAX]; /**< Functions mapping */
uint8_t rssi; /**< Receive signal strength index */
bool signal_lost; /**< Control signal lost, should be checked together with topic timeout */
}; /**< radio control channels. */
/**
src/modules/uORB/topics/satellite_info.h
+89
View File
@@ -0,0 +1,89 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
* Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
*
* 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 satellite_info.h
* Definition of the GNSS satellite info uORB topic.
*/
#ifndef TOPIC_SAT_INFO_H_
#define TOPIC_SAT_INFO_H_
#include <stdint.h>
#include "../uORB.h"
/**
* @addtogroup topics
* @{
*/
/**
* GNSS Satellite Info.
*/
#define SAT_INFO_MAX_SATELLITES 20
struct satellite_info_s {
uint64_t timestamp; /**< Timestamp of satellite info */
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. */
};
/**
* NAV_SVINFO space vehicle ID (svid) scheme according to u-blox protocol specs
* u-bloxM8-V15_ReceiverDescriptionProtocolSpec_Public_(UBX-13003221).pdf
*
* GPS 1-32
* SBAS 120-158
* Galileo 211-246
* BeiDou 159-163, 33-64
* QZSS 193-197
* GLONASS 65-96, 255
*
*/
/**
* @}
*/
/* register this as object request broker structure */
ORB_DECLARE(satellite_info);
#endif
src/modules/uORB/topics/tecs_status.h
+1
View File
@@ -68,6 +68,7 @@ struct tecs_status_s {
float altitude;
float flightPathAngleSp;
float flightPathAngle;
float flightPathAngleFiltered;
float airspeedSp;
float airspeed;
float airspeedFiltered;
src/modules/uORB/topics/vehicle_gps_position.h
+1 -8
View File
@@ -82,14 +82,7 @@ struct vehicle_gps_position_s {
uint64_t timestamp_time; /**< Timestamp for time information */
uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module */
uint64_t timestamp_satellites; /**< Timestamp for sattelite information */
uint8_t satellites_visible; /**< Number of satellites visible. If unknown, set to 255 */
uint8_t satellite_prn[20]; /**< Global satellite ID */
uint8_t satellite_used[20]; /**< 0: Satellite not used, 1: used for localization */
uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
uint8_t satellite_snr[20]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */
bool satellite_info_available; /**< 0 for no info, 1 for info available */
uint8_t satellites_used; /**< Number of satellites used */
};
/**
src/modules/uORB/topics/vehicle_status.h
+4
View File
@@ -191,6 +191,8 @@ struct vehicle_status_s {
bool condition_local_altitude_valid;
bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */
bool condition_landed; /**< true if vehicle is landed, always true if disarmed */
bool condition_power_input_valid; /**< set if input power is valid */
float avionics_power_rail_voltage; /**< voltage of the avionics power rail */
bool rc_signal_found_once;
bool rc_signal_lost; /**< true if RC reception lost */
@@ -220,6 +222,8 @@ struct vehicle_status_s {
uint16_t errors_count2;
uint16_t errors_count3;
uint16_t errors_count4;
bool circuit_breaker_engaged_power_check;
};
/**
src/systemcmds/mtd/24xxxx_mtd.c
+3 -1
View File
@@ -237,8 +237,10 @@ void at24c_test(void)
} else if (result != 1) {
vdbg("unexpected %u\n", result);
}
if ((count % 100) == 0)
if ((count % 100) == 0) {
vdbg("test %u errors %u\n", count, errors);
}
}
}
src/systemcmds/mtd/mtd.c
+21 -17
View File
@@ -91,7 +91,7 @@ static void mtd_test(void);
static void mtd_erase(char *partition_names[], unsigned n_partitions);
static void mtd_readtest(char *partition_names[], unsigned n_partitions);
static void mtd_rwtest(char *partition_names[], unsigned n_partitions);
static void mtd_print_info();
static void mtd_print_info(void);
static int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks,
unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions);
@@ -104,6 +104,16 @@ static unsigned n_partitions_current = 0;
static char *partition_names_default[] = {"/fs/mtd_params", "/fs/mtd_waypoints"};
static const int n_partitions_default = sizeof(partition_names_default) / sizeof(partition_names_default[0]);
static void
mtd_status(void)
{
if (!attached)
errx(1, "MTD driver not started");
mtd_print_info();
exit(0);
}
int mtd_main(int argc, char *argv[])
{
if (argc >= 2) {
@@ -193,8 +203,12 @@ ramtron_attach(void)
errx(1, "failed to initialize mtd driver");
int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_SETSPEED, (unsigned long)10*1000*1000);
if (ret != OK)
warnx(1, "failed to set bus speed");
if (ret != OK) {
// FIXME: From the previous warnx call, it looked like this should have been an errx instead. Tried
// that but setting the bug speed does fail all the time. Which was then exiting and the board would
// not run correctly. So changed to warnx.
warnx("failed to set bus speed");
}
attached = true;
}
@@ -351,7 +365,7 @@ static ssize_t mtd_get_partition_size(void)
return partsize;
}
void mtd_print_info()
void mtd_print_info(void)
{
if (!attached)
exit(1);
@@ -381,16 +395,6 @@ mtd_test(void)
exit(1);
}
void
mtd_status(void)
{
if (!attached)
errx(1, "MTD driver not started");
mtd_print_info();
exit(0);
}
void
mtd_erase(char *partition_names[], unsigned n_partitions)
{
@@ -424,7 +428,7 @@ mtd_readtest(char *partition_names[], unsigned n_partitions)
uint8_t v[128];
for (uint8_t i = 0; i < n_partitions; i++) {
uint32_t count = 0;
ssize_t count = 0;
printf("reading %s expecting %u bytes\n", partition_names[i], expected_size);
int fd = open(partition_names[i], O_RDONLY);
if (fd == -1) {
@@ -455,8 +459,8 @@ mtd_rwtest(char *partition_names[], unsigned n_partitions)
uint8_t v[128], v2[128];
for (uint8_t i = 0; i < n_partitions; i++) {
uint32_t count = 0;
off_t offset = 0;
ssize_t count = 0;
off_t offset = 0;
printf("rwtest %s testing %u bytes\n", partition_names[i], expected_size);
int fd = open(partition_names[i], O_RDWR);
if (fd == -1) {
src/systemcmds/nshterm/nshterm.c
+1 -1
View File
@@ -60,7 +60,7 @@ nshterm_main(int argc, char *argv[])
printf("Usage: nshterm <device>\n");
exit(1);
}
uint8_t retries = 0;
unsigned retries = 0;
int fd = -1;
/* try the first 30 seconds */
src/systemcmds/param/param.c
+30 -2
View File
@@ -63,7 +63,8 @@ static void do_show(const char* search_string);
static void do_show_print(void *arg, param_t param);
static void do_set(const char* name, const char* val, bool fail_on_not_found);
static void do_compare(const char* name, const char* vals[], unsigned comparisons);
static void do_reset();
static void do_reset(void);
static void do_reset_nostart(void);
int
param_main(int argc, char *argv[])
@@ -142,6 +143,10 @@ param_main(int argc, char *argv[])
if (!strcmp(argv[1], "reset")) {
do_reset();
}
if (!strcmp(argv[1], "reset_nostart")) {
do_reset_nostart();
}
}
errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'compare', 'select' or 'save'");
@@ -416,7 +421,7 @@ do_compare(const char* name, const char* vals[], unsigned comparisons)
}
static void
do_reset()
do_reset(void)
{
param_reset_all();
@@ -427,3 +432,26 @@ do_reset()
exit(0);
}
}
static void
do_reset_nostart(void)
{
int32_t autostart;
int32_t autoconfig;
(void)param_get(param_find("SYS_AUTOSTART"), &autostart);
(void)param_get(param_find("SYS_AUTOCONFIG"), &autoconfig);
param_reset_all();
(void)param_set(param_find("SYS_AUTOSTART"), &autostart);
(void)param_set(param_find("SYS_AUTOCONFIG"), &autoconfig);
if (param_save_default()) {
warnx("Param export failed.");
exit(1);
} else {
exit(0);
}
}
src/systemcmds/pwm/pwm.c
+2 -1
View File
@@ -515,7 +515,8 @@ pwm_main(int argc, char *argv[])
ret = poll(&fds, 1, 0);
if (ret > 0) {
int ret = read(0, &c, 1);
ret = read(0, &c, 1);
if (ret > 0) {
/* reset output to the last value */
for (unsigned i = 0; i < servo_count; i++) {
src/systemcmds/tests/test_conv.cpp
+1 -1
View File
@@ -65,7 +65,7 @@ int test_conv(int argc, char *argv[])
float f = i/10000.0f;
float fres = REG_TO_FLOAT(FLOAT_TO_REG(f));
if (fabsf(f - fres) > 0.0001f) {
warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", f, REG_TO_SIGNED(FLOAT_TO_REG(f)), fres);
warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", (double)f, REG_TO_SIGNED(FLOAT_TO_REG(f)), (double)fres);
return 1;
}
}
src/systemcmds/tests/test_file.c
+4 -6
View File
@@ -102,7 +102,7 @@ test_file(int argc, char *argv[])
}
/* perform tests for a range of chunk sizes */
unsigned chunk_sizes[] = {1, 5, 8, 13, 16, 32, 33, 64, 70, 128, 133, 256, 300, 512, 555, 1024, 1500};
int chunk_sizes[] = {1, 5, 8, 13, 16, 32, 33, 64, 70, 128, 133, 256, 300, 512, 555, 1024, 1500};
for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) {
@@ -116,7 +116,7 @@ test_file(int argc, char *argv[])
uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64)));
/* fill write buffer with known values */
for (int i = 0; i < sizeof(write_buf); i++) {
for (size_t i = 0; i < sizeof(write_buf); i++) {
/* this will wrap, but we just need a known value with spacing */
write_buf[i] = i+11;
}
@@ -149,6 +149,8 @@ test_file(int argc, char *argv[])
}
end = hrt_absolute_time();
warnx("write took %llu us", (end - start));
close(fd);
fd = open("/fs/microsd/testfile", O_RDONLY);
@@ -192,7 +194,6 @@ test_file(int argc, char *argv[])
warnx("testing aligned writes - please wait.. (CTRL^C to abort)");
start = hrt_absolute_time();
for (unsigned i = 0; i < iterations; i++) {
int wret = write(fd, write_buf, chunk_sizes[c]);
@@ -224,9 +225,6 @@ test_file(int argc, char *argv[])
return 1;
}
/* compare value */
bool compare_ok = true;
for (int j = 0; j < chunk_sizes[c]; j++) {
if (read_buf[j] != write_buf[j]) {
warnx("COMPARISON ERROR: byte %d: %u != %u", j, (unsigned int)read_buf[j], (unsigned int)write_buf[j]);
src/systemcmds/tests/test_file2.c
+6 -4
View File
@@ -49,6 +49,8 @@
#include <stdlib.h>
#include <getopt.h>
#include "tests.h"
#define FLAG_FSYNC 1
#define FLAG_LSEEK 2
@@ -85,9 +87,9 @@ static void test_corruption(const char *filename, uint32_t write_chunk, uint32_t
buffer[j] = get_value(ofs);
ofs++;
}
if (write(fd, buffer, sizeof(buffer)) != sizeof(buffer)) {
printf("write failed at offset %u\n", ofs);
exit(1);
if (write(fd, buffer, sizeof(buffer)) != (int)sizeof(buffer)) {
printf("write failed at offset %u\n", ofs);
exit(1);
}
if (flags & FLAG_FSYNC) {
fsync(fd);
@@ -116,7 +118,7 @@ static void test_corruption(const char *filename, uint32_t write_chunk, uint32_t
printf("read ofs=%u\r", ofs);
}
counter++;
if (read(fd, buffer, sizeof(buffer)) != sizeof(buffer)) {
if (read(fd, buffer, sizeof(buffer)) != (int)sizeof(buffer)) {
printf("read failed at offset %u\n", ofs);
exit(1);
}
src/systemcmds/tests/test_float.c
+12 -12
View File
@@ -68,7 +68,7 @@ int test_float(int argc, char *argv[])
float sinf_one = sinf(1.0f);
float sqrt_two = sqrt(2.0f);
if (sinf_zero == 0.0f) {
if (fabsf(sinf_zero) < FLT_EPSILON) {
printf("\t success: sinf(0.0f) == 0.0f\n");
} else {
@@ -94,7 +94,7 @@ int test_float(int argc, char *argv[])
printf("\t success: asinf(1.0f) == 1.57079f\n");
} else {
printf("\t FAIL: asinf(1.0f) != 1.57079f, result: %f\n", asinf_one);
printf("\t FAIL: asinf(1.0f) != 1.57079f, result: %f\n", (double)asinf_one);
ret = -1;
}
@@ -128,7 +128,7 @@ int test_float(int argc, char *argv[])
float sinf_zero_one = sinf(0.1f);
if (fabs(sinf_zero_one - 0.0998334166f) < FLT_EPSILON) {
if (fabsf(sinf_zero_one - 0.0998334166f) < FLT_EPSILON) {
printf("\t success: sinf(0.1f) == 0.09983f\n");
} else {
@@ -136,7 +136,7 @@ int test_float(int argc, char *argv[])
ret = -2;
}
if (sqrt_two == 1.41421356f) {
if (fabsf(sqrt_two - 1.41421356f) < FLT_EPSILON) {
printf("\t success: sqrt(2.0f) == 1.41421f\n");
} else {
@@ -155,7 +155,7 @@ int test_float(int argc, char *argv[])
}
char sbuf[30];
sprintf(sbuf, "%8.4f", 0.553415f);
sprintf(sbuf, "%8.4f", (double)0.553415f);
if (sbuf[0] == ' ' && sbuf[1] == ' ' && sbuf[2] == '0' &&
sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
@@ -166,7 +166,7 @@ int test_float(int argc, char *argv[])
ret = -5;
}
sprintf(sbuf, "%8.4f", -0.553415f);
sprintf(sbuf, "%8.4f", (double)-0.553415f);
if (sbuf[0] == ' ' && sbuf[1] == '-' && sbuf[2] == '0' &&
sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5'
@@ -188,7 +188,7 @@ int test_float(int argc, char *argv[])
double d1d2 = d1 * d2;
if (d1d2 == 2.022200000000000219557705349871) {
if (fabs(d1d2 - 2.022200000000000219557705349871) < DBL_EPSILON) {
printf("\t success: 1.0111 * 2.0 == 2.0222\n");
} else {
@@ -201,11 +201,11 @@ int test_float(int argc, char *argv[])
// Assign value of f1 to d1
d1 = f1;
if (f1 == (float)d1) {
if (fabsf(f1 - (float)d1) < FLT_EPSILON) {
printf("\t success: (float) 1.55f == 1.55 (double)\n");
} else {
printf("\t FAIL: (float) 1.55f != 1.55 (double), result: %8.4f\n", f1);
printf("\t FAIL: (float) 1.55f != 1.55 (double), result: %8.4f\n", (double)f1);
ret = -8;
}
@@ -216,7 +216,7 @@ int test_float(int argc, char *argv[])
double sin_one = sin(1.0);
double atan2_ones = atan2(1.0, 1.0);
if (sin_zero == 0.0) {
if (fabs(sin_zero - 0.0) < DBL_EPSILON) {
printf("\t success: sin(0.0) == 0.0\n");
} else {
@@ -224,7 +224,7 @@ int test_float(int argc, char *argv[])
ret = -9;
}
if (sin_one == 0.841470984807896504875657228695) {
if (fabs(sin_one - 0.841470984807896504875657228695) < DBL_EPSILON) {
printf("\t success: sin(1.0) == 0.84147098480\n");
} else {
@@ -232,7 +232,7 @@ int test_float(int argc, char *argv[])
ret = -10;
}
if (atan2_ones != 0.785398) {
if (fabs(atan2_ones - 0.785398) < DBL_EPSILON) {
printf("\t success: atan2(1.0, 1.0) == 0.785398\n");
} else {
src/systemcmds/tests/test_mathlib.cpp
-4
View File
@@ -52,10 +52,6 @@
using namespace math;
const char* formatResult(bool res) {
return res ? "OK" : "ERROR";
}
int test_mathlib(int argc, char *argv[])
{
warnx("testing mathlib");
src/systemcmds/tests/test_mixer.cpp
+11 -23
View File
@@ -81,7 +81,7 @@ int test_mixer(int argc, char *argv[])
warnx("testing mixer");
char *filename = "/etc/mixers/IO_pass.mix";
const char *filename = "/etc/mixers/IO_pass.mix";
if (argc > 1)
filename = argv[1];
@@ -100,8 +100,6 @@ int test_mixer(int argc, char *argv[])
* e.g. on PX4IO.
*/
unsigned nused = 0;
const unsigned chunk_size = 64;
MixerGroup mixer_group(mixer_callback, 0);
@@ -124,7 +122,6 @@ int test_mixer(int argc, char *argv[])
return 1;
/* FIRST mark the mixer as invalid */
bool mixer_ok = false;
/* THEN actually delete it */
mixer_group.reset();
char mixer_text[256]; /* large enough for one mixer */
@@ -140,7 +137,6 @@ int test_mixer(int argc, char *argv[])
/* check for overflow - this would be really fatal */
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
bool mixer_ok = false;
return 1;
}
@@ -156,15 +152,6 @@ int test_mixer(int argc, char *argv[])
/* if anything was parsed */
if (resid != mixer_text_length) {
/* only set mixer ok if no residual is left over */
if (resid == 0) {
mixer_ok = true;
} else {
/* not yet reached the end of the mixer, set as not ok */
mixer_ok = false;
}
warnx("used %u", mixer_text_length - resid);
/* copy any leftover text to the base of the buffer for re-use */
@@ -192,7 +179,7 @@ int test_mixer(int argc, char *argv[])
should_arm = true;
/* run through arming phase */
for (int i = 0; i < output_max; i++) {
for (unsigned i = 0; i < output_max; i++) {
actuator_controls[i] = 0.1f;
r_page_servo_disarmed[i] = PWM_LOWEST_MIN;
r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
@@ -213,7 +200,7 @@ int test_mixer(int argc, char *argv[])
pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (int i = 0; i < mixed; i++)
for (unsigned i = 0; i < mixed; i++)
{
/* check mixed outputs to be zero during init phase */
if (hrt_elapsed_time(&starttime) < INIT_TIME_US &&
@@ -228,7 +215,7 @@ int test_mixer(int argc, char *argv[])
return 1;
}
//printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]);
//printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, (double)outputs_unlimited[i], (double)outputs[i], (int)r_page_servos[i]);
}
usleep(sleep_quantum_us);
sleepcount++;
@@ -244,7 +231,7 @@ int test_mixer(int argc, char *argv[])
for (int j = -jmax; j <= jmax; j++) {
for (int i = 0; i < output_max; i++) {
for (unsigned i = 0; i < output_max; i++) {
actuator_controls[i] = j/10.0f + 0.1f * i;
r_page_servo_disarmed[i] = PWM_LOWEST_MIN;
r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
@@ -257,11 +244,11 @@ int test_mixer(int argc, char *argv[])
pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
warnx("mixed %d outputs (max %d)", mixed, output_max);
for (int i = 0; i < mixed; i++)
for (unsigned i = 0; i < mixed; i++)
{
servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
if (fabsf(servo_predicted[i] - r_page_servos[i]) > 2) {
printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]);
printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]);
warnx("mixer violated predicted value");
return 1;
}
@@ -282,7 +269,7 @@ int test_mixer(int argc, char *argv[])
pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (int i = 0; i < mixed; i++)
for (unsigned i = 0; i < mixed; i++)
{
/* check mixed outputs to be zero during init phase */
if (r_page_servos[i] != r_page_servo_disarmed[i]) {
@@ -316,7 +303,7 @@ int test_mixer(int argc, char *argv[])
pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
for (int i = 0; i < mixed; i++)
for (unsigned i = 0; i < mixed; i++)
{
/* predict value */
servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
@@ -333,7 +320,7 @@ int test_mixer(int argc, char *argv[])
/* check post ramp phase */
if (hrt_elapsed_time(&starttime) > RAMP_TIME_US &&
fabsf(servo_predicted[i] - r_page_servos[i]) > 2) {
printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]);
printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]);
warnx("mixer violated predicted value");
return 1;
}
@@ -372,6 +359,7 @@ int test_mixer(int argc, char *argv[])
}
warnx("SUCCESS: No errors in mixer test");
return 0;
}
static int
src/systemcmds/tests/test_mtd.c
+2
View File
@@ -171,6 +171,8 @@ test_mtd(int argc, char *argv[])
}
end = hrt_absolute_time();
warnx("write took %llu us", (end - start));
close(fd);
fd = open(PARAM_FILE_NAME, O_RDONLY);
src/systemcmds/tests/test_ppm_loopback.c
-1
View File
@@ -65,7 +65,6 @@ int test_ppm_loopback(int argc, char *argv[])
int _rc_sub = orb_subscribe(ORB_ID(input_rc));
int servo_fd, result;
servo_position_t data[PWM_OUTPUT_MAX_CHANNELS];
servo_position_t pos;
servo_fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR);
src/systemcmds/tests/test_rc.c
+1
View File
@@ -52,6 +52,7 @@
#include <arch/board/board.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_hrt.h>
#include <systemlib/err.h>
#include "tests.h"

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