diff --git a/NuttX b/NuttX index 94818ce16f..678eea3d2c 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit 94818ce16f63c4728a1d9316628a3651287f16df +Subproject commit 678eea3d2c157c686439597abb0367b0f1e643f4 diff --git a/ROMFS/px4fmu_common/init.d/13002_firefly6 b/ROMFS/px4fmu_common/init.d/13002_firefly6 index 9489bfd9a2..191c58da4c 100644 --- a/ROMFS/px4fmu_common/init.d/13002_firefly6 +++ b/ROMFS/px4fmu_common/init.d/13002_firefly6 @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io index e957626ce0..86042220e4 100644 --- a/ROMFS/px4fmu_common/init.d/rc.io +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index fddcf5b3e9..e456eff7f1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -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 diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 67ec7821c9..1237d9bb11 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -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 diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 2e001cac73..2a7e493a39 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -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 diff --git a/nuttx-configs/aerocore/nsh/defconfig b/nuttx-configs/aerocore/nsh/defconfig index 08ff4a9885..e314bf1d82 100644 --- a/nuttx-configs/aerocore/nsh/defconfig +++ b/nuttx-configs/aerocore/nsh/defconfig @@ -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 diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index 389cb5ab5b..2c9a21f212 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -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 diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index d331aa26a7..4ccc5dacb6 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -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 diff --git a/src/drivers/blinkm/blinkm_nuttx.cpp b/src/drivers/blinkm/blinkm_nuttx.cpp index 0b9dfd2051..91cf50af91 100644 --- a/src/drivers/blinkm/blinkm_nuttx.cpp +++ b/src/drivers/blinkm/blinkm_nuttx.cpp @@ -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); diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c index 85abe0671c..2b2b5ccd7a 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c @@ -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; } diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index c95c2a46e5..6a718fdd24 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -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; diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 639012dcf9..92ffa80665 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -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); + } } /** diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 2adcf1ac9a..f35957caae 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -674,7 +674,7 @@ fail: g_dev = nullptr; } - errx(1, "driver start failed"); + errx(1, "no PX4 FLOW connected"); } /** diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 85049abc1f..6491253017 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -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; } } diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index bb01e18f1d..6c90f74a8f 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -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); diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 5567b43529..c4c674bf09 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -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 } diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index e4c1d201b2..f7aace7753 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -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; } diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 87a2998dee..f2f4f0c64f 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -38,6 +38,7 @@ #include "airspeed_calibration.h" #include "calibration_messages.h" +#include "calibration_routines.h" #include "commander_helper.h" #include @@ -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; } diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h index 29f44dc368..7b4baae623 100644 --- a/src/modules/commander/calibration_messages.h +++ b/src/modules/commander/calibration_messages.h @@ -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_ */ diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index fe256e60ad..cddf2ab2c7 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -52,6 +52,8 @@ #include #include +#include + #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; +} diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h index 8f34f02044..b8232730ac 100644 --- a/src/modules/commander/calibration_routines.h +++ b/src/modules/commander/calibration_routines.h @@ -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 - */ +/// @file calibration_routines.h +/// @authot Don Gagne /** * 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 diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e36bfd474a..73411bb1c3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -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, ¶m); @@ -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); diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 161d516f7b..c1d6b08423 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -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; diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index deceed011a..1f45c4be5c 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -39,6 +39,7 @@ #include "gyro_calibration.h" #include "calibration_messages.h" +#include "calibration_routines.h" #include "commander_helper.h" #include @@ -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; } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 82f404914a..2790d6838d 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -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(malloc(sizeof(float) * calibration_points_maxcount)); worker_data.z[cur_mag] = reinterpret_cast(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_maghil_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 diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 75f8c6dc86..08f3240b86 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -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); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5aa3a10a5d..947ee08229 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -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. diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 648b875800..33871a048e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -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), diff --git a/src/modules/mavlink/mavlink_messages.h b/src/modules/mavlink/mavlink_messages.h index 7e44166099..5b6b9d6335 100644 --- a/src/modules/mavlink/mavlink_messages.h +++ b/src/modules/mavlink/mavlink_messages.h @@ -56,6 +56,6 @@ public: ~StreamListItem() {}; }; -extern StreamListItem *streams_list[]; +extern const StreamListItem *streams_list[]; #endif /* MAVLINK_MESSAGES_H_ */ diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 20d7cfdbbe..9c8794017b 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -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; } } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index db508b6f04..13d6743063 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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); diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 37f7eae671..6ac1192744 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -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); diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 5191a4de3b..6ffb37d970 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -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); diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 904f442389..949231b159 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -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) { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e5e7103ca4..bc6d3ea9f1 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -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); diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index 2b3465eae6..a33ded28ab 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -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 * diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 7d27016703..0cf5e885cb 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -52,7 +52,6 @@ #include #include #include -#include #include #include #include @@ -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; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index c6f967af43..abdf518c51 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -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; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index adf231840c..b2867eeffa 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -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]); diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index e42109da4d..2b9a09b712 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -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 * diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h index da810abea5..50d7df8dbd 100644 --- a/src/modules/systemlib/param/param.h +++ b/src/modules/systemlib/param/param.h @@ -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. * diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 64ee78414d..a878853dff 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -41,6 +41,7 @@ #include #include +#include #include #include #include @@ -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) { diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index 44dc6614fe..a8b19d91f9 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -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 * diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 078decd92c..bc89b956b3 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -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 diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 1bc3df4b7b..716a5b2d0b 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -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.