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

Merge remote-tracking branch 'upstream/master' into linux

Browse Source 
Signed-off-by: Mark Charlebois <charlebm@gmail.com>

Conflicts:
	src/drivers/rgbled/rgbled.cpp
	src/modules/commander/PreflightCheck.cpp
	src/modules/commander/airspeed_calibration.cpp
	src/modules/commander/calibration_routines.cpp
	src/modules/commander/gyro_calibration.cpp
	src/modules/commander/mag_calibration.cpp
	src/modules/mc_att_control/mc_att_control_main.cpp
This commit is contained in:
Mark Charlebois 2015-04-28 11:48:26 -07:00
commit 190814bc97
47 changed files with 935 additions and 656 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
NuttX

@ -1 +1 @@
Subproject commit 94818ce16f63c4728a1d9316628a3651287f16df
Subproject commit 678eea3d2c157c686439597abb0367b0f1e643f4

4
ROMFS/px4fmu_common/init.d/13002_firefly6
View File

@ -8,7 +8,7 @@
sh /etc/init.d/rc.vtol_defaults
set MIXER firefly6
set PWM_OUT 123456
set PWM_OUT 12345678
set MIXER_AUX firefly6
set PWM_AUX_RATE 50
@ -17,5 +17,7 @@ set PWM_AUX_DISARMED 1000
set PWM_AUX_MIN 1000
set PWM_AUX_MAX 2000
set MAV_TYPE 21
param set VT_MOT_COUNT 6
param set VT_IDLE_PWM_MC 1080

7
ROMFS/px4fmu_common/init.d/rc.io
View File

@ -16,5 +16,8 @@ then
set PX4IO_LIMIT 200
fi
echo "[i] Set PX4IO update rate limit: $PX4IO_LIMIT Hz"
px4io limit $PX4IO_LIMIT
if px4io limit $PX4IO_LIMIT
then
else
echo "[i] Set PX4IO update rate to $PX4IO_LIMIT Hz failed!"
fi

2
ROMFS/px4fmu_common/init.d/rc.usb
View File

@ -3,7 +3,7 @@
# USB MAVLink start
#
mavlink start -r 20000 -d /dev/ttyACM0 -x
mavlink start -r 30000 -d /dev/ttyACM0 -x
# Enable a number of interesting streams we want via USB
mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 200
mavlink stream -d /dev/ttyACM0 -s MISSION_ITEM -r 50

20
ROMFS/px4fmu_common/init.d/rcS
View File

@ -24,7 +24,6 @@ set LOG_FILE /fs/microsd/bootlog.txt
# Try to mount the microSD card.
#
# REBOOTWORK this needs to start after the flight control loop
echo "[i] Looking for microSD..."
if mount -t vfat /dev/mmcsd0 /fs/microsd
then
echo "[i] microSD mounted: /fs/microsd"
@ -53,11 +52,9 @@ fi
#
if [ -f $FRC ]
then
echo "[i] Executing init script: $FRC"
echo "[i] Executing script: $FRC"
sh $FRC
set MODE custom
else
echo "[i] Init script not found: $FRC"
fi
unset FRC
@ -126,6 +123,7 @@ then
set PWM_AUX_DISARMED none
set PWM_AUX_MIN none
set PWM_AUX_MAX none
set FAILSAFE_AUX none
set MK_MODE none
set FMU_MODE pwm
set MAVLINK_F default
@ -174,10 +172,8 @@ then
#
if [ -f $FCONFIG ]
then
echo "[i] Config: $FCONFIG"
echo "[i] Custom config: $FCONFIG"
sh $FCONFIG
else
echo "[i] Config not found: $FCONFIG"
fi
unset FCONFIG
@ -330,10 +326,8 @@ then
then
if px4io start
then
echo "[i] PX4IO started"
sh /etc/init.d/rc.io
else
echo "[i] ERROR: PX4IO start failed"
tone_alarm $TUNE_ERR
fi
fi
@ -461,10 +455,16 @@ then
#
if ver hwcmp PX4FMU_V2
then
# XXX We need a better way for runtime eval of shell variables,
# but this works for now
if param compare SYS_COMPANION 921600
then
mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 20000
fi
if param compare SYS_COMPANION 57600
then
mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 1000
fi
fi
# UAVCAN
@ -675,8 +675,6 @@ then
then
echo "[i] Addons script: $FEXTRAS"
sh $FEXTRAS
else
echo "[i] No addons script: $FEXTRAS"
fi
unset FEXTRAS

6
msg/vehicle_status.msg
View File

@ -72,8 +72,10 @@ uint8 VEHICLE_TYPE_FLAPPING_WING = 16 # Flapping wing
uint8 VEHICLE_TYPE_KITE = 17 # Kite
uint8 VEHICLE_TYPE_ONBOARD_CONTROLLER=18 # Onboard companion controller
uint8 VEHICLE_TYPE_VTOL_DUOROTOR = 19 # Vtol with two engines
uint8 VEHICLE_TYPE_VTOL_QUADROTOR = 20 # Vtol with four engines*/
uint8 VEHICLE_TYPE_ENUM_END = 21
uint8 VEHICLE_TYPE_VTOL_QUADROTOR = 20 # Vtol with four engines
uint8 VEHICLE_TYPE_VTOL_HEXAROTOR = 21 # Vtol with six engines
uint8 VEHICLE_TYPE_VTOL_OCTOROTOR = 22 # Vtol with eight engines
uint8 VEHICLE_TYPE_ENUM_END = 23
uint8 VEHICLE_BATTERY_WARNING_NONE = 0 # no battery low voltage warning active
uint8 VEHICLE_BATTERY_WARNING_LOW = 1 # warning of low voltage

2
nuttx-configs/aerocore/nsh/defconfig
View File

@ -403,7 +403,7 @@ CONFIG_SIG_SIGWORK=4
CONFIG_MAX_TASKS=32
CONFIG_MAX_TASK_ARGS=10
CONFIG_NPTHREAD_KEYS=4
CONFIG_NFILE_DESCRIPTORS=36
CONFIG_NFILE_DESCRIPTORS=42
CONFIG_NFILE_STREAMS=8
CONFIG_NAME_MAX=32
CONFIG_PREALLOC_MQ_MSGS=4

4
nuttx-configs/px4fmu-v1/nsh/defconfig
View File

@ -326,7 +326,7 @@ CONFIG_DRAM_START=0x20000000
CONFIG_DRAM_SIZE=196608
CONFIG_ARCH_HAVE_INTERRUPTSTACK=y
# The actual usage is 420 bytes
CONFIG_ARCH_INTERRUPTSTACK=1500
CONFIG_ARCH_INTERRUPTSTACK=750
#
# Boot options
@ -405,7 +405,7 @@ CONFIG_SIG_SIGWORK=4
CONFIG_MAX_TASKS=32
CONFIG_MAX_TASK_ARGS=10
CONFIG_NPTHREAD_KEYS=4
CONFIG_NFILE_DESCRIPTORS=44
CONFIG_NFILE_DESCRIPTORS=42
CONFIG_NFILE_STREAMS=8
CONFIG_NAME_MAX=32
CONFIG_PREALLOC_MQ_MSGS=4

2
nuttx-configs/px4fmu-v2/nsh/defconfig
View File

@ -439,7 +439,7 @@ CONFIG_SIG_SIGWORK=4
CONFIG_MAX_TASKS=32
CONFIG_MAX_TASK_ARGS=10
CONFIG_NPTHREAD_KEYS=4
CONFIG_NFILE_DESCRIPTORS=44
CONFIG_NFILE_DESCRIPTORS=42
CONFIG_NFILE_STREAMS=8
CONFIG_NAME_MAX=32
CONFIG_PREALLOC_MQ_MSGS=4

3
src/drivers/blinkm/blinkm_nuttx.cpp
View File

@ -316,7 +316,6 @@ BlinkM::init()
ret = I2C::init();
if (ret != OK) {
warnx("I2C init failed");
return ret;
}
@ -970,7 +969,7 @@ blinkm_main(int argc, char *argv[])
if (OK != g_blinkm->init()) {
delete g_blinkm;
g_blinkm = nullptr;
errx(1, "init failed");
errx(1, "no BlinkM found");
}
exit(0);

7
src/drivers/boards/px4fmu-v1/px4fmu_init.c
View File

@ -216,8 +216,6 @@ __EXPORT int nsh_archinitialize(void)
SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
up_udelay(20);
message("[boot] Successfully initialized SPI port 1\r\n");
/*
* If SPI2 is enabled in the defconfig, we loose some ADC pins as chip selects.
* Keep the SPI2 init optional and conditionally initialize the ADC pins
@ -243,7 +241,6 @@ __EXPORT int nsh_archinitialize(void)
/* Get the SPI port for the microSD slot */
message("[boot] Initializing SPI port 3\n");
spi3 = up_spiinitialize(3);
if (!spi3) {
@ -252,8 +249,6 @@ __EXPORT int nsh_archinitialize(void)
return -ENODEV;
}
message("[boot] Successfully initialized SPI port 3\n");
/* Now bind the SPI interface to the MMCSD driver */
result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3);
@ -263,7 +258,5 @@ __EXPORT int nsh_archinitialize(void)
return -ENODEV;
}
message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
return OK;
}

7
src/drivers/boards/px4fmu-v2/px4fmu2_init.c
View File

@ -296,8 +296,6 @@ __EXPORT int nsh_archinitialize(void)
SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
up_udelay(20);
message("[boot] Initialized SPI port 1 (SENSORS)\n");
/* Get the SPI port for the FRAM */
spi2 = up_spiinitialize(2);
@ -317,8 +315,6 @@ __EXPORT int nsh_archinitialize(void)
SPI_SETMODE(spi2, SPIDEV_MODE3);
SPI_SELECT(spi2, SPIDEV_FLASH, false);
message("[boot] Initialized SPI port 2 (RAMTRON FRAM)\n");
spi4 = up_spiinitialize(4);
/* Default SPI4 to 1MHz and de-assert the known chip selects. */
@ -328,8 +324,6 @@ __EXPORT int nsh_archinitialize(void)
SPI_SELECT(spi4, PX4_SPIDEV_EXT0, false);
SPI_SELECT(spi4, PX4_SPIDEV_EXT1, false);
message("[boot] Initialized SPI port 4\n");
#ifdef CONFIG_MMCSD
/* First, get an instance of the SDIO interface */
@ -350,7 +344,6 @@ __EXPORT int nsh_archinitialize(void)
/* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */
sdio_mediachange(sdio, true);
message("[boot] Initialized SDIO\n");
#endif
return OK;

5
src/drivers/hmc5883/hmc5883.cpp
View File

@ -1490,8 +1490,9 @@ start(enum HMC5883_BUS busid, enum Rotation rotation)
started |= start_bus(bus_options[i], rotation);
}
if (!started)
errx(1, "driver start failed");
if (!started) {
exit(1);
}
}
/**

2
src/drivers/px4flow/px4flow.cpp
View File

@ -674,7 +674,7 @@ fail:
g_dev = nullptr;
}
errx(1, "driver start failed");
errx(1, "no PX4 FLOW connected");
}
/**

4
src/drivers/rgbled/rgbled.cpp
View File

@ -638,7 +638,7 @@ rgbled_main(int argc, char *argv[])
if (g_rgbled == nullptr) {
// fall back to default bus
if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) {
warnx("init failed");
warnx("no RGB led on bus #%d", i2cdevice);
return 1;
}
i2cdevice = PX4_I2C_BUS_LED;
@ -656,7 +656,7 @@ rgbled_main(int argc, char *argv[])
if (OK != g_rgbled->init()) {
delete g_rgbled;
g_rgbled = nullptr;
warnx("init failed");
warnx("no RGB led on bus #%d", i2cdevice);
return 1;
}
}

4
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
View File

@ -465,8 +465,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
math::Vector<3> v(1.0f, 0.0f, 0.4f);
math::Vector<3> vn = Rvis * v;
math::Vector<3> vn = Rvis.transposed() * v; //Rvis is Rwr (robot respect to world) while v is respect to world. Hence Rvis must be transposed having (Rwr)' * Vw
// Rrw * Vw = vn. This way we have consistency
z_k[6] = vn(0);
z_k[7] = vn(1);
z_k[8] = vn(2);

12
src/modules/commander/PreflightCheck.cpp
View File

@ -94,7 +94,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
if (devid != calibration_devid) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: MAG #%u UNCALIBRATED (NO ID)", instance);
"PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance);
success = false;
goto out;
}
@ -138,7 +138,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
if (devid != calibration_devid) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED (NO ID)", instance);
"PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
success = false;
goto out;
}
@ -162,7 +162,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still");
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming");
/* this is frickin' fatal */
success = false;
goto out;
@ -205,7 +205,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional)
if (devid != calibration_devid) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: GYRO #%u UNCALIBRATED (NO ID)", instance);
"PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
success = false;
goto out;
}
@ -255,13 +255,13 @@ static bool airspeedCheck(int mavlink_fd, bool optional)
if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
(hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) {
mavlink_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
success = false;
goto out;
}
if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) {
mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
// XXX do not make this fatal yet
}

87
src/modules/commander/accelerometer_calibration.cpp
View File

@ -153,11 +153,10 @@ static const int ERROR = -1;
static const char *sensor_name = "accel";
int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors);
int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num);
calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors);
calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num);
int mat_invert3(float src[3][3], float dst[3][3]);
int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g);
int accel_calibration_worker(detect_orientation_return orientation, void* worker_data);
calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g);
/// Data passed to calibration worker routine
typedef struct {
@ -172,7 +171,7 @@ int do_accel_calibration(int mavlink_fd)
int fd;
int32_t device_id[max_accel_sens];
mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
struct accel_scale accel_scale = {
0.0f,
@ -203,7 +202,7 @@ int do_accel_calibration(int mavlink_fd)
px4_close(fd);
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, s);
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s);
}
}
@ -211,13 +210,23 @@ int do_accel_calibration(int mavlink_fd)
float accel_T[max_accel_sens][3][3];
unsigned active_sensors;
/* measure and calculate offsets & scales */
if (res == OK) {
/* measure and calculate offsets & scales */
res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T, &active_sensors);
calibrate_return cal_return = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T, &active_sensors);
if (cal_return == calibrate_return_cancelled) {
// Cancel message already displayed, nothing left to do
return ERROR;
} else if (cal_return == calibrate_return_ok) {
res = OK;
} else {
res = ERROR;
}
}
if (res != OK || active_sensors == 0) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
if (res != OK) {
if (active_sensors == 0) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SENSOR_MSG);
}
return ERROR;
}
@ -264,7 +273,7 @@ int do_accel_calibration(int mavlink_fd)
failed |= (OK != param_set_no_notification(param_find(str), &(device_id[i])));
if (failed) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG, i);
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, i);
return ERROR;
}
@ -272,7 +281,7 @@ int do_accel_calibration(int mavlink_fd)
fd = px4_open(str, 0);
if (fd < 0) {
mavlink_and_console_log_critical(mavlink_fd, "sensor does not exist");
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "sensor does not exist");
res = ERROR;
} else {
res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
@ -280,7 +289,7 @@ int do_accel_calibration(int mavlink_fd)
}
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG, i);
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, i);
}
}
@ -289,41 +298,47 @@ int do_accel_calibration(int mavlink_fd)
res = param_save_default();
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
}
mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
/* if there is a any preflight-check system response, let the barrage of messages through */
usleep(200000);
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
} else {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
}
/* give this message enough time to propagate */
usleep(600000);
return res;
}
int accel_calibration_worker(detect_orientation_return orientation, void* data)
static calibrate_return accel_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
{
const unsigned samples_num = 3000;
accel_worker_data_t* worker_data = (accel_worker_data_t*)(data);
mavlink_and_console_log_info(worker_data->mavlink_fd, "Hold still, starting to measure %s side", detect_orientation_str(orientation));
mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Hold still, measuring %s side", detect_orientation_str(orientation));
read_accelerometer_avg(worker_data->subs, worker_data->accel_ref, orientation, samples_num);
mavlink_and_console_log_info(worker_data->mavlink_fd, "%s side result: [ %8.4f %8.4f %8.4f ]", detect_orientation_str(orientation),
mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side result: [%8.4f %8.4f %8.4f]", detect_orientation_str(orientation),
(double)worker_data->accel_ref[0][orientation][0],
(double)worker_data->accel_ref[0][orientation][1],
(double)worker_data->accel_ref[0][orientation][2]);
worker_data->done_count++;
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * worker_data->done_count);
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, 17 * worker_data->done_count);
return OK;
return calibrate_return_ok;
}
int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors)
calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors)
{
int result = OK;
calibrate_return result = calibrate_return_ok;
*active_sensors = 0;
@ -344,7 +359,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac
for (unsigned i = 0; i < max_accel_sens; i++) {
worker_data.subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
if (worker_data.subs[i] < 0) {
result = ERROR;
result = calibrate_return_error;
break;
}
@ -354,8 +369,10 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac
timestamps[i] = arp.timestamp;
}
if (result == OK) {
result = calibrate_from_orientation(mavlink_fd, data_collected, accel_calibration_worker, &worker_data);
if (result == calibrate_return_ok) {
int cancel_sub = calibrate_cancel_subscribe();
result = calibrate_from_orientation(mavlink_fd, cancel_sub, data_collected, accel_calibration_worker, &worker_data, false /* normal still */);
calibrate_cancel_unsubscribe(cancel_sub);
}
/* close all subscriptions */
@ -371,13 +388,13 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac
}
}
if (result == OK) {
if (result == calibrate_return_ok) {
/* calculate offsets and transform matrix */
for (unsigned i = 0; i < (*active_sensors); i++) {
result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
if (result != OK) {
mavlink_and_console_log_critical(mavlink_fd, "ERROR: calibration values calculation error");
if (result != calibrate_return_ok) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: calibration calculation error");
break;
}
}
@ -389,7 +406,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac
/*
* Read specified number of accelerometer samples, calculate average and dispersion.
*/
int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num)
calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num)
{
px4_pollfd_struct_t fds[max_accel_sens];
@ -433,7 +450,7 @@ int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_a
}
if (errcount > samples_num / 10) {
return ERROR;
return calibrate_return_error;
}
}
@ -443,7 +460,7 @@ int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_a
}
}
return OK;
return calibrate_return_ok;
}
int mat_invert3(float src[3][3], float dst[3][3])
@ -469,7 +486,7 @@ int mat_invert3(float src[3][3], float dst[3][3])
return OK;
}
int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g)
calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g)
{
/* calculate offsets */
for (unsigned i = 0; i < 3; i++) {
@ -491,7 +508,7 @@ int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_s
float mat_A_inv[3][3];
if (mat_invert3(mat_A, mat_A_inv) != OK) {
return ERROR;
return calibrate_return_error;
}
/* copy results to accel_T */
@ -502,5 +519,5 @@ int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_s
}
}
return OK;
return calibrate_return_ok;
}

106
src/modules/commander/airspeed_calibration.cpp
View File

@ -38,6 +38,7 @@
#include "airspeed_calibration.h"
#include "calibration_messages.h"
#include "calibration_routines.h"
#include "commander_helper.h"
#include <px4_posix.h>
@ -64,19 +65,20 @@ static const int ERROR = -1;
static const char *sensor_name = "dpress";
#define HUMAN_ASPD_CAL_FAILED_MSG "Calibration failed, see http://px4.io/help/aspd"
static void feedback_calibration_failed(int mavlink_fd)
{
sleep(5);
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
mavlink_log_critical(mavlink_fd, HUMAN_ASPD_CAL_FAILED_MSG);
mavlink_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
}
int do_airspeed_calibration(int mavlink_fd)
{
int result = OK;
unsigned calibration_counter = 0;
const unsigned maxcount = 3000;
/* give directions */
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
const unsigned calibration_count = 2000;
@ -99,11 +101,13 @@ int do_airspeed_calibration(int mavlink_fd)
paramreset_successful = true;
} else {
mavlink_log_critical(mavlink_fd, "airspeed offset zero failed");
mavlink_log_critical(mavlink_fd, "[cal] airspeed offset zero failed");
}
px4_close(fd);
}
int cancel_sub = calibrate_cancel_subscribe();
if (!paramreset_successful) {
@ -111,26 +115,26 @@ int do_airspeed_calibration(int mavlink_fd)
float analog_scaling = 0.0f;
param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
if (fabsf(analog_scaling) < 0.1f) {
mavlink_log_critical(mavlink_fd, "No airspeed sensor, see http://px4.io/help/aspd");
px4_close(diff_pres_sub);
return ERROR;
mavlink_log_critical(mavlink_fd, "[cal] No airspeed sensor, see http://px4.io/help/aspd");
goto error_return;
}
/* set scaling offset parameter */
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
px4_close(diff_pres_sub);
return ERROR;
mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
goto error_return;
}
}
unsigned calibration_counter = 0;
mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind");
mavlink_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind");
usleep(500 * 1000);
while (calibration_counter < calibration_count) {
if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
goto error_return;
}
/* wait blocking for new data */
px4_pollfd_struct_t fds[1];
fds[0].fd = diff_pres_sub;
@ -145,14 +149,13 @@ int do_airspeed_calibration(int mavlink_fd)
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0) {
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 80) / calibration_count);
mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count);
}
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
feedback_calibration_failed(mavlink_fd);
px4_close(diff_pres_sub);
return ERROR;
goto error_return;
}
}
@ -164,16 +167,15 @@ int do_airspeed_calibration(int mavlink_fd)
airscale.offset_pa = diff_pres_offset;
if (fd_scale > 0) {
if (OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
mavlink_log_critical(mavlink_fd, "airspeed offset update failed");
mavlink_log_critical(mavlink_fd, "[cal] airspeed offset update failed");
}
px4_close(fd_scale);
}
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
px4_close(diff_pres_sub);
return ERROR;
mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
goto error_return;
}
/* auto-save to EEPROM */
@ -181,30 +183,31 @@ int do_airspeed_calibration(int mavlink_fd)
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
px4_close(diff_pres_sub);
return ERROR;
mavlink_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
goto error_return;
}
} else {
feedback_calibration_failed(mavlink_fd);
px4_close(diff_pres_sub);
return ERROR;
goto error_return;
}
mavlink_log_critical(mavlink_fd, "Offset of %d Pascal", (int)diff_pres_offset);
mavlink_log_critical(mavlink_fd, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
/* wait 500 ms to ensure parameter propagated through the system */
usleep(500 * 1000);
mavlink_log_critical(mavlink_fd, "Create airflow now");
mavlink_log_critical(mavlink_fd, "[cal] Create airflow now");
calibration_counter = 0;
const unsigned maxcount = 3000;
/* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */
while (calibration_counter < maxcount) {
if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
goto error_return;
}
/* wait blocking for new data */
px4_pollfd_struct_t fds[1];
fds[0].fd = diff_pres_sub;
@ -219,7 +222,7 @@ int do_airspeed_calibration(int mavlink_fd)
if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
if (calibration_counter % 500 == 0) {
mavlink_log_info(mavlink_fd, "Create air pressure! (got %d, wanted: 50 Pa)",
mavlink_log_info(mavlink_fd, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
(int)diff_pres.differential_pressure_raw_pa);
}
continue;
@ -227,30 +230,26 @@ int do_airspeed_calibration(int mavlink_fd)
/* do not allow negative values */
if (diff_pres.differential_pressure_raw_pa < 0.0f) {
mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)",
mavlink_log_info(mavlink_fd, "[cal] Negative pressure difference detected (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!");
px4_close(diff_pres_sub);
mavlink_log_info(mavlink_fd, "[cal] Swap static and dynamic ports!");
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */
diff_pres_offset = 0.0f;
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
px4_close(diff_pres_sub);
return ERROR;
mavlink_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG);
goto error_return;
}
/* save */
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 0);
mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 0);
(void)param_save_default();
px4_close(diff_pres_sub);
feedback_calibration_failed(mavlink_fd);
return ERROR;
goto error_return;
} else {
mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)",
mavlink_log_info(mavlink_fd, "[cal] Positive pressure: OK (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
break;
}
@ -258,21 +257,30 @@ int do_airspeed_calibration(int mavlink_fd)
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
feedback_calibration_failed(mavlink_fd);
px4_close(diff_pres_sub);
return ERROR;
goto error_return;
}
}
if (calibration_counter == maxcount) {
feedback_calibration_failed(mavlink_fd);
px4_close(diff_pres_sub);
return ERROR;
goto error_return;
}
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100);
mavlink_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
tune_neutral(true);
normal_return:
calibrate_cancel_unsubscribe(cancel_sub);
px4_close(diff_pres_sub);
return OK;
// This give a chance for the log messages to go out of the queue before someone else stomps on then
sleep(1);
return result;
error_return:
result = ERROR;
goto normal_return;
}

30
src/modules/commander/calibration_messages.h
View File

@ -42,17 +42,25 @@
#ifndef CALIBRATION_MESSAGES_H_
#define CALIBRATION_MESSAGES_H_
#define CAL_STARTED_MSG "%s calibration: started"
#define CAL_DONE_MSG "%s calibration: done"
#define CAL_FAILED_MSG "%s calibration: failed"
#define CAL_PROGRESS_MSG "%s calibration: progress <%u>"
// The calibration message defines which begin with CAL_QGC_ are used by QGroundControl to run a state
// machine to provide visual feedback for calibration. As such, the text for them or semantics of when
// they are displayed cannot be modified without causing QGC to break. If modifications are made, make
// sure to bump the calibration version number which will cause QGC to perform log based calibration
// instead of visual calibration until such a time as QGC is update to the new version.
#define CAL_FAILED_UNKNOWN_ERROR "ERROR: unknown error"
#define CAL_FAILED_SENSOR_MSG "ERROR: failed reading sensor"
#define CAL_FAILED_RESET_CAL_MSG "ERROR: failed to reset calibration, sensor %u"
#define CAL_FAILED_APPLY_CAL_MSG "ERROR: failed to apply calibration, sensor %u"
#define CAL_FAILED_SET_PARAMS_MSG "ERROR: failed to set parameters, sensor %u"
#define CAL_FAILED_SAVE_PARAMS_MSG "ERROR: failed to save parameters"
#define CAL_FAILED_ORIENTATION_TIMEOUT "ERROR: timed out waiting for correct orientation"
// The number in the cal started message is used to indicate the version stamp for the current calibration code.
#define CAL_QGC_STARTED_MSG "[cal] calibration started: 1 %s"
#define CAL_QGC_DONE_MSG "[cal] calibration done: %s"
#define CAL_QGC_FAILED_MSG "[cal] calibration failed: %s"
#define CAL_QGC_CANCELLED_MSG "[cal] calibration cancelled"
#define CAL_QGC_PROGRESS_MSG "[cal] progress <%u>"
#define CAL_QGC_ORIENTATION_DETECTED_MSG "[cal] %s orientation detected"
#define CAL_QGC_SIDE_DONE_MSG "[cal] %s side done, rotate to a different side"
#define CAL_ERROR_SENSOR_MSG "[cal] calibration failed: reading sensor"
#define CAL_ERROR_RESET_CAL_MSG "[cal] calibration failed: to reset, sensor %u"
#define CAL_ERROR_APPLY_CAL_MSG "[cal] calibration failed: to apply calibration, sensor %u"
#define CAL_ERROR_SET_PARAMS_MSG "[cal] calibration failed: to set parameters, sensor %u"
#define CAL_ERROR_SAVE_PARAMS_MSG "[cal] calibration failed: failed to save parameters"
#endif /* CALIBRATION_MESSAGES_H_ */

145
src/modules/commander/calibration_routines.cpp
View File

@ -52,6 +52,8 @@
#include <geo/geo.h>
#include <string.h>
#include <uORB/topics/vehicle_command.h>
#include "calibration_routines.h"
#include "calibration_messages.h"
#include "commander_helper.h"
@ -231,23 +233,19 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
return 0;
}
enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub)
enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub, int accel_sub, bool lenient_still_position)
{
const unsigned ndim = 3;
struct accel_report sensor;
/* exponential moving average of accel */
float accel_ema[ndim] = { 0.0f };
/* max-hold dispersion of accel */
float accel_disp[3] = { 0.0f, 0.0f, 0.0f };
/* EMA time constant in seconds*/
float ema_len = 0.5f;
/* set "still" threshold to 0.25 m/s^2 */
float still_thr2 = powf(0.25f, 2);
/* set accel error threshold to 5m/s^2 */
float accel_err_thr = 5.0f;
/* still time required in us */
hrt_abstime still_time = 2000000;
float accel_ema[ndim] = { 0.0f }; // exponential moving average of accel
float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; // max-hold dispersion of accel
float ema_len = 0.5f; // EMA time constant in seconds
const float normal_still_thr = 0.25; // normal still threshold
float still_thr2 = powf(lenient_still_position ? (normal_still_thr * 3) : normal_still_thr, 2);
float accel_err_thr = 5.0f; // set accel error threshold to 5m/s^2
hrt_abstime still_time = lenient_still_position ? 1000000 : 1500000; // still time required in us
px4_pollfd_struct_t fds[1];
fds[0].fd = accel_sub;
fds[0].events = POLLIN;
@ -309,7 +307,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub)
/* is still now */
if (t_still == 0) {
/* first time */
mavlink_and_console_log_info(mavlink_fd, "detected rest position, hold still...");
mavlink_and_console_log_info(mavlink_fd, "[cal] detected rest position, hold still...");
t_still = t;
t_timeout = t + timeout;
@ -326,8 +324,8 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub)
accel_disp[2] > still_thr2 * 4.0f) {
/* not still, reset still start time */
if (t_still != 0) {
mavlink_and_console_log_info(mavlink_fd, "detected motion, hold still...");
sleep(3);
mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still...");
usleep(500000);
t_still = 0;
}
}
@ -341,7 +339,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub)
}
if (poll_errcount > 1000) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SENSOR_MSG);
return DETECT_ORIENTATION_ERROR;
}
}
@ -382,7 +380,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub)
return DETECT_ORIENTATION_RIGHTSIDE_UP; // [ 0, 0, -g ]
}
mavlink_and_console_log_critical(mavlink_fd, "ERROR: invalid orientation");
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: invalid orientation");
return DETECT_ORIENTATION_ERROR; // Can't detect orientation
}
@ -402,28 +400,35 @@ const char* detect_orientation_str(enum detect_orientation_return orientation)
return rgOrientationStrs[orientation];
}
int calibrate_from_orientation(int mavlink_fd,
bool side_data_collected[detect_orientation_side_count],
calibration_from_orientation_worker_t calibration_worker,
void* worker_data)
calibrate_return calibrate_from_orientation(int mavlink_fd,
int cancel_sub,
bool side_data_collected[detect_orientation_side_count],
calibration_from_orientation_worker_t calibration_worker,
void* worker_data,
bool lenient_still_position)
{
int result = OK;
calibrate_return result = calibrate_return_ok;
// Setup subscriptions to onboard accel sensor
int sub_accel = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
if (sub_accel < 0) {
mavlink_and_console_log_critical(mavlink_fd, "No onboard accel found, abort");
return PX4_ERROR;
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "No onboard accel");
return calibrate_return_error;
}
unsigned orientation_failures = 0;
// Rotate through all three main positions
// Rotate through all requested orientation
while (true) {
if (orientation_failures > 10) {
result = PX4_ERROR;
mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_ORIENTATION_TIMEOUT);
if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
result = calibrate_return_cancelled;
break;
}
if (orientation_failures > 4) {
result = calibrate_return_error;
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "timeout: no motion");
break;
}
@ -451,44 +456,102 @@ int calibrate_from_orientation(int mavlink_fd,
strcat(pendingStr, detect_orientation_str((enum detect_orientation_return)cur_orientation));
}
}
mavlink_and_console_log_info(mavlink_fd, "pending:%s", pendingStr);
mavlink_and_console_log_info(mavlink_fd, "[cal] pending:%s", pendingStr);
mavlink_and_console_log_info(mavlink_fd, "hold the vehicle still on one of the pending sides");
enum detect_orientation_return orient = detect_orientation(mavlink_fd, sub_accel);
mavlink_and_console_log_info(mavlink_fd, "[cal] hold vehicle still on a pending side");
enum detect_orientation_return orient = detect_orientation(mavlink_fd, cancel_sub, sub_accel, lenient_still_position);
if (orient == DETECT_ORIENTATION_ERROR) {
orientation_failures++;
mavlink_and_console_log_info(mavlink_fd, "detected motion, hold still...");
mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still...");
continue;
}
/* inform user about already handled side */
if (side_data_collected[orient]) {
orientation_failures++;
mavlink_and_console_log_info(mavlink_fd, "%s side already completed or not needed", detect_orientation_str(orient));
mavlink_and_console_log_info(mavlink_fd, "rotate to a pending side");
mavlink_and_console_log_info(mavlink_fd, "[cal] %s side completed or not needed", detect_orientation_str(orient));
mavlink_and_console_log_info(mavlink_fd, "[cal] rotate to a pending side");
continue;
}
mavlink_and_console_log_info(mavlink_fd, "%s orientation detected", detect_orientation_str(orient));
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient));
orientation_failures = 0;
// Call worker routine
calibration_worker(orient, worker_data);
result = calibration_worker(orient, cancel_sub, worker_data);
if (result != calibrate_return_ok ) {
break;
}
mavlink_and_console_log_info(mavlink_fd, "%s side done, rotate to a different side", detect_orientation_str(orient));
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient));
// Note that this side is complete
side_data_collected[orient] = true;
tune_neutral(true);
sleep(1);
usleep(500000);
}
if (sub_accel >= 0) {
close(sub_accel);
}
// FIXME: Do we need an orientation complete routine?
return result;
}
int calibrate_cancel_subscribe(void)
{
return orb_subscribe(ORB_ID(vehicle_command));
}
void calibrate_cancel_unsubscribe(int cmd_sub)
{
orb_unsubscribe(cmd_sub);
}
static void calibrate_answer_command(int mavlink_fd, struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result)
{
switch (result) {
case VEHICLE_CMD_RESULT_ACCEPTED:
tune_positive(true);
break;
case VEHICLE_CMD_RESULT_DENIED:
mavlink_log_critical(mavlink_fd, "command denied during calibration: %u", cmd.command);
tune_negative(true);
break;
default:
break;
}
}
bool calibrate_cancel_check(int mavlink_fd, int cancel_sub)
{
struct pollfd fds[1];
fds[0].fd = cancel_sub;
fds[0].events = POLLIN;
if (poll(&fds[0], 1, 0) > 0) {
struct vehicle_command_s cmd;
memset(&cmd, 0, sizeof(cmd));
orb_copy(ORB_ID(vehicle_command), cancel_sub, &cmd);
if (cmd.command == VEHICLE_CMD_PREFLIGHT_CALIBRATION &&
(int)cmd.param1 == 0 &&
(int)cmd.param2 == 0 &&
(int)cmd.param3 == 0 &&
(int)cmd.param4 == 0 &&
(int)cmd.param5 == 0 &&
(int)cmd.param6 == 0) {
calibrate_answer_command(mavlink_fd, cmd, VEHICLE_CMD_RESULT_ACCEPTED);
mavlink_log_critical(mavlink_fd, CAL_QGC_CANCELLED_MSG);
return true;
} else {
calibrate_answer_command(mavlink_fd, cmd, VEHICLE_CMD_RESULT_DENIED);
}
}
return false;
}

71
src/modules/commander/calibration_routines.h
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012 PX4 Development Team. All rights reserved.
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -31,12 +31,8 @@
*
****************************************************************************/
/**
* @file calibration_routines.h
* Calibration routines definitions.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
*/
/// @file calibration_routines.h
/// @authot Don Gagne <don@thegagnes.com>
/**
* Least-squares fit of a sphere to a set of points.
@ -75,30 +71,45 @@ enum detect_orientation_return {
};
static const unsigned detect_orientation_side_count = 6;
/**
* Wait for vehicle to become still and detect it's orientation.
*
* @param mavlink_fd the MAVLink file descriptor to print output to
* @param accel_sub Subscription to onboard accel
*
* @return detect_orientation)_return according to orientation when vehicle is still and ready for measurements,
* DETECT_ORIENTATION_ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2
*/
enum detect_orientation_return detect_orientation(int mavlink_fd, int accel_sub);
/// Wait for vehicle to become still and detect it's orientation
/// @return Returns detect_orientation_return according to orientation when vehicle
/// and ready for measurements
enum detect_orientation_return detect_orientation(int mavlink_fd, ///< Mavlink fd to write output to
int cancel_sub, ///< Cancel subscription from calibration_cancel_subscribe
int accel_sub, ///< Orb subcription to accel sensor
bool lenient_still_detection); ///< true: Use more lenient still position detection
/**
* Returns the human readable string representation of the orientation
*
* @param orientation Orientation to return string for, "error" if buffer is too small
*
* @return str Returned orientation string
*/
/// Returns the human readable string representation of the orientation
/// @param orientation Orientation to return string for, "error" if buffer is too small
const char* detect_orientation_str(enum detect_orientation_return orientation);
typedef int (*calibration_from_orientation_worker_t)(detect_orientation_return orientation, void* worker_data);
enum calibrate_return {
calibrate_return_ok,
calibrate_return_error,
calibrate_return_cancelled
};
int calibrate_from_orientation(int mavlink_fd,
bool side_data_collected[detect_orientation_side_count],
calibration_from_orientation_worker_t calibration_worker,
void* worker_data);
typedef calibrate_return (*calibration_from_orientation_worker_t)(detect_orientation_return orientation, ///< Orientation which was detected
int cancel_sub, ///< Cancel subscription from calibration_cancel_subscribe
void* worker_data); ///< Opaque worker data
/// Perform calibration sequence which require a rest orientation detection prior to calibration.
/// @return OK: Calibration succeeded, ERROR: Calibration failed
calibrate_return calibrate_from_orientation(int mavlink_fd, ///< Mavlink fd to write output to
int cancel_sub, ///< Cancel subscription from calibration_cancel_subscribe
bool side_data_collected[detect_orientation_side_count], ///< Sides for which data still needs calibration
calibration_from_orientation_worker_t calibration_worker, ///< Worker routine which performs the actual calibration
void* worker_data, ///< Opaque data passed to worker routine
bool lenient_still_detection); ///< true: Use more lenient still position detection
/// Called at the beginning of calibration in order to subscribe to the cancel command
/// @return Handle to vehicle_command subscription
int calibrate_cancel_subscribe(void);
/// Called to cancel the subscription to the cancel command
/// @param cancel_sub Cancel subcription from calibration_cancel_subscribe
void calibrate_cancel_unsubscribe(int cancel_sub);
/// Used to periodically check for a cancel command
bool calibrate_cancel_check(int mavlink_fd, ///< Mavlink fd for output
int cancel_sub); ///< Cancel subcription fromcalibration_cancel_subscribe

149
src/modules/commander/commander.cpp
View File

@ -186,21 +186,6 @@ static struct safety_s safety;
static struct vehicle_control_mode_s control_mode;
static struct offboard_control_mode_s offboard_control_mode;
/* tasks waiting for low prio thread */
typedef enum {
LOW_PRIO_TASK_NONE = 0,
LOW_PRIO_TASK_PARAM_SAVE,
LOW_PRIO_TASK_PARAM_LOAD,
LOW_PRIO_TASK_GYRO_CALIBRATION,
LOW_PRIO_TASK_MAG_CALIBRATION,
LOW_PRIO_TASK_ALTITUDE_CALIBRATION,
LOW_PRIO_TASK_RC_CALIBRATION,
LOW_PRIO_TASK_ACCEL_CALIBRATION,
LOW_PRIO_TASK_AIRSPEED_CALIBRATION
} low_prio_task_t;
static low_prio_task_t low_prio_task = LOW_PRIO_TASK_NONE;
/**
* The daemon app only briefly exists to start
* the background job. The stack size assigned in the
@ -232,6 +217,8 @@ int commander_thread_main(int argc, char *argv[]);
void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed);
void get_circuit_breaker_params();
void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man);
@ -558,8 +545,8 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
}
else {
//Refuse to arm if preflight checks have failed
if(!status.condition_system_sensors_initialized) {
// Refuse to arm if preflight checks have failed
if (!status.hil_state != vehicle_status_s::HIL_STATE_ON && !status.condition_system_sensors_initialized) {
mavlink_log_critical(mavlink_fd, "Arming DENIED. Preflight checks have failed.");
cmd_result = VEHICLE_CMD_RESULT_DENIED;
break;
@ -966,10 +953,10 @@ int commander_thread_main(int argc, char *argv[])
if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
if (mission.dataman_id >= 0 && mission.dataman_id <= 1) {
warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count,
mission.current_seq);
mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d",
mission.dataman_id, mission.count, mission.current_seq);
if (mission.count > 0) {
mavlink_log_info(mavlink_fd, "[cmd] Mission #%d loaded, %u WPs, curr: %d",
mission.dataman_id, mission.count, mission.current_seq);
}
} else {
const char *missionfail = "reading mission state failed";
@ -1038,7 +1025,7 @@ int commander_thread_main(int argc, char *argv[])
bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM];
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
telemetry_subs[i] = -1;
telemetry_last_heartbeat[i] = 0;
telemetry_last_dl_loss[i] = 0;
telemetry_lost[i] = true;
@ -1135,6 +1122,8 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_sys_type, &(status.system_type)); // get system type
status.is_rotary_wing = is_rotary_wing(&status) || is_vtol(&status);
get_circuit_breaker_params();
bool checkAirspeed = false;
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
@ -1142,9 +1131,9 @@ int commander_thread_main(int argc, char *argv[])
checkAirspeed = true;
}
//Run preflight check
// Run preflight check
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
if(!status.condition_system_sensors_initialized) {
if (!status.condition_system_sensors_initialized) {
set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune
}
else {
@ -1176,7 +1165,7 @@ int commander_thread_main(int argc, char *argv[])
/* initialize low priority thread */
pthread_attr_t commander_low_prio_attr;
pthread_attr_init(&commander_low_prio_attr);
pthread_attr_setstacksize(&commander_low_prio_attr, 2100);
pthread_attr_setstacksize(&commander_low_prio_attr, 2000);
struct sched_param param;
(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
@ -1228,14 +1217,7 @@ int commander_thread_main(int argc, char *argv[])
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.circuit_breaker_engaged_airspd_check =
circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
status.circuit_breaker_engaged_enginefailure_check =
circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY);
status.circuit_breaker_engaged_gpsfailure_check =
circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY);
get_circuit_breaker_params();
status_changed = true;
@ -1296,6 +1278,11 @@ int commander_thread_main(int argc, char *argv[])
}
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
if (telemetry_subs[i] < 0 && (OK == orb_exists(telemetry_status_orb_id[i], 0))) {
telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
}
orb_check(telemetry_subs[i], &updated);
if (updated) {
@ -1585,7 +1572,9 @@ int commander_thread_main(int argc, char *argv[])
/* if battery voltage is getting lower, warn using buzzer, etc. */
if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) {
low_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
if (armed.armed) {
mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
}
status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW;
status_changed = true;
@ -1593,7 +1582,6 @@ int commander_thread_main(int argc, char *argv[])
&& !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
/* critical battery voltage, this is rather an emergency, change state machine */
critical_battery_voltage_actions_done = true;
mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL;
if (!armed.armed) {
@ -1616,8 +1604,7 @@ int commander_thread_main(int argc, char *argv[])
/* End battery voltage check */
/* If in INIT state, try to proceed to STANDBY state */
if (status.arming_state == vehicle_status_s::ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
/* TODO: check for sensors */
if (status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
@ -1625,8 +1612,6 @@ int commander_thread_main(int argc, char *argv[])
arming_state_changed = true;
}
} else {
/* TODO: Add emergency stuff if sensors are lost */
}
@ -1833,13 +1818,17 @@ int commander_thread_main(int argc, char *argv[])
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
if (telemetry_last_heartbeat[i] != 0 &&
hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) {
/* handle the case where data link was regained,
/* handle the case where data link was gained first time or regained,
* accept datalink as healthy only after datalink_regain_timeout seconds
* */
if (telemetry_lost[i] &&
hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) {
mavlink_log_info(mavlink_fd, "data link %i regained", i);
/* only report a regain */
if (telemetry_last_dl_loss[i] > 0) {
mavlink_and_console_log_critical(mavlink_fd, "data link #%i regained", i);
}
telemetry_lost[i] = false;
have_link = true;
@ -1850,10 +1839,12 @@ int commander_thread_main(int argc, char *argv[])
}
} else {
telemetry_last_dl_loss[i] = hrt_absolute_time();
if (!telemetry_lost[i]) {
mavlink_log_info(mavlink_fd, "data link %i lost", i);
/* only reset the timestamp to a different time on state change */
telemetry_last_dl_loss[i] = hrt_absolute_time();
mavlink_and_console_log_critical(mavlink_fd, "data link #%i lost", i);
telemetry_lost[i] = true;
}
}
@ -1868,7 +1859,7 @@ int commander_thread_main(int argc, char *argv[])
} else {
if (!status.data_link_lost) {
mavlink_log_info(mavlink_fd, "ALL DATA LINKS LOST");
mavlink_and_console_log_critical(mavlink_fd, "ALL DATA LINKS LOST");
status.data_link_lost = true;
status.data_link_lost_counter++;
status_changed = true;
@ -2119,6 +2110,19 @@ int commander_thread_main(int argc, char *argv[])
return 0;
}
void
get_circuit_breaker_params()
{
status.circuit_breaker_engaged_power_check =
circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
status.circuit_breaker_engaged_airspd_check =
circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY);
status.circuit_breaker_engaged_enginefailure_check =
circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY);
status.circuit_breaker_engaged_gpsfailure_check =
circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY);
}
void
check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed)
{
@ -2670,7 +2674,7 @@ void *commander_low_prio_loop(void *arg)
/* try to go to INIT/PREFLIGHT arming state */
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_INIT, &armed,
true /* fRunPreArmChecks */, mavlink_fd)) {
false /* fRunPreArmChecks */, mavlink_fd)) {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
break;
}
@ -2719,53 +2723,38 @@ void *commander_low_prio_loop(void *arg)
/* enable RC control input */
status.rc_input_blocked = false;
mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN");
calib_ret = OK;
}
/* this always succeeds */
calib_ret = OK;
}
if (calib_ret == OK) {
tune_positive(true);
// Update preflight check status
// we do not set the calibration return value based on it because the calibration
// might have worked just fine, but the preflight check fails for a different reason,
// so this would be prone to false negatives.
bool checkAirspeed = false;
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
checkAirspeed = true;
}
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
} else {
tune_negative(true);
}
// Update preflight check status
// we do not set the calibration return value based on it because the calibration
// might have worked just fine, but the preflight check fails for a different reason,
// so this would be prone to false negatives.
bool checkAirspeed = false;
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
checkAirspeed = true;
}
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
break;
}
case VEHICLE_CMD_PREFLIGHT_STORAGE: {
bool checkAirspeed = false;
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) {
checkAirspeed = true;
}
// Update preflight check status
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true);
arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
if (((int)(cmd.param1)) == 0) {
int ret = param_load_default();
@ -2789,9 +2778,15 @@ void *commander_low_prio_loop(void *arg)
}
} else if (((int)(cmd.param1)) == 1) {
int ret = param_save_default();
if (ret == OK) {
if (need_param_autosave) {
need_param_autosave = false;
need_param_autosave_timeout = 0;
}
mavlink_log_info(mavlink_fd, "settings saved");
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);

6
src/modules/commander/commander_helper.cpp
View File

@ -87,8 +87,10 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status)
}
bool is_vtol(const struct vehicle_status_s * current_status) {
return current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR ||
current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR;
return (current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_DUOROTOR ||
current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_QUADROTOR ||
current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_HEXAROTOR ||
current_status->system_type == vehicle_status_s::VEHICLE_TYPE_VTOL_OCTOROTOR);
}
static int buzzer = -1;

303
src/modules/commander/gyro_calibration.cpp
View File

@ -39,6 +39,7 @@
#include "gyro_calibration.h"
#include "calibration_messages.h"
#include "calibration_routines.h"
#include "commander_helper.h"
#include <px4_posix.h>
@ -65,142 +66,195 @@ static const int ERROR = -1;
static const char *sensor_name = "gyro";
int do_gyro_calibration(int mavlink_fd)
static const unsigned max_gyros = 3;
/// Data passed to calibration worker routine
typedef struct {
int mavlink_fd;
int32_t device_id[max_gyros];
int gyro_sensor_sub[max_gyros];
struct gyro_scale gyro_scale[max_gyros];
struct gyro_report gyro_report_0;
} gyro_worker_data_t;
static calibrate_return gyro_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
{
const unsigned max_gyros = 3;
int32_t device_id[3];
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "HOLD STILL");
/* wait for the user to respond */
sleep(2);
struct gyro_scale gyro_scale_zero = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
struct gyro_scale gyro_scale[max_gyros] = {};
int res = OK;
char str[30];
gyro_worker_data_t* worker_data = (gyro_worker_data_t*)(data);
unsigned calibration_counter[max_gyros] = { 0 };
const unsigned calibration_count = 5000;
struct gyro_report gyro_report;
unsigned poll_errcount = 0;
struct pollfd fds[max_gyros];
for (unsigned s = 0; s < max_gyros; s++) {
/* ensure all scale fields are initialized tha same as the first struct */
(void)memcpy(&gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale[0]));
sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
/* reset all offsets to zero and all scales to one */
int fd = px4_open(str, 0);
if (fd < 0) {
continue;
fds[s].fd = worker_data->gyro_sensor_sub[s];
fds[s].events = POLLIN;
}
memset(&worker_data->gyro_report_0, 0, sizeof(worker_data->gyro_report_0));
/* use first gyro to pace, but count correctly per-gyro for statistics */
while (calibration_counter[0] < calibration_count) {
if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) {
return calibrate_return_cancelled;
}
device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero);
px4_close(fd);
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, s);
int poll_ret = px4_poll(&fds[0], max_gyros, 1000);
if (poll_ret > 0) {
for (unsigned s = 0; s < max_gyros; s++) {
bool changed;
orb_check(worker_data->gyro_sensor_sub[s], &changed);
if (changed) {
orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &gyro_report);
if (s == 0) {
orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &worker_data->gyro_report_0);
}
worker_data->gyro_scale[s].x_offset += gyro_report.x;
worker_data->gyro_scale[s].y_offset += gyro_report.y;
worker_data->gyro_scale[s].z_offset += gyro_report.z;
calibration_counter[s]++;
}
if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) {
mavlink_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter[0] * 100) / calibration_count);
}
}
} else {
poll_errcount++;
}
if (poll_errcount > 1000) {
mavlink_log_critical(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG);
return calibrate_return_error;
}
}
unsigned calibration_counter[max_gyros] = { 0 };
const unsigned calibration_count = 5000;
struct gyro_report gyro_report_0 = {};
if (res == OK) {
/* determine gyro mean values */
unsigned poll_errcount = 0;
/* subscribe to gyro sensor topic */
int sub_sensor_gyro[max_gyros];
px4_pollfd_struct_t fds[max_gyros];
for (unsigned s = 0; s < max_gyros; s++) {
sub_sensor_gyro[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
fds[s].fd = sub_sensor_gyro[s];
fds[s].events = POLLIN;
for (unsigned s = 0; s < max_gyros; s++) {
if (worker_data->device_id[s] != 0 && calibration_counter[s] < calibration_count / 2) {
mavlink_log_critical(worker_data->mavlink_fd, "[cal] ERROR: missing data, sensor %d", s)
return calibrate_return_error;
}
struct gyro_report gyro_report;
worker_data->gyro_scale[s].x_offset /= calibration_counter[s];
worker_data->gyro_scale[s].y_offset /= calibration_counter[s];
worker_data->gyro_scale[s].z_offset /= calibration_counter[s];
}
/* use first gyro to pace, but count correctly per-gyro for statistics */
while (calibration_counter[0] < calibration_count) {
/* wait blocking for new data */
return calibrate_return_ok;
}
int poll_ret = px4_poll(&fds[0], max_gyros, 1000);
int do_gyro_calibration(int mavlink_fd)
{
int res = OK;
gyro_worker_data_t worker_data;
if (poll_ret > 0) {
mavlink_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
for (unsigned s = 0; s < max_gyros; s++) {
bool changed;
orb_check(sub_sensor_gyro[s], &changed);
if (changed) {
orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report);
if (s == 0) {
orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro[s], &gyro_report_0);
}
gyro_scale[s].x_offset += gyro_report.x;
gyro_scale[s].y_offset += gyro_report.y;
gyro_scale[s].z_offset += gyro_report.z;
calibration_counter[s]++;
}
if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) {
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter[0] * 100) / calibration_count);
}
}
} else {
poll_errcount++;
}
if (poll_errcount > 1000) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
res = ERROR;
break;
}
worker_data.mavlink_fd = mavlink_fd;
struct gyro_scale gyro_scale_zero = {
0.0f, // x offset
1.0f, // x scale
0.0f, // y offset
1.0f, // y scale
0.0f, // z offset
1.0f, // z scale
};
for (unsigned s = 0; s < max_gyros; s++) {
char str[30];
// Reset gyro ids to unavailable
worker_data.device_id[s] = 0;
(void)sprintf(str, "CAL_GYRO%u_ID", s);
res = param_set_no_notification(param_find(str), &(worker_data.device_id[s]));
if (res != OK) {
mavlink_log_critical(mavlink_fd, "[cal] Unable to reset CAL_GYRO%u_ID", s);
return ERROR;
}
// Reset all offsets to 0 and scales to 1
(void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale));
sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
int fd = open(str, 0);
if (fd >= 0) {
worker_data.device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0);
res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero);
close(fd);
<<<<<<< HEAD
for (unsigned s = 0; s < max_gyros; s++) {
px4_close(sub_sensor_gyro[s]);
gyro_scale[s].x_offset /= calibration_counter[s];
gyro_scale[s].y_offset /= calibration_counter[s];
gyro_scale[s].z_offset /= calibration_counter[s];
=======
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s);
return ERROR;
}
>>>>>>> upstream/master
}
}
for (unsigned s = 0; s < max_gyros; s++) {
worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
}
// Calibrate right-side up
bool side_collected[detect_orientation_side_count] = { true, true, true, true, true, false };
int cancel_sub = calibrate_cancel_subscribe();
calibrate_return cal_return = calibrate_from_orientation(mavlink_fd, // Mavlink fd to write output
cancel_sub, // Subscription to vehicle_command for cancel support
side_collected, // Sides to calibrate
gyro_calibration_worker, // Calibration worker
&worker_data, // Opaque data for calibration worked
true); // true: lenient still detection
calibrate_cancel_unsubscribe(cancel_sub);
for (unsigned s = 0; s < max_gyros; s++) {
close(worker_data.gyro_sensor_sub[s]);
}
if (cal_return == calibrate_return_cancelled) {
// Cancel message already sent, we are done here
return ERROR;
} else if (cal_return == calibrate_return_error) {
res = ERROR;
}
if (res == OK) {
/* check offsets */
float xdiff = gyro_report_0.x - gyro_scale[0].x_offset;
float ydiff = gyro_report_0.y - gyro_scale[0].y_offset;
float zdiff = gyro_report_0.z - gyro_scale[0].z_offset;
float xdiff = worker_data.gyro_report_0.x - worker_data.gyro_scale[0].x_offset;
float ydiff = worker_data.gyro_report_0.y - worker_data.gyro_scale[0].y_offset;
float zdiff = worker_data.gyro_report_0.z - worker_data.gyro_scale[0].z_offset;
/* maximum allowable calibration error in radians */
const float maxoff = 0.0055f;
<<<<<<< HEAD
if (!PX4_ISFINITE(gyro_scale[0].x_offset) ||
!PX4_ISFINITE(gyro_scale[0].y_offset) ||
!PX4_ISFINITE(gyro_scale[0].z_offset) ||
=======
if (!isfinite(worker_data.gyro_scale[0].x_offset) ||
!isfinite(worker_data.gyro_scale[0].y_offset) ||
!isfinite(worker_data.gyro_scale[0].z_offset) ||
>>>>>>> upstream/master
fabsf(xdiff) > maxoff ||
fabsf(ydiff) > maxoff ||
fabsf(zdiff) > maxoff) {
mavlink_and_console_log_critical(mavlink_fd, "ERROR: Motion during calibration");
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: Motion during calibration");
res = ERROR;
}
}
@ -210,7 +264,28 @@ int do_gyro_calibration(int mavlink_fd)
bool failed = false;
for (unsigned s = 0; s < max_gyros; s++) {
if (worker_data.device_id[s] != 0) {
char str[30];
(void)sprintf(str, "CAL_GYRO%u_XOFF", s);
failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].x_offset)));
(void)sprintf(str, "CAL_GYRO%u_YOFF", s);
failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].y_offset)));
(void)sprintf(str, "CAL_GYRO%u_ZOFF", s);
failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].z_offset)));
(void)sprintf(str, "CAL_GYRO%u_ID", s);
failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.device_id[s])));
/* apply new scaling and offsets */
(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
int fd = open(str, 0);
if (fd < 0) {
failed = true;
continue;
}
<<<<<<< HEAD
/* if any reasonable amount of data is missing, skip */
if (calibration_counter[s] < calibration_count / 2) {
continue;
@ -236,14 +311,19 @@ int do_gyro_calibration(int mavlink_fd)
res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale[s]);
px4_close(fd);
=======
res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&worker_data.gyro_scale[s]);
close(fd);
>>>>>>> upstream/master
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG);
}
}
}
if (failed) {
mavlink_and_console_log_critical(mavlink_fd, "ERROR: failed to set offset params");
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: failed to set offset params");
res = ERROR;
}
}
@ -260,16 +340,21 @@ int do_gyro_calibration(int mavlink_fd)
res = param_save_default();
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
mavlink_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
}
}
if (res == OK) {
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
/* if there is a any preflight-check system response, let the barrage of messages through */
usleep(200000);
if (res == OK) {
mavlink_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
} else {
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
}
/* give this message enough time to propagate */
usleep(600000);
return res;
}

192
src/modules/commander/mag_calibration.cpp
View File

@ -68,8 +68,7 @@ static const int ERROR = -1;
static const char *sensor_name = "mag";
static const unsigned max_mags = 3;
int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
int mag_calibration_worker(detect_orientation_return orientation, void* worker_data);
calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
/// Data passed to calibration worker routine
typedef struct {
@ -89,7 +88,7 @@ typedef struct {
int do_mag_calibration(int mavlink_fd)
{
mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
struct mag_scale mscale_null = {
0.0f,
@ -116,7 +115,7 @@ int do_mag_calibration(int mavlink_fd)
(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
result = param_set_no_notification(param_find(str), &(device_ids[cur_mag]));
if (result != OK) {
mavlink_and_console_log_info(mavlink_fd, "Unabled to reset CAL_MAG%u_ID", cur_mag);
mavlink_and_console_log_info(mavlink_fd, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag);
break;
}
@ -134,15 +133,15 @@ int do_mag_calibration(int mavlink_fd)
result = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
if (result != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, cur_mag);
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, cur_mag);
}
/* calibrate range */
if (result == OK) {
/* calibrate range */
result = px4_ioctl(fd, MAGIOCCALIBRATE, fd);
if (result != OK) {
mavlink_and_console_log_info(mavlink_fd, "Skipped scale calibration, sensor %u", cur_mag);
mavlink_and_console_log_info(mavlink_fd, "[cal] Skipped scale calibration, sensor %u", cur_mag);
/* this is non-fatal - mark it accordingly */
result = OK;
}
@ -151,39 +150,52 @@ int do_mag_calibration(int mavlink_fd)
px4_close(fd);
}
// Calibrate all mags at the same time
if (result == OK) {
// Calibrate all mags at the same time
result = mag_calibrate_all(mavlink_fd, device_ids);
}
if (result == OK) {
/* auto-save to EEPROM */
result = param_save_default();
if (result != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
switch (mag_calibrate_all(mavlink_fd, device_ids)) {
case calibrate_return_cancelled:
// Cancel message already displayed, we're done here
result = ERROR;
break;
case calibrate_return_ok:
/* auto-save to EEPROM */
result = param_save_default();
/* if there is a any preflight-check system response, let the barrage of messages through */
usleep(200000);
if (result == OK) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
break;
} else {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
}
// Fall through
default:
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
break;
}
}
if (result == OK) {
mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100);
mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
} else {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
}
/* give this message enough time to propagate */
usleep(600000);
return result;
}
int mag_calibration_worker(detect_orientation_return orientation, void* data)
static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
{
int result = OK;
calibrate_return result = calibrate_return_ok;
unsigned int calibration_counter_side;
mag_worker_data_t* worker_data = (mag_worker_data_t*)(data);
mavlink_and_console_log_info(worker_data->mavlink_fd, "Rotate vehicle around the detected orientation");
mavlink_and_console_log_info(worker_data->mavlink_fd, "Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds);
mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Rotate vehicle around the detected orientation");
mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds);
sleep(2);
uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds;
@ -194,6 +206,11 @@ int mag_calibration_worker(detect_orientation_return orientation, void* data)
while (hrt_absolute_time() < calibration_deadline &&
calibration_counter_side < worker_data->calibration_points_perside) {
if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) {
result = calibrate_return_cancelled;
break;
}
// Wait clocking for new data on all mags
px4_pollfd_struct_t fds[max_mags];
size_t fd_count = 0;
@ -225,8 +242,7 @@ int mag_calibration_worker(detect_orientation_return orientation, void* data)
// Progress indicator for side
mavlink_and_console_log_info(worker_data->mavlink_fd,
"%s %s side calibration: progress <%u>",
sensor_name,
"[cal] %s side calibration: progress <%u>",
detect_orientation_str(orientation),
(unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)));
} else {
@ -234,50 +250,25 @@ int mag_calibration_worker(detect_orientation_return orientation, void* data)
}
if (poll_errcount > worker_data->calibration_points_perside * 3) {
result = ERROR;
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_FAILED_SENSOR_MSG);
result = calibrate_return_error;
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG);
break;
}
}
// Mark the opposite side as collected as well. No need to collect opposite side since it
// would generate similar points.
detect_orientation_return alternateOrientation = orientation;
switch (orientation) {
case DETECT_ORIENTATION_TAIL_DOWN:
alternateOrientation = DETECT_ORIENTATION_NOSE_DOWN;
break;
case DETECT_ORIENTATION_NOSE_DOWN:
alternateOrientation = DETECT_ORIENTATION_TAIL_DOWN;
break;
case DETECT_ORIENTATION_LEFT:
alternateOrientation = DETECT_ORIENTATION_RIGHT;
break;
case DETECT_ORIENTATION_RIGHT:
alternateOrientation = DETECT_ORIENTATION_LEFT;
break;
case DETECT_ORIENTATION_UPSIDE_DOWN:
alternateOrientation = DETECT_ORIENTATION_RIGHTSIDE_UP;
break;
case DETECT_ORIENTATION_RIGHTSIDE_UP:
alternateOrientation = DETECT_ORIENTATION_UPSIDE_DOWN;
break;
case DETECT_ORIENTATION_ERROR:
warnx("Invalid orientation in mag_calibration_worker");
break;
if (result == calibrate_return_ok) {
mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side done, rotate to a different side", detect_orientation_str(orientation));
worker_data->done_count++;
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, 34 * worker_data->done_count);
}
worker_data->side_data_collected[alternateOrientation] = true;
mavlink_and_console_log_info(worker_data->mavlink_fd, "%s side done, rotate to a different side", detect_orientation_str(alternateOrientation));
worker_data->done_count++;
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 34 * worker_data->done_count);
return result;
}
int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
{
int result = OK;
calibrate_return result = calibrate_return_ok;
mag_worker_data_t worker_data;
@ -288,10 +279,13 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
worker_data.calibration_interval_perside_seconds = 20;
worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;
// Initialize to collect all sides
for (size_t cur_side=0; cur_side<6; cur_side++) {
worker_data.side_data_collected[cur_side] = false;
}
// Collect: Right-side up, Left Side, Nose down
worker_data.side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = false;
worker_data.side_data_collected[DETECT_ORIENTATION_LEFT] = false;
worker_data.side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = false;
worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = true;
worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = true;
worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = true;
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
// Initialize to no subscription
@ -313,21 +307,21 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
worker_data.y[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
worker_data.z[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) {
mavlink_and_console_log_critical(mavlink_fd, "ERROR: out of memory");
result = ERROR;
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: out of memory");
result = calibrate_return_error;
}
}
// Setup subscriptions to mag sensors
if (result == OK) {
if (result == calibrate_return_ok) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
// Mag in this slot is available
worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag);
if (worker_data.sub_mag[cur_mag] < 0) {
mavlink_and_console_log_critical(mavlink_fd, "Mag #%u not found, abort", cur_mag);
result = ERROR;
mavlink_and_console_log_critical(mavlink_fd, "[cal] Mag #%u not found, abort", cur_mag);
result = calibrate_return_error;
break;
}
}
@ -335,7 +329,7 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
}
// Limit update rate to get equally spaced measurements over time (in ms)
if (result == OK) {
if (result == calibrate_return_ok) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
// Mag in this slot is available
@ -347,8 +341,18 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
}
}
if (result == calibrate_return_ok) {
int cancel_sub = calibrate_cancel_subscribe();
result = calibrate_from_orientation(mavlink_fd, worker_data.side_data_collected, mag_calibration_worker, &worker_data);
result = calibrate_from_orientation(mavlink_fd, // Mavlink fd to write output
cancel_sub, // Subscription to vehicle_command for cancel support
worker_data.side_data_collected, // Sides to calibrate
mag_calibration_worker, // Calibration worker
&worker_data, // Opaque data for calibration worked
true); // true: lenient still detection
calibrate_cancel_unsubscribe(cancel_sub);
}
// Close subscriptions
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
@ -366,7 +370,7 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
float sphere_radius[max_mags];
// Sphere fit the data to get calibration values
if (result == OK) {
if (result == calibrate_return_ok) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
// Mag in this slot is available and we should have values for it to calibrate
@ -378,8 +382,8 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
&sphere_radius[cur_mag]);
if (!PX4_ISFINITE(sphere_x[cur_mag]) || !PX4_ISFINITE(sphere_y[cur_mag]) || !PX4_ISFINITE(sphere_z[cur_mag])) {
mavlink_and_console_log_info(mavlink_fd, "ERROR: NaN in sphere fit for mag #%u", cur_mag);
result = ERROR;
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: NaN in sphere fit for mag #%u", cur_mag);
result = calibrate_return_error;
}
}
}
@ -392,7 +396,7 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
free(worker_data.z[cur_mag]);
}
if (result == OK) {
if (result == calibrate_return_ok) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
int fd_mag = -1;
@ -403,27 +407,25 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
fd_mag = px4_open(str, 0);
if (fd_mag < 0) {
mavlink_and_console_log_info(mavlink_fd, "ERROR: unable to open mag device #%u", cur_mag);
result = ERROR;
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: unable to open mag device #%u", cur_mag);
result = calibrate_return_error;
}
if (result == OK) {
result = px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale);
if (result != OK) {
mavlink_and_console_log_info(mavlink_fd, "ERROR: failed to get current calibration #%u", cur_mag);
result = ERROR;
if (result == calibrate_return_ok) {
if (px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != OK) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: failed to get current calibration #%u", cur_mag);
result = calibrate_return_error;
}
}
if (result == OK) {
if (result == calibrate_return_ok) {
mscale.x_offset = sphere_x[cur_mag];
mscale.y_offset = sphere_y[cur_mag];
mscale.z_offset = sphere_z[cur_mag];
result = px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale);
if (result != OK) {
mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG, cur_mag);
result = ERROR;
if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, cur_mag);
result = calibrate_return_error;
}
}
@ -432,7 +434,7 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
px4_close(fd_mag);
}
if (result == OK) {
if (result == calibrate_return_ok) {
bool failed = false;
/* set parameters */
@ -452,13 +454,13 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale)));
if (failed) {
mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG, cur_mag);
result = ERROR;
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, cur_mag);
result = calibrate_return_error;
} else {
mavlink_and_console_log_info(mavlink_fd, "mag #%u off: x:%.2f y:%.2f z:%.2f Ga",
mavlink_and_console_log_info(mavlink_fd, "[cal] mag #%u off: x:%.2f y:%.2f z:%.2f Ga",
cur_mag,
(double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset);
mavlink_and_console_log_info(mavlink_fd, "mag #%u scale: x:%.2f y:%.2f z:%.2f",
mavlink_and_console_log_info(mavlink_fd, "[cal] mag #%u scale: x:%.2f y:%.2f z:%.2f",
cur_mag,
(double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale);
}

43
src/modules/commander/state_machine_helper.cpp
View File

@ -140,6 +140,13 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
/* enforce lockdown in HIL */
if (status->hil_state == vehicle_status_s::HIL_STATE_ON) {
armed->lockdown = true;
prearm_ret = OK;
status->condition_system_sensors_initialized = true;
/* recover from a prearm fail */
if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY;
}
} else {
armed->lockdown = false;
@ -177,7 +184,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
// Fail transition if power is not good
if (!status->condition_power_input_valid) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
feedback_provided = true;
valid_transition = false;
}
@ -187,7 +194,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) {
// Check avionics rail voltages
if (status->avionics_power_rail_voltage < 4.75f) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
valid_transition = false;
} else if (status->avionics_power_rail_voltage < 4.9f) {
@ -212,23 +219,35 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
valid_transition = true;
}
/* Sensors need to be initialized for STANDBY state */
if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
mavlink_log_critical(mavlink_fd, "Not ready to fly: Sensors not operational.");
// Sensors need to be initialized for STANDBY state, except for HIL
if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) &&
(new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
(!status->condition_system_sensors_initialized)) {
mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection");
feedback_provided = true;
valid_transition = false;
status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
}
/* Check if we are trying to arm, checks look good but we are in STANDBY_ERROR */
if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR &&
new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
if (status->condition_system_sensors_initialized) {
mavlink_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming");
// Check if we are trying to arm, checks look good but we are in STANDBY_ERROR
if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
if (status->condition_system_sensors_initialized) {
mavlink_and_console_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming");
} else {
mavlink_and_console_log_critical(mavlink_fd, "Preflight check failed, refusing to arm");
}
feedback_provided = true;
} else if ((new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
status->condition_system_sensors_initialized) {
mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, power cycle to complete");
feedback_provided = true;
} else {
mavlink_log_critical(mavlink_fd, "Preflight check failed, refusing to arm");
}
feedback_provided = true;
}
// Finish up the state transition

11
src/modules/dataman/dataman.c
View File

@ -688,20 +688,21 @@ task_main(int argc, char *argv[])
fsync(g_task_fd);
printf("dataman: ");
/* see if we need to erase any items based on restart type */
int sys_restart_val;
if (param_get(param_find("SYS_RESTART_TYPE"), &sys_restart_val) == OK) {
if (sys_restart_val == DM_INIT_REASON_POWER_ON) {
warnx("Power on restart");
printf("Power on restart");
_restart(DM_INIT_REASON_POWER_ON);
} else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) {
warnx("In flight restart");
printf("In flight restart");
_restart(DM_INIT_REASON_IN_FLIGHT);
} else {
warnx("Unknown restart");
printf("Unknown restart");
}
} else {
warnx("Unknown restart");
printf("Unknown restart");
}
/* We use two file descriptors, one for the caller context and one for the worker thread */
@ -709,7 +710,7 @@ task_main(int argc, char *argv[])
/* worker thread is shutting down but still processing requests */
g_fd = g_task_fd;
warnx("Initialized, data manager file '%s' size is %d bytes", k_data_manager_device_path, max_offset);
printf(", data manager file '%s' size is %d bytes\n", k_data_manager_device_path, max_offset);
/* Tell startup that the worker thread has completed its initialization */
sem_post(&g_init_sema);

2
src/modules/mavlink/mavlink_main.cpp
View File

@ -93,7 +93,7 @@
static const int ERROR = -1;
#define DEFAULT_DEVICE_NAME "/dev/ttyS1"
#define MAX_DATA_RATE 20000 ///< max data rate in bytes/s
#define MAX_DATA_RATE 60000 ///< max data rate in bytes/s
#define MAIN_LOOP_DELAY 10000 ///< 100 Hz @ 1000 bytes/s data rate
#define FLOW_CONTROL_DISABLE_THRESHOLD 40 ///< picked so that some messages still would fit it.

5
src/modules/mavlink/mavlink_messages.cpp
View File

@ -542,7 +542,8 @@ protected:
msg.errors_count2 = status.errors_count2;
msg.errors_count3 = status.errors_count3;
msg.errors_count4 = status.errors_count4;
msg.battery_remaining = status.battery_remaining * 100.0f;
msg.battery_remaining = (msg.voltage_battery > 0) ?
status.battery_remaining * 100.0f : -1;
_mavlink->send_message(MAVLINK_MSG_ID_SYS_STATUS, &msg);
}
@ -2233,7 +2234,7 @@ protected:
};
StreamListItem *streams_list[] = {
const StreamListItem *streams_list[] = {
new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static),
new StreamListItem(&MavlinkStreamStatustext::new_instance, &MavlinkStreamStatustext::get_name_static),
new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static),

2
src/modules/mavlink/mavlink_messages.h
View File

@ -56,6 +56,6 @@ public:
~StreamListItem() {};
};
extern StreamListItem *streams_list[];
extern const StreamListItem *streams_list[];
#endif /* MAVLINK_MESSAGES_H_ */

5
src/modules/mavlink/mavlink_parameters.cpp
View File

@ -130,7 +130,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
} else {
/* when index is >= 0, send this parameter again */
send_param(param_for_index(req_read.param_index));
send_param(param_for_used_index(req_read.param_index));
}
}
break;
@ -192,6 +192,7 @@ MavlinkParametersManager::send(const hrt_abstime t)
/* look for the first parameter which is used */
param_t p;
do {
/* walk through all parameters, including unused ones */
p = param_for_index(_send_all_index);
_send_all_index++;
} while (p != PARAM_INVALID && !param_used(p));
@ -200,7 +201,7 @@ MavlinkParametersManager::send(const hrt_abstime t)
send_param(p);
}
if (_send_all_index >= (int) param_count()) {
if ((p == PARAM_INVALID) || (_send_all_index >= (int) param_count())) {
_send_all_index = -1;
}
}

2
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -824,7 +824,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
/* Publish attitude setpoint if attitude and thrust ignore bits are not set */
if (!(_offboard_control_mode.ignore_attitude)) {
static struct vehicle_attitude_setpoint_s att_sp = {};
struct vehicle_attitude_setpoint_s att_sp = {};
att_sp.timestamp = hrt_absolute_time();
mavlink_quaternion_to_euler(set_attitude_target.q,
&att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body);

2
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -908,7 +908,7 @@ MulticopterAttitudeControl::start()
_control_task = task_spawn_cmd("mc_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
1600,
1500,
(px4_main_t)&MulticopterAttitudeControl::task_main_trampoline,
nullptr);

2
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -1426,7 +1426,7 @@ MulticopterPositionControl::start()
_control_task = task_spawn_cmd("mc_pos_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
1600,
1500,
(main_t)&MulticopterPositionControl::task_main_trampoline,
nullptr);

2
src/modules/navigator/mission_feasibility_checker.cpp
View File

@ -134,7 +134,7 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
{
/* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */
for (size_t i = 0; i < nMissionItems; i++) {
static struct mission_item_s missionitem;
struct mission_item_s missionitem;
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(dm_current, i, &missionitem, len) != len) {

2
src/modules/navigator/navigator_main.cpp
View File

@ -518,7 +518,7 @@ Navigator::start()
_navigator_task = task_spawn_cmd("navigator",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT + 20,
1800,
1700,
(main_t)&Navigator::task_main_trampoline,
nullptr);

12
src/modules/navigator/rtl_params.c
View File

@ -47,18 +47,6 @@
* RTL parameters, accessible via MAVLink
*/
/**
* Loiter radius after RTL (FW only)
*
* Default value of loiter radius after RTL (fixedwing only).
*
* @unit meters
* @min 20
* @max 200
* @group Return To Land
*/
PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f);
/**
* RTL altitude
*

184
src/modules/sdlog2/sdlog2.c
View File

@ -52,7 +52,6 @@
#include <errno.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <stdlib.h>
#include <string.h>
#include <ctype.h>
@ -152,11 +151,6 @@ PARAM_DEFINE_INT32(SDLOG_EXT, -1);
log_msgs_skipped++; \
}
#define LOG_ORB_SUBSCRIBE(_var, _topic) subs.##_var##_sub = orb_subscribe(ORB_ID(##_topic##)); \
fds[fdsc_count].fd = subs.##_var##_sub; \
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 */
@ -219,7 +213,7 @@ static void *logwriter_thread(void *arg);
*/
__EXPORT int sdlog2_main(int argc, char *argv[]);
static bool copy_if_updated(orb_id_t topic, int handle, void *buffer);
static bool copy_if_updated(orb_id_t topic, int *handle, void *buffer);
/**
* Mainloop of sd log deamon.
@ -814,14 +808,25 @@ int write_parameters(int fd)
return written;
}
bool copy_if_updated(orb_id_t topic, int handle, void *buffer)
bool copy_if_updated(orb_id_t topic, int *handle, void *buffer)
{
bool updated;
bool updated = false;
orb_check(handle, &updated);
if (*handle < 0) {
if (OK == orb_exists(topic, 0)) {
*handle = orb_subscribe(topic);
/* copy first data */
if (*handle >= 0) {
orb_copy(topic, *handle, buffer);
updated = true;
}
}
} else {
orb_check(*handle, &updated);
if (updated) {
orb_copy(topic, handle, buffer);
if (updated) {
orb_copy(topic, *handle, buffer);
}
}
return updated;
@ -1127,54 +1132,47 @@ int sdlog2_thread_main(int argc, char *argv[])
int mc_att_ctrl_status_sub;
} subs;
subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
subs.status_sub = orb_subscribe(ORB_ID(vehicle_status));
subs.vtol_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status));
subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
subs.act_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs));
subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
subs.act_controls_1_sub = orb_subscribe(ORB_ID(actuator_controls_1));
subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
subs.local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
subs.vision_pos_sub = orb_subscribe(ORB_ID(vision_position_estimate));
subs.flow_sub = orb_subscribe(ORB_ID(optical_flow));
subs.rc_sub = orb_subscribe(ORB_ID(rc_channels));
subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
subs.esc_sub = orb_subscribe(ORB_ID(esc_status));
subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
subs.battery_sub = orb_subscribe(ORB_ID(battery_status));
subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status));
subs.system_power_sub = orb_subscribe(ORB_ID(system_power));
subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status));
subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate));
subs.tsync_sub = orb_subscribe(ORB_ID(time_offset));
subs.mc_att_ctrl_status_sub = orb_subscribe(ORB_ID(mc_att_ctrl_status));
/* we need to rate-limit wind, as we do not need the full update rate */
orb_set_interval(subs.wind_sub, 90);
subs.encoders_sub = orb_subscribe(ORB_ID(encoders));
subs.cmd_sub = -1;
subs.status_sub = -1;
subs.vtol_status_sub = -1;
subs.gps_pos_sub = -1;
subs.sensor_sub = -1;
subs.att_sub = -1;
subs.att_sp_sub = -1;
subs.rates_sp_sub = -1;
subs.act_outputs_sub = -1;
subs.act_controls_sub = -1;
subs.act_controls_1_sub = -1;
subs.local_pos_sub = -1;
subs.local_pos_sp_sub = -1;
subs.global_pos_sub = -1;
subs.triplet_sub = -1;
subs.vicon_pos_sub = -1;
subs.vision_pos_sub = -1;
subs.flow_sub = -1;
subs.rc_sub = -1;
subs.airspeed_sub = -1;
subs.esc_sub = -1;
subs.global_vel_sp_sub = -1;
subs.battery_sub = -1;
subs.range_finder_sub = -1;
subs.estimator_status_sub = -1;
subs.tecs_status_sub = -1;
subs.system_power_sub = -1;
subs.servorail_status_sub = -1;
subs.wind_sub = -1;
subs.tsync_sub = -1;
subs.mc_att_ctrl_status_sub = -1;
subs.encoders_sub = -1;
/* add new topics HERE */
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
subs.telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
}
if (_extended_logging) {
subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info));
} else {
subs.sat_info_sub = 0;
for (unsigned i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
subs.telemetry_subs[i] = -1;
}
subs.sat_info_sub = -1;
/* close non-needed fd's */
@ -1222,12 +1220,12 @@ int sdlog2_thread_main(int argc, char *argv[])
usleep(sleep_delay);
/* --- VEHICLE COMMAND - LOG MANAGEMENT --- */
if (copy_if_updated(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd)) {
if (copy_if_updated(ORB_ID(vehicle_command), &subs.cmd_sub, &buf.cmd)) {
handle_command(&buf.cmd);
}
/* --- VEHICLE STATUS - LOG MANAGEMENT --- */
bool status_updated = copy_if_updated(ORB_ID(vehicle_status), subs.status_sub, &buf_status);
bool status_updated = copy_if_updated(ORB_ID(vehicle_status), &subs.status_sub, &buf_status);
if (status_updated) {
if (log_when_armed) {
@ -1236,7 +1234,7 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- GPS POSITION - LOG MANAGEMENT --- */
bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos);
bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), &subs.gps_pos_sub, &buf_gps_pos);
if (gps_pos_updated && log_name_timestamp) {
gps_time = buf_gps_pos.time_utc_usec;
@ -1267,7 +1265,7 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- VTOL VEHICLE STATUS --- */
if(copy_if_updated(ORB_ID(vtol_vehicle_status), subs.vtol_status_sub, &buf.vtol_status)) {
if(copy_if_updated(ORB_ID(vtol_vehicle_status), &subs.vtol_status_sub, &buf.vtol_status)) {
log_msg.msg_type = LOG_VTOL_MSG;
log_msg.body.log_VTOL.airspeed_tot = buf.vtol_status.airspeed_tot;
LOGBUFFER_WRITE_AND_COUNT(VTOL);
@ -1298,7 +1296,7 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- SATELLITE INFO - UNIT #1 --- */
if (_extended_logging) {
if (copy_if_updated(ORB_ID(satellite_info), subs.sat_info_sub, &buf.sat_info)) {
if (copy_if_updated(ORB_ID(satellite_info), &subs.sat_info_sub, &buf.sat_info)) {
/* log the SNR of each satellite for a detailed view of signal quality */
unsigned sat_info_count = MIN(buf.sat_info.count, sizeof(buf.sat_info.snr) / sizeof(buf.sat_info.snr[0]));
@ -1344,7 +1342,7 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- SENSOR COMBINED --- */
if (copy_if_updated(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor)) {
if (copy_if_updated(ORB_ID(sensor_combined), &subs.sensor_sub, &buf.sensor)) {
bool write_IMU = false;
bool write_IMU1 = false;
bool write_IMU2 = false;
@ -1486,7 +1484,7 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- ATTITUDE --- */
if (copy_if_updated(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att)) {
if (copy_if_updated(ORB_ID(vehicle_attitude), &subs.att_sub, &buf.att)) {
log_msg.msg_type = LOG_ATT_MSG;
log_msg.body.log_ATT.q_w = buf.att.q[0];
log_msg.body.log_ATT.q_x = buf.att.q[1];
@ -1505,7 +1503,7 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- ATTITUDE SETPOINT --- */
if (copy_if_updated(ORB_ID(vehicle_attitude_setpoint), subs.att_sp_sub, &buf.att_sp)) {
if (copy_if_updated(ORB_ID(vehicle_attitude_setpoint), &subs.att_sp_sub, &buf.att_sp)) {
log_msg.msg_type = LOG_ATSP_MSG;
log_msg.body.log_ATSP.roll_sp = buf.att_sp.roll_body;
log_msg.body.log_ATSP.pitch_sp = buf.att_sp.pitch_body;
@ -1519,7 +1517,7 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- RATES SETPOINT --- */
if (copy_if_updated(ORB_ID(vehicle_rates_setpoint), subs.rates_sp_sub, &buf.rates_sp)) {
if (copy_if_updated(ORB_ID(vehicle_rates_setpoint), &subs.rates_sp_sub, &buf.rates_sp)) {
log_msg.msg_type = LOG_ARSP_MSG;
log_msg.body.log_ARSP.roll_rate_sp = buf.rates_sp.roll;
log_msg.body.log_ARSP.pitch_rate_sp = buf.rates_sp.pitch;
@ -1528,14 +1526,14 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- ACTUATOR OUTPUTS --- */
if (copy_if_updated(ORB_ID(actuator_outputs), subs.act_outputs_sub, &buf.act_outputs)) {
if (copy_if_updated(ORB_ID(actuator_outputs), &subs.act_outputs_sub, &buf.act_outputs)) {
log_msg.msg_type = LOG_OUT0_MSG;
memcpy(log_msg.body.log_OUT0.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT0.output));
LOGBUFFER_WRITE_AND_COUNT(OUT0);
}
/* --- ACTUATOR CONTROL --- */
if (copy_if_updated(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.act_controls_sub, &buf.act_controls)) {
if (copy_if_updated(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &subs.act_controls_sub, &buf.act_controls)) {
log_msg.msg_type = LOG_ATTC_MSG;
log_msg.body.log_ATTC.roll = buf.act_controls.control[0];
log_msg.body.log_ATTC.pitch = buf.act_controls.control[1];
@ -1545,7 +1543,7 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- ACTUATOR CONTROL FW VTOL --- */
if(copy_if_updated(ORB_ID(actuator_controls_1),subs.act_controls_1_sub,&buf.act_controls)) {
if(copy_if_updated(ORB_ID(actuator_controls_1), &subs.act_controls_1_sub,&buf.act_controls)) {
log_msg.msg_type = LOG_ATC1_MSG;
log_msg.body.log_ATTC.roll = buf.act_controls.control[0];
log_msg.body.log_ATTC.pitch = buf.act_controls.control[1];
@ -1555,7 +1553,7 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- LOCAL POSITION --- */
if (copy_if_updated(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos)) {
if (copy_if_updated(ORB_ID(vehicle_local_position), &subs.local_pos_sub, &buf.local_pos)) {
log_msg.msg_type = LOG_LPOS_MSG;
log_msg.body.log_LPOS.x = buf.local_pos.x;
log_msg.body.log_LPOS.y = buf.local_pos.y;
@ -1581,7 +1579,7 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- LOCAL POSITION SETPOINT --- */
if (copy_if_updated(ORB_ID(vehicle_local_position_setpoint), subs.local_pos_sp_sub, &buf.local_pos_sp)) {
if (copy_if_updated(ORB_ID(vehicle_local_position_setpoint), &subs.local_pos_sp_sub, &buf.local_pos_sp)) {
log_msg.msg_type = LOG_LPSP_MSG;
log_msg.body.log_LPSP.x = buf.local_pos_sp.x;
log_msg.body.log_LPSP.y = buf.local_pos_sp.y;
@ -1597,7 +1595,7 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- GLOBAL POSITION --- */
if (copy_if_updated(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos)) {
if (copy_if_updated(ORB_ID(vehicle_global_position), &subs.global_pos_sub, &buf.global_pos)) {
log_msg.msg_type = LOG_GPOS_MSG;
log_msg.body.log_GPOS.lat = buf.global_pos.lat * 1e7;
log_msg.body.log_GPOS.lon = buf.global_pos.lon * 1e7;
@ -1616,7 +1614,7 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- GLOBAL POSITION SETPOINT --- */
if (copy_if_updated(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet)) {
if (copy_if_updated(ORB_ID(position_setpoint_triplet), &subs.triplet_sub, &buf.triplet)) {
if (buf.triplet.current.valid) {
log_msg.msg_type = LOG_GPSP_MSG;
@ -1634,7 +1632,7 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- VICON POSITION --- */
if (copy_if_updated(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos)) {
if (copy_if_updated(ORB_ID(vehicle_vicon_position), &subs.vicon_pos_sub, &buf.vicon_pos)) {
log_msg.msg_type = LOG_VICN_MSG;
log_msg.body.log_VICN.x = buf.vicon_pos.x;
log_msg.body.log_VICN.y = buf.vicon_pos.y;
@ -1646,7 +1644,7 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- VISION POSITION --- */
if (copy_if_updated(ORB_ID(vision_position_estimate), subs.vision_pos_sub, &buf.vision_pos)) {
if (copy_if_updated(ORB_ID(vision_position_estimate), &subs.vision_pos_sub, &buf.vision_pos)) {
log_msg.msg_type = LOG_VISN_MSG;
log_msg.body.log_VISN.x = buf.vision_pos.x;
log_msg.body.log_VISN.y = buf.vision_pos.y;
@ -1654,15 +1652,15 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_VISN.vx = buf.vision_pos.vx;
log_msg.body.log_VISN.vy = buf.vision_pos.vy;
log_msg.body.log_VISN.vz = buf.vision_pos.vz;
log_msg.body.log_VISN.qx = buf.vision_pos.q[0];
log_msg.body.log_VISN.qy = buf.vision_pos.q[1];
log_msg.body.log_VISN.qz = buf.vision_pos.q[2];
log_msg.body.log_VISN.qw = buf.vision_pos.q[3];
log_msg.body.log_VISN.qw = buf.vision_pos.q[0]; // vision_position_estimate uses [w,x,y,z] convention
log_msg.body.log_VISN.qx = buf.vision_pos.q[1];
log_msg.body.log_VISN.qy = buf.vision_pos.q[2];
log_msg.body.log_VISN.qz = buf.vision_pos.q[3];
LOGBUFFER_WRITE_AND_COUNT(VISN);
}
/* --- FLOW --- */
if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) {
if (copy_if_updated(ORB_ID(optical_flow), &subs.flow_sub, &buf.flow)) {
log_msg.msg_type = LOG_FLOW_MSG;
log_msg.body.log_FLOW.ground_distance_m = buf.flow.ground_distance_m;
log_msg.body.log_FLOW.gyro_temperature = buf.flow.gyro_temperature;
@ -1678,7 +1676,7 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- RC CHANNELS --- */
if (copy_if_updated(ORB_ID(rc_channels), subs.rc_sub, &buf.rc)) {
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.channels, sizeof(log_msg.body.log_RC.channel));
@ -1688,7 +1686,7 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- AIRSPEED --- */
if (copy_if_updated(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed)) {
if (copy_if_updated(ORB_ID(airspeed), &subs.airspeed_sub, &buf.airspeed)) {
log_msg.msg_type = LOG_AIRS_MSG;
log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s;
log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s;
@ -1697,7 +1695,7 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- ESCs --- */
if (copy_if_updated(ORB_ID(esc_status), subs.esc_sub, &buf.esc)) {
if (copy_if_updated(ORB_ID(esc_status), &subs.esc_sub, &buf.esc)) {
for (uint8_t i = 0; i < buf.esc.esc_count; i++) {
log_msg.msg_type = LOG_ESC_MSG;
log_msg.body.log_ESC.counter = buf.esc.counter;
@ -1717,7 +1715,7 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- GLOBAL VELOCITY SETPOINT --- */
if (copy_if_updated(ORB_ID(vehicle_global_velocity_setpoint), subs.global_vel_sp_sub, &buf.global_vel_sp)) {
if (copy_if_updated(ORB_ID(vehicle_global_velocity_setpoint), &subs.global_vel_sp_sub, &buf.global_vel_sp)) {
log_msg.msg_type = LOG_GVSP_MSG;
log_msg.body.log_GVSP.vx = buf.global_vel_sp.vx;
log_msg.body.log_GVSP.vy = buf.global_vel_sp.vy;
@ -1726,7 +1724,7 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- BATTERY --- */
if (copy_if_updated(ORB_ID(battery_status), subs.battery_sub, &buf.battery)) {
if (copy_if_updated(ORB_ID(battery_status), &subs.battery_sub, &buf.battery)) {
log_msg.msg_type = LOG_BATT_MSG;
log_msg.body.log_BATT.voltage = buf.battery.voltage_v;
log_msg.body.log_BATT.voltage_filtered = buf.battery.voltage_filtered_v;
@ -1736,7 +1734,7 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- SYSTEM POWER RAILS --- */
if (copy_if_updated(ORB_ID(system_power), subs.system_power_sub, &buf.system_power)) {
if (copy_if_updated(ORB_ID(system_power), &subs.system_power_sub, &buf.system_power)) {
log_msg.msg_type = LOG_PWR_MSG;
log_msg.body.log_PWR.peripherals_5v = buf.system_power.voltage5V_v;
log_msg.body.log_PWR.usb_ok = buf.system_power.usb_connected;
@ -1754,8 +1752,8 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- TELEMETRY --- */
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
if (copy_if_updated(telemetry_status_orb_id[i], subs.telemetry_subs[i], &buf.telemetry)) {
for (unsigned i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
if (copy_if_updated(telemetry_status_orb_id[i], &subs.telemetry_subs[i], &buf.telemetry)) {
log_msg.msg_type = LOG_TEL0_MSG + i;
log_msg.body.log_TEL.rssi = buf.telemetry.rssi;
log_msg.body.log_TEL.remote_rssi = buf.telemetry.remote_rssi;
@ -1770,7 +1768,7 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- BOTTOM DISTANCE --- */
if (copy_if_updated(ORB_ID(sensor_range_finder), subs.range_finder_sub, &buf.range_finder)) {
if (copy_if_updated(ORB_ID(sensor_range_finder), &subs.range_finder_sub, &buf.range_finder)) {
log_msg.msg_type = LOG_DIST_MSG;
log_msg.body.log_DIST.bottom = buf.range_finder.distance;
log_msg.body.log_DIST.bottom_rate = 0.0f;
@ -1779,7 +1777,7 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- ESTIMATOR STATUS --- */
if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) {
if (copy_if_updated(ORB_ID(estimator_status), &subs.estimator_status_sub, &buf.estimator_status)) {
log_msg.msg_type = LOG_EST0_MSG;
unsigned maxcopy0 = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_EST0.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_EST0.s);
memset(&(log_msg.body.log_EST0.s), 0, sizeof(log_msg.body.log_EST0.s));
@ -1798,7 +1796,7 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- TECS STATUS --- */
if (copy_if_updated(ORB_ID(tecs_status), subs.tecs_status_sub, &buf.tecs_status)) {
if (copy_if_updated(ORB_ID(tecs_status), &subs.tecs_status_sub, &buf.tecs_status)) {
log_msg.msg_type = LOG_TECS_MSG;
log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp;
log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitude_filtered;
@ -1818,7 +1816,7 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- WIND ESTIMATE --- */
if (copy_if_updated(ORB_ID(wind_estimate), subs.wind_sub, &buf.wind_estimate)) {
if (copy_if_updated(ORB_ID(wind_estimate), &subs.wind_sub, &buf.wind_estimate)) {
log_msg.msg_type = LOG_WIND_MSG;
log_msg.body.log_WIND.x = buf.wind_estimate.windspeed_north;
log_msg.body.log_WIND.y = buf.wind_estimate.windspeed_east;
@ -1828,7 +1826,7 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- ENCODERS --- */
if (copy_if_updated(ORB_ID(encoders), subs.encoders_sub, &buf.encoders)) {
if (copy_if_updated(ORB_ID(encoders), &subs.encoders_sub, &buf.encoders)) {
log_msg.msg_type = LOG_ENCD_MSG;
log_msg.body.log_ENCD.cnt0 = buf.encoders.counts[0];
log_msg.body.log_ENCD.vel0 = buf.encoders.velocity[0];
@ -1838,14 +1836,14 @@ int sdlog2_thread_main(int argc, char *argv[])
}
/* --- TIMESYNC OFFSET --- */
if (copy_if_updated(ORB_ID(time_offset), subs.tsync_sub, &buf.time_offset)) {
if (copy_if_updated(ORB_ID(time_offset), &subs.tsync_sub, &buf.time_offset)) {
log_msg.msg_type = LOG_TSYN_MSG;
log_msg.body.log_TSYN.time_offset = buf.time_offset.offset_ns;
LOGBUFFER_WRITE_AND_COUNT(TSYN);
}
/* --- MULTIROTOR ATTITUDE CONTROLLER STATUS --- */
if (copy_if_updated(ORB_ID(mc_att_ctrl_status), subs.mc_att_ctrl_status_sub, &buf.mc_att_ctrl_status)) {
if (copy_if_updated(ORB_ID(mc_att_ctrl_status), &subs.mc_att_ctrl_status_sub, &buf.mc_att_ctrl_status)) {
log_msg.msg_type = LOG_MACS_MSG;
log_msg.body.log_MACS.roll_rate_integ = buf.mc_att_ctrl_status.roll_rate_integ;
log_msg.body.log_MACS.pitch_rate_integ = buf.mc_att_ctrl_status.pitch_rate_integ;

4
src/modules/sdlog2/sdlog2_messages.h
View File

@ -441,7 +441,7 @@ struct log_ENCD_s {
};
/* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */
#define LOG_AIR1_MSG 40
#define LOG_AIR1_MSG 41
/* --- VTOL - VTOL VEHICLE STATUS */
#define LOG_VTOL_MSG 42
@ -456,7 +456,7 @@ struct log_TSYN_s {
};
/* --- MACS - MULTIROTOR ATTITUDE CONTROLLER STATUS */
#define LOG_MACS_MSG 42
#define LOG_MACS_MSG 44
struct log_MACS_s {
float roll_rate_integ;
float pitch_rate_integ;

39
src/modules/sensors/sensors.cpp
View File

@ -149,7 +149,7 @@
#endif
static const int ERROR = -1;
#define CAL_FAILED_APPLY_CAL_MSG "FAILED APPLYING %s CAL #%u"
#define CAL_ERROR_APPLY_CAL_MSG "FAILED APPLYING %s CAL #%u"
/**
* Sensor app start / stop handling function
@ -250,6 +250,7 @@ private:
struct differential_pressure_s _diff_pres;
struct airspeed_s _airspeed;
struct rc_parameter_map_s _rc_parameter_map;
float _param_rc_values[RC_PARAM_MAP_NCHAN]; /**< parameter values for RC control */
math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
math::Matrix<3, 3> _mag_rotation[3]; /**< rotation matrix for the orientation that the external mag0 is mounted */
@ -527,6 +528,7 @@ Sensors::Sensors() :
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
_param_rc_values{},
_board_rotation{},
_mag_rotation{},
@ -624,6 +626,18 @@ Sensors::Sensors() :
/* Barometer QNH */
_parameter_handles.baro_qnh = param_find("SENS_BARO_QNH");
// These are parameters for which QGroundControl always expects to be returned in a list request.
// We do a param_find here to force them into the list.
(void)param_find("RC_CHAN_CNT");
(void)param_find("RC_TH_USER");
(void)param_find("CAL_MAG0_ID");
(void)param_find("CAL_MAG1_ID");
(void)param_find("CAL_MAG2_ID");
(void)param_find("CAL_MAG0_ROT");
(void)param_find("CAL_MAG1_ROT");
(void)param_find("CAL_MAG2_ROT");
(void)param_find("SYS_PARAM_VER");
/* fetch initial parameter values */
parameters_update();
}
@ -1386,12 +1400,12 @@ Sensors::parameter_update_poll(bool forced)
failed = failed || (OK != param_get(param_find(str), &gscale.z_scale));
if (failed) {
warnx(CAL_FAILED_APPLY_CAL_MSG, "gyro", i);
warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i);
} else {
/* apply new scaling and offsets */
res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale);
if (res) {
warnx(CAL_FAILED_APPLY_CAL_MSG, "gyro", i);
warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i);
} else {
config_ok = true;
}
@ -1452,12 +1466,12 @@ Sensors::parameter_update_poll(bool forced)
failed = failed || (OK != param_get(param_find(str), &gscale.z_scale));
if (failed) {
warnx(CAL_FAILED_APPLY_CAL_MSG, "accel", i);
warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i);
} else {
/* apply new scaling and offsets */
res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&gscale);
if (res) {
warnx(CAL_FAILED_APPLY_CAL_MSG, "accel", i);
warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i);
} else {
config_ok = true;
}
@ -1575,12 +1589,12 @@ Sensors::parameter_update_poll(bool forced)
}
if (failed) {
warnx(CAL_FAILED_APPLY_CAL_MSG, "mag", i);
warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i);
} else {
/* apply new scaling and offsets */
res = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&gscale);
if (res) {
warnx(CAL_FAILED_APPLY_CAL_MSG, "mag", i);
warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i);
} else {
config_ok = true;
}
@ -1613,7 +1627,8 @@ Sensors::parameter_update_poll(bool forced)
px4_close(fd);
}
warnx("valid configs: %u gyros, %u mags, %u accels", gyro_count, mag_count, accel_count);
/* do not output this for now, as its covered in preflight checks */
// warnx("valid configs: %u gyros, %u mags, %u accels", gyro_count, mag_count, accel_count);
}
}
@ -1637,7 +1652,7 @@ Sensors::rc_parameter_map_poll(bool forced)
/* Set the handle by index if the index is set, otherwise use the id */
if (_rc_parameter_map.param_index[i] >= 0) {
_parameter_handles.rc_param[i] = param_for_index((unsigned)_rc_parameter_map.param_index[i]);
_parameter_handles.rc_param[i] = param_for_used_index((unsigned)_rc_parameter_map.param_index[i]);
} else {
_parameter_handles.rc_param[i] = param_find(_rc_parameter_map.param_id[i]);
@ -1842,8 +1857,6 @@ Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv)
void
Sensors::set_params_from_rc()
{
static float param_rc_values[RC_PARAM_MAP_NCHAN] = {};
for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
/* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0)
@ -1855,8 +1868,8 @@ Sensors::set_params_from_rc()
float rc_val = get_rc_value((rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0);
/* Check if the value has changed,
* maybe we need to introduce a more aggressive limit here */
if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) {
param_rc_values[i] = rc_val;
if (rc_val > _param_rc_values[i] + FLT_EPSILON || rc_val < _param_rc_values[i] - FLT_EPSILON) {
_param_rc_values[i] = rc_val;
float param_val = math::constrain(
_rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val,
_rc_parameter_map.value_min[i], _rc_parameter_map.value_max[i]);

47
src/modules/systemlib/param/param.c
View File

@ -93,7 +93,7 @@ struct param_wbuf_s {
};
// XXX this should be param_info_count, but need to work out linking
uint8_t param_changed_storage[(600 / sizeof(uint8_t)) + 1] = {};
uint8_t param_changed_storage[(700 / sizeof(uint8_t)) + 1] = {};
/** flexible array holding modified parameter values */
UT_array *param_values;
@ -266,8 +266,37 @@ param_count_used(void)
param_t
param_for_index(unsigned index)
{
if (index < param_info_count)
if (index < param_info_count) {
return (param_t)index;
}
return PARAM_INVALID;
}
param_t
param_for_used_index(unsigned index)
{
if (index < param_info_count) {
/* walk all params and count */
int count = 0;
for (unsigned i = 0; i < (unsigned)param_info_count + 1; i++) {
for (unsigned j = 0; j < 8; j++) {
if (param_changed_storage[i] & (1 << j)) {
/* we found the right used count,
* return the param value
*/
if (index == count) {
return (param_t)i;
}
count++;
}
}
}
}
return PARAM_INVALID;
}
@ -275,8 +304,9 @@ param_for_index(unsigned index)
int
param_get_index(param_t param)
{
if (handle_in_range(param))
if (handle_in_range(param)) {
return (unsigned)param;
}
return -1;
}
@ -284,7 +314,9 @@ param_get_index(param_t param)
int
param_get_used_index(param_t param)
{
if (!handle_in_range(param)) {
int param_storage_index = param_get_index(param);
if (param_storage_index < 0) {
return -1;
}
@ -294,12 +326,17 @@ param_get_used_index(param_t param)
for (unsigned i = 0; i < (unsigned)param + 1; i++) {
for (unsigned j = 0; j < 8; j++) {
if (param_changed_storage[i] & (1 << j)) {
if (param_storage_index == i) {
return count;
}
count++;
}
}
}
return count;
return -1;
}
const char *

8
src/modules/systemlib/param/param.h
View File

@ -129,6 +129,14 @@ __EXPORT bool param_used(param_t param);
*/
__EXPORT param_t param_for_index(unsigned index);
/**
* Look up an used parameter by index.
*
* @param param The parameter to obtain the index for.
* @return The index of the parameter in use, or -1 if the parameter does not exist.
*/
__EXPORT param_t param_for_used_index(unsigned index);
/**
* Look up the index of a parameter.
*

20
src/modules/uORB/uORB.cpp
View File

@ -41,6 +41,7 @@
#include <drivers/device/device.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
@ -1196,6 +1197,25 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve
} // namespace
int
orb_exists(const struct orb_metadata *meta, int instance)
{
/*
* Generate the path to the node and try to open it.
*/
char path[orb_maxpath];
int inst = instance;
int ret = node_mkpath(path, PUBSUB, meta, &inst);
if (ret != OK) {
errno = -ret;
return ERROR;
}
struct stat buffer;
return stat(path, &buffer);
}
orb_advert_t
orb_advertise(const struct orb_metadata *meta, const void *data)
{

9
src/modules/uORB/uORB.h
View File

@ -318,6 +318,15 @@ extern int orb_check(int handle, bool *updated) __EXPORT;
*/
extern int orb_stat(int handle, uint64_t *time) __EXPORT;
/**
* Check if a topic has already been created.
*
* @param meta ORB topic metadata.
* @param instance ORB instance
* @return OK if the topic exists, ERROR otherwise with errno set accordingly.
*/
extern int orb_exists(const struct orb_metadata *meta, int instance) __EXPORT;
/**
* Return the priority of the topic
*

4
src/modules/uavcan/uavcan_main.cpp
View File

@ -698,7 +698,9 @@ int uavcan_main(int argc, char *argv[])
if (!std::strcmp(argv[1], "start")) {
if (UavcanNode::instance()) {
errx(1, "already started");
// Already running, no error
warnx("already started");
::exit(0);
}
// Node ID

4
src/systemcmds/param/param.c
View File

@ -294,9 +294,9 @@ do_show_print(void *arg, param_t param)
}
}
printf("%c %c %s: ", (param_used(param) ? 'x' : ' '),
printf("%c %c %s [%d,%d] : ", (param_used(param) ? 'x' : ' '),
param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'),
param_name(param));
param_name(param), param_get_used_index(param), param_get_index(param));
/*
* This case can be expanded to handle printing common structure types.