fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 11:43: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/attitude_estimator_q/attitude_estimator_q_main.cpp
+7 -16
View File
@@ -68,12 +68,12 @@
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/geo/geo.h>
#include <lib/ecl/validation/data_validator_group.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/mavlink_log.h>
extern "C" __EXPORT int attitude_estimator_q_main(int argc, char *argv[]);
@@ -192,7 +192,7 @@ private:
bool _vibration_warning = false;
bool _ext_hdg_good = false;
int _mavlink_fd = -1;
orb_advert_t _mavlink_log_pub = nullptr;
perf_counter_t _update_perf;
perf_counter_t _loop_perf;
@@ -326,15 +326,6 @@ void AttitudeEstimatorQ::task_main()
while (!_task_should_exit) {
int ret = px4_poll(fds, 1, 1000);
#ifndef __PX4_QURT
if (_mavlink_fd < 0) {
/* TODO: This call currently stalls the thread on QURT */
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
}
#endif
if (ret < 0) {
// Poll error, sleep and try again
usleep(10000);
@@ -422,7 +413,7 @@ void AttitudeEstimatorQ::task_main()
if (_voter_gyro.failover_count() > 0) {
_failsafe = true;
flags = _voter_gyro.failover_state();
mavlink_and_console_log_emergency(_mavlink_fd, "Gyro #%i failure :%s%s%s%s%s!",
mavlink_and_console_log_emergency(&_mavlink_log_pub, "Gyro #%i failure :%s%s%s%s%s!",
_voter_gyro.failover_index(),
((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
@@ -434,7 +425,7 @@ void AttitudeEstimatorQ::task_main()
if (_voter_accel.failover_count() > 0) {
_failsafe = true;
flags = _voter_accel.failover_state();
mavlink_and_console_log_emergency(_mavlink_fd, "Accel #%i failure :%s%s%s%s%s!",
mavlink_and_console_log_emergency(&_mavlink_log_pub, "Accel #%i failure :%s%s%s%s%s!",
_voter_accel.failover_index(),
((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
@@ -446,7 +437,7 @@ void AttitudeEstimatorQ::task_main()
if (_voter_mag.failover_count() > 0) {
_failsafe = true;
flags = _voter_mag.failover_state();
mavlink_and_console_log_emergency(_mavlink_fd, "Mag #%i failure :%s%s%s%s%s!",
mavlink_and_console_log_emergency(&_mavlink_log_pub, "Mag #%i failure :%s%s%s%s%s!",
_voter_mag.failover_index(),
((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""),
((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""),
@@ -456,7 +447,7 @@ void AttitudeEstimatorQ::task_main()
}
if (_failsafe) {
mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
mavlink_and_console_log_emergency(&_mavlink_log_pub, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
}
}
@@ -469,7 +460,7 @@ void AttitudeEstimatorQ::task_main()
} else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) {
_vibration_warning = true;
mavlink_and_console_log_critical(_mavlink_fd, "HIGH VIBRATION! g: %d a: %d m: %d",
mavlink_and_console_log_critical(&_mavlink_log_pub, "HIGH VIBRATION! g: %d a: %d m: %d",
(int)(100 * _voter_gyro.get_vibration_factor(curr_time)),
(int)(100 * _voter_accel.get_vibration_factor(curr_time)),
(int)(100 * _voter_mag.get_vibration_factor(curr_time)));
src/modules/bottle_drop/bottle_drop.cpp
+19 -21
View File
@@ -67,10 +67,10 @@
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/mavlink_log.h>
#include <geo/geo.h>
#include <dataman/dataman.h>
#include <mathlib/mathlib.h>
#include <mavlink/mavlink_log.h>
/**
@@ -113,7 +113,7 @@ public:
private:
bool _task_should_exit; /**< if true, task should exit */
int _main_task; /**< handle for task */
int _mavlink_fd;
orb_advert_t _mavlink_log_pub;
int _command_sub;
int _wind_estimate_sub;
@@ -174,7 +174,6 @@ BottleDrop::BottleDrop() :
_task_should_exit(false),
_main_task(-1),
_mavlink_fd(-1),
_command_sub(-1),
_wind_estimate_sub(-1),
_command {},
@@ -341,8 +340,7 @@ void
BottleDrop::task_main()
{
_mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(_mavlink_fd, "[bottle_drop] started");
mavlink_log_info(&_mavlink_log_pub, "[bottle_drop] started");
_command_sub = orb_subscribe(ORB_ID(vehicle_command));
_wind_estimate_sub = orb_subscribe(ORB_ID(wind_estimate));
@@ -533,7 +531,7 @@ BottleDrop::task_main()
approach_error = _wrap_pi(ground_direction - approach_direction);
if (counter % 90 == 0) {
mavlink_log_info(_mavlink_fd, "drop distance %u, heading error %u", (unsigned)distance_real,
mavlink_log_info(&_mavlink_log_pub, "drop distance %u, heading error %u", (unsigned)distance_real,
(unsigned)math::degrees(approach_error));
}
}
@@ -638,7 +636,7 @@ BottleDrop::task_main()
float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat,
flight_vector_e.lon);
mavlink_log_critical(_mavlink_fd, "position set, approach heading: %u", (unsigned)distance_real,
mavlink_log_critical(&_mavlink_log_pub, "position set, approach heading: %u", (unsigned)distance_real,
(unsigned)math::degrees(approach_direction + M_PI_F));
_drop_state = DROP_STATE_TARGET_SET;
@@ -662,7 +660,7 @@ BottleDrop::task_main()
fabsf(approach_error) < math::radians(20.0f)) {
open_bay();
_drop_state = DROP_STATE_BAY_OPEN;
mavlink_log_info(_mavlink_fd, "#audio: opening bay");
mavlink_log_info(&_mavlink_log_pub, "#audio: opening bay");
}
}
}
@@ -680,7 +678,7 @@ BottleDrop::task_main()
(distance_real < precision) && ((distance_real < future_distance))) {
drop();
_drop_state = DROP_STATE_DROPPED;
mavlink_log_info(_mavlink_fd, "#audio: payload dropped");
mavlink_log_info(&_mavlink_log_pub, "#audio: payload dropped");
} else {
@@ -704,7 +702,7 @@ BottleDrop::task_main()
_drop_approval = false;
lock_release();
close_bay();
mavlink_log_info(_mavlink_fd, "#audio: closing bay");
mavlink_log_info(&_mavlink_log_pub, "#audio: closing bay");
// remove onboard mission
_onboard_mission.current_seq = -1;
@@ -751,16 +749,16 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd)
if (cmd->param1 > 0.5f && cmd->param2 > 0.5f) {
open_bay();
drop();
mavlink_log_critical(_mavlink_fd, "drop bottle");
mavlink_log_critical(&_mavlink_log_pub, "drop bottle");
} else if (cmd->param1 > 0.5f) {
open_bay();
mavlink_log_critical(_mavlink_fd, "opening bay");
mavlink_log_critical(&_mavlink_log_pub, "opening bay");
} else {
lock_release();
close_bay();
mavlink_log_critical(_mavlink_fd, "closing bay");
mavlink_log_critical(&_mavlink_log_pub, "closing bay");
}
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
@@ -771,12 +769,12 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd)
switch ((int)(cmd->param1 + 0.5f)) {
case 0:
_drop_approval = false;
mavlink_log_critical(_mavlink_fd, "got drop position, no approval");
mavlink_log_critical(&_mavlink_log_pub, "got drop position, no approval");
break;
case 1:
_drop_approval = true;
mavlink_log_critical(_mavlink_fd, "got drop position and approval");
mavlink_log_critical(&_mavlink_log_pub, "got drop position and approval");
break;
default:
@@ -791,7 +789,7 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd)
_target_position.lon = cmd->param6;
_target_position.alt = cmd->param7;
_drop_state = DROP_STATE_TARGET_VALID;
mavlink_log_info(_mavlink_fd, "got target: %8.4f, %8.4f, %8.4f", (double)_target_position.lat,
mavlink_log_info(&_mavlink_log_pub, "got target: %8.4f, %8.4f, %8.4f", (double)_target_position.lat,
(double)_target_position.lon, (double)_target_position.alt);
map_projection_init(&ref, _target_position.lat, _target_position.lon);
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
@@ -820,7 +818,7 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd)
case 1:
_drop_approval = true;
mavlink_log_info(_mavlink_fd, "#audio: got drop approval");
mavlink_log_info(&_mavlink_log_pub, "#audio: got drop approval");
break;
default:
@@ -846,19 +844,19 @@ BottleDrop::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);
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);
break;
case vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
mavlink_log_critical(_mavlink_fd, "command temporarily rejected: %u", cmd->command);
mavlink_log_critical(&_mavlink_log_pub, "command temporarily rejected: %u", cmd->command);
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);
break;
default:
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_ */
src/modules/ekf2/ekf2_main.cpp
+1 -1
View File
@@ -58,9 +58,9 @@
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <systemlib/mavlink_log.h>
#include <mathlib/mathlib.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <mavlink/mavlink_log.h>
#include <platforms/px4_defines.h>
#include <drivers/drv_hrt.h>
#include <controllib/uorb/blocks.hpp>
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+2 -2
View File
@@ -229,7 +229,7 @@ private:
bool _newDataMag;
bool _newRangeData;
int _mavlink_fd;
orb_advert_t _mavlink_log_pub;
control::BlockParamFloat _mag_offset_x;
control::BlockParamFloat _mag_offset_y;
@@ -356,7 +356,7 @@ private:
* Runs the sensor fusion step of the filter. The parameters determine which of the sensors
* are fused with each other
**/
void updateSensorFusion(const bool fuseGPS, const bool fuseMag, const bool fuseRangeSensor,
void updateSensorFusion(const bool fuseGPS, const bool fuseMag, const bool fuseRangeSensor,
const bool fuseBaro, const bool fuseAirSpeed);
/**
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+8 -11
View File
@@ -63,9 +63,9 @@
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <systemlib/mavlink_log.h>
#include <mathlib/mathlib.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <mavlink/mavlink_log.h>
#include <platforms/px4_defines.h>
static uint64_t IMUusec = 0;
@@ -213,7 +213,6 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_newDataMag(false),
_newRangeData(false),
_mavlink_fd(-1),
_mag_offset_x(this, "MAGB_X"),
_mag_offset_y(this, "MAGB_Y"),
_mag_offset_z(this, "MAGB_Z"),
@@ -405,7 +404,7 @@ int AttitudePositionEstimatorEKF::check_filter_state()
// Do not warn about accel offset if we have no position updates
if (!(warn_index == 5 && _ekf->staticMode)) {
PX4_WARN("reset: %s", feedback[warn_index]);
mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]);
mavlink_log_critical(&_mavlink_log_pub, "[ekf check] %s", feedback[warn_index]);
}
}
@@ -511,8 +510,6 @@ void AttitudePositionEstimatorEKF::task_main_trampoline(int argc, char *argv[])
void AttitudePositionEstimatorEKF::task_main()
{
_mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);
_ekf = new AttPosEKF();
if (!_ekf) {
@@ -769,7 +766,7 @@ void AttitudePositionEstimatorEKF::initReferencePosition(hrt_abstime timestamp,
_local_pos.ref_timestamp = timestamp;
map_projection_init(&_pos_ref, lat, lon);
mavlink_and_console_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
mavlink_and_console_log_info(&_mavlink_log_pub, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
}
}
@@ -1442,7 +1439,7 @@ void AttitudePositionEstimatorEKF::pollData()
_voter_mag.failover_count() > 0)) {
_failsafe = true;
mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
mavlink_and_console_log_emergency(&_mavlink_log_pub, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
}
if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
@@ -1454,7 +1451,7 @@ void AttitudePositionEstimatorEKF::pollData()
} else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) {
_vibration_warning = true;
mavlink_and_console_log_critical(_mavlink_fd, "HIGH VIBRATION! g: %d a: %d m: %d",
mavlink_and_console_log_critical(&_mavlink_log_pub, "HIGH VIBRATION! g: %d a: %d m: %d",
(int)(100 * _voter_gyro.get_vibration_factor(curr_time)),
(int)(100 * _voter_accel.get_vibration_factor(curr_time)),
(int)(100 * _voter_mag.get_vibration_factor(curr_time)));
@@ -1570,7 +1567,7 @@ void AttitudePositionEstimatorEKF::pollData()
//Stop dead-reckoning mode
if (_global_pos.dead_reckoning) {
mavlink_log_info(_mavlink_fd, "[ekf] stop dead-reckoning");
mavlink_log_info(&_mavlink_log_pub, "[ekf] stop dead-reckoning");
}
_global_pos.dead_reckoning = false;
@@ -1637,7 +1634,7 @@ void AttitudePositionEstimatorEKF::pollData()
if (dtLastGoodGPS >= POS_RESET_THRESHOLD) {
if (_global_pos.dead_reckoning) {
mavlink_log_info(_mavlink_fd, "[ekf] gave up dead-reckoning after long timeout");
mavlink_log_info(&_mavlink_log_pub, "[ekf] gave up dead-reckoning after long timeout");
}
_gpsIsGood = false;
@@ -1648,7 +1645,7 @@ void AttitudePositionEstimatorEKF::pollData()
else if (dtLastGoodGPS >= 0.5f) {
if (_armed.armed) {
if (!_global_pos.dead_reckoning) {
mavlink_log_info(_mavlink_fd, "[ekf] dead-reckoning enabled");
mavlink_log_info(&_mavlink_log_pub, "[ekf] dead-reckoning enabled");
}
_global_pos.dead_reckoning = true;
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+11 -17
View File
@@ -86,11 +86,11 @@
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/pid/pid.h>
#include <systemlib/mavlink_log.h>
#include <geo/geo.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <mathlib/mathlib.h>
#include <mavlink/mavlink_log.h>
#include <launchdetection/LaunchDetector.h>
#include <ecl/l1/ecl_l1_pos_controller.h>
#include <external_lgpl/tecs/tecs.h>
@@ -151,7 +151,7 @@ public:
bool task_running() { return _task_running; }
private:
int _mavlink_fd;
orb_advert_t _mavlink_log_pub;
bool _task_should_exit; /**< if true, sensor task should exit */
bool _task_running; /**< if true, task is running in its mainloop */
@@ -506,7 +506,7 @@ FixedwingPositionControl *g_control = nullptr;
FixedwingPositionControl::FixedwingPositionControl() :
_mavlink_fd(-1),
_mavlink_log_pub(nullptr),
_task_should_exit(false),
_task_running(false),
@@ -1063,7 +1063,7 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint
* for the whole landing */
if (_parameters.land_use_terrain_estimate && global_pos.terrain_alt_valid) {
if (!land_useterrain) {
mavlink_log_info(_mavlink_fd, "Landing, using terrain estimate");
mavlink_log_info(&_mavlink_log_pub, "Landing, using terrain estimate");
land_useterrain = true;
}
@@ -1400,7 +1400,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
target_bearing = _yaw;
}
mavlink_log_info(_mavlink_fd, "#Landing, heading hold");
mavlink_log_info(&_mavlink_log_pub, "#Landing, heading hold");
}
// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_yaw));
@@ -1506,7 +1506,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (!land_motor_lim) {
land_motor_lim = true;
mavlink_log_info(_mavlink_fd, "#Landing, limiting throttle");
mavlink_log_info(&_mavlink_log_pub, "#Landing, limiting throttle");
}
}
@@ -1534,7 +1534,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
// just started with the flaring phase
_att_sp.pitch_body = 0.0f;
height_flare = _global_pos.alt - terrain_alt;
mavlink_log_info(_mavlink_fd, "#Landing, flaring");
mavlink_log_info(&_mavlink_log_pub, "#Landing, flaring");
land_noreturn_vertical = true;
} else {
@@ -1567,7 +1567,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
altitude_desired_rel = landing_slope_alt_rel_desired;
if (!land_onslope) {
mavlink_log_info(_mavlink_fd, "#Landing, on slope");
mavlink_log_info(&_mavlink_log_pub, "#Landing, on slope");
land_onslope = true;
}
@@ -1602,7 +1602,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
* doesn't matter if it gets reset when takeoff is detected eventually */
_takeoff_ground_alt = _global_pos.alt;
mavlink_log_info(_mavlink_fd, "#Takeoff on runway");
mavlink_log_info(&_mavlink_log_pub, "#Takeoff on runway");
}
float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos);
@@ -1613,7 +1613,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_global_pos.alt - terrain_alt,
_global_pos.lat,
_global_pos.lon,
_mavlink_fd);
&_mavlink_log_pub);
/*
* Update navigation: _runway_takeoff returns the start WP according to mode and phase.
@@ -1665,7 +1665,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
static hrt_abstime last_sent = 0;
if (hrt_absolute_time() - last_sent > 4e6) {
mavlink_log_critical(_mavlink_fd, "#Launchdetection running");
mavlink_log_critical(&_mavlink_log_pub, "#Launchdetection running");
last_sent = hrt_absolute_time();
}
@@ -2126,12 +2126,6 @@ FixedwingPositionControl::task_main()
if (fds[1].revents & POLLIN) {
perf_begin(_loop_perf);
/* XXX Hack to get mavlink output going */
if (_mavlink_fd < 0) {
/* try to open the mavlink log device every once in a while */
_mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);
}
/* load local copies */
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+42 -45
View File
@@ -1,5 +1,5 @@
#include "BlockLocalPositionEstimator.hpp"
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <fcntl.h>
#include <systemlib/err.h>
#include <matrix/math.hpp>
@@ -135,9 +135,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
_time_last_vision_p(0),
_time_last_mocap(0),
// mavlink log
_mavlink_fd(open(MAVLINK_LOG_DEVICE, 0)),
// initialization flags
_baroInitialized(false),
_gpsInitialized(false),
@@ -342,7 +339,7 @@ void BlockLocalPositionEstimator::update()
_x(i) = 0;
}
mavlink_log_info(_mavlink_fd, "[lpe] reinit x");
mavlink_log_info(&_mavlink_log_pub, "[lpe] reinit x");
warnx("[lpe] reinit x");
}
@@ -361,7 +358,7 @@ void BlockLocalPositionEstimator::update()
}
if (reinit_P) {
mavlink_log_info(_mavlink_fd, "[lpe] reinit P");
mavlink_log_info(&_mavlink_log_pub, "[lpe] reinit P");
warnx("[lpe] reinit P");
initP();
}
@@ -471,7 +468,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
if (_timeStamp - _time_last_xy > EST_SRC_TIMEOUT) {
if (!_xyTimeout) {
_xyTimeout = true;
mavlink_log_info(_mavlink_fd, "[lpe] xy timeout ");
mavlink_log_info(&_mavlink_log_pub, "[lpe] xy timeout ");
warnx("[lpe] xy timeout ");
}
@@ -484,7 +481,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
if (_timeStamp - _time_last_z > EST_SRC_TIMEOUT) {
if (!_zTimeout) {
_zTimeout = true;
mavlink_log_info(_mavlink_fd, "[lpe] z timeout ");
mavlink_log_info(&_mavlink_log_pub, "[lpe] z timeout ");
warnx("[lpe] z timeout ");
}
@@ -497,7 +494,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
if (_timeStamp - _time_last_tz > EST_SRC_TIMEOUT) {
if (!_tzTimeout) {
_tzTimeout = true;
mavlink_log_info(_mavlink_fd, "[lpe] tz timeout ");
mavlink_log_info(&_mavlink_log_pub, "[lpe] tz timeout ");
warnx("[lpe] tz timeout ");
}
@@ -510,7 +507,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
if (_timeStamp - _time_last_baro > BARO_TIMEOUT) {
if (_baroInitialized) {
_baroInitialized = false;
mavlink_log_info(_mavlink_fd, "[lpe] baro timeout ");
mavlink_log_info(&_mavlink_log_pub, "[lpe] baro timeout ");
warnx("[lpe] baro timeout ");
}
}
@@ -518,7 +515,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
if (_timeStamp - _time_last_gps > GPS_TIMEOUT) {
if (_gpsInitialized) {
_gpsInitialized = false;
mavlink_log_info(_mavlink_fd, "[lpe] GPS timeout ");
mavlink_log_info(&_mavlink_log_pub, "[lpe] GPS timeout ");
warnx("[lpe] GPS timeout ");
}
}
@@ -526,7 +523,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
if (_timeStamp - _time_last_flow > FLOW_TIMEOUT) {
if (_flowInitialized) {
_flowInitialized = false;
mavlink_log_info(_mavlink_fd, "[lpe] flow timeout ");
mavlink_log_info(&_mavlink_log_pub, "[lpe] flow timeout ");
warnx("[lpe] flow timeout ");
}
}
@@ -534,7 +531,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
if (_timeStamp - _time_last_sonar > RANGER_TIMEOUT) {
if (_sonarInitialized) {
_sonarInitialized = false;
mavlink_log_info(_mavlink_fd, "[lpe] sonar timeout ");
mavlink_log_info(&_mavlink_log_pub, "[lpe] sonar timeout ");
warnx("[lpe] sonar timeout ");
}
}
@@ -542,7 +539,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
if (_timeStamp - _time_last_lidar > RANGER_TIMEOUT) {
if (_lidarInitialized) {
_lidarInitialized = false;
mavlink_log_info(_mavlink_fd, "[lpe] lidar timeout ");
mavlink_log_info(&_mavlink_log_pub, "[lpe] lidar timeout ");
warnx("[lpe] lidar timeout ");
}
}
@@ -550,7 +547,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
if (_timeStamp - _time_last_vision_p > VISION_TIMEOUT) {
if (_visionInitialized) {
_visionInitialized = false;
mavlink_log_info(_mavlink_fd, "[lpe] vision position timeout ");
mavlink_log_info(&_mavlink_log_pub, "[lpe] vision position timeout ");
warnx("[lpe] vision position timeout ");
}
}
@@ -558,7 +555,7 @@ void BlockLocalPositionEstimator::checkTimeouts()
if (_timeStamp - _time_last_mocap > MOCAP_TIMEOUT) {
if (_mocapInitialized) {
_mocapInitialized = false;
mavlink_log_info(_mavlink_fd, "[lpe] mocap timeout ");
mavlink_log_info(&_mavlink_log_pub, "[lpe] mocap timeout ");
warnx("[lpe] mocap timeout ");
}
}
@@ -574,7 +571,7 @@ void BlockLocalPositionEstimator::updateHome()
// like gps and baro to be off, need to allow it
// to reset by resetting covariance
initP();
mavlink_log_info(_mavlink_fd, "[lpe] home "
mavlink_log_info(&_mavlink_log_pub, "[lpe] home "
"lat %6.2f lon %6.2f alt %5.1f m",
lat, lon, double(alt));
warnx("[lpe] home "
@@ -598,7 +595,7 @@ void BlockLocalPositionEstimator::initBaro()
if (_baroStats.getCount() > REQ_BARO_INIT_COUNT) {
_baroAltHome = _baroStats.getMean()(0);
mavlink_log_info(_mavlink_fd,
mavlink_log_info(&_mavlink_log_pub,
"[lpe] baro init %d m std %d cm",
(int)_baroStats.getMean()(0),
(int)(100 * _baroStats.getStdDev()(0)));
@@ -643,7 +640,7 @@ void BlockLocalPositionEstimator::initGps()
_gpsAltHome = _gpsStats.getMean()(2);
map_projection_init(&_map_ref,
_gpsLatHome, _gpsLonHome);
mavlink_log_info(_mavlink_fd, "[lpe] gps init "
mavlink_log_info(&_mavlink_log_pub, "[lpe] gps init "
"lat %6.2f lon %6.2f alt %5.1f m",
_gpsLatHome,
_gpsLonHome,
@@ -691,7 +688,7 @@ void BlockLocalPositionEstimator::initLidar()
}
// not, might want to hard code this to zero
mavlink_log_info(_mavlink_fd, "[lpe] lidar init: "
mavlink_log_info(&_mavlink_log_pub, "[lpe] lidar init: "
"mean %d cm stddev %d cm",
int(100 * _lidarStats.getMean()(0)),
int(100 * _lidarStats.getStdDev()(0)));
@@ -731,7 +728,7 @@ void BlockLocalPositionEstimator::initSonar()
}
// not, might want to hard code this to zero
mavlink_log_info(_mavlink_fd, "[lpe] sonar init "
mavlink_log_info(&_mavlink_log_pub, "[lpe] sonar init "
"mean %d cm std %d cm",
int(100 * _sonarStats.getMean()(0)),
int(100 * _sonarStats.getStdDev()(0)));
@@ -758,7 +755,7 @@ void BlockLocalPositionEstimator::initFlow()
_time_last_flow = _timeStamp;
if (_flowQStats.getCount() > REQ_FLOW_INIT_COUNT) {
mavlink_log_info(_mavlink_fd, "[lpe] flow init: "
mavlink_log_info(&_mavlink_log_pub, "[lpe] flow init: "
"quality %d std %d",
int(_flowQStats.getMean()(0)),
int(_flowQStats.getStdDev()(0)));
@@ -785,7 +782,7 @@ void BlockLocalPositionEstimator::initVision()
if (_visionStats.getCount() > REQ_VISION_INIT_COUNT) {
_visionHome = _visionStats.getMean();
mavlink_log_info(_mavlink_fd, "[lpe] vision position init: "
mavlink_log_info(&_mavlink_log_pub, "[lpe] vision position init: "
"%5.2f %5.2f %5.2f m std %5.2f %5.2f %5.2f m",
double(_visionStats.getMean()(0)),
double(_visionStats.getMean()(1)),
@@ -825,7 +822,7 @@ void BlockLocalPositionEstimator::initMocap()
if (_mocapStats.getCount() > REQ_MOCAP_INIT_COUNT) {
_mocapHome = _mocapStats.getMean();
mavlink_log_info(_mavlink_fd, "[lpe] mocap position init: "
mavlink_log_info(&_mavlink_log_pub, "[lpe] mocap position init: "
"%5.2f, %5.2f, %5.2f m std %5.2f, %5.2f, %5.2f m",
double(_mocapStats.getMean()(0)),
double(_mocapStats.getMean()(1)),
@@ -1071,7 +1068,7 @@ void BlockLocalPositionEstimator::correctFlow()
if (qual < _flow_min_q.get()) {
if (_flowFault < FAULT_SEVERE) {
mavlink_log_info(_mavlink_fd, "[lpe] low flow quality %d", int(qual));
mavlink_log_info(&_mavlink_log_pub, "[lpe] low flow quality %d", int(qual));
warnx("[lpe] low flow quality %d", int(qual));
_flowFault = FAULT_SEVERE;
}
@@ -1096,7 +1093,7 @@ void BlockLocalPositionEstimator::correctFlow()
} else {
// no valid distance sensor, so return
if (_flowFault < FAULT_SEVERE) {
mavlink_log_info(_mavlink_fd, "[lpe] no distance for flow");
mavlink_log_info(&_mavlink_log_pub, "[lpe] no distance for flow");
warnx("[lpe] no distance for flow");
_flowFault = FAULT_SEVERE;
}
@@ -1167,14 +1164,14 @@ void BlockLocalPositionEstimator::correctFlow()
if (beta > BETA_TABLE[n_y_flow]) {
if (_flowFault < FAULT_MINOR) {
mavlink_log_info(_mavlink_fd, "[lpe] flow fault, beta %5.2f", double(beta));
mavlink_log_info(&_mavlink_log_pub, "[lpe] flow fault, beta %5.2f", double(beta));
warnx("[lpe] flow fault, beta %5.2f", double(beta));
_flowFault = FAULT_MINOR;
}
} else if (_flowFault) {
_flowFault = FAULT_NONE;
mavlink_log_info(_mavlink_fd, "[lpe] flow OK");
mavlink_log_info(&_mavlink_log_pub, "[lpe] flow OK");
warnx("[lpe] flow OK");
}
@@ -1207,7 +1204,7 @@ void BlockLocalPositionEstimator::correctSonar()
} else if (d > max_dist) {
if (_sonarFault < FAULT_SEVERE) {
mavlink_log_info(_mavlink_fd, "[lpe] sonar max distance");
mavlink_log_info(&_mavlink_log_pub, "[lpe] sonar max distance");
warnx("[lpe] sonar max distance");
_sonarFault = FAULT_SEVERE;
}
@@ -1262,7 +1259,7 @@ void BlockLocalPositionEstimator::correctSonar()
if (_sonarFault < FAULT_MINOR) {
// avoid printing messages near ground
if (_x(X_tz) > 1.0f) {
mavlink_log_info(_mavlink_fd, "[lpe] sonar fault, beta %5.2f", double(beta));
mavlink_log_info(&_mavlink_log_pub, "[lpe] sonar fault, beta %5.2f", double(beta));
warnx("[lpe] sonar fault, beta %5.2f", double(beta));
}
@@ -1274,7 +1271,7 @@ void BlockLocalPositionEstimator::correctSonar()
// avoid printing messages near ground
if (_x(X_tz) > 1.0f) {
mavlink_log_info(_mavlink_fd, "[lpe] sonar OK");
mavlink_log_info(&_mavlink_log_pub, "[lpe] sonar OK");
warnx("[lpe] sonar OK");
}
}
@@ -1324,7 +1321,7 @@ void BlockLocalPositionEstimator::correctBaro()
if (beta > BETA_TABLE[n_y_baro]) {
if (_baroFault < FAULT_MINOR) {
mavlink_log_info(_mavlink_fd, "[lpe] baro fault, r %5.2f m, beta %5.2f",
mavlink_log_info(&_mavlink_log_pub, "[lpe] baro fault, r %5.2f m, beta %5.2f",
double(r(0)), double(beta));
warnx("[lpe] baro fault, r %5.2f m, beta %5.2f",
double(r(0)), double(beta));
@@ -1333,7 +1330,7 @@ void BlockLocalPositionEstimator::correctBaro()
} else if (_baroFault) {
_baroFault = FAULT_NONE;
mavlink_log_info(_mavlink_fd, "[lpe] baro OK");
mavlink_log_info(&_mavlink_log_pub, "[lpe] baro OK");
warnx("[lpe] baro OK");
}
@@ -1369,7 +1366,7 @@ void BlockLocalPositionEstimator::correctLidar()
} else if (d > max_dist) {
if (_lidarFault < FAULT_SEVERE) {
mavlink_log_info(_mavlink_fd, "[lpe] lidar out of range");
mavlink_log_info(&_mavlink_log_pub, "[lpe] lidar out of range");
warnx("[lpe] lidar out of range");
_lidarFault = FAULT_SEVERE;
}
@@ -1416,7 +1413,7 @@ void BlockLocalPositionEstimator::correctLidar()
// only print message if above 1 meter, avoids
// message clutter when on ground
if (_x(X_tz) > 1.0f) {
mavlink_log_info(_mavlink_fd, "[lpe] lidar fault, r %5.2f m, beta %5.2f",
mavlink_log_info(&_mavlink_log_pub, "[lpe] lidar fault, r %5.2f m, beta %5.2f",
double(r(0)), double(beta));
warnx("[lpe] lidar fault, r %5.2f m, beta %5.2f",
double(r(0)), double(beta));
@@ -1431,7 +1428,7 @@ void BlockLocalPositionEstimator::correctLidar()
// only print message if above 1 meter, avoids
// message clutter when on ground
if (_x(X_tz) > 1.0f) {
mavlink_log_info(_mavlink_fd, "[lpe] lidar OK");
mavlink_log_info(&_mavlink_log_pub, "[lpe] lidar OK");
warnx("[lpe] lidar OK");
}
}
@@ -1461,7 +1458,7 @@ void BlockLocalPositionEstimator::correctGps()
if (nSat < 6 || eph > _gps_eph_max.get()) {
if (_gpsFault < FAULT_SEVERE) {
mavlink_log_info(_mavlink_fd, "[lpe] gps fault nSat: %d eph: %5.2f", nSat, double(eph));
mavlink_log_info(&_mavlink_log_pub, "[lpe] gps fault nSat: %d eph: %5.2f", nSat, double(eph));
warnx("[lpe] gps fault nSat: %d eph: %5.2f", nSat, double(eph));
_gpsFault = FAULT_SEVERE;
}
@@ -1554,12 +1551,12 @@ void BlockLocalPositionEstimator::correctGps()
if (beta > BETA_TABLE[n_y_gps]) {
if (_gpsFault < FAULT_MINOR) {
mavlink_log_info(_mavlink_fd, "[lpe] gps fault, beta: %5.2f", double(beta));
mavlink_log_info(&_mavlink_log_pub, "[lpe] gps fault, beta: %5.2f", double(beta));
warnx("[lpe] gps fault, beta: %5.2f", double(beta));
mavlink_log_info(_mavlink_fd, "[lpe] r: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f",
mavlink_log_info(&_mavlink_log_pub, "[lpe] r: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f",
double(r(0)), double(r(1)), double(r(2)),
double(r(3)), double(r(4)), double(r(5)));
mavlink_log_info(_mavlink_fd, "[lpe] S_I: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f",
mavlink_log_info(&_mavlink_log_pub, "[lpe] S_I: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f",
double(S_I(0, 0)), double(S_I(1, 1)), double(S_I(2, 2)),
double(S_I(3, 3)), double(S_I(4, 4)), double(S_I(5, 5)));
warnx("[lpe] r: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f",
@@ -1573,7 +1570,7 @@ void BlockLocalPositionEstimator::correctGps()
} else if (_gpsFault) {
_gpsFault = FAULT_NONE;
mavlink_log_info(_mavlink_fd, "[lpe] GPS OK");
mavlink_log_info(&_mavlink_log_pub, "[lpe] GPS OK");
warnx("[lpe] GPS OK");
}
@@ -1618,14 +1615,14 @@ void BlockLocalPositionEstimator::correctVision()
if (beta > BETA_TABLE[n_y_vision]) {
if (_visionFault < FAULT_MINOR) {
mavlink_log_info(_mavlink_fd, "[lpe] vision position fault, beta %5.2f", double(beta));
mavlink_log_info(&_mavlink_log_pub, "[lpe] vision position fault, beta %5.2f", double(beta));
warnx("[lpe] vision position fault, beta %5.2f", double(beta));
_visionFault = FAULT_MINOR;
}
} else if (_visionFault) {
_visionFault = FAULT_NONE;
mavlink_log_info(_mavlink_fd, "[lpe] vision position OK");
mavlink_log_info(&_mavlink_log_pub, "[lpe] vision position OK");
warnx("[lpe] vision position OK");
}
@@ -1672,14 +1669,14 @@ void BlockLocalPositionEstimator::correctMocap()
if (beta > BETA_TABLE[n_y_mocap]) {
if (_mocapFault < FAULT_MINOR) {
mavlink_log_info(_mavlink_fd, "[lpe] mocap fault, beta %5.2f", double(beta));
mavlink_log_info(&_mavlink_log_pub, "[lpe] mocap fault, beta %5.2f", double(beta));
warnx("[lpe] mocap fault, beta %5.2f", double(beta));
_mocapFault = FAULT_MINOR;
}
} else if (_mocapFault) {
_mocapFault = FAULT_NONE;
mavlink_log_info(_mavlink_fd, "[lpe] mocap OK");
mavlink_log_info(&_mavlink_log_pub, "[lpe] mocap OK");
warnx("[lpe] mocap OK");
}
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+1 -1
View File
@@ -275,7 +275,7 @@ private:
uint64_t _time_last_sonar;
uint64_t _time_last_vision_p;
uint64_t _time_last_mocap;
int _mavlink_fd;
orb_advert_t _mavlink_log_pub;
// initialization flags
bool _baroInitialized;
src/modules/mc_pos_control/mc_pos_control_main.cpp
+4 -6
View File
@@ -79,9 +79,9 @@
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <systemlib/systemlib.h>
#include <systemlib/mavlink_log.h>
#include <mathlib/mathlib.h>
#include <lib/geo/geo.h>
#include <mavlink/mavlink_log.h>
#include <platforms/px4_defines.h>
#include <controllib/blocks.hpp>
@@ -123,7 +123,7 @@ public:
private:
bool _task_should_exit; /**< if true, task should exit */
int _control_task; /**< task handle for task */
int _mavlink_fd; /**< mavlink fd */
orb_advert_t _mavlink_log_pub; /**< mavlink log advert */
int _vehicle_status_sub; /**< vehicle status subscription */
int _ctrl_state_sub; /**< control state subscription */
@@ -348,7 +348,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
SuperBlock(NULL, "MPC"),
_task_should_exit(false),
_control_task(-1),
_mavlink_fd(-1),
_mavlink_log_pub(nullptr),
/* subscriptions */
_ctrl_state_sub(-1),
@@ -1146,8 +1146,6 @@ void
MulticopterPositionControl::task_main()
{
_mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);
/*
* do subscriptions
*/
@@ -1960,7 +1958,7 @@ MulticopterPositionControl::task_main()
&& !_control_mode.flag_control_climb_rate_enabled;
}
mavlink_log_info(_mavlink_fd, "[mpc] stopped");
mavlink_log_info(&_mavlink_log_pub, "[mpc] stopped");
_control_task = -1;
}
src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
+3 -3
View File
@@ -53,7 +53,7 @@ MulticopterPositionControlMultiplatform::MulticopterPositionControlMultiplatform
_task_should_exit(false),
_control_task(-1),
_mavlink_fd(-1),
_mavlink_log_pub(nullptr),
/* publications */
_att_sp_pub(nullptr),
@@ -232,7 +232,7 @@ MulticopterPositionControlMultiplatform::reset_pos_sp()
// - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
//XXX: port this once a mavlink like interface is available
// mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
// mavlink_log_info(&_mavlink_log_pub, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
PX4_INFO("[mpc] reset pos sp: %2.3f, %2.3f", (double)_pos_sp(0), (double)_pos_sp(1));
}
}
@@ -249,7 +249,7 @@ MulticopterPositionControlMultiplatform::reset_alt_sp()
_att_sp_msg.data().yaw_body = _att->data().yaw;
//XXX: port this once a mavlink like interface is available
// mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2));
// mavlink_log_info(&_mavlink_log_pub, "[mpc] reset alt sp: %d", -(int)_pos_sp(2));
PX4_INFO("[mpc] reset alt sp: %2.3f", -(double)_pos_sp(2));
}
}
src/modules/mc_pos_control_multiplatform/mc_pos_control.h
+1 -1
View File
@@ -84,7 +84,7 @@ protected:
bool _task_should_exit; /**< if true, task should exit */
int _control_task; /**< task handle for task */
int _mavlink_fd; /**< mavlink fd */
orb_advert_t _mavlink_log_pub; /**< mavlink log advert */
Publisher<px4_vehicle_attitude_setpoint> *_att_sp_pub; /**< attitude setpoint publication */
Publisher<px4_vehicle_local_position_setpoint> *_local_pos_sp_pub; /**< vehicle local position setpoint publication */
src/modules/navigator/datalinkloss.cpp
+6 -6
View File
@@ -42,7 +42,7 @@
#include <math.h>
#include <fcntl.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <navigator/navigation.h>
@@ -187,7 +187,7 @@ DataLinkLoss::advance_dll()
if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) {
warnx("%d data link losses, limit is %d, fly to airfield home",
_navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get());
mavlink_log_critical(_navigator->get_mavlink_fd(), "too many DL losses, fly to airfield home");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "too many DL losses, fly to airfield home");
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->set_mission_result_updated();
reset_mission_item_reached();
@@ -195,19 +195,19 @@ DataLinkLoss::advance_dll()
} else {
if (!_param_skipcommshold.get()) {
warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter);
mavlink_log_critical(_navigator->get_mavlink_fd(), "fly to comms hold");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "fly to comms hold");
_dll_state = DLL_STATE_FLYTOCOMMSHOLDWP;
} else {
/* comms hold wp not active, fly to airfield home directly */
warnx("Skipping comms hold wp. Flying directly to airfield home");
mavlink_log_critical(_navigator->get_mavlink_fd(), "fly to airfield home, comms hold skipped");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "fly to airfield home, comms hold skipped");
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
}
}
break;
case DLL_STATE_FLYTOCOMMSHOLDWP:
warnx("fly to airfield home");
mavlink_log_critical(_navigator->get_mavlink_fd(), "fly to airfield home");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "fly to airfield home");
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->set_mission_result_updated();
@@ -216,7 +216,7 @@ DataLinkLoss::advance_dll()
case DLL_STATE_FLYTOAIRFIELDHOMEWP:
_dll_state = DLL_STATE_TERMINATE;
warnx("time is up, state should have been changed manually by now");
mavlink_log_critical(_navigator->get_mavlink_fd(), "no manual control, terminating");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no manual control, terminating");
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->set_mission_result_updated();
reset_mission_item_reached();
src/modules/navigator/enginefailure.cpp
+2 -2
View File
@@ -41,7 +41,7 @@
#include <math.h>
#include <fcntl.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <navigator/navigation.h>
@@ -141,7 +141,7 @@ EngineFailure::advance_ef()
{
switch (_ef_state) {
case EF_STATE_NONE:
mavlink_log_emergency(_navigator->get_mavlink_fd(), "Engine failure. Loitering down");
mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "Engine failure. Loitering down");
_ef_state = EF_STATE_LOITERDOWN;
break;
default:
src/modules/navigator/geofence.cpp
+14 -10
View File
@@ -44,14 +44,15 @@
#include <string.h>
#include <dataman/dataman.h>
#include <systemlib/err.h>
#include <systemlib/mavlink_log.h>
#include <stdlib.h>
#include <stdio.h>
#include <ctype.h>
#include <px4_config.h>
#include <unistd.h>
#include <mavlink/mavlink_log.h>
#include <geo/geo.h>
#include <drivers/drv_hrt.h>
#include "navigator.h"
#define GEOFENCE_RANGE_WARNING_LIMIT 3000000
@@ -61,8 +62,10 @@
#endif
static const int ERROR = -1;
Geofence::Geofence() :
Geofence::Geofence(Navigator *navigator) :
SuperBlock(NULL, "GF"),
_navigator(navigator),
_fence_pub(nullptr),
_home_pos{},
_home_pos_set(false),
@@ -77,8 +80,7 @@ Geofence::Geofence() :
_param_counter_threshold(this, "COUNT"),
_param_max_hor_distance(this, "MAX_HOR_DIST"),
_param_max_ver_distance(this, "MAX_VER_DIST"),
_outside_counter(0),
_mavlinkFd(-1)
_outside_counter(0)
{
/* Load initial params */
updateParams();
@@ -145,8 +147,9 @@ bool Geofence::inside(double lat, double lon, float altitude)
if (max_vertical_distance > 0 && (dist_z > max_vertical_distance)) {
if (hrt_elapsed_time(&_last_vertical_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
mavlink_and_console_log_critical(_mavlinkFd, "Geofence exceeded max vertical distance by %.1f m",
(double)(dist_z - max_vertical_distance));
mavlink_and_console_log_critical(_navigator->get_mavlink_log_pub(),
"Geofence exceeded max vertical distance by %.1f m",
(double)(dist_z - max_vertical_distance));
_last_vertical_range_warning = hrt_absolute_time();
}
@@ -155,8 +158,9 @@ bool Geofence::inside(double lat, double lon, float altitude)
if (max_horizontal_distance > 0 && (dist_xy > max_horizontal_distance)) {
if (hrt_elapsed_time(&_last_horizontal_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) {
mavlink_and_console_log_critical(_mavlinkFd, "Geofence exceeded max horizontal distance by %.1f m",
(double)(dist_xy - max_horizontal_distance));
mavlink_and_console_log_critical(_navigator->get_mavlink_log_pub(),
"Geofence exceeded max horizontal distance by %.1f m",
(double)(dist_xy - max_horizontal_distance));
_last_horizontal_range_warning = hrt_absolute_time();
}
@@ -403,12 +407,12 @@ Geofence::loadFromFile(const char *filename)
if (gotVertical && pointCounter > 0) {
_vertices_count = pointCounter;
warnx("Geofence: imported successfully");
mavlink_log_info(_mavlinkFd, "Geofence imported");
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Geofence imported");
rc = OK;
} else {
warnx("Geofence: import error");
mavlink_log_critical(_mavlinkFd, "Geofence import error");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence import error");
}
error:
src/modules/navigator/geofence.h
+5 -5
View File
@@ -53,10 +53,12 @@
#define GEOFENCE_FILENAME PX4_ROOTFSDIR"/fs/microsd/etc/geofence.txt"
class Navigator;
class Geofence : public control::SuperBlock
{
public:
Geofence();
Geofence(Navigator *navigator);
~Geofence();
/* Altitude mode, corresponding to the param GF_ALTMODE */
@@ -105,9 +107,9 @@ public:
int getGeofenceAction() { return _param_action.get(); }
void setMavlinkFd(int value) { _mavlinkFd = value; }
private:
Navigator *_navigator;
orb_advert_t _fence_pub; /**< publish fence topic */
home_position_s _home_pos;
@@ -131,8 +133,6 @@ private:
unsigned _outside_counter;
int _mavlinkFd;
bool inside(double lat, double lon, float altitude);
bool inside(const struct vehicle_global_position_s &global_position);
bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl);
src/modules/navigator/gpsfailure.cpp
+3 -3
View File
@@ -42,7 +42,7 @@
#include <math.h>
#include <fcntl.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <navigator/navigation.h>
@@ -162,12 +162,12 @@ GpsFailure::advance_gpsf()
case GPSF_STATE_NONE:
_gpsf_state = GPSF_STATE_LOITER;
warnx("gpsf loiter");
mavlink_log_critical(_navigator->get_mavlink_fd(), "GPS failed: open loop loiter");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "GPS failed: open loop loiter");
break;
case GPSF_STATE_LOITER:
_gpsf_state = GPSF_STATE_TERMINATE;
warnx("gpsf terminate");
mavlink_log_emergency(_navigator->get_mavlink_fd(), "no gps recovery, termination");
mavlink_log_emergency(_navigator->get_mavlink_log_pub(), "no gps recovery, termination");
warnx("mavlink sent");
break;
case GPSF_STATE_TERMINATE:
src/modules/navigator/land.cpp
+1 -1
View File
@@ -44,8 +44,8 @@
#include <math.h>
#include <fcntl.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/err.h>
#include <systemlib/mavlink_log.h>
#include <uORB/uORB.h>
#include <uORB/topics/position_setpoint_triplet.h>
src/modules/navigator/loiter.cpp
+1 -1
View File
@@ -45,7 +45,7 @@
#include <math.h>
#include <fcntl.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/err.h>
#include <uORB/uORB.h>
src/modules/navigator/mission.cpp
+18 -22
View File
@@ -52,7 +52,7 @@
#include <drivers/drv_hrt.h>
#include <dataman/dataman.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>
#include <lib/mathlib/mathlib.h>
@@ -257,7 +257,7 @@ Mission::update_offboard_mission()
* however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
failed = !_missionFeasibilityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol),
failed = !_missionFeasibilityChecker.checkMissionFeasible(_navigator->get_mavlink_log_pub(), (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol),
dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
_navigator->get_home_position()->alt, _navigator->home_position_valid(),
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
@@ -351,7 +351,7 @@ Mission::set_mission_items()
if (_param_onboard_enabled.get() && prepare_mission_items(true, &_mission_item, &mission_item_next_position, &has_next_position_item)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_ONBOARD) {
mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "onboard mission now running");
user_feedback_done = true;
}
_mission_type = MISSION_TYPE_ONBOARD;
@@ -360,7 +360,7 @@ Mission::set_mission_items()
} else if (prepare_mission_items(false, &_mission_item, &mission_item_next_position, &has_next_position_item)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_OFFBOARD) {
mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running");
mavlink_log_info(_navigator->get_mavlink_log_pub(), "offboard mission now running");
user_feedback_done = true;
}
_mission_type = MISSION_TYPE_OFFBOARD;
@@ -368,7 +368,7 @@ Mission::set_mission_items()
/* no mission available or mission finished, switch to loiter */
if (_mission_type != MISSION_TYPE_NONE) {
/* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */
mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "mission finished, loitering");
user_feedback_done = true;
/* use last setpoint for loiter */
@@ -400,9 +400,9 @@ Mission::set_mission_items()
if (_navigator->get_vstatus()->condition_landed) {
/* landed, refusing to take off without a mission */
mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, refusing takeoff");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no valid mission available, refusing takeoff");
} else {
mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, loitering");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no valid mission available, loitering");
}
user_feedback_done = true;
@@ -432,7 +432,7 @@ Mission::set_mission_items()
float takeoff_alt = calculate_takeoff_altitude(&_mission_item);
mavlink_log_info(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
mavlink_log_info(_navigator->get_mavlink_log_pub(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
_mission_item.lat = _navigator->get_global_position()->lat;
@@ -821,7 +821,7 @@ Mission::prepare_mission_items(bool onboard, struct mission_item_s *mission_item
int offset = 1;
if (read_mission_item(onboard, 0, mission_item)) {
first_res = true;
/* trying to find next position mission item */
@@ -873,7 +873,7 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
/* mission item index out of bounds - if they are equal, we just reached the end */
if (*mission_index_ptr != (int)mission->count) {
mavlink_and_console_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count);
mavlink_and_console_log_critical(_navigator->get_mavlink_log_pub(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count);
}
return false;
}
@@ -886,8 +886,7 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
/* read mission item from datamanager */
if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
mavlink_and_console_log_critical(_navigator->get_mavlink_fd(),
"ERROR waypoint could not be read");
mavlink_and_console_log_critical(_navigator->get_mavlink_log_pub(), "ERROR waypoint could not be read");
return false;
}
@@ -906,8 +905,7 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
&mission_item_tmp, len) != len) {
/* not supposed to happen unless the datamanager can't access the
* dataman */
mavlink_log_critical(_navigator->get_mavlink_fd(),
"ERROR DO JUMP waypoint could not be written");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "ERROR DO JUMP waypoint could not be written");
return false;
}
report_do_jump_mission_changed(*mission_index_ptr,
@@ -919,8 +917,7 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
} else {
if (offset == 0) {
mavlink_log_info(_navigator->get_mavlink_fd(),
"DO JUMP repetitions completed");
mavlink_log_info(_navigator->get_mavlink_log_pub(), "DO JUMP repetitions completed");
}
/* no more DO_JUMPS, therefore just try to continue with next mission item */
(*mission_index_ptr)++;
@@ -934,8 +931,7 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
}
/* we have given up, we don't want to cycle forever */
mavlink_log_critical(_navigator->get_mavlink_fd(),
"ERROR DO JUMP is cycling, giving up");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "ERROR DO JUMP is cycling, giving up");
return false;
}
@@ -957,7 +953,7 @@ Mission::save_offboard_mission_state()
if (mission_state.current_seq != _current_offboard_mission_index) {
if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) {
warnx("ERROR: can't save mission state");
mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: can't save mission state");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "ERROR: can't save mission state");
}
}
}
@@ -969,12 +965,12 @@ Mission::save_offboard_mission_state()
mission_state.current_seq = _current_offboard_mission_index;
warnx("ERROR: invalid mission state");
mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: invalid mission state");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "ERROR: invalid mission state");
/* write modified state only if changed */
if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) {
warnx("ERROR: can't save mission state");
mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: can't save mission state");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "ERROR: can't save mission state");
}
}
@@ -1027,7 +1023,7 @@ Mission::check_mission_valid()
dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
_navigator->get_mission_result()->valid = _missionFeasibilityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol),
_navigator->get_mission_result()->valid = _missionFeasibilityChecker.checkMissionFeasible(_navigator->get_mavlink_log_pub(), (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol),
dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
_navigator->get_home_position()->alt, _navigator->home_position_valid(),
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
src/modules/navigator/mission_block.cpp
+3 -3
View File
@@ -49,7 +49,7 @@
#include <systemlib/err.h>
#include <geo/geo.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <mathlib/mathlib.h>
#include <uORB/uORB.h>
@@ -233,7 +233,7 @@ MissionBlock::is_mission_item_reached()
_time_first_inside_orbit = now;
// if (_mission_item.time_inside > 0.01f) {
// mavlink_log_critical(_mavlink_fd, "waypoint reached, wait for %.1fs",
// mavlink_log_critical(_mavlink_log_pub, "waypoint reached, wait for %.1fs",
// (double)_mission_item.time_inside);
// }
}
@@ -486,7 +486,7 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio
if (at_current_location) {
item->lat = _navigator->get_global_position()->lat;
item->lon = _navigator->get_global_position()->lon;
/* use home position */
} else {
item->lat = _navigator->get_home_position()->lat;
src/modules/navigator/mission_feasibility_checker.cpp
+24 -24
View File
@@ -45,7 +45,7 @@
#include <geo/geo.h>
#include <math.h>
#include <mathlib/mathlib.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <fw_pos_control_l1/landingslope.h>
#include <systemlib/err.h>
#include <stdio.h>
@@ -54,7 +54,7 @@
#include <uORB/topics/fence.h>
MissionFeasibilityChecker::MissionFeasibilityChecker() :
_mavlink_fd(-1),
_mavlink_log_pub(nullptr),
_capabilities_sub(-1),
_initDone(false),
_dist_1wp_ok(false)
@@ -63,7 +63,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() :
}
bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing,
bool MissionFeasibilityChecker::checkMissionFeasible(orb_advert_t *mavlink_log_pub, bool isRotarywing,
dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
float home_alt, bool home_valid, double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued,
float default_acceptance_rad,
@@ -74,13 +74,13 @@ bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRota
/* Init if not done yet */
init();
_mavlink_fd = mavlink_fd;
_mavlink_log_pub = mavlink_log_pub;
// first check if we have a valid position
if (!home_valid /* can later use global / local pos for finer granularity */) {
failed = true;
warned = true;
mavlink_log_info(_mavlink_fd, "Not yet ready for mission, no position lock.");
mavlink_log_info(_mavlink_log_pub, "Not yet ready for mission, no position lock.");
} else {
failed = failed || !check_dist_1wp(dm_current, nMissionItems, curr_lat, curr_lon, max_waypoint_distance, warning_issued);
}
@@ -127,7 +127,7 @@ bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_curr
}
if (takeoff_alt - 1.0f < acceptance_radius) {
mavlink_log_critical(_mavlink_fd, "Mission rejected: Takeoff altitude too low!");
mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Takeoff altitude too low!");
return false;
}
}
@@ -165,7 +165,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
if (MissionBlock::item_contains_position(&missionitem) &&
!geofence.inside_polygon(missionitem.lat, missionitem.lon, missionitem.altitude)) {
mavlink_log_critical(_mavlink_fd, "Geofence violation for waypoint %d", i);
mavlink_log_critical(_mavlink_log_pub, "Geofence violation for waypoint %d", i);
return false;
}
}
@@ -194,10 +194,10 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
warning_issued = true;
if (throw_error) {
mavlink_log_critical(_mavlink_fd, "Rejecting mission: No home pos, WP %d uses rel alt", i+1);
mavlink_log_critical(_mavlink_log_pub, "Rejecting mission: No home pos, WP %d uses rel alt", i+1);
return false;
} else {
mavlink_log_critical(_mavlink_fd, "Warning: No home pos, WP %d uses rel alt", i+1);
mavlink_log_critical(_mavlink_log_pub, "Warning: No home pos, WP %d uses rel alt", i+1);
return true;
}
}
@@ -210,10 +210,10 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
warning_issued = true;
if (throw_error) {
mavlink_log_critical(_mavlink_fd, "Rejecting mission: Waypoint %d below home", i+1);
mavlink_log_critical(_mavlink_log_pub, "Rejecting mission: Waypoint %d below home", i+1);
return false;
} else {
mavlink_log_critical(_mavlink_fd, "Warning: Waypoint %d below home", i+1);
mavlink_log_critical(_mavlink_log_pub, "Warning: Waypoint %d below home", i+1);
return true;
}
}
@@ -230,7 +230,7 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
if (dm_read(dm_current, i, &missionitem, len) != len) {
// not supposed to happen unless the datamanager can't access the SD card, etc.
mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Cannot access SD card");
mavlink_log_critical(_mavlink_log_pub, "Rejecting Mission: Cannot access SD card");
return false;
}
@@ -250,7 +250,7 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST &&
missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) {
mavlink_log_critical(_mavlink_fd, "Rejecting mission item %i: unsupported cmd: %d", (int)(i+1), (int)missionitem.nav_cmd);
mavlink_log_critical(_mavlink_log_pub, "Rejecting mission item %i: unsupported cmd: %d", (int)(i+1), (int)missionitem.nav_cmd);
return false;
}
@@ -259,7 +259,7 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
i == 0 &&
condition_landed) {
mavlink_log_critical(_mavlink_fd, "Rejecting mission that starts with LAND command while vehicle is landed.");
mavlink_log_critical(_mavlink_log_pub, "Rejecting mission that starts with LAND command while vehicle is landed.");
return false;
}
@@ -308,25 +308,25 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
return true;
} else {
/* Landing waypoint is above altitude of slope at the given waypoint distance */
mavlink_log_critical(_mavlink_fd, "Landing: last waypoint too high/too close");
mavlink_log_critical(_mavlink_fd, "Move down to %.1fm or move further away by %.1fm",
mavlink_log_critical(_mavlink_log_pub, "Landing: last waypoint too high/too close");
mavlink_log_critical(_mavlink_log_pub, "Move down to %.1fm or move further away by %.1fm",
(double)(slope_alt_req),
(double)(wp_distance_req - wp_distance));
return false;
}
} else {
/* Landing waypoint is above last waypoint */
mavlink_log_critical(_mavlink_fd, "Landing waypoint above last nav waypoint");
mavlink_log_critical(_mavlink_log_pub, "Landing waypoint above last nav waypoint");
return false;
}
} else {
/* Last wp is in flare region */
//xxx give recommendations
mavlink_log_critical(_mavlink_fd, "Warning: Landing: last waypoint in flare region");
mavlink_log_critical(_mavlink_log_pub, "Warning: Landing: last waypoint in flare region");
return false;
}
} else {
mavlink_log_critical(_mavlink_fd, "Warning: starting with land waypoint");
mavlink_log_critical(_mavlink_log_pub, "Warning: starting with land waypoint");
return false;
}
}
@@ -353,13 +353,13 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
/* check actuator number */
if (mission_item.params[0] < 0 || mission_item.params[0] > 5) {
mavlink_log_critical(_mavlink_fd, "Actuator number %d is out of bounds 0..5", (int)mission_item.params[0]);
mavlink_log_critical(_mavlink_log_pub, "Actuator number %d is out of bounds 0..5", (int)mission_item.params[0]);
warning_issued = true;
return false;
}
/* check actuator value */
if (mission_item.params[1] < -2000 || mission_item.params[1] > 2000) {
mavlink_log_critical(_mavlink_fd, "Actuator value %d is out of bounds -2000..2000", (int)mission_item.params[1]);
mavlink_log_critical(_mavlink_log_pub, "Actuator value %d is out of bounds -2000..2000", (int)mission_item.params[1]);
warning_issued = true;
return false;
}
@@ -375,14 +375,14 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
_dist_1wp_ok = true;
if (dist_to_1wp > ((dist_first_wp * 3) / 2)) {
/* allow at 2/3 distance, but warn */
mavlink_log_critical(_mavlink_fd, "Warning: First waypoint very far: %d m", (int)dist_to_1wp);
mavlink_log_critical(_mavlink_log_pub, "Warning: First waypoint very far: %d m", (int)dist_to_1wp);
warning_issued = true;
}
return true;
} else {
/* item is too far from home */
mavlink_log_critical(_mavlink_fd, "First waypoint too far: %d m,refusing mission", (int)dist_to_1wp, (int)dist_first_wp);
mavlink_log_critical(_mavlink_log_pub, "First waypoint too far: %d m,refusing mission", (int)dist_to_1wp, (int)dist_first_wp);
warning_issued = true;
return false;
}
@@ -390,7 +390,7 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
} else {
/* error reading, mission is invalid */
mavlink_log_info(_mavlink_fd, "error reading offboard mission");
mavlink_log_info(_mavlink_log_pub, "error reading offboard mission");
return false;
}
}
src/modules/navigator/mission_feasibility_checker.h
+2 -2
View File
@@ -52,7 +52,7 @@
class MissionFeasibilityChecker
{
private:
int _mavlink_fd;
orb_advert_t *_mavlink_log_pub;
int _capabilities_sub;
struct navigation_capabilities_s _nav_caps;
@@ -83,7 +83,7 @@ public:
/*
* Returns true if mission is feasible and false otherwise
*/
bool checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current,
bool checkMissionFeasible(orb_advert_t *mavlink_log_pub, bool isRotarywing, dm_item_t dm_current,
size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid,
double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued, float default_acceptance_rad,
bool condition_landed);
src/modules/navigator/navigator.h
+2 -2
View File
@@ -176,7 +176,7 @@ public:
* @return the distance at which the next waypoint should be used
*/
float get_acceptance_radius(float mission_item_radius);
int get_mavlink_fd() { return _mavlink_fd; }
orb_advert_t *get_mavlink_log_pub() { return &_mavlink_log_pub; }
void increment_mission_instance_count() { _mission_instance_count++; }
@@ -187,7 +187,7 @@ private:
bool _task_should_exit; /**< if true, sensor task should exit */
int _navigator_task; /**< task handle for sensor task */
int _mavlink_fd; /**< the file descriptor to send messages over mavlink */
orb_advert_t _mavlink_log_pub; /**< the uORB advert to send messages over mavlink */
int _global_pos_sub; /**< global position subscription */
int _gps_pos_sub; /**< gps position subscription */
src/modules/navigator/navigator_main.cpp
+6 -18
View File
@@ -79,7 +79,7 @@
#include <geo/geo.h>
#include <dataman/dataman.h>
#include <mathlib/mathlib.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include "navigator.h"
@@ -102,7 +102,7 @@ Navigator::Navigator() :
SuperBlock(NULL, "NAV"),
_task_should_exit(false),
_navigator_task(-1),
_mavlink_fd(-1),
_mavlink_log_pub(nullptr),
_global_pos_sub(-1),
_gps_pos_sub(-1),
_home_pos_sub(-1),
@@ -131,7 +131,7 @@ Navigator::Navigator() :
_mission_item_valid(false),
_mission_instance_count(0),
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
_geofence{},
_geofence(this),
_geofence_violation_warning_sent(false),
_inside_fence(true),
_can_loiter_at_sp(false),
@@ -265,9 +265,6 @@ Navigator::task_main_trampoline(int argc, char *argv[])
void
Navigator::task_main()
{
_mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);
_geofence.setMavlinkFd(_mavlink_fd);
bool have_geofence_position_data = false;
/* Try to load the geofence:
@@ -281,7 +278,7 @@ Navigator::task_main()
} else {
if (_geofence.clearDm() != OK) {
mavlink_log_critical(_mavlink_fd, "failed clearing geofence");
mavlink_log_critical(&_mavlink_log_pub, "failed clearing geofence");
}
}
@@ -308,9 +305,6 @@ Navigator::task_main()
navigation_capabilities_update();
params_update();
hrt_abstime mavlink_open_time = 0;
const hrt_abstime mavlink_open_interval = 500000;
/* wakeup source(s) */
px4_pollfd_struct_t fds[1] = {};
@@ -342,12 +336,6 @@ Navigator::task_main()
perf_begin(_loop_perf);
if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time) {
/* try to reopen the mavlink log device with specified interval */
mavlink_open_time = hrt_abstime() + mavlink_open_interval;
_mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);
}
bool updated;
/* gps updated */
@@ -435,7 +423,7 @@ Navigator::task_main()
/* Issue a warning about the geofence violation once */
if (!_geofence_violation_warning_sent) {
mavlink_log_critical(_mavlink_fd, "Geofence violation");
mavlink_log_critical(&_mavlink_log_pub, "Geofence violation");
_geofence_violation_warning_sent = true;
}
} else {
@@ -777,6 +765,6 @@ Navigator::set_mission_failure(const char* reason)
if (!_mission_result.mission_failure) {
_mission_result.mission_failure = true;
set_mission_result_updated();
mavlink_log_critical(_mavlink_fd, "%s", reason);
mavlink_log_critical(&_mavlink_log_pub, "%s", reason);
}
}
src/modules/navigator/rcloss.cpp
+4 -4
View File
@@ -42,7 +42,7 @@
#include <math.h>
#include <fcntl.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>
@@ -155,11 +155,11 @@ RCLoss::advance_rcl()
case RCL_STATE_NONE:
if (_param_loitertime.get() > 0.0f) {
warnx("RC loss, OBC mode, loiter");
mavlink_log_critical(_navigator->get_mavlink_fd(), "rc loss, loitering");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "rc loss, loitering");
_rcl_state = RCL_STATE_LOITER;
} else {
warnx("RC loss, OBC mode, slip loiter, terminate");
mavlink_log_critical(_navigator->get_mavlink_fd(), "rc loss, terminating");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "rc loss, terminating");
_rcl_state = RCL_STATE_TERMINATE;
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->set_mission_result_updated();
@@ -169,7 +169,7 @@ RCLoss::advance_rcl()
case RCL_STATE_LOITER:
_rcl_state = RCL_STATE_TERMINATE;
warnx("time is up, no RC regain, terminating");
mavlink_log_critical(_navigator->get_mavlink_fd(), "RC not regained, terminating");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RC not regained, terminating");
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->set_mission_result_updated();
reset_mission_item_reached();
src/modules/navigator/rtl.cpp
+9 -9
View File
@@ -42,7 +42,7 @@
#include <math.h>
#include <fcntl.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>
@@ -96,7 +96,7 @@ RTL::on_activation()
/* for safety reasons don't go into RTL if landed */
if (_navigator->get_vstatus()->condition_landed) {
_rtl_state = RTL_STATE_LANDED;
mavlink_log_critical(_navigator->get_mavlink_fd(), "no RTL when landed");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "no RTL when landed");
/* if lower than return altitude, climb up first */
} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
@@ -157,7 +157,7 @@ RTL::set_rtl_item()
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: climb to %d m (%d m above home)",
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)",
(int)(climb_alt),
(int)(climb_alt - _navigator->get_home_position()->alt));
break;
@@ -189,7 +189,7 @@ RTL::set_rtl_item()
_mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD;
mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: return at %d m (%d m above home)",
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)",
(int)(_mission_item.altitude),
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
@@ -212,7 +212,7 @@ RTL::set_rtl_item()
_mission_item.autocontinue = false;
_mission_item.origin = ORIGIN_ONBOARD;
mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: descend to %d m (%d m above home)",
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above home)",
(int)(_mission_item.altitude),
(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
break;
@@ -238,10 +238,10 @@ RTL::set_rtl_item()
_navigator->set_can_loiter_at_sp(true);
if (autoland) {
mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: loiter %.1fs", (double)_mission_item.time_inside);
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs", (double)_mission_item.time_inside);
} else {
mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: completed, loiter");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: completed, loiter");
}
break;
}
@@ -249,14 +249,14 @@ RTL::set_rtl_item()
case RTL_STATE_LAND: {
set_land_item(&_mission_item, false);
mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: land at home");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: land at home");
break;
}
case RTL_STATE_LANDED: {
set_idle_item(&_mission_item);
mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: completed, landed");
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: completed, landed");
break;
}
src/modules/navigator/takeoff.cpp
+1 -1
View File
@@ -44,7 +44,7 @@
#include <math.h>
#include <fcntl.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <systemlib/err.h>
#include <uORB/uORB.h>
src/modules/position_estimator_inav/position_estimator_inav_main.cpp
+19 -20
View File
@@ -66,9 +66,9 @@
#include <uORB/topics/att_pos_mocap.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/distance_sensor.h>
#include <mavlink/mavlink_log.h>
#include <poll.h>
#include <systemlib/err.h>
#include <systemlib/mavlink_log.h>
#include <geo/geo.h>
#include <systemlib/systemlib.h>
#include <drivers/drv_hrt.h>
@@ -234,8 +234,7 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e
****************************************************************************/
int position_estimator_inav_thread_main(int argc, char *argv[])
{
int mavlink_fd;
mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);
orb_advert_t mavlink_log_pub = nullptr;
float x_est[2] = { 0.0f, 0.0f }; // pos, vel
float y_est[2] = { 0.0f, 0.0f }; // pos, vel
@@ -421,10 +420,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (ret < 0) {
/* poll error */
mavlink_log_info(mavlink_fd, "[inav] poll error on init");
mavlink_log_info(&mavlink_log_pub, "[inav] poll error on init");
} else if (hrt_absolute_time() - baro_wait_for_sample_time > MAX_WAIT_FOR_BARO_SAMPLE) {
wait_baro = false;
mavlink_log_info(mavlink_fd, "[inav] timed out waiting for a baro sample");
mavlink_log_info(&mavlink_log_pub, "[inav] timed out waiting for a baro sample");
}
else if (ret > 0) {
if (fds_init[0].revents & POLLIN) {
@@ -466,7 +465,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (ret < 0) {
/* poll error */
mavlink_log_info(mavlink_fd, "[inav] poll error on init");
mavlink_log_info(&mavlink_log_pub, "[inav] poll error on init");
continue;
} else if (ret > 0) {
@@ -549,9 +548,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar);
lidar.current_distance += params.lidar_calibration_offset;
}
if (updated) { //check if altitude estimation for lidar is enabled and new sensor data
if (params.enable_lidar_alt_est && lidar.current_distance > lidar.min_distance && lidar.current_distance < lidar.max_distance
&& (PX4_R(att.R, 2, 2) > 0.7f)) {
@@ -567,7 +566,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (lidar_first) {
lidar_first = false;
lidar_offset = dist_ground + z_est[0];
mavlink_log_info(mavlink_fd, "[inav] LIDAR: new ground offset");
mavlink_log_info(&mavlink_log_pub, "[inav] LIDAR: new ground offset");
warnx("[inav] LIDAR: new ground offset");
}
@@ -764,7 +763,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
last_vision_z = vision.z;
warnx("VISION estimate valid");
mavlink_log_info(mavlink_fd, "[inav] VISION estimate valid");
mavlink_log_info(&mavlink_log_pub, "[inav] VISION estimate valid");
}
/* calculate correction for position */
@@ -818,7 +817,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
mocap_valid = true;
warnx("MOCAP data valid");
mavlink_log_info(mavlink_fd, "[inav] MOCAP data valid");
mavlink_log_info(&mavlink_log_pub, "[inav] MOCAP data valid");
}
/* calculate correction for position */
@@ -842,7 +841,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (gps_valid) {
if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3) {
gps_valid = false;
mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
mavlink_log_info(&mavlink_log_pub, "[inav] GPS signal lost");
warnx("[inav] GPS signal lost");
}
@@ -850,7 +849,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (gps.eph < max_eph_epv * 0.7f && gps.epv < max_eph_epv * 0.7f && gps.fix_type >= 3) {
gps_valid = true;
reset_est = true;
mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
mavlink_log_info(&mavlink_log_pub, "[inav] GPS signal found");
warnx("[inav] GPS signal found");
}
}
@@ -883,7 +882,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
map_projection_init(&ref, lat, lon);
// XXX replace this print
warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt);
mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt);
mavlink_log_info(&mavlink_log_pub, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt);
}
}
@@ -945,35 +944,35 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if ((flow_valid || lidar_valid) && t > (flow_time + flow_topic_timeout)) {
flow_valid = false;
warnx("FLOW timeout");
mavlink_log_info(mavlink_fd, "[inav] FLOW timeout");
mavlink_log_info(&mavlink_log_pub, "[inav] FLOW timeout");
}
/* check for timeout on GPS topic */
if (gps_valid && (t > (gps.timestamp_position + gps_topic_timeout))) {
gps_valid = false;
warnx("GPS timeout");
mavlink_log_info(mavlink_fd, "[inav] GPS timeout");
mavlink_log_info(&mavlink_log_pub, "[inav] GPS timeout");
}
/* check for timeout on vision topic */
if (vision_valid && (t > (vision.timestamp_boot + vision_topic_timeout))) {
vision_valid = false;
warnx("VISION timeout");
mavlink_log_info(mavlink_fd, "[inav] VISION timeout");
mavlink_log_info(&mavlink_log_pub, "[inav] VISION timeout");
}
/* check for timeout on mocap topic */
if (mocap_valid && (t > (mocap.timestamp_boot + mocap_topic_timeout))) {
mocap_valid = false;
warnx("MOCAP timeout");
mavlink_log_info(mavlink_fd, "[inav] MOCAP timeout");
mavlink_log_info(&mavlink_log_pub, "[inav] MOCAP timeout");
}
/* check for lidar measurement timeout */
if (lidar_valid && (t > (lidar_time + lidar_timeout))) {
lidar_valid = false;
warnx("LIDAR timeout");
mavlink_log_info(mavlink_fd, "[inav] LIDAR timeout");
mavlink_log_info(&mavlink_log_pub, "[inav] LIDAR timeout");
}
float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
@@ -1380,7 +1379,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
warnx("stopped");
mavlink_log_info(mavlink_fd, "[inav] stopped");
mavlink_log_info(&mavlink_log_pub, "[inav] stopped");
thread_running = false;
return 0;
}
src/modules/sdlog2/sdlog2.c
+13 -22
View File
@@ -118,10 +118,9 @@
#include <systemlib/perf_counter.h>
#include <systemlib/git_version.h>
#include <systemlib/printload.h>
#include <systemlib/mavlink_log.h>
#include <version/version.h>
#include <mavlink/mavlink_log.h>
#include "logbuffer.h"
#include "sdlog2_format.h"
#include "sdlog2_messages.h"
@@ -159,7 +158,7 @@ static int32_t _utc_offset = 0;
#endif
static const char *mountpoint = MOUNTPOINT;
static const char *log_root = MOUNTPOINT "/log";
static int mavlink_fd = -1;
static orb_advert_t mavlink_log_pub = NULL;
struct logbuffer_s lb;
/* mutex / condition to synchronize threads */
@@ -480,7 +479,7 @@ int create_log_dir()
}
/* print logging path, important to find log file later */
mavlink_and_console_log_info(mavlink_fd, "[blackbox] %s", log_dir);
mavlink_and_console_log_info(&mavlink_log_pub, "[blackbox] %s", log_dir);
return 0;
}
@@ -517,7 +516,7 @@ int open_log_file()
if (file_number > MAX_NO_LOGFILE) {
/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
mavlink_and_console_log_critical(mavlink_fd, "[blackbox] ERR: max files %d", MAX_NO_LOGFILE);
mavlink_and_console_log_critical(&mavlink_log_pub, "[blackbox] ERR: max files %d", MAX_NO_LOGFILE);
return -1;
}
}
@@ -529,10 +528,10 @@ int open_log_file()
#endif
if (fd < 0) {
mavlink_and_console_log_critical(mavlink_fd, "[blackbox] failed: %s", log_file_name);
mavlink_and_console_log_critical(&mavlink_log_pub, "[blackbox] failed: %s", log_file_name);
} else {
mavlink_and_console_log_info(mavlink_fd, "[blackbox] recording: %s", log_file_name);
mavlink_and_console_log_info(&mavlink_log_pub, "[blackbox] recording: %s", log_file_name);
}
return fd;
@@ -569,7 +568,7 @@ int open_perf_file(const char* str)
if (file_number > MAX_NO_LOGFILE) {
/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
mavlink_and_console_log_critical(mavlink_fd, "[blackbox] ERR: max files %d", MAX_NO_LOGFILE);
mavlink_and_console_log_critical(&mavlink_log_pub, "[blackbox] ERR: max files %d", MAX_NO_LOGFILE);
return -1;
}
}
@@ -581,7 +580,7 @@ int open_perf_file(const char* str)
#endif
if (fd < 0) {
mavlink_and_console_log_critical(mavlink_fd, "[blackbox] failed: %s", log_file_name);
mavlink_and_console_log_critical(&mavlink_log_pub, "[blackbox] failed: %s", log_file_name);
}
@@ -710,7 +709,7 @@ void sdlog2_start_log()
/* create log dir if needed */
if (create_log_dir() != 0) {
mavlink_and_console_log_critical(mavlink_fd, "[blackbox] error creating log dir");
mavlink_and_console_log_critical(&mavlink_log_pub, "[blackbox] error creating log dir");
return;
}
@@ -811,7 +810,7 @@ void sdlog2_stop_log()
/* free log buffer */
logbuffer_free(&lb);
mavlink_and_console_log_info(mavlink_fd, "[blackbox] stopped (%lu drops)", skipped_count);
mavlink_and_console_log_info(&mavlink_log_pub, "[blackbox] stopped (%lu drops)", skipped_count);
sdlog2_status();
}
@@ -925,12 +924,6 @@ bool copy_if_updated_multi(orb_id_t topic, int multi_instance, int *handle, void
int sdlog2_thread_main(int argc, char *argv[])
{
mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);
if (mavlink_fd < 0) {
warnx("ERR: log stream, start mavlink app first");
}
/* default log rate: 50 Hz */
int32_t log_rate = 50;
int log_buffer_size = LOG_BUFFER_SIZE_DEFAULT;
@@ -2163,7 +2156,7 @@ void sdlog2_status()
float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f;
warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped);
mavlink_log_info(mavlink_fd, "[blackbox] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped);
mavlink_log_info(&mavlink_log_pub, "[blackbox] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped);
}
}
@@ -2187,16 +2180,14 @@ int check_free_space()
/* use a threshold of 50 MiB */
if (statfs_buf.f_bavail < (px4_statfs_buf_f_bavail_t)(50 * 1024 * 1024 / statfs_buf.f_bsize)) {
mavlink_and_console_log_critical(mavlink_fd,
"[blackbox] no space on MicroSD: %u MiB",
mavlink_and_console_log_critical(&mavlink_log_pub, "[blackbox] no space on MicroSD: %u MiB",
(unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U));
/* we do not need a flag to remember that we sent this warning because we will exit anyway */
return PX4_ERROR;
/* use a threshold of 100 MiB to send a warning */
} else if (!space_warning_sent && statfs_buf.f_bavail < (px4_statfs_buf_f_bavail_t)(100 * 1024 * 1024 / statfs_buf.f_bsize)) {
mavlink_and_console_log_critical(mavlink_fd,
"[blackbox] space on MicroSD low: %u MiB",
mavlink_and_console_log_critical(&mavlink_log_pub, "[blackbox] space on MicroSD low: %u MiB",
(unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U));
/* we don't want to flood the user with warnings */
space_warning_sent = true;
src/modules/systemlib/rc_check.c
+11 -11
View File
@@ -47,12 +47,12 @@
#include <systemlib/err.h>
#include <systemlib/rc_check.h>
#include <systemlib/param/param.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
#include <drivers/drv_rc_input.h>
#define RC_INPUT_MAP_UNMAPPED 0
int rc_calibration_check(int mavlink_fd, bool report_fail)
int rc_calibration_check(orb_advert_t *mavlink_log_pub, bool report_fail)
{
char nbuf[20];
@@ -74,7 +74,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail)
param_t map_parm = param_find(rc_map_mandatory[j]);
if (map_parm == PARAM_INVALID) {
if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]); }
if (report_fail) { mavlink_and_console_log_critical(mavlink_log_pub, "RC ERR: PARAM %s MISSING", rc_map_mandatory[j]); }
/* give system time to flush error message in case there are more */
usleep(100000);
@@ -87,7 +87,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail)
param_get(map_parm, &mapping);
if (mapping > RC_INPUT_MAX_CHANNELS) {
if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]); }
if (report_fail) { mavlink_and_console_log_critical(mavlink_log_pub, "RC ERR: %s >= # CHANS", rc_map_mandatory[j]); }
/* give system time to flush error message in case there are more */
usleep(100000);
@@ -95,7 +95,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail)
}
if (mapping == 0) {
if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]); }
if (report_fail) { mavlink_and_console_log_critical(mavlink_log_pub, "RC ERR: Mandatory %s is unmapped", rc_map_mandatory[j]); }
/* give system time to flush error message in case there are more */
usleep(100000);
@@ -148,7 +148,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail)
if (param_min < RC_INPUT_LOWEST_MIN_US) {
count++;
if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US); }
if (report_fail) { mavlink_and_console_log_critical(mavlink_log_pub, "RC ERR: RC_%d_MIN < %u", i + 1, RC_INPUT_LOWEST_MIN_US); }
/* give system time to flush error message in case there are more */
usleep(100000);
@@ -157,7 +157,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail)
if (param_max > RC_INPUT_HIGHEST_MAX_US) {
count++;
if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US); }
if (report_fail) { mavlink_and_console_log_critical(mavlink_log_pub, "RC ERR: RC_%d_MAX > %u", i + 1, RC_INPUT_HIGHEST_MAX_US); }
/* give system time to flush error message in case there are more */
usleep(100000);
@@ -166,7 +166,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail)
if (param_trim < param_min) {
count++;
if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min); }
if (report_fail) { mavlink_and_console_log_critical(mavlink_log_pub, "RC ERR: RC_%d_TRIM < MIN (%d/%d)", i + 1, (int)param_trim, (int)param_min); }
/* give system time to flush error message in case there are more */
usleep(100000);
@@ -175,7 +175,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail)
if (param_trim > param_max) {
count++;
if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max); }
if (report_fail) { mavlink_and_console_log_critical(mavlink_log_pub, "RC ERR: RC_%d_TRIM > MAX (%d/%d)", i + 1, (int)param_trim, (int)param_max); }
/* give system time to flush error message in case there are more */
usleep(100000);
@@ -183,7 +183,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail)
/* assert deadzone is sane */
if (param_dz > RC_INPUT_MAX_DEADZONE_US) {
if (report_fail) { mavlink_and_console_log_critical(mavlink_fd, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US); }
if (report_fail) { mavlink_and_console_log_critical(mavlink_log_pub, "RC ERR: RC_%d_DZ > %u", i + 1, RC_INPUT_MAX_DEADZONE_US); }
/* give system time to flush error message in case there are more */
usleep(100000);
@@ -201,7 +201,7 @@ int rc_calibration_check(int mavlink_fd, bool report_fail)
sleep(2);
if (report_fail) {
mavlink_and_console_log_critical(mavlink_fd, "%d config error%s for %d RC channel%s.",
mavlink_and_console_log_critical(mavlink_log_pub, "%d config error%s for %d RC channel%s.",
total_fail_count,
(total_fail_count > 1) ? "s" : "", channels_failed, (channels_failed > 1) ? "s" : "");
}
src/modules/systemlib/rc_check.h
+2 -1
View File
@@ -37,6 +37,7 @@
* RC calibration check
*/
#include <stdbool.h>
#include <uORB/uORB.h>
#pragma once
@@ -48,6 +49,6 @@ __BEGIN_DECLS
* @return 0 / OK if RC calibration is ok, index + 1 of the first
* channel that failed else (so 1 == first channel failed)
*/
__EXPORT int rc_calibration_check(int mavlink_fd, bool report_fail);
__EXPORT int rc_calibration_check(orb_advert_t *mavlink_log_pub, bool report_fail);
__END_DECLS
src/modules/vtol_att_control/vtol_att_control_main.cpp
+3 -15
View File
@@ -47,7 +47,7 @@
*
*/
#include "vtol_att_control_main.h"
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
namespace VTOL_att_control
{
@@ -62,7 +62,7 @@ VtolAttitudeControl::VtolAttitudeControl() :
_control_task(-1),
// mavlink log
_mavlink_fd(-1),
_mavlink_log_pub(nullptr),
//init subscription handlers
_v_att_sub(-1),
@@ -491,7 +491,7 @@ void
VtolAttitudeControl::abort_front_transition()
{
if(!_abort_front_transition) {
mavlink_log_critical(_mavlink_fd, "Front transition timeout occured, aborting");
mavlink_log_critical(&_mavlink_log_pub, "Front transition timeout occured, aborting");
_abort_front_transition = true;
_vtol_vehicle_status.vtol_transition_failsafe = true;
}
@@ -596,8 +596,6 @@ void VtolAttitudeControl::task_main()
{
fflush(stdout);
_mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);
/* do subscriptions */
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_mc_virtual_att_sp_sub = orb_subscribe(ORB_ID(mc_virtual_attitude_setpoint));
@@ -628,9 +626,6 @@ void VtolAttitudeControl::task_main()
// make sure we start with idle in mc mode
_vtol_type->set_idle_mc();
hrt_abstime mavlink_open_time = 0;
const hrt_abstime mavlink_open_interval = 500000;
/* wakeup source*/
px4_pollfd_struct_t fds[3] = {}; /*input_mc, input_fw, parameters*/
@@ -668,13 +663,6 @@ void VtolAttitudeControl::task_main()
continue;
}
if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time) {
/* try to reopen the mavlink log device with specified interval */
mavlink_open_time = hrt_abstime() + mavlink_open_interval;
_mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);
}
if (fds[2].revents & POLLIN) { //parameters were updated, read them now
/* read from param to clear updated flag */
struct parameter_update_s update;
src/modules/vtol_att_control/vtol_att_control_main.h
+1 -1
View File
@@ -140,7 +140,7 @@ private:
//******************flags & handlers******************************************************
bool _task_should_exit;
int _control_task; //task handle for VTOL attitude controller
int _mavlink_fd; // mavlink log device
orb_advert_t _mavlink_log_pub; // mavlink log uORB handle
/* handlers for subscriptions */
int _v_att_sub; //vehicle attitude subscription