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

drivers/modules: changes after mavlink_log change

Browse Source 
The mavlink_log API changes lead to changes in all drivers/modules using
it.
This commit is contained in:
Julian Oes
2016-03-24 13:10:02 +01:00
parent b49b012d35
commit bba0d0384d
62 changed files with 731 additions and 831 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/PreflightCheck.cpp
+39 -50
View File
@@ -54,6 +54,7 @@
#include <systemlib/err.h>
#include <systemlib/param/param.h>
#include <systemlib/rc_check.h>
#include <systemlib/mavlink_log.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mag.h>
@@ -65,7 +66,6 @@
#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <mavlink/mavlink_log.h>
#include "PreflightCheck.h"
@@ -84,7 +84,7 @@ static int check_calibration(DevHandle &h, const char* param_template, int &devi
int ret = h.ioctl(SENSORIOCCALTEST, 0);
calibration_found = (ret == OK);
devid = h.ioctl(DEVIOCGDEVICEID, 0);
char s[20];
@@ -116,7 +116,7 @@ static int check_calibration(DevHandle &h, const char* param_template, int &devi
return !calibration_found;
}
static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail)
static bool magnometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, int &device_id, bool report_fail)
{
bool success = true;
@@ -128,8 +128,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
if (!h.isValid()) {
if (!optional) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO MAG SENSOR #%u", instance);
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO MAG SENSOR #%u", instance);
}
}
@@ -140,8 +139,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
if (ret) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance);
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance);
}
success = false;
goto out;
@@ -151,8 +149,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
if (ret != OK) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance);
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance);
}
success = false;
goto out;
@@ -163,7 +160,7 @@ out:
return success;
}
static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id, bool report_fail)
static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, bool dynamic, int &device_id, bool report_fail)
{
bool success = true;
@@ -175,8 +172,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
if (!h.isValid()) {
if (!optional) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance);
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance);
}
}
@@ -187,8 +183,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
if (ret) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
}
success = false;
goto out;
@@ -198,8 +193,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
if (ret != OK) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: ACCEL #%u TEST FAILED: %d", instance, ret);
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL #%u TEST FAILED: %d", instance, ret);
}
success = false;
goto out;
@@ -217,7 +211,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming");
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL RANGE, hold still on arming");
}
/* this is frickin' fatal */
success = false;
@@ -225,7 +219,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
}
} else {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL READ");
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ACCEL READ");
}
/* this is frickin' fatal */
success = false;
@@ -239,7 +233,7 @@ out:
return success;
}
static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail)
static bool gyroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, int &device_id, bool report_fail)
{
bool success = true;
@@ -251,8 +245,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
if (!h.isValid()) {
if (!optional) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance);
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance);
}
}
@@ -263,8 +256,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
if (ret) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
}
success = false;
goto out;
@@ -274,8 +266,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
if (ret != OK) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance);
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance);
}
success = false;
goto out;
@@ -286,7 +277,7 @@ out:
return success;
}
static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id, bool report_fail)
static bool baroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool optional, int &device_id, bool report_fail)
{
bool success = true;
@@ -298,8 +289,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
if (!h.isValid()) {
if (!optional) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO BARO SENSOR #%u", instance);
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: NO BARO SENSOR #%u", instance);
}
}
@@ -312,8 +302,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
// int ret = check_calibration(fd, "CAL_BARO%u_ID");
// if (ret) {
// mavlink_and_console_log_critical(mavlink_fd,
// "PREFLIGHT FAIL: BARO #%u UNCALIBRATED", instance);
// mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: BARO #%u UNCALIBRATED", instance);
// success = false;
// goto out;
// }
@@ -324,7 +313,7 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
return success;
}
static bool airspeedCheck(int mavlink_fd, bool optional, bool report_fail)
static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail)
{
bool success = true;
int ret;
@@ -335,7 +324,7 @@ static bool airspeedCheck(int mavlink_fd, bool optional, bool report_fail)
if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
(hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
}
success = false;
goto out;
@@ -343,7 +332,7 @@ static bool airspeedCheck(int mavlink_fd, bool optional, bool report_fail)
if (fabsf(airspeed.confidence) < 0.99f) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: AIRSPEED SENSOR COMM ERROR");
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR COMM ERROR");
}
success = false;
goto out;
@@ -351,7 +340,7 @@ static bool airspeedCheck(int mavlink_fd, bool optional, bool report_fail)
if (fabsf(airspeed.indicated_airspeed_m_s) > 6.0f) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
mavlink_and_console_log_critical(mavlink_log_pub, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
}
// XXX do not make this fatal yet
}
@@ -361,7 +350,7 @@ out:
return success;
}
static bool gnssCheck(int mavlink_fd, bool report_fail)
static bool gnssCheck(orb_advert_t *mavlink_log_pub, bool report_fail)
{
bool success = true;
@@ -385,7 +374,7 @@ static bool gnssCheck(int mavlink_fd, bool report_fail)
//Report failure to detect module
if (!success) {
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING");
mavlink_and_console_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: GPS RECEIVER MISSING");
}
}
@@ -393,7 +382,7 @@ static bool gnssCheck(int mavlink_fd, bool report_fail)
return success;
}
bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro,
bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc, bool checkGyro,
bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool reportFailures)
{
bool failed = false;
@@ -409,7 +398,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
bool required = (i < max_mandatory_mag_count);
int device_id = -1;
if (!magnometerCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) {
if (!magnometerCheck(mavlink_log_pub, i, !required, device_id, reportFailures) && required) {
failed = true;
}
@@ -421,7 +410,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary compass not found");
mavlink_and_console_log_critical(mavlink_log_pub, "Warning: Primary compass not found");
}
failed = true;
}
@@ -438,7 +427,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
bool required = (i < max_mandatory_accel_count);
int device_id = -1;
if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id, reportFailures) && required) {
if (!accelerometerCheck(mavlink_log_pub, i, !required, checkDynamic, device_id, reportFailures) && required) {
failed = true;
}
@@ -450,7 +439,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary accelerometer not found");
mavlink_and_console_log_critical(mavlink_log_pub, "Warning: Primary accelerometer not found");
}
failed = true;
}
@@ -467,7 +456,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
bool required = (i < max_mandatory_gyro_count);
int device_id = -1;
if (!gyroCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) {
if (!gyroCheck(mavlink_log_pub, i, !required, device_id, reportFailures) && required) {
failed = true;
}
@@ -479,7 +468,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_fd, "Warning: Primary gyro not found");
mavlink_and_console_log_critical(mavlink_log_pub, "Warning: Primary gyro not found");
}
failed = true;
}
@@ -496,7 +485,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
bool required = (i < max_mandatory_baro_count);
int device_id = -1;
if (!baroCheck(mavlink_fd, i, !required, device_id, reportFailures) && required) {
if (!baroCheck(mavlink_log_pub, i, !required, device_id, reportFailures) && required) {
failed = true;
}
@@ -506,10 +495,10 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
}
// TODO there is no logic in place to calibrate the primary baro yet
// // check if the primary device is present
// // check if the primary device is present
if (!prime_found && prime_id != 0) {
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_fd, "warning: primary barometer not operational");
mavlink_and_console_log_critical(mavlink_log_pub, "warning: primary barometer not operational");
}
failed = true;
}
@@ -517,16 +506,16 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* ---- AIRSPEED ---- */
if (checkAirspeed) {
if (!airspeedCheck(mavlink_fd, true, reportFailures)) {
if (!airspeedCheck(mavlink_log_pub, true, reportFailures)) {
failed = true;
}
}
/* ---- RC CALIBRATION ---- */
if (checkRC) {
if (rc_calibration_check(mavlink_fd, reportFailures) != OK) {
if (rc_calibration_check(mavlink_log_pub, reportFailures) != OK) {
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_fd, "RC calibration check failed");
mavlink_and_console_log_critical(mavlink_log_pub, "RC calibration check failed");
}
failed = true;
}
@@ -534,7 +523,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* ---- Global Navigation Satellite System receiver ---- */
if (checkGNSS) {
if (!gnssCheck(mavlink_fd, reportFailures)) {
if (!gnssCheck(mavlink_log_pub, reportFailures)) {
failed = true;
}
}
src/modules/commander/PreflightCheck.h
+3 -3
View File
@@ -49,8 +49,8 @@ namespace Commander
* The function won't fail the test if optional sensors are not found, however,
* it will fail the test if optional sensors are found but not in working condition.
*
* @param mavlink_fd
* Mavlink output file descriptor for feedback when a sensor fails
* @param mavlink_log_pub
* Mavlink output orb handle reference for feedback when a sensor fails
* @param checkMag
* true if the magneteometer should be checked
* @param checkAcc
@@ -66,7 +66,7 @@ namespace Commander
* @param checkGNSS
* true if the GNSS receiver should be checked
**/
bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc,
bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc,
bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic, bool reportFailures = false);
const unsigned max_mandatory_gyro_count = 1;
src/modules/commander/accelerometer_calibration.cpp
+32 -32
View File
@@ -145,7 +145,7 @@
#include <conversion/rotation.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <uORB/topics/vehicle_attitude.h>
/* oddly, ERROR is not defined for c++ */
@@ -160,26 +160,26 @@ static int32_t device_id[max_accel_sens];
static int device_prio_max = 0;
static int32_t device_id_primary = 0;
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 do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub, 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]);
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 {
int mavlink_fd;
orb_advert_t *mavlink_log_pub;
unsigned done_count;
int subs[max_accel_sens];
float accel_ref[max_accel_sens][detect_orientation_side_count][3];
} accel_worker_data_t;
int do_accel_calibration(int mavlink_fd)
int do_accel_calibration(orb_advert_t *mavlink_log_pub)
{
#ifndef __PX4_QURT
int fd;
#endif
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
struct accel_calibration_s accel_scale;
accel_scale.x_offset = 0.0f;
@@ -210,7 +210,7 @@ int do_accel_calibration(int mavlink_fd)
px4_close(fd);
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, s);
}
#else
(void)sprintf(str, "CAL_ACC%u_XOFF", s);
@@ -252,7 +252,7 @@ int do_accel_calibration(int mavlink_fd)
/* measure and calculate offsets & scales */
if (res == OK) {
calibrate_return cal_return = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T, &active_sensors);
calibrate_return cal_return = do_accel_calibration_measurements(mavlink_log_pub, accel_offs, accel_T, &active_sensors);
if (cal_return == calibrate_return_cancelled) {
// Cancel message already displayed, nothing left to do
return ERROR;
@@ -265,7 +265,7 @@ int do_accel_calibration(int mavlink_fd)
if (res != OK) {
if (active_sensors == 0) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SENSOR_MSG);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
}
return ERROR;
}
@@ -325,7 +325,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_ERROR_SET_PARAMS_MSG, i);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, i);
return ERROR;
}
@@ -334,7 +334,7 @@ int do_accel_calibration(int mavlink_fd)
fd = px4_open(str, 0);
if (fd < 0) {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "sensor does not exist");
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "sensor does not exist");
res = ERROR;
} else {
res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
@@ -342,7 +342,7 @@ int do_accel_calibration(int mavlink_fd)
}
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, i);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, i);
}
#endif
}
@@ -352,16 +352,16 @@ int do_accel_calibration(int mavlink_fd)
res = param_save_default();
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SAVE_PARAMS_MSG);
}
/* 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);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
} else {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
}
/* give this message enough time to propagate */
@@ -375,22 +375,22 @@ static calibrate_return accel_calibration_worker(detect_orientation_return orien
const unsigned samples_num = 750;
accel_worker_data_t* worker_data = (accel_worker_data_t*)(data);
mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Hold still, measuring %s side", detect_orientation_str(orientation));
mavlink_and_console_log_info(worker_data->mavlink_log_pub, "[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, "[cal] %s side result: [%8.4f %8.4f %8.4f]", detect_orientation_str(orientation),
mavlink_and_console_log_info(worker_data->mavlink_log_pub, "[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_QGC_PROGRESS_MSG, 17 * worker_data->done_count);
mavlink_and_console_log_info(worker_data->mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 17 * worker_data->done_count);
return calibrate_return_ok;
}
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 do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors)
{
calibrate_return result = calibrate_return_ok;
@@ -398,7 +398,7 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel
accel_worker_data_t worker_data;
worker_data.mavlink_fd = mavlink_fd;
worker_data.mavlink_log_pub = mavlink_log_pub;
worker_data.done_count = 0;
bool data_collected[detect_orientation_side_count] = { false, false, false, false, false, false };
@@ -448,7 +448,7 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel
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 */);
result = calibrate_from_orientation(mavlink_log_pub, cancel_sub, data_collected, accel_calibration_worker, &worker_data, false /* normal still */);
calibrate_cancel_unsubscribe(cancel_sub);
}
@@ -471,7 +471,7 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel
result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
if (result != calibrate_return_ok) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: calibration calculation error");
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] ERROR: calibration calculation error");
break;
}
}
@@ -631,7 +631,7 @@ calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref
return calibrate_return_ok;
}
int do_level_calibration(int mavlink_fd) {
int do_level_calibration(orb_advert_t *mavlink_log_pub) {
const unsigned cal_time = 5;
const unsigned cal_hz = 100;
unsigned settle_time = 30;
@@ -641,7 +641,7 @@ int do_level_calibration(int mavlink_fd) {
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, "level");
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "level");
param_t roll_offset_handle = param_find("SENS_BOARD_X_OFF");
param_t pitch_offset_handle = param_find("SENS_BOARD_Y_OFF");
@@ -676,7 +676,7 @@ int do_level_calibration(int mavlink_fd) {
// sleep for some time
hrt_abstime start = hrt_absolute_time();
while(hrt_elapsed_time(&start) < settle_time * 1000000) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (int)(90*hrt_elapsed_time(&start)/1e6f/(float)settle_time));
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (int)(90*hrt_elapsed_time(&start)/1e6f/(float)settle_time));
sleep(settle_time / 10);
}
@@ -687,8 +687,8 @@ int do_level_calibration(int mavlink_fd) {
if (pollret <= 0) {
// attitude estimator is not running
mavlink_and_console_log_critical(mavlink_fd, "attitude estimator not running - check system boot");
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "level");
mavlink_and_console_log_critical(mavlink_log_pub, "attitude estimator not running - check system boot");
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "level");
goto out;
}
@@ -698,21 +698,21 @@ int do_level_calibration(int mavlink_fd) {
counter++;
}
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);
if (counter > (cal_time * cal_hz / 2 )) {
roll_mean /= counter;
pitch_mean /= counter;
} else {
mavlink_and_console_log_info(mavlink_fd, "not enough measurements taken");
mavlink_and_console_log_info(mavlink_log_pub, "not enough measurements taken");
success = false;
goto out;
}
if (fabsf(roll_mean) > 0.8f ) {
mavlink_and_console_log_critical(mavlink_fd, "excess roll angle");
mavlink_and_console_log_critical(mavlink_log_pub, "excess roll angle");
} else if (fabsf(pitch_mean) > 0.8f ) {
mavlink_and_console_log_critical(mavlink_fd, "excess pitch angle");
mavlink_and_console_log_critical(mavlink_log_pub, "excess pitch angle");
} else {
roll_mean *= (float)M_RAD_TO_DEG;
pitch_mean *= (float)M_RAD_TO_DEG;
@@ -723,13 +723,13 @@ int do_level_calibration(int mavlink_fd) {
out:
if (success) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, "level");
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "level");
return 0;
} else {
// set old parameters
param_set(roll_offset_handle, &roll_offset_current);
param_set(pitch_offset_handle, &pitch_offset_current);
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "level");
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "level");
return 1;
}
}
src/modules/commander/accelerometer_calibration.h
+3 -2
View File
@@ -43,8 +43,9 @@
#define ACCELEROMETER_CALIBRATION_H_
#include <stdint.h>
#include <uORB/uORB.h>
int do_accel_calibration(int mavlink_fd);
int do_level_calibration(int mavlink_fd);
int do_accel_calibration(orb_advert_t *mavlink_log_pub);
int do_level_calibration(orb_advert_t *mavlink_log_pub);
#endif /* ACCELEROMETER_CALIBRATION_H_ */
src/modules/commander/airspeed_calibration.cpp
+35 -35
View File
@@ -52,7 +52,7 @@
#include <drivers/drv_airspeed.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/differential_pressure.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/airspeed.h>
@@ -65,20 +65,20 @@ static const int ERROR = -1;
static const char *sensor_name = "dpress";
static void feedback_calibration_failed(int mavlink_fd)
static void feedback_calibration_failed(orb_advert_t *mavlink_log_pub)
{
sleep(5);
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
}
int do_airspeed_calibration(int mavlink_fd)
int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
{
int result = OK;
unsigned calibration_counter = 0;
const unsigned maxcount = 2400;
/* give directions */
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
const unsigned calibration_count = (maxcount * 2) / 3;
@@ -101,12 +101,12 @@ int do_airspeed_calibration(int mavlink_fd)
paramreset_successful = true;
} else {
mavlink_and_console_log_critical(mavlink_fd, "[cal] airspeed offset zero failed");
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] airspeed offset zero failed");
}
px4_close(fd);
}
int cancel_sub = calibrate_cancel_subscribe();
if (!paramreset_successful) {
@@ -115,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_and_console_log_critical(mavlink_fd, "[cal] No airspeed sensor, see http://px4.io/help/aspd");
mavlink_and_console_log_critical(mavlink_log_pub, "[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_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, 1);
goto error_return;
}
}
mavlink_and_console_log_critical(mavlink_fd, "[cal] Ensure sensor is not measuring wind");
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] Ensure sensor is not measuring wind");
usleep(500 * 1000);
while (calibration_counter < calibration_count) {
if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
if (calibrate_cancel_check(mavlink_log_pub, cancel_sub)) {
goto error_return;
}
/* wait blocking for new data */
px4_pollfd_struct_t fds[1];
fds[0].fd = diff_pres_sub;
@@ -149,12 +149,12 @@ int do_airspeed_calibration(int mavlink_fd)
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count);
mavlink_and_console_log_info(mavlink_log_pub, 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);
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
}
}
@@ -167,14 +167,14 @@ 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_and_console_log_critical(mavlink_fd, "[cal] airspeed offset update failed");
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] airspeed offset update failed");
}
px4_close(fd_scale);
}
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, 1);
goto error_return;
}
@@ -183,28 +183,28 @@ int do_airspeed_calibration(int mavlink_fd)
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SAVE_PARAMS_MSG);
goto error_return;
}
} else {
feedback_calibration_failed(mavlink_fd);
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
}
mavlink_and_console_log_critical(mavlink_fd, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] Offset of %d Pascal", (int)diff_pres_offset);
/* wait 500 ms to ensure parameter propagated through the system */
usleep(500 * 1000);
mavlink_and_console_log_critical(mavlink_fd, "[cal] Create airflow now");
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] Create airflow now");
calibration_counter = 0;
/* 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)) {
if (calibrate_cancel_check(mavlink_log_pub, cancel_sub)) {
goto error_return;
}
@@ -222,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_and_console_log_info(mavlink_fd, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
mavlink_and_console_log_info(mavlink_log_pub, "[cal] Create air pressure! (got %d, wanted: 50 Pa)",
(int)diff_pres.differential_pressure_raw_pa);
}
continue;
@@ -230,56 +230,56 @@ int do_airspeed_calibration(int mavlink_fd)
/* do not allow negative values */
if (diff_pres.differential_pressure_raw_pa < 0.0f) {
mavlink_and_console_log_info(mavlink_fd, "[cal] Negative pressure difference detected (%d Pa)",
mavlink_and_console_log_info(mavlink_log_pub, "[cal] Negative pressure difference detected (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
mavlink_and_console_log_info(mavlink_fd, "[cal] Swap static and dynamic ports!");
mavlink_and_console_log_info(mavlink_log_pub, "[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_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, 1);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, 1);
goto error_return;
}
/* save */
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 0);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 0);
(void)param_save_default();
feedback_calibration_failed(mavlink_fd);
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
} else {
mavlink_and_console_log_info(mavlink_fd, "[cal] Positive pressure: OK (%d Pa)",
mavlink_and_console_log_info(mavlink_log_pub, "[cal] Positive pressure: OK (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa);
break;
}
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
feedback_calibration_failed(mavlink_fd);
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
}
}
if (calibration_counter == maxcount) {
feedback_calibration_failed(mavlink_fd);
feedback_calibration_failed(mavlink_log_pub);
goto error_return;
}
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
tune_neutral(true);
normal_return:
calibrate_cancel_unsubscribe(cancel_sub);
px4_close(diff_pres_sub);
// 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;
src/modules/commander/airspeed_calibration.h
+2 -1
View File
@@ -40,7 +40,8 @@
#define AIRSPEED_CALIBRATION_H_
#include <stdint.h>
#include <uORB/uORB.h>
int do_airspeed_calibration(int mavlink_fd);
int do_airspeed_calibration(orb_advert_t *mavlink_log_pub);
#endif /* AIRSPEED_CALIBRATION_H_ */
src/modules/commander/baro_calibration.cpp
+2 -2
View File
@@ -44,7 +44,7 @@
#include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_combined.h>
#include <drivers/drv_baro.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/param/param.h>
/* oddly, ERROR is not defined for c++ */
@@ -53,7 +53,7 @@
#endif
static const int ERROR = -1;
int do_baro_calibration(int mavlink_fd)
int do_baro_calibration(orb_advert_t *mavlink_log_pub)
{
// TODO implement this
return ERROR;
src/modules/commander/baro_calibration.h
+2 -1
View File
@@ -40,7 +40,8 @@
#define BARO_CALIBRATION_H_
#include <stdint.h>
#include <uORB/uORB.h>
int do_baro_calibration(int mavlink_fd);
int do_baro_calibration(orb_advert_t *mavlink_log_pub);
#endif /* BARO_CALIBRATION_H_ */
src/modules/commander/calibration_routines.cpp
+73 -73
View File
@@ -47,7 +47,7 @@
#include <float.h>
#include <poll.h>
#include <drivers/drv_hrt.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <geo/geo.h>
#include <string.h>
@@ -233,10 +233,10 @@ 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 cancel_sub, int accel_sub, bool lenient_still_position)
enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, int cancel_sub, int accel_sub, bool lenient_still_position)
{
const unsigned ndim = 3;
struct sensor_combined_s sensor;
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
@@ -245,11 +245,11 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
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 ? 500000 : 1300000; // still time required in us
px4_pollfd_struct_t fds[1];
fds[0].fd = accel_sub;
fds[0].events = POLLIN;
hrt_abstime t_start = hrt_absolute_time();
/* set timeout to 30s */
hrt_abstime timeout = 30000000;
@@ -257,22 +257,22 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
hrt_abstime t = t_start;
hrt_abstime t_prev = t_start;
hrt_abstime t_still = 0;
unsigned poll_errcount = 0;
while (true) {
/* wait blocking for new data */
int poll_ret = px4_poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(sensor_combined), accel_sub, &sensor);
t = hrt_absolute_time();
float dt = (t - t_prev) / 1000000.0f;
t_prev = t;
float w = dt / ema_len;
for (unsigned i = 0; i < ndim; i++) {
float di = 0.0f;
switch (i) {
case 0:
@@ -285,21 +285,21 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
di = sensor.accelerometer_m_s2[2];
break;
}
float d = di - accel_ema[i];
accel_ema[i] += d * w;
d = d * d;
accel_disp[i] = accel_disp[i] * (1.0f - w);
if (d > still_thr2 * 8.0f) {
d = still_thr2 * 8.0f;
}
if (d > accel_disp[i]) {
accel_disp[i] = d;
}
}
/* still detector with hysteresis */
if (accel_disp[0] < still_thr2 &&
accel_disp[1] < still_thr2 &&
@@ -307,10 +307,10 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
/* is still now */
if (t_still == 0) {
/* first time */
mavlink_and_console_log_info(mavlink_fd, "[cal] detected rest position, hold still...");
mavlink_and_console_log_info(mavlink_log_pub, "[cal] detected rest position, hold still...");
t_still = t;
t_timeout = t + timeout;
} else {
/* still since t_still */
if (t > t_still + still_time) {
@@ -318,28 +318,28 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
break;
}
}
} else if (accel_disp[0] > still_thr2 * 4.0f ||
accel_disp[1] > still_thr2 * 4.0f ||
accel_disp[2] > still_thr2 * 4.0f) {
/* not still, reset still start time */
if (t_still != 0) {
mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still...");
mavlink_and_console_log_info(mavlink_log_pub, "[cal] detected motion, hold still...");
usleep(200000);
t_still = 0;
}
}
} else if (poll_ret == 0) {
poll_errcount++;
}
if (t > t_timeout) {
poll_errcount++;
}
if (poll_errcount > 1000) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SENSOR_MSG);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
return DETECT_ORIENTATION_ERROR;
}
}
@@ -349,39 +349,39 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
fabsf(accel_ema[2]) < accel_err_thr) {
return DETECT_ORIENTATION_TAIL_DOWN; // [ g, 0, 0 ]
}
if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr) {
return DETECT_ORIENTATION_NOSE_DOWN; // [ -g, 0, 0 ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr) {
return DETECT_ORIENTATION_LEFT; // [ 0, g, 0 ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
fabsf(accel_ema[2]) < accel_err_thr) {
return DETECT_ORIENTATION_RIGHT; // [ 0, -g, 0 ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) {
return DETECT_ORIENTATION_UPSIDE_DOWN; // [ 0, 0, g ]
}
if (fabsf(accel_ema[0]) < accel_err_thr &&
fabsf(accel_ema[1]) < accel_err_thr &&
fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) {
return DETECT_ORIENTATION_RIGHTSIDE_UP; // [ 0, 0, -g ]
}
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: invalid orientation");
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] ERROR: invalid orientation");
return DETECT_ORIENTATION_ERROR; // Can't detect orientation
}
@@ -396,11 +396,11 @@ const char* detect_orientation_str(enum detect_orientation_return orientation)
"down", // right-side up
"error"
};
return rgOrientationStrs[orientation];
}
calibrate_return calibrate_from_orientation(int mavlink_fd,
calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
int cancel_sub,
bool side_data_collected[detect_orientation_side_count],
calibration_from_orientation_worker_t calibration_worker,
@@ -408,94 +408,94 @@ calibrate_return calibrate_from_orientation(int mavlink_fd,
bool lenient_still_position)
{
calibrate_return result = calibrate_return_ok;
// Setup subscriptions to onboard accel sensor
int sub_accel = orb_subscribe(ORB_ID(sensor_combined));
if (sub_accel < 0) {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "No onboard accel");
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "No onboard accel");
return calibrate_return_error;
}
unsigned orientation_failures = 0;
// Rotate through all requested orientation
while (true) {
if (calibrate_cancel_check(mavlink_fd, cancel_sub)) {
if (calibrate_cancel_check(mavlink_log_pub, 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");
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "timeout: no motion");
break;
}
unsigned int side_complete_count = 0;
// Update the number of completed sides
for (unsigned i = 0; i < detect_orientation_side_count; i++) {
if (side_data_collected[i]) {
side_complete_count++;
}
}
if (side_complete_count == detect_orientation_side_count) {
// We have completed all sides, move on
break;
}
/* inform user which orientations are still needed */
char pendingStr[256];
pendingStr[0] = 0;
for (unsigned int cur_orientation=0; cur_orientation<detect_orientation_side_count; cur_orientation++) {
if (!side_data_collected[cur_orientation]) {
strcat(pendingStr, " ");
strcat(pendingStr, detect_orientation_str((enum detect_orientation_return)cur_orientation));
}
}
mavlink_and_console_log_info(mavlink_fd, "[cal] pending:%s", pendingStr);
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);
mavlink_and_console_log_info(mavlink_log_pub, "[cal] pending:%s", pendingStr);
mavlink_and_console_log_info(mavlink_log_pub, "[cal] hold vehicle still on a pending side");
enum detect_orientation_return orient = detect_orientation(mavlink_log_pub, cancel_sub, sub_accel, lenient_still_position);
if (orient == DETECT_ORIENTATION_ERROR) {
orientation_failures++;
mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still...");
mavlink_and_console_log_info(mavlink_log_pub, "[cal] detected motion, hold still...");
continue;
}
/* inform user about already handled side */
if (side_data_collected[orient]) {
orientation_failures++;
mavlink_and_console_log_critical(mavlink_fd, "%s side already completed", detect_orientation_str(orient));
mavlink_and_console_log_critical(mavlink_fd, "rotate to a pending side");
mavlink_and_console_log_critical(mavlink_log_pub, "%s side already completed", detect_orientation_str(orient));
mavlink_and_console_log_critical(mavlink_log_pub, "rotate to a pending side");
continue;
}
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient));
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_ORIENTATION_DETECTED_MSG, detect_orientation_str(orient));
orientation_failures = 0;
// Call worker routine
result = calibration_worker(orient, cancel_sub, worker_data);
if (result != calibrate_return_ok ) {
break;
}
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient));
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_SIDE_DONE_MSG, detect_orientation_str(orient));
// Note that this side is complete
side_data_collected[orient] = true;
tune_neutral(true);
usleep(200000);
}
if (sub_accel >= 0) {
px4_close(sub_accel);
}
return result;
}
@@ -509,24 +509,24 @@ void calibrate_cancel_unsubscribe(int cmd_sub)
orb_unsubscribe(cmd_sub);
}
static void calibrate_answer_command(int mavlink_fd, struct vehicle_command_s &cmd, unsigned result)
static void calibrate_answer_command(orb_advert_t *mavlink_log_pub, struct vehicle_command_s &cmd, unsigned result)
{
switch (result) {
case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED:
tune_positive(true);
break;
case vehicle_command_s::VEHICLE_CMD_RESULT_DENIED:
mavlink_log_critical(mavlink_fd, "command denied during calibration: %u", cmd.command);
mavlink_log_critical(mavlink_log_pub, "command denied during calibration: %u", cmd.command);
tune_negative(true);
break;
default:
break;
}
}
bool calibrate_cancel_check(int mavlink_fd, int cancel_sub)
bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, int cancel_sub)
{
px4_pollfd_struct_t fds[1];
fds[0].fd = cancel_sub;
@@ -535,9 +535,9 @@ bool calibrate_cancel_check(int mavlink_fd, int cancel_sub)
if (px4_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_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION &&
(int)cmd.param1 == 0 &&
(int)cmd.param2 == 0 &&
@@ -545,13 +545,13 @@ bool calibrate_cancel_check(int mavlink_fd, int cancel_sub)
(int)cmd.param4 == 0 &&
(int)cmd.param5 == 0 &&
(int)cmd.param6 == 0) {
calibrate_answer_command(mavlink_fd, cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
mavlink_log_critical(mavlink_fd, CAL_QGC_CANCELLED_MSG);
calibrate_answer_command(mavlink_log_pub, cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
mavlink_log_critical(mavlink_log_pub, CAL_QGC_CANCELLED_MSG);
return true;
} else {
calibrate_answer_command(mavlink_fd, cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
calibrate_answer_command(mavlink_log_pub, cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
}
}
return false;
}
src/modules/commander/calibration_routines.h
+3 -3
View File
@@ -74,7 +74,7 @@ static const unsigned detect_orientation_side_count = 6;
/// 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
enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, ///< uORB handle 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
@@ -95,7 +95,7 @@ typedef calibrate_return (*calibration_from_orientation_worker_t)(detect_orienta
/// 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
calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub, ///< uORB handle 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
@@ -111,5 +111,5 @@ int calibrate_cancel_subscribe(void);
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
bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, ///< uORB handle to write output to
int cancel_sub); ///< Cancel subcription fromcalibration_cancel_subscribe
src/modules/commander/commander.cpp
+111 -133
View File
@@ -59,6 +59,7 @@
#include <errno.h>
#include <systemlib/err.h>
#include <systemlib/circuit_breaker.h>
#include <systemlib/mavlink_log.h>
//#include <debug.h>
#include <sys/stat.h>
#include <string.h>
@@ -97,12 +98,12 @@
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/input_rc.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/mavlink_log.h>
#include <drivers/drv_led.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_tone_alarm.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
@@ -173,8 +174,8 @@ enum MAV_MODE_FLAG {
MAV_MODE_FLAG_ENUM_END = 129, /* | */
};
/* Mavlink file descriptors */
static int mavlink_fd = 0;
/* Mavlink log uORB handle */
static orb_advert_t mavlink_log_pub = 0;
/* System autostart ID */
static int autostart_id;
@@ -260,7 +261,7 @@ void print_status();
transition_result_t check_navigation_state_machine(struct vehicle_status_s *status,
struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy);
transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub, const char *armedBy);
/**
* @brief This function initializes the home position of the vehicle. This happens first time we get a good GPS fix and each
@@ -359,17 +360,17 @@ int commander_main(int argc, char *argv[])
if (argc > 2) {
int calib_ret = OK;
if (!strcmp(argv[2], "mag")) {
calib_ret = do_mag_calibration(mavlink_fd);
calib_ret = do_mag_calibration(&mavlink_log_pub);
} else if (!strcmp(argv[2], "accel")) {
calib_ret = do_accel_calibration(mavlink_fd);
calib_ret = do_accel_calibration(&mavlink_log_pub);
} else if (!strcmp(argv[2], "gyro")) {
calib_ret = do_gyro_calibration(mavlink_fd);
calib_ret = do_gyro_calibration(&mavlink_log_pub);
} else if (!strcmp(argv[2], "level")) {
calib_ret = do_level_calibration(mavlink_fd);
calib_ret = do_level_calibration(&mavlink_log_pub);
} else if (!strcmp(argv[2], "esc")) {
calib_ret = do_esc_calibration(mavlink_fd, &armed);
calib_ret = do_esc_calibration(&mavlink_log_pub, &armed);
} else if (!strcmp(argv[2], "airspeed")) {
calib_ret = do_airspeed_calibration(mavlink_fd);
calib_ret = do_airspeed_calibration(&mavlink_log_pub);
} else {
warnx("argument %s unsupported.", argv[2]);
}
@@ -386,41 +387,34 @@ int commander_main(int argc, char *argv[])
}
if (!strcmp(argv[1], "check")) {
int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0);
int checkres = 0;
checkres = preflight_check(&status, mavlink_fd_local, false, true);
checkres = preflight_check(&status, &mavlink_log_pub, false, true);
warnx("Preflight check: %s", (checkres == 0) ? "OK" : "FAILED");
checkres = preflight_check(&status, mavlink_fd_local, true, true);
checkres = preflight_check(&status, &mavlink_log_pub, true, true);
warnx("Prearm check: %s", (checkres == 0) ? "OK" : "FAILED");
px4_close(mavlink_fd_local);
return 0;
}
if (!strcmp(argv[1], "arm")) {
int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0);
if (TRANSITION_CHANGED != arm_disarm(true, mavlink_fd_local, "command line")) {
if (TRANSITION_CHANGED != arm_disarm(true, &mavlink_log_pub, "command line")) {
warnx("arming failed");
}
px4_close(mavlink_fd_local);
return 0;
}
if (!strcmp(argv[1], "disarm")) {
int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0);
if (TRANSITION_DENIED == arm_disarm(false, mavlink_fd_local, "command line")) {
if (TRANSITION_DENIED == arm_disarm(false, &mavlink_log_pub, "command line")) {
warnx("rejected disarm");
}
px4_close(mavlink_fd_local);
return 0;
}
if (!strcmp(argv[1], "takeoff")) {
int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0);
/* see if we got a home position */
if (status.condition_home_position_valid) {
if (TRANSITION_DENIED != arm_disarm(true, mavlink_fd_local, "command line")) {
if (TRANSITION_DENIED != arm_disarm(true, &mavlink_log_pub, "command line")) {
vehicle_command_s cmd = {};
cmd.target_system = status.system_id;
@@ -445,12 +439,10 @@ int commander_main(int argc, char *argv[])
warnx("rejecting takeoff, no position lock yet. Please retry..");
}
px4_close(mavlink_fd_local);
return 0;
}
if (!strcmp(argv[1], "land")) {
int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0);
vehicle_command_s cmd = {};
cmd.target_system = status.system_id;
@@ -467,7 +459,6 @@ int commander_main(int argc, char *argv[])
// XXX inspect use of publication handle
(void)orb_advertise(ORB_ID(vehicle_command), &cmd);
px4_close(mavlink_fd_local);
return 0;
}
@@ -519,8 +510,6 @@ int commander_main(int argc, char *argv[])
return 1;
}
int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0);
vehicle_command_s cmd = {};
cmd.target_system = status.system_id;
cmd.target_component = status.component_id;
@@ -532,7 +521,6 @@ int commander_main(int argc, char *argv[])
// XXX inspect use of publication handle
(void)orb_advertise(ORB_ID(vehicle_command), &cmd);
px4_close(mavlink_fd_local);
return 0;
}
@@ -613,24 +601,23 @@ void print_status()
static orb_advert_t status_pub;
transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char *armedBy)
transition_result_t arm_disarm(bool arm, orb_advert_t *mavlink_log_pub_local, const char *armedBy)
{
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
// For HIL platforms, require that simulated sensors are connected
if (arm && hrt_absolute_time() > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL &&
is_hil_setup(autostart_id) && status.hil_state != vehicle_status_s::HIL_STATE_ON) {
mavlink_and_console_log_critical(mavlink_fd_local, "HIL platform: Connect to simulator before arming");
mavlink_and_console_log_critical(mavlink_log_pub_local, "HIL platform: Connect to simulator before arming");
return TRANSITION_DENIED;
}
// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
// output appropriate error messages if the state cannot transition.
// Transition the armed state.
arming_res = arming_state_transition(&status, &safety, arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, &armed,
true /* fRunPreArmChecks */, mavlink_fd_local);
true /* fRunPreArmChecks */, mavlink_log_pub_local);
if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_log_pub_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
} else if (arming_res == TRANSITION_DENIED) {
tune_negative(true);
@@ -667,12 +654,12 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
/* set HIL state */
hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? vehicle_status_s::HIL_STATE_ON : vehicle_status_s::HIL_STATE_OFF;
transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, mavlink_fd);
transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, &mavlink_log_pub);
// Transition the arming state
bool cmd_arm = base_mode & MAV_MODE_FLAG_SAFETY_ARMED;
arming_ret = arm_disarm(cmd_arm, mavlink_fd, "set mode command");
arming_ret = arm_disarm(cmd_arm, &mavlink_log_pub, "set mode command");
/* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */
if (cmd_arm && (arming_ret == TRANSITION_CHANGED) &&
@@ -717,7 +704,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
default:
main_ret = TRANSITION_DENIED;
mavlink_log_critical(mavlink_fd, "Unsupported auto mode");
mavlink_log_critical(&mavlink_log_pub, "Unsupported auto mode");
break;
}
@@ -770,11 +757,11 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
if (arming_ret == TRANSITION_DENIED) {
mavlink_log_critical(mavlink_fd, "Rejecting arming cmd");
mavlink_log_critical(&mavlink_log_pub, "Rejecting arming cmd");
}
if (main_ret == TRANSITION_DENIED) {
mavlink_log_critical(mavlink_fd, "Rejecting mode switch cmd");
mavlink_log_critical(&mavlink_log_pub, "Rejecting mode switch cmd");
}
}
}
@@ -785,7 +772,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
// Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints
// for logic state parameters
if (static_cast<int>(cmd->param1 + 0.5f) != 0 && static_cast<int>(cmd->param1 + 0.5f) != 1) {
mavlink_log_critical(mavlink_fd, "Unsupported ARM_DISARM param: %.3f", (double)cmd->param1);
mavlink_log_critical(&mavlink_log_pub, "Unsupported ARM_DISARM param: %.3f", (double)cmd->param1);
} else {
@@ -799,17 +786,17 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
// 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.");
mavlink_log_critical(&mavlink_log_pub, "Arming DENIED. Preflight checks have failed.");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
break;
}
}
transition_result_t arming_res = arm_disarm(cmd_arms, mavlink_fd, "arm/disarm component command");
transition_result_t arming_res = arm_disarm(cmd_arms,&mavlink_log_pub, "arm/disarm component command");
if (arming_res == TRANSITION_DENIED) {
mavlink_log_critical(mavlink_fd, "REJECTING component arm cmd");
mavlink_log_critical(&mavlink_log_pub, "REJECTING component arm cmd");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
} else {
@@ -836,16 +823,16 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
if (mav_goto == 0) { // MAV_GOTO_DO_HOLD
status_local->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
mavlink_log_critical(mavlink_fd, "Pause mission cmd");
mavlink_log_critical(&mavlink_log_pub, "Pause mission cmd");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE
status_local->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
mavlink_log_critical(mavlink_fd, "Continue mission cmd");
mavlink_log_critical(&mavlink_log_pub, "Continue mission cmd");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f",
mavlink_log_critical(&mavlink_log_pub, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f",
(double)cmd->param1,
(double)cmd->param2,
(double)cmd->param3,
@@ -945,7 +932,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
}
if (cmd_result == vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED) {
mavlink_and_console_log_info(mavlink_fd, "Home position: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt);
mavlink_and_console_log_info(&mavlink_log_pub, "Home position: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt);
/* announce new home position */
if (*home_pub != nullptr) {
@@ -1198,17 +1185,16 @@ int commander_thread_main(int argc, char *argv[])
/* initialize */
if (led_init() != OK) {
mavlink_and_console_log_critical(mavlink_fd, "ERROR: LED INIT FAIL");
mavlink_and_console_log_critical(&mavlink_log_pub, "ERROR: LED INIT FAIL");
}
if (buzzer_init() != OK) {
mavlink_and_console_log_critical(mavlink_fd, "ERROR: BUZZER INIT FAIL");
mavlink_and_console_log_critical(&mavlink_log_pub, "ERROR: BUZZER INIT FAIL");
}
if (battery_init() != OK) {
mavlink_and_console_log_critical(mavlink_fd, "ERROR: BATTERY INIT FAIL");
mavlink_and_console_log_critical(&mavlink_log_pub, "ERROR: BATTERY INIT FAIL");
}
mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);
/* vehicle status topic */
memset(&status, 0, sizeof(status));
@@ -1295,14 +1281,14 @@ 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) {
if (mission.count > 0) {
mavlink_log_info(mavlink_fd, "[cmd] Mission #%d loaded, %u WPs, curr: %d",
mavlink_log_info(&mavlink_log_pub, "[cmd] Mission #%d loaded, %u WPs, curr: %d",
mission.dataman_id, mission.count, mission.current_seq);
}
} else {
const char *missionfail = "reading mission state failed";
warnx("%s", missionfail);
mavlink_log_critical(mavlink_fd, missionfail);
mavlink_log_critical(&mavlink_log_pub, missionfail);
/* initialize mission state in dataman */
mission.dataman_id = 0;
@@ -1482,7 +1468,7 @@ int commander_thread_main(int argc, char *argv[])
set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
} else {
// sensor diagnostics done continiously, not just at boot so don't warn about any issues just yet
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true,
status.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true, true, true, true,
checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check, false);
set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
}
@@ -1533,11 +1519,6 @@ int commander_thread_main(int argc, char *argv[])
while (!thread_should_exit) {
if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) {
/* try to open the mavlink log device every once in a while */
mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);
}
arming_ret = TRANSITION_NOT_CHANGED;
@@ -1656,10 +1637,7 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(telemetry_status), telemetry_subs[i], &telemetry);
/* perform system checks when new telemetry link connected */
if (
/* we actually have a way to talk to the user */
mavlink_fd &&
/* we first connect a link or re-connect a link after loosing it */
if (/* we first connect a link or re-connect a link after loosing it */
(telemetry_last_heartbeat[i] == 0 || (hrt_elapsed_time(&telemetry_last_heartbeat[i]) > 3 * 1000 * 1000)) &&
/* and this link has a communication partner */
(telemetry.heartbeat_time > 0) &&
@@ -1681,11 +1659,11 @@ int commander_thread_main(int argc, char *argv[])
/* provide RC and sensor status feedback to the user */
if (is_hil_setup(autostart_id)) {
/* HIL configuration: check only RC input */
(void)Commander::preflightCheck(mavlink_fd, false, false, false, false, false,
(void)Commander::preflightCheck(&mavlink_log_pub, false, false, false, false, false,
(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), false, true);
} else {
/* check sensors also */
(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed,
(void)Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, chAirspeed,
(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
}
}
@@ -1716,14 +1694,14 @@ int commander_thread_main(int argc, char *argv[])
if (status.barometer_failure) {
status.barometer_failure = false;
status_changed = true;
mavlink_log_critical(mavlink_fd, "baro healthy");
mavlink_log_critical(&mavlink_log_pub, "baro healthy");
}
} else {
if (!status.barometer_failure) {
status.barometer_failure = true;
status_changed = true;
mavlink_log_critical(mavlink_fd, "baro failed");
mavlink_log_critical(&mavlink_log_pub, "baro failed");
}
}
}
@@ -1759,7 +1737,7 @@ int commander_thread_main(int argc, char *argv[])
* apparently the USB cable went away but we are still powered,
* so lets reset to a classic non-usb state.
*/
mavlink_log_critical(mavlink_fd, "USB disconnected, rebooting.")
mavlink_log_critical(&mavlink_log_pub, "USB disconnected, rebooting.")
usleep(400000);
px4_systemreset(false);
}
@@ -1784,8 +1762,8 @@ int commander_thread_main(int argc, char *argv[])
vehicle_status_s::ARMING_STATE_STANDBY_ERROR);
if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed,
true /* fRunPreArmChecks */, mavlink_fd)) {
mavlink_log_info(mavlink_fd, "DISARMED by safety switch");
true /* fRunPreArmChecks */, &mavlink_log_pub)) {
mavlink_log_info(&mavlink_log_pub, "DISARMED by safety switch");
arming_state_changed = true;
}
}
@@ -1918,10 +1896,10 @@ int commander_thread_main(int argc, char *argv[])
status_changed = true;
if (status.condition_landed) {
mavlink_and_console_log_info(mavlink_fd, "LANDING DETECTED");
mavlink_and_console_log_info(&mavlink_log_pub, "LANDING DETECTED");
} else {
mavlink_and_console_log_info(mavlink_fd, "TAKEOFF DETECTED");
mavlink_and_console_log_info(&mavlink_log_pub, "TAKEOFF DETECTED");
}
}
@@ -1933,8 +1911,8 @@ int commander_thread_main(int argc, char *argv[])
}
if (_inair_last_time > 0 && ((hrt_absolute_time() - _inair_last_time) > (hrt_abstime)disarm_when_landed * 1000 * 1000)) {
mavlink_log_critical(mavlink_fd, "AUTO DISARMING AFTER LANDING");
arm_disarm(false, mavlink_fd, "auto disarm on land");
mavlink_log_critical(&mavlink_log_pub, "AUTO DISARMING AFTER LANDING");
arm_disarm(false, &mavlink_log_pub, "auto disarm on land");
_inair_last_time = 0;
check_for_disarming = false;
}
@@ -2023,9 +2001,9 @@ int commander_thread_main(int argc, char *argv[])
if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) {
low_battery_voltage_actions_done = true;
if (armed.armed) {
mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
mavlink_log_critical(&mavlink_log_pub, "LOW BATTERY, RETURN TO LAND ADVISED");
} else {
mavlink_log_critical(mavlink_fd, "LOW BATTERY, TAKEOFF DISCOURAGED");
mavlink_log_critical(&mavlink_log_pub, "LOW BATTERY, TAKEOFF DISCOURAGED");
}
status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW;
status_changed = true;
@@ -2039,15 +2017,15 @@ int commander_thread_main(int argc, char *argv[])
if (!armed.armed) {
arming_ret = arming_state_transition(&status, &safety,
vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
&armed, true /* fRunPreArmChecks */, mavlink_fd);
&armed, true /* fRunPreArmChecks */, &mavlink_log_pub);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
mavlink_and_console_log_critical(mavlink_fd, "LOW BATTERY, LOCKING ARMING DOWN");
mavlink_and_console_log_critical(&mavlink_log_pub, "LOW BATTERY, LOCKING ARMING DOWN");
}
} else {
mavlink_and_console_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
mavlink_and_console_log_emergency(&mavlink_log_pub, "CRITICAL BATTERY, LAND IMMEDIATELY");
}
status_changed = true;
@@ -2058,7 +2036,7 @@ int commander_thread_main(int argc, char *argv[])
/* If in INIT state, try to proceed to STANDBY state */
if (!status.calibration_enabled && 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);
&mavlink_log_pub);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
@@ -2102,7 +2080,7 @@ int commander_thread_main(int argc, char *argv[])
//Check if GPS receiver is too noisy while we are disarmed
if (!armed.armed && gpsIsNoisy) {
if (!status.gps_failure) {
mavlink_log_critical(mavlink_fd, "GPS signal noisy");
mavlink_log_critical(&mavlink_log_pub, "GPS signal noisy");
set_tune_override(TONE_GPS_WARNING_TUNE);
//GPS suffers from signal jamming or excessive noise, disable GPS-aided flight
@@ -2116,13 +2094,13 @@ int commander_thread_main(int argc, char *argv[])
if (status.gps_failure && !gpsIsNoisy) {
status.gps_failure = false;
status_changed = true;
mavlink_log_critical(mavlink_fd, "gps fix regained");
mavlink_log_critical(&mavlink_log_pub, "gps fix regained");
}
} else if (!status.gps_failure) {
status.gps_failure = true;
status_changed = true;
mavlink_log_critical(mavlink_fd, "gps fix lost");
mavlink_log_critical(&mavlink_log_pub, "gps fix lost");
}
}
@@ -2137,7 +2115,7 @@ int commander_thread_main(int argc, char *argv[])
status_changed = true;
if (status.mission_failure) {
mavlink_log_critical(mavlink_fd, "mission cannot be completed");
mavlink_log_critical(&mavlink_log_pub, "mission cannot be completed");
}
}
}
@@ -2188,7 +2166,7 @@ int commander_thread_main(int argc, char *argv[])
}
case (geofence_result_s::GF_ACTION_TERMINATE) : {
warnx("Flight termination because of geofence");
mavlink_log_critical(mavlink_fd, "Geofence violation: flight termination");
mavlink_log_critical(&mavlink_log_pub, "Geofence violation: flight termination");
armed.force_failsafe = true;
status_changed = true;
break;
@@ -2241,12 +2219,12 @@ int commander_thread_main(int argc, char *argv[])
static bool flight_termination_printed = false;
if (!flight_termination_printed) {
mavlink_and_console_log_critical(mavlink_fd, "Geofence violation: flight termination");
mavlink_and_console_log_critical(&mavlink_log_pub, "Geofence violation: flight termination");
flight_termination_printed = true;
}
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
mavlink_and_console_log_critical(mavlink_fd, "Flight termination active");
mavlink_and_console_log_critical(&mavlink_log_pub, "Flight termination active");
}
}
@@ -2290,7 +2268,7 @@ int commander_thread_main(int argc, char *argv[])
} else {
if (status.rc_signal_lost) {
mavlink_log_info(mavlink_fd, "MANUAL CONTROL REGAINED after %llums",
mavlink_log_info(&mavlink_log_pub, "MANUAL CONTROL REGAINED after %llums",
(hrt_absolute_time() - status.rc_signal_lost_timestamp) / 1000);
status_changed = true;
}
@@ -2314,7 +2292,7 @@ int commander_thread_main(int argc, char *argv[])
arming_state_t new_arming_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED ? vehicle_status_s::ARMING_STATE_STANDBY :
vehicle_status_s::ARMING_STATE_STANDBY_ERROR);
arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
&mavlink_log_pub);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
@@ -2351,7 +2329,7 @@ int commander_thread_main(int argc, char *argv[])
} else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */,
mavlink_fd);
&mavlink_log_pub);
if (arming_ret == TRANSITION_CHANGED) {
arming_state_changed = true;
@@ -2371,10 +2349,10 @@ int commander_thread_main(int argc, char *argv[])
if (arming_ret == TRANSITION_CHANGED) {
if (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
mavlink_log_info(mavlink_fd, "ARMED by RC");
mavlink_log_info(&mavlink_log_pub, "ARMED by RC");
} else {
mavlink_log_info(mavlink_fd, "DISARMED by RC");
mavlink_log_info(&mavlink_log_pub, "DISARMED by RC");
}
arming_state_changed = true;
@@ -2400,19 +2378,19 @@ int commander_thread_main(int argc, char *argv[])
} else if (main_res == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
mavlink_log_critical(mavlink_fd, "main state transition denied");
mavlink_log_critical(&mavlink_log_pub, "main state transition denied");
}
/* check throttle kill switch */
if (sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
/* set lockdown flag */
if (!armed.lockdown) {
mavlink_log_emergency(mavlink_fd, "MANUAL KILL SWITCH ENGAGED");
mavlink_log_emergency(&mavlink_log_pub, "MANUAL KILL SWITCH ENGAGED");
}
armed.lockdown = true;
} else if (sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
if (armed.lockdown) {
mavlink_log_emergency(mavlink_fd, "MANUAL KILL SWITCH OFF");
mavlink_log_emergency(&mavlink_log_pub, "MANUAL KILL SWITCH OFF");
}
armed.lockdown = false;
}
@@ -2420,7 +2398,7 @@ int commander_thread_main(int argc, char *argv[])
} else {
if (!status.rc_input_blocked && !status.rc_signal_lost) {
mavlink_log_critical(mavlink_fd, "MANUAL CONTROL LOST (at t=%llums)", hrt_absolute_time() / 1000);
mavlink_log_critical(&mavlink_log_pub, "MANUAL CONTROL LOST (at t=%llums)", hrt_absolute_time() / 1000);
status.rc_signal_lost = true;
status.rc_signal_lost_timestamp = sp_man.timestamp;
status_changed = true;
@@ -2441,7 +2419,7 @@ int commander_thread_main(int argc, char *argv[])
/* report a regain */
if (telemetry_last_dl_loss[i] > 0) {
mavlink_and_console_log_info(mavlink_fd, "data link #%i regained", i);
mavlink_and_console_log_info(&mavlink_log_pub, "data link #%i regained", i);
} else if (telemetry_last_dl_loss[i] == 0) {
/* new link */
}
@@ -2465,7 +2443,7 @@ int commander_thread_main(int argc, char *argv[])
/* only reset the timestamp to a different time on state change */
telemetry_last_dl_loss[i] = hrt_absolute_time();
mavlink_and_console_log_info(mavlink_fd, "data link #%i lost", i);
mavlink_and_console_log_info(&mavlink_log_pub, "data link #%i lost", i);
telemetry_lost[i] = true;
}
}
@@ -2481,7 +2459,7 @@ int commander_thread_main(int argc, char *argv[])
} else {
if (!status.data_link_lost) {
if (armed.armed) {
mavlink_and_console_log_critical(mavlink_fd, "ALL DATA LINKS LOST");
mavlink_and_console_log_critical(&mavlink_log_pub, "ALL DATA LINKS LOST");
}
status.data_link_lost = true;
status.data_link_lost_counter++;
@@ -2506,7 +2484,7 @@ int commander_thread_main(int argc, char *argv[])
!status.engine_failure) {
status.engine_failure = true;
status_changed = true;
mavlink_log_critical(mavlink_fd, "Engine Failure");
mavlink_log_critical(&mavlink_log_pub, "Engine Failure");
}
} else {
@@ -2566,12 +2544,12 @@ int commander_thread_main(int argc, char *argv[])
if (!flight_termination_printed) {
warnx("Flight termination because of data link loss && gps failure");
mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination");
mavlink_log_critical(&mavlink_log_pub, "DL and GPS lost: flight termination");
flight_termination_printed = true;
}
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination");
mavlink_log_critical(&mavlink_log_pub, "DL and GPS lost: flight termination");
}
}
@@ -2596,7 +2574,7 @@ int commander_thread_main(int argc, char *argv[])
}
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination");
mavlink_log_critical(&mavlink_log_pub, "RC and GPS lost: flight termination");
}
}
}
@@ -2633,10 +2611,10 @@ int commander_thread_main(int argc, char *argv[])
status_changed = true;
if (status.failsafe) {
mavlink_log_critical(mavlink_fd, "failsafe mode on");
mavlink_log_critical(&mavlink_log_pub, "failsafe mode on");
} else {
mavlink_log_critical(mavlink_fd, "failsafe mode off");
mavlink_log_critical(&mavlink_log_pub, "failsafe mode off");
}
failsafe_old = status.failsafe;
@@ -3038,7 +3016,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
} while (0);
}
return res;
}
@@ -3357,7 +3335,7 @@ print_reject_mode(struct vehicle_status_s *status_local, const char *msg)
if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
last_print_mode_reject_time = t;
mavlink_log_critical(mavlink_fd, "REJECT %s", msg);
mavlink_log_critical(&mavlink_log_pub, "REJECT %s", msg);
/* only buzz if armed, because else we're driving people nuts indoors
they really need to look at the leds as well. */
@@ -3372,7 +3350,7 @@ print_reject_arm(const char *msg)
if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
last_print_mode_reject_time = t;
mavlink_log_critical(mavlink_fd, msg);
mavlink_log_critical(&mavlink_log_pub, msg);
tune_negative(true);
}
}
@@ -3386,23 +3364,23 @@ void answer_command(struct vehicle_command_s &cmd, unsigned result,
break;
case vehicle_command_s::VEHICLE_CMD_RESULT_DENIED:
mavlink_log_critical(mavlink_fd, "command denied: %u", cmd.command);
mavlink_log_critical(&mavlink_log_pub, "command denied: %u", cmd.command);
tune_negative(true);
break;
case vehicle_command_s::VEHICLE_CMD_RESULT_FAILED:
mavlink_log_critical(mavlink_fd, "command failed: %u", cmd.command);
mavlink_log_critical(&mavlink_log_pub, "command failed: %u", cmd.command);
tune_negative(true);
break;
case vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
/* this needs additional hints to the user - so let other messages pass and be spoken */
mavlink_log_critical(mavlink_fd, "command temporarily rejected: %u", cmd.command);
mavlink_log_critical(&mavlink_log_pub, "command temporarily rejected: %u", cmd.command);
tune_negative(true);
break;
case vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED:
mavlink_log_critical(mavlink_fd, "command unsupported: %u", cmd.command);
mavlink_log_critical(&mavlink_log_pub, "command unsupported: %u", cmd.command);
tune_negative(true);
break;
@@ -3459,7 +3437,7 @@ void *commander_low_prio_loop(void *arg)
int ret = param_save_default();
if (ret != OK) {
mavlink_and_console_log_critical(mavlink_fd, "settings auto save error");
mavlink_and_console_log_critical(&mavlink_log_pub, "settings auto save error");
} else {
warnx("settings saved.");
}
@@ -3522,7 +3500,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,
false /* fRunPreArmChecks */, mavlink_fd)) {
false /* fRunPreArmChecks */, &mavlink_log_pub)) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub, command_ack);
break;
} else {
@@ -3532,12 +3510,12 @@ void *commander_low_prio_loop(void *arg)
if ((int)(cmd.param1) == 1) {
/* gyro calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack);
calib_ret = do_gyro_calibration(mavlink_fd);
calib_ret = do_gyro_calibration(&mavlink_log_pub);
} else if ((int)(cmd.param2) == 1) {
/* magnetometer calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack);
calib_ret = do_mag_calibration(mavlink_fd);
calib_ret = do_mag_calibration(&mavlink_log_pub);
} else if ((int)(cmd.param3) == 1) {
/* zero-altitude pressure calibration */
@@ -3549,30 +3527,30 @@ void *commander_low_prio_loop(void *arg)
/* disable RC control input completely */
status.rc_input_blocked = true;
calib_ret = OK;
mavlink_log_info(mavlink_fd, "CAL: Disabling RC IN");
mavlink_log_info(&mavlink_log_pub, "CAL: Disabling RC IN");
} else if ((int)(cmd.param4) == 2) {
/* RC trim calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack);
calib_ret = do_trim_calibration(mavlink_fd);
calib_ret = do_trim_calibration(&mavlink_log_pub);
} else if ((int)(cmd.param5) == 1) {
/* accelerometer calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack);
calib_ret = do_accel_calibration(mavlink_fd);
calib_ret = do_accel_calibration(&mavlink_log_pub);
} else if ((int)(cmd.param5) == 2) {
// board offset calibration
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack);
calib_ret = do_level_calibration(mavlink_fd);
calib_ret = do_level_calibration(&mavlink_log_pub);
} else if ((int)(cmd.param6) == 1) {
/* airspeed calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack);
calib_ret = do_airspeed_calibration(mavlink_fd);
calib_ret = do_airspeed_calibration(&mavlink_log_pub);
} else if ((int)(cmd.param7) == 1) {
/* do esc calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack);
calib_ret = do_esc_calibration(mavlink_fd, &armed);
calib_ret = do_esc_calibration(&mavlink_log_pub, &armed);
} else if ((int)(cmd.param4) == 0) {
/* RC calibration ended - have we been in one worth confirming? */
@@ -3580,7 +3558,7 @@ void *commander_low_prio_loop(void *arg)
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack);
/* enable RC control input */
status.rc_input_blocked = false;
mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN");
mavlink_log_info(&mavlink_log_pub, "CAL: Re-enabling RC IN");
calib_ret = OK;
}
/* this always succeeds */
@@ -3605,10 +3583,10 @@ void *commander_low_prio_loop(void *arg)
checkAirspeed = true;
}
status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed,
status.condition_system_sensors_initialized = Commander::preflightCheck(&mavlink_log_pub, true, true, true, true, checkAirspeed,
!(status.rc_input_mode >= vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check, hotplug_timeout);
arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd);
arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, &mavlink_log_pub);
} else {
tune_negative(true);
@@ -3623,11 +3601,11 @@ void *commander_low_prio_loop(void *arg)
int ret = param_load_default();
if (ret == OK) {
mavlink_log_info(mavlink_fd, "settings loaded");
mavlink_log_info(&mavlink_log_pub, "settings loaded");
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack);
} else {
mavlink_log_critical(mavlink_fd, "settings load ERROR");
mavlink_log_critical(&mavlink_log_pub, "settings load ERROR");
/* convenience as many parts of NuttX use negative errno */
if (ret < 0) {
@@ -3635,7 +3613,7 @@ void *commander_low_prio_loop(void *arg)
}
if (ret < 1000) {
mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret));
mavlink_log_critical(&mavlink_log_pub, "ERROR: %s", strerror(ret));
}
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub, command_ack);
@@ -3655,7 +3633,7 @@ void *commander_low_prio_loop(void *arg)
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack);
} else {
mavlink_log_critical(mavlink_fd, "settings save error");
mavlink_log_critical(&mavlink_log_pub, "settings save error");
/* convenience as many parts of NuttX use negative errno */
if (ret < 0) {
@@ -3663,7 +3641,7 @@ void *commander_low_prio_loop(void *arg)
}
if (ret < 1000) {
mavlink_log_critical(mavlink_fd, "ERROR: %s", strerror(ret));
mavlink_log_critical(&mavlink_log_pub, "ERROR: %s", strerror(ret));
}
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub, command_ack);
@@ -3676,11 +3654,11 @@ void *commander_low_prio_loop(void *arg)
if (ret == OK) {
/* do not spam MAVLink, but provide the answer / green led mechanism */
mavlink_log_critical(mavlink_fd, "onboard parameters reset");
mavlink_log_critical(&mavlink_log_pub, "onboard parameters reset");
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED, command_ack_pub, command_ack);
} else {
mavlink_log_critical(mavlink_fd, "param reset error");
mavlink_log_critical(&mavlink_log_pub, "param reset error");
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED, command_ack_pub, command_ack);
}
}
src/modules/commander/commander_tests/state_machine_helper_test.cpp
+1 -1
View File
@@ -288,7 +288,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
armed.ready_to_arm = test->current_state.ready_to_arm;
// Attempt transition
transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, 0 /* no mavlink_fd */);
transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, nullptr /* no mavlink_log_pub */);
// Validate result of transition
ut_assert(test->assertMsg, test->expected_transition_result == result);
src/modules/commander/esc_calibration.cpp
+40 -40
View File
@@ -57,7 +57,7 @@
#include <uORB/topics/vehicle_command.h>
#include <uORB/uORB.h>
#include <drivers/drv_hrt.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
@@ -65,82 +65,82 @@
#endif
static const int ERROR = -1;
int do_esc_calibration(int mavlink_fd, struct actuator_armed_s* armed)
int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* armed)
{
int return_code = OK;
int fd = -1;
struct battery_status_s battery;
int batt_sub = -1;
bool batt_updated = false;
bool batt_connected = false;
hrt_abstime battery_connect_wait_timeout = 30000000;
hrt_abstime pwm_high_timeout = 10000000;
hrt_abstime timeout_start;
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, "esc");
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc");
batt_sub = orb_subscribe(ORB_ID(battery_status));
if (batt_sub < 0) {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Subscribe to battery");
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Subscribe to battery");
goto Error;
}
// Make sure battery is disconnected
orb_copy(ORB_ID(battery_status), batt_sub, &battery);
if (battery.voltage_filtered_v > 3.0f) {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Disconnect battery and try again");
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Disconnect battery and try again");
goto Error;
}
armed->in_esc_calibration_mode = true;
fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0);
if (fd < 0) {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Can't open PWM device");
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Can't open PWM device");
goto Error;
}
/* tell IO/FMU that its ok to disable its safety with the switch */
if (px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0) != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Unable to disable safety switch");
goto Error;
}
/* tell IO/FMU that the system is armed (it will output values if safety is off) */
if (px4_ioctl(fd, PWM_SERVO_ARM, 0) != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Unable to arm system");
goto Error;
}
/* tell IO to switch off safety without using the safety switch */
if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0) != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Unable to force safety off");
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to disable safety switch");
goto Error;
}
mavlink_and_console_log_info(mavlink_fd, "[cal] Connect battery now");
/* tell IO/FMU that the system is armed (it will output values if safety is off) */
if (px4_ioctl(fd, PWM_SERVO_ARM, 0) != OK) {
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to arm system");
goto Error;
}
/* tell IO to switch off safety without using the safety switch */
if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0) != OK) {
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to force safety off");
goto Error;
}
mavlink_and_console_log_info(mavlink_log_pub, "[cal] Connect battery now");
timeout_start = hrt_absolute_time();
while (true) {
// We are either waiting for the user to connect the battery. Or we are waiting to let the PWM
// sit high.
hrt_abstime timeout_wait = batt_connected ? pwm_high_timeout : battery_connect_wait_timeout;
if (hrt_absolute_time() - timeout_start > timeout_wait) {
if (!batt_connected) {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Timeout waiting for battery");
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Timeout waiting for battery");
goto Error;
}
// PWM was high long enough
break;
}
if (!batt_connected) {
orb_check(batt_sub, &batt_updated);
if (batt_updated) {
@@ -149,7 +149,7 @@ int do_esc_calibration(int mavlink_fd, struct actuator_armed_s* armed)
// Battery is connected, signal to user and start waiting again
batt_connected = true;
timeout_start = hrt_absolute_time();
mavlink_and_console_log_info(mavlink_fd, "[cal] Battery connected");
mavlink_and_console_log_info(mavlink_log_pub, "[cal] Battery connected");
}
}
}
@@ -162,24 +162,24 @@ Out:
}
if (fd != -1) {
if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0) != OK) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_WARNING_MSG, "Safety switch still off");
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_WARNING_MSG, "Safety switch still off");
}
if (px4_ioctl(fd, PWM_SERVO_DISARM, 0) != OK) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_WARNING_MSG, "Servos still armed");
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_WARNING_MSG, "Servos still armed");
}
if (px4_ioctl(fd, PWM_SERVO_CLEAR_ARM_OK, 0) != OK) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_WARNING_MSG, "Safety switch still deactivated");
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_WARNING_MSG, "Safety switch still deactivated");
}
px4_close(fd);
}
armed->in_esc_calibration_mode = false;
if (return_code == OK) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, "esc");
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc");
}
return return_code;
Error:
return_code = ERROR;
goto Out;
src/modules/commander/esc_calibration.h
+2 -2
View File
@@ -44,6 +44,6 @@
#include <uORB/topics/actuator_armed.h>
int do_esc_calibration(int mavlink_fd, struct actuator_armed_s* armed);
int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s* armed);
#endif
#endif
src/modules/commander/gyro_calibration.cpp
+18 -18
View File
@@ -53,7 +53,7 @@
#include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_combined.h>
#include <drivers/drv_gyro.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/mcu_version.h>
@@ -70,7 +70,7 @@ static const unsigned max_gyros = 3;
/// Data passed to calibration worker routine
typedef struct {
int mavlink_fd;
orb_advert_t *mavlink_log_pub;
int32_t device_id[max_gyros];
int gyro_sensor_sub[max_gyros];
struct gyro_calibration_s gyro_scale[max_gyros];
@@ -96,7 +96,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
/* 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)) {
if (calibrate_cancel_check(worker_data->mavlink_log_pub, cancel_sub)) {
return calibrate_return_cancelled;
}
@@ -122,7 +122,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
}
if (s == 0 && calibration_counter[0] % (calibration_count / 20) == 0) {
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, (calibration_counter[0] * 100) / calibration_count);
mavlink_and_console_log_info(worker_data->mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (calibration_counter[0] * 100) / calibration_count);
}
}
@@ -131,14 +131,14 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
}
if (poll_errcount > 1000) {
mavlink_and_console_log_critical(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG);
mavlink_and_console_log_critical(worker_data->mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
return calibrate_return_error;
}
}
for (unsigned s = 0; s < max_gyros; s++) {
if (worker_data->device_id[s] != 0 && calibration_counter[s] < calibration_count / 2) {
mavlink_and_console_log_critical(worker_data->mavlink_fd, "[cal] ERROR: missing data, sensor %d", s)
mavlink_and_console_log_critical(worker_data->mavlink_log_pub, "[cal] ERROR: missing data, sensor %d", s)
return calibrate_return_error;
}
@@ -150,14 +150,14 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data)
return calibrate_return_ok;
}
int do_gyro_calibration(int mavlink_fd)
int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
{
int res = OK;
gyro_worker_data_t worker_data = {};
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
worker_data.mavlink_fd = mavlink_fd;
worker_data.mavlink_log_pub = mavlink_log_pub;
struct gyro_calibration_s gyro_scale_zero;
gyro_scale_zero.x_offset = 0.0f;
@@ -180,7 +180,7 @@ int do_gyro_calibration(int mavlink_fd)
(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_and_console_log_critical(mavlink_fd, "[cal] Unable to reset CAL_GYRO%u_ID", s);
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] Unable to reset CAL_GYRO%u_ID", s);
return ERROR;
}
@@ -195,7 +195,7 @@ int do_gyro_calibration(int mavlink_fd)
px4_close(fd);
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, s);
return ERROR;
}
}
@@ -294,7 +294,7 @@ int do_gyro_calibration(int mavlink_fd)
fabsf(ydiff) > maxoff ||
fabsf(zdiff) > maxoff) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] motion, retrying..");
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] motion, retrying..");
res = ERROR;
} else {
@@ -306,7 +306,7 @@ int do_gyro_calibration(int mavlink_fd)
} while (res == ERROR && try_count <= max_tries);
if (try_count >= max_tries) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: Motion during calibration");
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] ERROR: Motion during calibration");
res = ERROR;
}
@@ -351,14 +351,14 @@ int do_gyro_calibration(int mavlink_fd)
px4_close(fd);
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, 1);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, 1);
}
#endif
}
}
if (failed) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: failed to set offset params");
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] ERROR: failed to set offset params");
res = ERROR;
}
}
@@ -375,7 +375,7 @@ int do_gyro_calibration(int mavlink_fd)
res = param_save_default();
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SAVE_PARAMS_MSG);
}
}
@@ -383,9 +383,9 @@ int do_gyro_calibration(int mavlink_fd)
usleep(200000);
if (res == OK) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
} else {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
}
/* give this message enough time to propagate */
src/modules/commander/gyro_calibration.h
+2 -1
View File
@@ -40,7 +40,8 @@
#define GYRO_CALIBRATION_H_
#include <stdint.h>
#include <uORB/uORB.h>
int do_gyro_calibration(int mavlink_fd);
int do_gyro_calibration(orb_advert_t *mavlink_log_pub);
#endif /* GYRO_CALIBRATION_H_ */
src/modules/commander/mag_calibration.cpp
+39 -48
View File
@@ -56,7 +56,7 @@
#include <drivers/drv_gyro.h>
#include <uORB/topics/sensor_combined.h>
#include <drivers/drv_mag.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
@@ -80,11 +80,11 @@ bool internal[max_mags];
int device_prio_max = 0;
int32_t device_id_primary = 0;
calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t (&device_ids)[max_mags]);
/// Data passed to calibration worker routine
typedef struct {
int mavlink_fd;
orb_advert_t *mavlink_log_pub;
unsigned done_count;
int sub_mag[max_mags];
unsigned int calibration_points_perside;
@@ -92,15 +92,15 @@ typedef struct {
uint64_t calibration_interval_perside_useconds;
unsigned int calibration_counter_total[max_mags];
bool side_data_collected[detect_orientation_side_count];
float* x[max_mags];
float* y[max_mags];
float* z[max_mags];
float *x[max_mags];
float *y[max_mags];
float *z[max_mags];
} mag_worker_data_t;
int do_mag_calibration(int mavlink_fd)
int do_mag_calibration(orb_advert_t *mavlink_log_pub)
{
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
struct mag_calibration_s mscale_null;
mscale_null.x_offset = 0.0f;
@@ -126,7 +126,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, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag);
mavlink_and_console_log_info(mavlink_log_pub, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag);
break;
}
#else
@@ -179,7 +179,7 @@ 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_ERROR_RESET_CAL_MSG, cur_mag);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, cur_mag);
}
/* calibrate range */
@@ -187,7 +187,7 @@ int do_mag_calibration(int mavlink_fd)
result = px4_ioctl(fd, MAGIOCCALIBRATE, fd);
if (result != OK) {
mavlink_and_console_log_info(mavlink_fd, "[cal] Skipped scale calibration, sensor %u", cur_mag);
mavlink_and_console_log_info(mavlink_log_pub, "[cal] Skipped scale calibration, sensor %u", cur_mag);
/* this is non-fatal - mark it accordingly */
result = OK;
}
@@ -199,7 +199,7 @@ int do_mag_calibration(int mavlink_fd)
// Calibrate all mags at the same time
if (result == OK) {
switch (mag_calibrate_all(mavlink_fd, device_ids)) {
switch (mag_calibrate_all(mavlink_log_pub, device_ids)) {
case calibrate_return_cancelled:
// Cancel message already displayed, we're done here
result = ERROR;
@@ -213,16 +213,16 @@ int do_mag_calibration(int mavlink_fd)
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);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
break;
} else {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SAVE_PARAMS_MSG);
}
// Fall through
default:
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
break;
}
}
@@ -263,8 +263,8 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
mag_worker_data_t* worker_data = (mag_worker_data_t*)(data);
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);
mavlink_and_console_log_info(worker_data->mavlink_log_pub, "[cal] Rotate vehicle around the detected orientation");
mavlink_and_console_log_info(worker_data->mavlink_log_pub, "[cal] Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds);
/*
* Detect if the system is rotating.
@@ -289,7 +289,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
fabsf(gyro_z_integral) < gyro_int_thresh_rad) {
/* abort on request */
if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) {
if (calibrate_cancel_check(worker_data->mavlink_log_pub, cancel_sub)) {
result = calibrate_return_cancelled;
px4_close(sub_gyro);
return result;
@@ -299,7 +299,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
if (hrt_absolute_time() > detection_deadline) {
result = calibrate_return_error;
warnx("int: %8.4f, %8.4f, %8.4f", (double)gyro_x_integral, (double)gyro_y_integral, (double)gyro_z_integral);
mavlink_and_console_log_critical(worker_data->mavlink_fd, "Failed: This calibration requires rotation.");
mavlink_and_console_log_critical(worker_data->mavlink_log_pub, "Failed: This calibration requires rotation.");
break;
}
@@ -339,7 +339,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
while (hrt_absolute_time() < calibration_deadline &&
calibration_counter_side < worker_data->calibration_points_perside) {
if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) {
if (calibrate_cancel_check(worker_data->mavlink_log_pub, cancel_sub)) {
result = calibrate_return_cancelled;
break;
}
@@ -394,7 +394,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
calibration_counter_side++;
// Progress indicator for side
mavlink_and_console_log_info(worker_data->mavlink_fd,
mavlink_and_console_log_info(worker_data->mavlink_log_pub,
"[cal] %s side calibration: progress <%u>",
detect_orientation_str(orientation), progress_percentage(worker_data) +
(unsigned)((100 / calibration_sides) * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)));
@@ -405,28 +405,28 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
if (poll_errcount > worker_data->calibration_points_perside * 3) {
result = calibrate_return_error;
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG);
mavlink_and_console_log_info(worker_data->mavlink_log_pub, CAL_ERROR_SENSOR_MSG);
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));
mavlink_and_console_log_info(worker_data->mavlink_log_pub, "[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, progress_percentage(worker_data));
mavlink_and_console_log_info(worker_data->mavlink_log_pub, CAL_QGC_PROGRESS_MSG, progress_percentage(worker_data));
}
return result;
}
calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t (&device_ids)[max_mags])
{
calibrate_return result = calibrate_return_ok;
mag_worker_data_t worker_data;
worker_data.mavlink_fd = mavlink_fd;
worker_data.mavlink_log_pub = mavlink_log_pub;
worker_data.done_count = 0;
worker_data.calibration_points_perside = calibration_total_points / calibration_sides;
worker_data.calibration_interval_perside_seconds = calibraton_duration_seconds / calibration_sides;
@@ -460,7 +460,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
worker_data.y[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
worker_data.z[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: out of memory");
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] ERROR: out of memory");
result = calibrate_return_error;
}
}
@@ -512,7 +512,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
// Mag in this slot is available
unsigned int orb_interval_msecs = (worker_data.calibration_interval_perside_useconds / 1000) / worker_data.calibration_points_perside;
//mavlink_and_console_log_info(mavlink_fd, "Orb interval %u msecs", orb_interval_msecs);
//mavlink_and_console_log_info(mavlink_log_pub, "Orb interval %u msecs", orb_interval_msecs);
orb_set_interval(worker_data.sub_mag[cur_mag], orb_interval_msecs);
}
}
@@ -522,7 +522,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
if (result == calibrate_return_ok) {
int cancel_sub = calibrate_cancel_subscribe();
result = calibrate_from_orientation(mavlink_fd, // Mavlink fd to write output
result = calibrate_from_orientation(mavlink_log_pub, // uORB handle to write output
cancel_sub, // Subscription to vehicle_command for cancel support
worker_data.side_data_collected, // Sides to calibrate
mag_calibration_worker, // Calibration worker
@@ -559,15 +559,15 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
&sphere_radius[cur_mag]);
if (!PX4_ISFINITE(sphere_x[cur_mag]) || !PX4_ISFINITE(sphere_y[cur_mag]) || !PX4_ISFINITE(sphere_z[cur_mag])) {
mavlink_and_console_log_emergency(mavlink_fd, "ERROR: Retry calibration (sphere NaN, #%u)", cur_mag);
mavlink_and_console_log_emergency(mavlink_log_pub, "ERROR: Retry calibration (sphere NaN, #%u)", cur_mag);
result = calibrate_return_error;
}
if (fabsf(sphere_x[cur_mag]) > MAG_MAX_OFFSET_LEN ||
fabsf(sphere_y[cur_mag]) > MAG_MAX_OFFSET_LEN ||
fabsf(sphere_z[cur_mag]) > MAG_MAX_OFFSET_LEN) {
mavlink_and_console_log_info(mavlink_fd, "WARNING: Large offsets %s mag", (internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
mavlink_and_console_log_info(mavlink_fd, "offsets: %8.4f, %8.4f, %8.4f, #%u", (double)sphere_x[cur_mag],
mavlink_and_console_log_emergency(mavlink_log_pub, "ERROR: Replace %s mag fault", (internal[cur_mag]) ? "autopilot, internal" : "GPS unit, external");
mavlink_and_console_log_info(mavlink_log_pub, "Excessive offsets: %8.4f, %8.4f, %8.4f, #%u", (double)sphere_x[cur_mag],
(double)sphere_y[cur_mag], (double)sphere_z[cur_mag], cur_mag);
result = calibrate_return_ok;
}
@@ -639,13 +639,13 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
fd_mag = px4_open(str, 0);
if (fd_mag < 0) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: unable to open mag device #%u", cur_mag);
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] ERROR: unable to open mag device #%u", cur_mag);
result = calibrate_return_error;
}
if (result == calibrate_return_ok) {
if (px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != OK) {
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: failed to get current calibration #%u", cur_mag);
mavlink_and_console_log_critical(mavlink_log_pub, "[cal] ERROR: failed to get current calibration #%u", cur_mag);
result = calibrate_return_error;
}
}
@@ -658,7 +658,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
#ifndef __PX4_QURT
if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, cur_mag);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, cur_mag);
result = calibrate_return_error;
}
#endif
@@ -674,15 +674,6 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
if (result == calibrate_return_ok) {
bool failed = false;
PX4_DEBUG("found offset %d: x: %.6f, y: %.6f, z: %.6f", cur_mag,
(double)mscale.x_offset,
(double)mscale.y_offset,
(double)mscale.z_offset);
PX4_DEBUG("found scale %d: x: %.6f, y: %.6f, z: %.6f", cur_mag,
(double)mscale.x_scale,
(double)mscale.y_scale,
(double)mscale.z_scale);
/* set parameters */
(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
@@ -705,14 +696,14 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
#endif
if (failed) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, cur_mag);
mavlink_and_console_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, cur_mag);
result = calibrate_return_error;
} else {
mavlink_and_console_log_info(mavlink_fd, "[cal] mag #%u off: x:%.2f y:%.2f z:%.2f Ga",
mavlink_and_console_log_info(mavlink_log_pub, "[cal] mag #%u off: x:%.2f y:%.2f z:%.2f Ga",
cur_mag,
(double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset);
#ifndef __PX4_QURT
mavlink_and_console_log_info(mavlink_fd, "[cal] mag #%u scale: x:%.2f y:%.2f z:%.2f",
mavlink_and_console_log_info(mavlink_log_pub, "[cal] mag #%u scale: x:%.2f y:%.2f z:%.2f",
cur_mag,
(double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale);
#endif
src/modules/commander/mag_calibration.h
+2 -1
View File
@@ -40,7 +40,8 @@
#define MAG_CALIBRATION_H_
#include <stdint.h>
#include <uORB/uORB.h>
int do_mag_calibration(int mavlink_fd);
int do_mag_calibration(orb_advert_t *mavlink_log_pub);
#endif /* MAG_CALIBRATION_H_ */
src/modules/commander/rc_calibration.cpp
+5 -5
View File
@@ -46,7 +46,7 @@
#include <unistd.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
@@ -56,7 +56,7 @@
#endif
static const int ERROR = -1;
int do_trim_calibration(int mavlink_fd)
int do_trim_calibration(orb_advert_t *mavlink_log_pub)
{
int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
usleep(400000);
@@ -65,7 +65,7 @@ int do_trim_calibration(int mavlink_fd)
orb_check(sub_man, &changed);
if (!changed) {
mavlink_log_critical(mavlink_fd, "no inputs, aborting");
mavlink_log_critical(mavlink_log_pub, "no inputs, aborting");
return ERROR;
}
@@ -98,12 +98,12 @@ int do_trim_calibration(int mavlink_fd)
int save_ret = param_save_default();
if (save_ret != 0 || p1r != 0 || p2r != 0 || p3r != 0) {
mavlink_log_critical(mavlink_fd, "TRIM: PARAM SET FAIL");
mavlink_log_critical(mavlink_log_pub, "TRIM: PARAM SET FAIL");
px4_close(sub_man);
return ERROR;
}
mavlink_log_info(mavlink_fd, "trim cal done");
mavlink_log_info(mavlink_log_pub, "trim cal done");
px4_close(sub_man);
return OK;
}
src/modules/commander/rc_calibration.h
+2 -1
View File
@@ -40,7 +40,8 @@
#define RC_CALIBRATION_H_
#include <stdint.h>
#include <uORB/uORB.h>
int do_trim_calibration(int mavlink_fd);
int do_trim_calibration(orb_advert_t *mavlink_log_pub);
#endif /* RC_CALIBRATION_H_ */
src/modules/commander/state_machine_helper.cpp
+30 -30
View File
@@ -58,10 +58,10 @@
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/mavlink_log.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_device.h>
#include <mavlink/mavlink_log.h>
#include "state_machine_helper.h"
#include "commander_helper.h"
@@ -107,7 +107,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
arming_state_t new_arming_state, ///< arming state requested
struct actuator_armed_s *armed, ///< current armed status
bool fRunPreArmChecks, ///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing
const int mavlink_fd) ///< mavlink fd for error reporting, 0 for none
orb_advert_t *mavlink_log_pub) ///< uORB handle for mavlink log
{
// Double check that our static arrays are still valid
ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0);
@@ -132,16 +132,16 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
prearm_ret = preflight_check(status, mavlink_fd, true /* pre-arm */ );
prearm_ret = preflight_check(status, mavlink_log_pub, true /* pre-arm */ );
}
/* re-run the pre-flight check as long as sensors are failing */
if (!status->condition_system_sensors_initialized
if (!status->condition_system_sensors_initialized
&& (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
|| new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY)
&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
if (last_preflight_check == 0 || hrt_absolute_time() - last_preflight_check > 1000 * 1000) {
prearm_ret = preflight_check(status, mavlink_fd, false /* pre-flight */);
prearm_ret = preflight_check(status, mavlink_log_pub, false /* pre-flight */);
status->condition_system_sensors_initialized = !prearm_ret;
last_preflight_check = hrt_absolute_time();
last_prearm_ret = prearm_ret;
@@ -193,7 +193,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
// Fail transition if we need safety switch press
} else if (safety->safety_switch_available && !safety->safety_off) {
mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!");
mavlink_and_console_log_critical(mavlink_log_pub, "NOT ARMING: Press safety switch first!");
feedback_provided = true;
valid_transition = false;
}
@@ -204,7 +204,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_and_console_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
mavlink_and_console_log_critical(mavlink_log_pub, "NOT ARMING: Connect power module.");
feedback_provided = true;
valid_transition = false;
}
@@ -214,14 +214,14 @@ 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.5f) {
mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
mavlink_and_console_log_critical(mavlink_log_pub, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
valid_transition = false;
} else if (status->avionics_power_rail_voltage < 4.9f) {
mavlink_and_console_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
mavlink_and_console_log_critical(mavlink_log_pub, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
} else if (status->avionics_power_rail_voltage > 5.4f) {
mavlink_and_console_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage);
mavlink_and_console_log_critical(mavlink_log_pub, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage);
feedback_provided = true;
}
}
@@ -245,15 +245,15 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
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 resolved, reboot before arming");
mavlink_and_console_log_critical(mavlink_log_pub, "Preflight check resolved, reboot before arming");
} else {
mavlink_and_console_log_critical(mavlink_fd, "Preflight check failed, refusing to arm");
mavlink_and_console_log_critical(mavlink_log_pub, "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, reboot to complete");
mavlink_and_console_log_critical(mavlink_log_pub, "Preflight check resolved, reboot to complete");
feedback_provided = true;
} else {
// Silent ignore
@@ -265,10 +265,10 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
(new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
(status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) &&
(!status->condition_system_sensors_initialized)) {
if ((!status->condition_system_prearm_error_reported &&
status->condition_system_hotplug_timeout) ||
if ((!status->condition_system_prearm_error_reported &&
status->condition_system_hotplug_timeout) ||
(new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection");
mavlink_and_console_log_critical(mavlink_log_pub, "Not ready to fly: Sensors need inspection");
status->condition_system_prearm_error_reported = true;
}
feedback_provided = true;
@@ -299,7 +299,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
if (ret == TRANSITION_DENIED) {
/* print to MAVLink and console if we didn't provide any feedback yet */
if (!feedback_provided) {
mavlink_and_console_log_critical(mavlink_fd, "INVAL: %s - %s", state_names[status->arming_state], state_names[new_arming_state]);
mavlink_and_console_log_critical(mavlink_log_pub, "INVAL: %s - %s", state_names[status->arming_state], state_names[new_arming_state]);
}
}
@@ -397,7 +397,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
/**
* Transition from one hil state to another
*/
transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_status, orb_advert_t *mavlink_log_pub)
{
transition_result_t ret = TRANSITION_DENIED;
@@ -408,7 +408,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
switch (new_state) {
case vehicle_status_s::HIL_STATE_OFF:
/* we're in HIL and unexpected things can happen if we disable HIL now */
mavlink_and_console_log_critical(mavlink_fd, "Not switching off HIL (safety)");
mavlink_and_console_log_critical(mavlink_log_pub, "Not switching off HIL (safety)");
ret = TRANSITION_DENIED;
break;
@@ -481,11 +481,11 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
closedir(d);
ret = TRANSITION_CHANGED;
mavlink_and_console_log_critical(mavlink_fd, "Switched to ON hil state");
mavlink_and_console_log_critical(mavlink_log_pub, "Switched to ON hil state");
} else {
/* failed opening dir */
mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY");
mavlink_log_info(mavlink_log_pub, "FAILED LISTING DEVICE ROOT DIRECTORY");
ret = TRANSITION_DENIED;
}
@@ -542,11 +542,11 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
}
ret = TRANSITION_CHANGED;
mavlink_and_console_log_critical(mavlink_fd, "Switched to ON hil state");
mavlink_and_console_log_critical(mavlink_log_pub, "Switched to ON hil state");
#endif
} else {
mavlink_and_console_log_critical(mavlink_fd, "Not switching to HIL when armed");
mavlink_and_console_log_critical(mavlink_log_pub, "Not switching to HIL when armed");
ret = TRANSITION_DENIED;
}
break;
@@ -830,13 +830,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
return status->nav_state != nav_state_old;
}
int preflight_check(struct vehicle_status_s *status, const int mavlink_fd, bool prearm, bool force_report)
int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report)
{
/*
/*
*/
bool reportFailures = force_report || (!status->condition_system_prearm_error_reported &&
status->condition_system_hotplug_timeout);
bool checkAirspeed = false;
/* Perform airspeed check only if circuit breaker is not
* engaged and it's not a rotary wing */
@@ -844,19 +844,19 @@ int preflight_check(struct vehicle_status_s *status, const int mavlink_fd, bool
checkAirspeed = true;
}
bool preflight_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true,
bool preflight_ok = Commander::preflightCheck(mavlink_log_pub, true, true, true, true,
checkAirspeed, (status->rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT),
!status->circuit_breaker_engaged_gpsfailure_check, true, reportFailures);
if (!status->cb_usb && status->usb_connected && prearm) {
preflight_ok = false;
if (reportFailures) {
mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited");
mavlink_and_console_log_critical(mavlink_log_pub, "NOT ARMING: Flying with USB connected prohibited");
}
}
/* report once, then set the flag */
if (mavlink_fd >= 0 && reportFailures && !preflight_ok) {
if (reportFailures && !preflight_ok) {
status->condition_system_prearm_error_reported = true;
}
src/modules/commander/state_machine_helper.h
+3 -3
View File
@@ -57,14 +57,14 @@ typedef enum {
bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
arming_state_t new_arming_state, struct actuator_armed_s *armed, bool fRunPreArmChecks, const int mavlink_fd);
arming_state_t new_arming_state, struct actuator_armed_s *armed, bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub);
transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);
transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_state, const int mavlink_fd);
transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_state, orb_advert_t *mavlink_log_pub);
bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe);
int preflight_check(struct vehicle_status_s *status, const int mavlink_fd, bool prearm, bool force_report=false);
int preflight_check(struct vehicle_status_s *status, orb_advert_t *mavlink_log_pub, bool prearm, bool force_report=false);
#endif /* STATE_MACHINE_HELPER_H_ */