Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 19:32:36 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

mavlink_log_info: always print to console and merge with mavlink_and_console_log_info

Browse Source
This commit is contained in:
Daniel Agar
2020-05-11 11:27:33 -04:00
committed by Beat Küng
parent b933557f56
commit 05886e053f
19 changed files with 125 additions and 146 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/px4io/px4io.cpp
+1 -1
View File
@@ -2200,7 +2200,7 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
} while (retries > 0); } while (retries > 0);
if (retries == 0) { if (retries == 0) {
mavlink_and_console_log_info(&_mavlink_log_pub, "[IO] mixer upload fail"); mavlink_log_info(&_mavlink_log_pub, "[IO] mixer upload fail");
/* load must have failed for some reason */ /* load must have failed for some reason */
return -EINVAL; return -EINVAL;
src/lib/systemlib/mavlink_log.h
+6 -27
View File
@@ -74,12 +74,16 @@ __EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, con
*/ */
/** /**
* Send a mavlink info message (not printed to console). * Send a mavlink info message (also printed to console).
* *
* @param _pub Pointer to the uORB advert; * @param _pub Pointer to the uORB advert;
* @param _text The text to log; * @param _text The text to log;
*/ */
#define mavlink_log_info(_pub, _text, ...) mavlink_vasprintf(_MSG_PRIO_INFO, _pub, _text, ##__VA_ARGS__); #define mavlink_log_info(_pub, _text, ...) \
do { \
mavlink_vasprintf(_MSG_PRIO_INFO, _pub, _text, ##__VA_ARGS__); \
PX4_INFO(_text, ##__VA_ARGS__); \
} while(0);
/** /**
* Send a mavlink warning message and print to console. * Send a mavlink warning message and print to console.
@@ -116,28 +120,3 @@ __EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, con
mavlink_vasprintf(_MSG_PRIO_CRITICAL, _pub, _text, ##__VA_ARGS__); \ mavlink_vasprintf(_MSG_PRIO_CRITICAL, _pub, _text, ##__VA_ARGS__); \
PX4_WARN(_text, ##__VA_ARGS__); \ PX4_WARN(_text, ##__VA_ARGS__); \
} while(0); } while(0);
/**
* Send a mavlink emergency message and print to console.
*
* @param _pub Pointer to the uORB advert;
* @param _text The text to log;
*/
#define mavlink_and_console_log_info(_pub, _text, ...) \
do { \
mavlink_log_info(_pub, _text, ##__VA_ARGS__); \
PX4_INFO(_text, ##__VA_ARGS__); \
} while(0);
struct mavlink_logmessage {
char text[MAVLINK_LOG_MAXLEN + 1];
unsigned char severity;
};
struct mavlink_logbuffer {
unsigned int start;
unsigned int size;
int count;
struct mavlink_logmessage *elems;
};
src/modules/airspeed_selector/airspeed_selector_main.cpp
+8 -8
View File
@@ -220,12 +220,12 @@ AirspeedModule::init()
_valid_airspeed_index = math::min(_param_airspeed_primary_index.get(), _number_of_airspeed_sensors); _valid_airspeed_index = math::min(_param_airspeed_primary_index.get(), _number_of_airspeed_sensors);
if (_number_of_airspeed_sensors == 0) { if (_number_of_airspeed_sensors == 0) {
mavlink_and_console_log_info(&_mavlink_log_pub, mavlink_log_info(&_mavlink_log_pub,
"No airspeed sensor detected. Switch to non-airspeed mode."); "No airspeed sensor detected. Switch to non-airspeed mode.");
} else { } else {
mavlink_and_console_log_info(&_mavlink_log_pub, mavlink_log_info(&_mavlink_log_pub,
"Primary airspeed index bigger than number connected sensors. Take last sensor."); "Primary airspeed index bigger than number connected sensors. Take last sensor.");
} }
} else { } else {
@@ -399,7 +399,7 @@ void AirspeedModule::update_params()
-1.0f); // set it to a negative value to start estimation inside wind estimator -1.0f); // set it to a negative value to start estimation inside wind estimator
} else { } else {
mavlink_and_console_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor."); mavlink_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor.");
_param_west_scale_estimation_on.set(0); // reset this param to 0 as estimation was not turned on _param_west_scale_estimation_on.set(0); // reset this param to 0 as estimation was not turned on
_param_west_scale_estimation_on.commit_no_notification(); _param_west_scale_estimation_on.commit_no_notification();
} }
@@ -413,11 +413,11 @@ void AirspeedModule::update_params()
_param_west_airspeed_scale.commit_no_notification(); _param_west_airspeed_scale.commit_no_notification();
_airspeed_validator[_valid_airspeed_index - 1].set_airspeed_scale_manual(_param_west_airspeed_scale.get()); _airspeed_validator[_valid_airspeed_index - 1].set_airspeed_scale_manual(_param_west_airspeed_scale.get());
mavlink_and_console_log_info(&_mavlink_log_pub, "Airspeed: estimated scale (ASPD_ASPD_SCALE): %0.2f", mavlink_log_info(&_mavlink_log_pub, "Airspeed: estimated scale (ASPD_ASPD_SCALE): %0.2f",
(double)_airspeed_validator[_valid_airspeed_index - 1].get_EAS_scale()); (double)_airspeed_validator[_valid_airspeed_index - 1].get_EAS_scale());
} else { } else {
mavlink_and_console_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor."); mavlink_log_info(&_mavlink_log_pub, "Airspeed: can't estimate scale as no valid sensor.");
} }
} }
src/modules/commander/Commander.cpp
+6 -6
View File
@@ -884,7 +884,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
/* switch to RTL which ends the mission */ /* switch to RTL which ends the mission */
if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags,
&_internal_state)) { &_internal_state)) {
mavlink_and_console_log_info(&mavlink_log_pub, "Returning to launch"); mavlink_log_info(&mavlink_log_pub, "Returning to launch");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else { } else {
@@ -913,7 +913,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
case vehicle_command_s::VEHICLE_CMD_NAV_LAND: { case vehicle_command_s::VEHICLE_CMD_NAV_LAND: {
if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags, if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags,
&_internal_state)) { &_internal_state)) {
mavlink_and_console_log_info(&mavlink_log_pub, "Landing at current position"); mavlink_log_info(&mavlink_log_pub, "Landing at current position");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else { } else {
@@ -926,7 +926,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
case vehicle_command_s::VEHICLE_CMD_NAV_PRECLAND: { case vehicle_command_s::VEHICLE_CMD_NAV_PRECLAND: {
if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_PRECLAND, if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_PRECLAND,
status_flags, &_internal_state)) { status_flags, &_internal_state)) {
mavlink_and_console_log_info(&mavlink_log_pub, "Precision landing"); mavlink_log_info(&mavlink_log_pub, "Precision landing");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else { } else {
@@ -1582,10 +1582,10 @@ Commander::run()
if (armed.armed) { if (armed.armed) {
if (_was_landed != _land_detector.landed) { if (_was_landed != _land_detector.landed) {
if (_land_detector.landed) { if (_land_detector.landed) {
mavlink_and_console_log_info(&mavlink_log_pub, "Landing detected"); mavlink_log_info(&mavlink_log_pub, "Landing detected");
} else { } else {
mavlink_and_console_log_info(&mavlink_log_pub, "Takeoff detected"); mavlink_log_info(&mavlink_log_pub, "Takeoff detected");
_have_taken_off_since_arming = true; _have_taken_off_since_arming = true;
// Set all position and velocity test probation durations to takeoff value // Set all position and velocity test probation durations to takeoff value
@@ -1599,7 +1599,7 @@ Commander::run()
if (was_falling != _land_detector.freefall) { if (was_falling != _land_detector.freefall) {
if (_land_detector.freefall) { if (_land_detector.freefall) {
mavlink_and_console_log_info(&mavlink_log_pub, "Freefall detected"); mavlink_log_info(&mavlink_log_pub, "Freefall detected");
} }
} }
} }
src/modules/commander/calibration_routines.h
+1 -1
View File
@@ -139,7 +139,7 @@ bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, ///< uORB handle to w
#define calibration_log_info(_pub, _text, ...) \ #define calibration_log_info(_pub, _text, ...) \
do { \ do { \
mavlink_and_console_log_info(_pub, _text, ##__VA_ARGS__); \ mavlink_log_info(_pub, _text, ##__VA_ARGS__); \
px4_usleep(10000); \ px4_usleep(10000); \
} while(0); } while(0);
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
+11 -11
View File
@@ -207,13 +207,13 @@ void BlockLocalPositionEstimator::Run()
s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING && s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING &&
_sub_lidar == nullptr) { _sub_lidar == nullptr) {
_sub_lidar = s; _sub_lidar = s;
mavlink_and_console_log_info(&mavlink_log_pub, "%sDownward-facing Lidar detected with ID %zu", msg_label, i); mavlink_log_info(&mavlink_log_pub, "%sDownward-facing Lidar detected with ID %zu", msg_label, i);
} else if (s->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND && } else if (s->get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND &&
s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING && s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING &&
_sub_sonar == nullptr) { _sub_sonar == nullptr) {
_sub_sonar = s; _sub_sonar = s;
mavlink_and_console_log_info(&mavlink_log_pub, "%sDownward-facing Sonar detected with ID %zu", msg_label, i); mavlink_log_info(&mavlink_log_pub, "%sDownward-facing Sonar detected with ID %zu", msg_label, i);
} }
} }
} }
@@ -346,8 +346,8 @@ void BlockLocalPositionEstimator::Run()
// set timestamp when origin was set to current time // set timestamp when origin was set to current time
_time_origin = _timeStamp; _time_origin = _timeStamp;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (parameter) : lat %6.2f lon %6.2f alt %5.1f m", mavlink_log_info(&mavlink_log_pub, "[lpe] global origin init (parameter) : lat %6.2f lon %6.2f alt %5.1f m",
double(_param_lpe_lat.get()), double(_param_lpe_lon.get()), double(_altOrigin)); double(_param_lpe_lat.get()), double(_param_lpe_lon.get()), double(_altOrigin));
} }
// reinitialize x if necessary // reinitialize x if necessary
@@ -359,7 +359,7 @@ void BlockLocalPositionEstimator::Run()
// don't want it to take too long // don't want it to take too long
if (!PX4_ISFINITE(_x(i))) { if (!PX4_ISFINITE(_x(i))) {
reinit_x = true; reinit_x = true;
mavlink_and_console_log_info(&mavlink_log_pub, "%sreinit x, x(%zu) not finite", msg_label, i); mavlink_log_info(&mavlink_log_pub, "%sreinit x, x(%zu) not finite", msg_label, i);
break; break;
} }
} }
@@ -369,7 +369,7 @@ void BlockLocalPositionEstimator::Run()
_x(i) = 0; _x(i) = 0;
} }
mavlink_and_console_log_info(&mavlink_log_pub, "%sreinit x", msg_label); mavlink_log_info(&mavlink_log_pub, "%sreinit x", msg_label);
} }
// force P symmetry and reinitialize P if necessary // force P symmetry and reinitialize P if necessary
@@ -378,16 +378,16 @@ void BlockLocalPositionEstimator::Run()
for (size_t i = 0; i < n_x; i++) { for (size_t i = 0; i < n_x; i++) {
for (size_t j = 0; j <= i; j++) { for (size_t j = 0; j <= i; j++) {
if (!PX4_ISFINITE(m_P(i, j))) { if (!PX4_ISFINITE(m_P(i, j))) {
mavlink_and_console_log_info(&mavlink_log_pub, mavlink_log_info(&mavlink_log_pub,
"%sreinit P (%zu, %zu) not finite", msg_label, i, j); "%sreinit P (%zu, %zu) not finite", msg_label, i, j);
reinit_P = true; reinit_P = true;
} }
if (i == j) { if (i == j) {
// make sure diagonal elements are positive // make sure diagonal elements are positive
if (m_P(i, i) <= 0) { if (m_P(i, i) <= 0) {
mavlink_and_console_log_info(&mavlink_log_pub, mavlink_log_info(&mavlink_log_pub,
"%sreinit P (%zu, %zu) negative", msg_label, i, j); "%sreinit P (%zu, %zu) negative", msg_label, i, j);
reinit_P = true; reinit_P = true;
} }
@@ -1006,7 +1006,7 @@ int BlockLocalPositionEstimator::getDelayPeriods(float delay, uint8_t *periods)
*periods = i_hist; *periods = i_hist;
if (t_delay > DELAY_MAX) { if (t_delay > DELAY_MAX) {
mavlink_and_console_log_info(&mavlink_log_pub, "%sdelayed data old: %8.4f", msg_label, double(t_delay)); mavlink_log_info(&mavlink_log_pub, "%sdelayed data old: %8.4f", msg_label, double(t_delay));
return -1; return -1;
} }
src/modules/local_position_estimator/sensors/baro.cpp
+6 -6
View File
@@ -22,10 +22,10 @@ void BlockLocalPositionEstimator::baroInit()
// if finished // if finished
if (_baroStats.getCount() > REQ_BARO_INIT_COUNT) { if (_baroStats.getCount() > REQ_BARO_INIT_COUNT) {
_baroAltOrigin = _baroStats.getMean()(0); _baroAltOrigin = _baroStats.getMean()(0);
mavlink_and_console_log_info(&mavlink_log_pub, mavlink_log_info(&mavlink_log_pub,
"[lpe] baro init %d m std %d cm", "[lpe] baro init %d m std %d cm",
(int)_baroStats.getMean()(0), (int)_baroStats.getMean()(0),
(int)(100 * _baroStats.getStdDev()(0))); (int)(100 * _baroStats.getStdDev()(0)));
_sensorTimeout &= ~SENSOR_BARO; _sensorTimeout &= ~SENSOR_BARO;
_sensorFault &= ~SENSOR_BARO; _sensorFault &= ~SENSOR_BARO;
@@ -83,7 +83,7 @@ void BlockLocalPositionEstimator::baroCorrect()
} else if (_sensorFault & SENSOR_BARO) { } else if (_sensorFault & SENSOR_BARO) {
_sensorFault &= ~SENSOR_BARO; _sensorFault &= ~SENSOR_BARO;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro OK"); mavlink_log_info(&mavlink_log_pub, "[lpe] baro OK");
} }
// kalman filter correction always // kalman filter correction always
@@ -99,7 +99,7 @@ void BlockLocalPositionEstimator::baroCheckTimeout()
if (!(_sensorTimeout & SENSOR_BARO)) { if (!(_sensorTimeout & SENSOR_BARO)) {
_sensorTimeout |= SENSOR_BARO; _sensorTimeout |= SENSOR_BARO;
_baroStats.reset(); _baroStats.reset();
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro timeout "); mavlink_log_info(&mavlink_log_pub, "[lpe] baro timeout ");
} }
} }
} }
src/modules/local_position_estimator/sensors/flow.cpp
+6 -6
View File
@@ -22,10 +22,10 @@ void BlockLocalPositionEstimator::flowInit()
// if finished // if finished
if (_flowQStats.getCount() > REQ_FLOW_INIT_COUNT) { if (_flowQStats.getCount() > REQ_FLOW_INIT_COUNT) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow init: " mavlink_log_info(&mavlink_log_pub, "[lpe] flow init: "
"quality %d std %d", "quality %d std %d",
int(_flowQStats.getMean()(0)), int(_flowQStats.getMean()(0)),
int(_flowQStats.getStdDev()(0))); int(_flowQStats.getStdDev()(0)));
_sensorTimeout &= ~SENSOR_FLOW; _sensorTimeout &= ~SENSOR_FLOW;
_sensorFault &= ~SENSOR_FLOW; _sensorFault &= ~SENSOR_FLOW;
} }
@@ -185,13 +185,13 @@ void BlockLocalPositionEstimator::flowCorrect()
if (beta > BETA_TABLE[n_y_flow]) { if (beta > BETA_TABLE[n_y_flow]) {
if (!(_sensorFault & SENSOR_FLOW)) { if (!(_sensorFault & SENSOR_FLOW)) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow fault, beta %5.2f", double(beta)); mavlink_log_info(&mavlink_log_pub, "[lpe] flow fault, beta %5.2f", double(beta));
_sensorFault |= SENSOR_FLOW; _sensorFault |= SENSOR_FLOW;
} }
} else if (_sensorFault & SENSOR_FLOW) { } else if (_sensorFault & SENSOR_FLOW) {
_sensorFault &= ~SENSOR_FLOW; _sensorFault &= ~SENSOR_FLOW;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow OK"); mavlink_log_info(&mavlink_log_pub, "[lpe] flow OK");
} }
if (!(_sensorFault & SENSOR_FLOW)) { if (!(_sensorFault & SENSOR_FLOW)) {
src/modules/local_position_estimator/sensors/gps.cpp
+3 -3
View File
@@ -75,8 +75,8 @@ void BlockLocalPositionEstimator::gpsInit()
_altOriginInitialized = true; _altOriginInitialized = true;
_altOriginGlobal = true; _altOriginGlobal = true;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (gps) : lat %6.2f lon %6.2f alt %5.1f m", mavlink_log_info(&mavlink_log_pub, "[lpe] global origin init (gps) : lat %6.2f lon %6.2f alt %5.1f m",
gpsLatOrigin, gpsLonOrigin, double(_gpsAltOrigin)); gpsLatOrigin, gpsLonOrigin, double(_gpsAltOrigin));
} }
PX4_INFO("[lpe] gps init " PX4_INFO("[lpe] gps init "
@@ -223,7 +223,7 @@ void BlockLocalPositionEstimator::gpsCorrect()
} else if (_sensorFault & SENSOR_GPS) { } else if (_sensorFault & SENSOR_GPS) {
_sensorFault &= ~SENSOR_GPS; _sensorFault &= ~SENSOR_GPS;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] GPS OK"); mavlink_log_info(&mavlink_log_pub, "[lpe] GPS OK");
} }
// kalman filter correction always for GPS // kalman filter correction always for GPS
src/modules/local_position_estimator/sensors/land.cpp
+4 -4
View File
@@ -21,7 +21,7 @@ void BlockLocalPositionEstimator::landInit()
// if finished // if finished
if (_landCount > REQ_LAND_INIT_COUNT) { if (_landCount > REQ_LAND_INIT_COUNT) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land init"); mavlink_log_info(&mavlink_log_pub, "[lpe] land init");
_sensorTimeout &= ~SENSOR_LAND; _sensorTimeout &= ~SENSOR_LAND;
_sensorFault &= ~SENSOR_LAND; _sensorFault &= ~SENSOR_LAND;
} }
@@ -73,7 +73,7 @@ void BlockLocalPositionEstimator::landCorrect()
if (beta / BETA_TABLE[n_y_land] > beta_thresh) { if (beta / BETA_TABLE[n_y_land] > beta_thresh) {
if (!(_sensorFault & SENSOR_LAND)) { if (!(_sensorFault & SENSOR_LAND)) {
_sensorFault |= SENSOR_LAND; _sensorFault |= SENSOR_LAND;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land fault, beta %5.2f", double(beta)); mavlink_log_info(&mavlink_log_pub, "[lpe] land fault, beta %5.2f", double(beta));
} }
// abort correction // abort correction
@@ -81,7 +81,7 @@ void BlockLocalPositionEstimator::landCorrect()
} else if (_sensorFault & SENSOR_LAND) { } else if (_sensorFault & SENSOR_LAND) {
_sensorFault &= ~SENSOR_LAND; _sensorFault &= ~SENSOR_LAND;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land OK"); mavlink_log_info(&mavlink_log_pub, "[lpe] land OK");
} }
// kalman filter correction always for land detector // kalman filter correction always for land detector
@@ -97,7 +97,7 @@ void BlockLocalPositionEstimator::landCheckTimeout()
if (!(_sensorTimeout & SENSOR_LAND)) { if (!(_sensorTimeout & SENSOR_LAND)) {
_sensorTimeout |= SENSOR_LAND; _sensorTimeout |= SENSOR_LAND;
_landCount = 0; _landCount = 0;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land timeout "); mavlink_log_info(&mavlink_log_pub, "[lpe] land timeout ");
} }
} }
} }
src/modules/local_position_estimator/sensors/landing_target.cpp
+4 -4
View File
@@ -16,7 +16,7 @@ void BlockLocalPositionEstimator::landingTargetInit()
Vector<float, n_y_target> y; Vector<float, n_y_target> y;
if (landingTargetMeasure(y) == OK) { if (landingTargetMeasure(y) == OK) {
mavlink_and_console_log_info(&mavlink_log_pub, "Landing target init"); mavlink_log_info(&mavlink_log_pub, "Landing target init");
_sensorTimeout &= ~SENSOR_LAND_TARGET; _sensorTimeout &= ~SENSOR_LAND_TARGET;
_sensorFault &= ~SENSOR_LAND_TARGET; _sensorFault &= ~SENSOR_LAND_TARGET;
} }
@@ -89,7 +89,7 @@ void BlockLocalPositionEstimator::landingTargetCorrect()
if (beta > BETA_TABLE[n_y_target]) { if (beta > BETA_TABLE[n_y_target]) {
if (!(_sensorFault & SENSOR_LAND_TARGET)) { if (!(_sensorFault & SENSOR_LAND_TARGET)) {
mavlink_and_console_log_info(&mavlink_log_pub, "Landing target fault, beta %5.2f", double(beta)); mavlink_log_info(&mavlink_log_pub, "Landing target fault, beta %5.2f", double(beta));
_sensorFault |= SENSOR_LAND_TARGET; _sensorFault |= SENSOR_LAND_TARGET;
} }
@@ -98,7 +98,7 @@ void BlockLocalPositionEstimator::landingTargetCorrect()
} else if (_sensorFault & SENSOR_LAND_TARGET) { } else if (_sensorFault & SENSOR_LAND_TARGET) {
_sensorFault &= ~SENSOR_LAND_TARGET; _sensorFault &= ~SENSOR_LAND_TARGET;
mavlink_and_console_log_info(&mavlink_log_pub, "Landing target OK"); mavlink_log_info(&mavlink_log_pub, "Landing target OK");
} }
// kalman filter correction // kalman filter correction
@@ -115,7 +115,7 @@ void BlockLocalPositionEstimator::landingTargetCheckTimeout()
if (_timeStamp - _time_last_target > TARGET_TIMEOUT) { if (_timeStamp - _time_last_target > TARGET_TIMEOUT) {
if (!(_sensorTimeout & SENSOR_LAND_TARGET)) { if (!(_sensorTimeout & SENSOR_LAND_TARGET)) {
_sensorTimeout |= SENSOR_LAND_TARGET; _sensorTimeout |= SENSOR_LAND_TARGET;
mavlink_and_console_log_info(&mavlink_log_pub, "Landing target timeout"); mavlink_log_info(&mavlink_log_pub, "Landing target timeout");
} }
} }
} }
src/modules/local_position_estimator/sensors/lidar.cpp
+7 -7
View File
@@ -21,10 +21,10 @@ void BlockLocalPositionEstimator::lidarInit()
// if finished // if finished
if (_lidarStats.getCount() > REQ_LIDAR_INIT_COUNT) { if (_lidarStats.getCount() > REQ_LIDAR_INIT_COUNT) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar init: " mavlink_log_info(&mavlink_log_pub, "[lpe] lidar init: "
"mean %d cm stddev %d cm", "mean %d cm stddev %d cm",
int(100 * _lidarStats.getMean()(0)), int(100 * _lidarStats.getMean()(0)),
int(100 * _lidarStats.getStdDev()(0))); int(100 * _lidarStats.getStdDev()(0)));
_sensorTimeout &= ~SENSOR_LIDAR; _sensorTimeout &= ~SENSOR_LIDAR;
_sensorFault &= ~SENSOR_LIDAR; _sensorFault &= ~SENSOR_LIDAR;
} }
@@ -103,7 +103,7 @@ void BlockLocalPositionEstimator::lidarCorrect()
if (beta > BETA_TABLE[n_y_lidar]) { if (beta > BETA_TABLE[n_y_lidar]) {
if (!(_sensorFault & SENSOR_LIDAR)) { if (!(_sensorFault & SENSOR_LIDAR)) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar fault, beta %5.2f", double(beta)); mavlink_log_info(&mavlink_log_pub, "[lpe] lidar fault, beta %5.2f", double(beta));
_sensorFault |= SENSOR_LIDAR; _sensorFault |= SENSOR_LIDAR;
} }
@@ -112,7 +112,7 @@ void BlockLocalPositionEstimator::lidarCorrect()
} else if (_sensorFault & SENSOR_LIDAR) { } else if (_sensorFault & SENSOR_LIDAR) {
_sensorFault &= ~SENSOR_LIDAR; _sensorFault &= ~SENSOR_LIDAR;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar OK"); mavlink_log_info(&mavlink_log_pub, "[lpe] lidar OK");
} }
// kalman filter correction always // kalman filter correction always
@@ -128,7 +128,7 @@ void BlockLocalPositionEstimator::lidarCheckTimeout()
if (!(_sensorTimeout & SENSOR_LIDAR)) { if (!(_sensorTimeout & SENSOR_LIDAR)) {
_sensorTimeout |= SENSOR_LIDAR; _sensorTimeout |= SENSOR_LIDAR;
_lidarStats.reset(); _lidarStats.reset();
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar timeout "); mavlink_log_info(&mavlink_log_pub, "[lpe] lidar timeout ");
} }
} }
} }
src/modules/local_position_estimator/sensors/mocap.cpp
+15 -15
View File
@@ -25,14 +25,14 @@ void BlockLocalPositionEstimator::mocapInit()
// if finished // if finished
if (_mocapStats.getCount() > REQ_MOCAP_INIT_COUNT) { if (_mocapStats.getCount() > REQ_MOCAP_INIT_COUNT) {
mavlink_and_console_log_info(&mavlink_log_pub, "[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", "%5.2f, %5.2f, %5.2f m std %5.2f, %5.2f, %5.2f m",
double(_mocapStats.getMean()(0)), double(_mocapStats.getMean()(0)),
double(_mocapStats.getMean()(1)), double(_mocapStats.getMean()(1)),
double(_mocapStats.getMean()(2)), double(_mocapStats.getMean()(2)),
double(_mocapStats.getStdDev()(0)), double(_mocapStats.getStdDev()(0)),
double(_mocapStats.getStdDev()(1)), double(_mocapStats.getStdDev()(1)),
double(_mocapStats.getStdDev()(2))); double(_mocapStats.getStdDev()(2)));
_sensorTimeout &= ~SENSOR_MOCAP; _sensorTimeout &= ~SENSOR_MOCAP;
_sensorFault &= ~SENSOR_MOCAP; _sensorFault &= ~SENSOR_MOCAP;
@@ -43,8 +43,8 @@ void BlockLocalPositionEstimator::mocapInit()
if (!_map_ref.init_done && _is_global_cov_init && !_visionUpdated) { if (!_map_ref.init_done && _is_global_cov_init && !_visionUpdated) {
// initialize global origin using the mocap estimator reference (only if the vision estimation is not being fused as well) // initialize global origin using the mocap estimator reference (only if the vision estimation is not being fused as well)
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (mocap) : lat %6.2f lon %6.2f alt %5.1f m", mavlink_log_info(&mavlink_log_pub, "[lpe] global origin init (mocap) : lat %6.2f lon %6.2f alt %5.1f m",
double(_ref_lat), double(_ref_lon), double(_ref_alt)); double(_ref_lat), double(_ref_lon), double(_ref_alt));
map_projection_init(&_map_ref, _ref_lat, _ref_lon); map_projection_init(&_map_ref, _ref_lat, _ref_lon);
// set timestamp when origin was set to current time // set timestamp when origin was set to current time
_time_origin = _timeStamp; _time_origin = _timeStamp;
@@ -106,8 +106,8 @@ void BlockLocalPositionEstimator::mocapCorrect()
Vector<float, n_y_mocap> y; Vector<float, n_y_mocap> y;
if (mocapMeasure(y) != OK) { if (mocapMeasure(y) != OK) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap data invalid. eph: %f epv: %f", (double)_mocap_eph, mavlink_log_info(&mavlink_log_pub, "[lpe] mocap data invalid. eph: %f epv: %f", (double)_mocap_eph,
(double)_mocap_epv); (double)_mocap_epv);
return; return;
} }
@@ -168,13 +168,13 @@ void BlockLocalPositionEstimator::mocapCorrect()
if (beta > BETA_TABLE[n_y_mocap]) { if (beta > BETA_TABLE[n_y_mocap]) {
if (!(_sensorFault & SENSOR_MOCAP)) { if (!(_sensorFault & SENSOR_MOCAP)) {
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap fault, beta %5.2f", double(beta)); //mavlink_log_info(&mavlink_log_pub, "[lpe] mocap fault, beta %5.2f", double(beta));
_sensorFault |= SENSOR_MOCAP; _sensorFault |= SENSOR_MOCAP;
} }
} else if (_sensorFault & SENSOR_MOCAP) { } else if (_sensorFault & SENSOR_MOCAP) {
_sensorFault &= ~SENSOR_MOCAP; _sensorFault &= ~SENSOR_MOCAP;
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap OK"); //mavlink_log_info(&mavlink_log_pub, "[lpe] mocap OK");
} }
// kalman filter correction always // kalman filter correction always
@@ -190,7 +190,7 @@ void BlockLocalPositionEstimator::mocapCheckTimeout()
if (!(_sensorTimeout & SENSOR_MOCAP)) { if (!(_sensorTimeout & SENSOR_MOCAP)) {
_sensorTimeout |= SENSOR_MOCAP; _sensorTimeout |= SENSOR_MOCAP;
_mocapStats.reset(); _mocapStats.reset();
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap timeout "); mavlink_log_info(&mavlink_log_pub, "[lpe] mocap timeout ");
} }
} }
} }
src/modules/local_position_estimator/sensors/sonar.cpp
+5 -5
View File
@@ -26,11 +26,11 @@ void BlockLocalPositionEstimator::sonarInit()
// if finished // if finished
if (_sonarStats.getCount() > REQ_SONAR_INIT_COUNT) { if (_sonarStats.getCount() > REQ_SONAR_INIT_COUNT) {
if (_sonarStats.getStdDev()(0) > SONAR_MAX_INIT_STD) { if (_sonarStats.getStdDev()(0) > SONAR_MAX_INIT_STD) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar init std > min"); mavlink_log_info(&mavlink_log_pub, "[lpe] sonar init std > min");
_sonarStats.reset(); _sonarStats.reset();
} else if ((_timeStamp - _time_init_sonar) > SONAR_TIMEOUT) { } else if ((_timeStamp - _time_init_sonar) > SONAR_TIMEOUT) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar init timeout "); mavlink_log_info(&mavlink_log_pub, "[lpe] sonar init timeout ");
_sonarStats.reset(); _sonarStats.reset();
} else { } else {
@@ -124,7 +124,7 @@ void BlockLocalPositionEstimator::sonarCorrect()
if (beta > BETA_TABLE[n_y_sonar]) { if (beta > BETA_TABLE[n_y_sonar]) {
if (!(_sensorFault & SENSOR_SONAR)) { if (!(_sensorFault & SENSOR_SONAR)) {
_sensorFault |= SENSOR_SONAR; _sensorFault |= SENSOR_SONAR;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar fault, beta %5.2f", double(beta)); mavlink_log_info(&mavlink_log_pub, "[lpe] sonar fault, beta %5.2f", double(beta));
} }
// abort correction // abort correction
@@ -132,7 +132,7 @@ void BlockLocalPositionEstimator::sonarCorrect()
} else if (_sensorFault & SENSOR_SONAR) { } else if (_sensorFault & SENSOR_SONAR) {
_sensorFault &= ~SENSOR_SONAR; _sensorFault &= ~SENSOR_SONAR;
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar OK"); //mavlink_log_info(&mavlink_log_pub, "[lpe] sonar OK");
} }
// kalman filter correction if no fault // kalman filter correction if no fault
@@ -151,7 +151,7 @@ void BlockLocalPositionEstimator::sonarCheckTimeout()
if (!(_sensorTimeout & SENSOR_SONAR)) { if (!(_sensorTimeout & SENSOR_SONAR)) {
_sensorTimeout |= SENSOR_SONAR; _sensorTimeout |= SENSOR_SONAR;
_sonarStats.reset(); _sonarStats.reset();
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar timeout "); mavlink_log_info(&mavlink_log_pub, "[lpe] sonar timeout ");
} }
} }
} }
src/modules/local_position_estimator/sensors/vision.cpp
+14 -14
View File
@@ -30,14 +30,14 @@ void BlockLocalPositionEstimator::visionInit()
// increament sums for mean // increament sums for mean
if (_visionStats.getCount() > REQ_VISION_INIT_COUNT) { if (_visionStats.getCount() > REQ_VISION_INIT_COUNT) {
mavlink_and_console_log_info(&mavlink_log_pub, "[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", "%5.2f %5.2f %5.2f m std %5.2f %5.2f %5.2f m",
double(_visionStats.getMean()(0)), double(_visionStats.getMean()(0)),
double(_visionStats.getMean()(1)), double(_visionStats.getMean()(1)),
double(_visionStats.getMean()(2)), double(_visionStats.getMean()(2)),
double(_visionStats.getStdDev()(0)), double(_visionStats.getStdDev()(0)),
double(_visionStats.getStdDev()(1)), double(_visionStats.getStdDev()(1)),
double(_visionStats.getStdDev()(2))); double(_visionStats.getStdDev()(2)));
_sensorTimeout &= ~SENSOR_VISION; _sensorTimeout &= ~SENSOR_VISION;
_sensorFault &= ~SENSOR_VISION; _sensorFault &= ~SENSOR_VISION;
@@ -48,8 +48,8 @@ void BlockLocalPositionEstimator::visionInit()
if (!_map_ref.init_done && _is_global_cov_init) { if (!_map_ref.init_done && _is_global_cov_init) {
// initialize global origin using the visual estimator reference // initialize global origin using the visual estimator reference
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] global origin init (vision) : lat %6.2f lon %6.2f alt %5.1f m", mavlink_log_info(&mavlink_log_pub, "[lpe] global origin init (vision) : lat %6.2f lon %6.2f alt %5.1f m",
double(_ref_lat), double(_ref_lon), double(_ref_alt)); double(_ref_lat), double(_ref_lon), double(_ref_alt));
map_projection_init(&_map_ref, _ref_lat, _ref_lon); map_projection_init(&_map_ref, _ref_lat, _ref_lon);
// set timestamp when origin was set to current time // set timestamp when origin was set to current time
_time_origin = _timeStamp; _time_origin = _timeStamp;
@@ -111,8 +111,8 @@ void BlockLocalPositionEstimator::visionCorrect()
Vector<float, n_y_vision> y; Vector<float, n_y_vision> y;
if (visionMeasure(y) != OK) { if (visionMeasure(y) != OK) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision data invalid. eph: %f epv: %f", (double)_vision_eph, mavlink_log_info(&mavlink_log_pub, "[lpe] vision data invalid. eph: %f epv: %f", (double)_vision_eph,
(double)_vision_epv); (double)_vision_epv);
return; return;
} }
@@ -185,13 +185,13 @@ void BlockLocalPositionEstimator::visionCorrect()
if (beta > BETA_TABLE[n_y_vision]) { if (beta > BETA_TABLE[n_y_vision]) {
if (!(_sensorFault & SENSOR_VISION)) { if (!(_sensorFault & SENSOR_VISION)) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position fault, beta %5.2f", double(beta)); mavlink_log_info(&mavlink_log_pub, "[lpe] vision position fault, beta %5.2f", double(beta));
_sensorFault |= SENSOR_VISION; _sensorFault |= SENSOR_VISION;
} }
} else if (_sensorFault & SENSOR_VISION) { } else if (_sensorFault & SENSOR_VISION) {
_sensorFault &= ~SENSOR_VISION; _sensorFault &= ~SENSOR_VISION;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision position OK"); mavlink_log_info(&mavlink_log_pub, "[lpe] vision position OK");
} }
// kalman filter correction if no fault // kalman filter correction if no fault
src/modules/mavlink/mavlink_main.cpp
+6 -6
View File
@@ -2253,11 +2253,11 @@ Mavlink::task_main(int argc, char *argv[])
(_first_heartbeat_sent)) { (_first_heartbeat_sent)) {
_transmitting_enabled = false; _transmitting_enabled = false;
mavlink_and_console_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s", _device_name); mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s", _device_name);
} else if (!_transmitting_enabled && !status.high_latency_data_link_lost) { } else if (!_transmitting_enabled && !status.high_latency_data_link_lost) {
_transmitting_enabled = true; _transmitting_enabled = true;
mavlink_and_console_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s", _device_name); mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s", _device_name);
} }
} }
} }
@@ -2269,8 +2269,8 @@ Mavlink::task_main(int argc, char *argv[])
(_mode == MAVLINK_MODE_IRIDIUM)) { (_mode == MAVLINK_MODE_IRIDIUM)) {
if (vehicle_cmd.param1 > 0.5f) { if (vehicle_cmd.param1 > 0.5f) {
if (!_transmitting_enabled) { if (!_transmitting_enabled) {
mavlink_and_console_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s by command", mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s by command",
_device_name); _device_name);
} }
_transmitting_enabled = true; _transmitting_enabled = true;
@@ -2278,8 +2278,8 @@ Mavlink::task_main(int argc, char *argv[])
} else { } else {
if (_transmitting_enabled) { if (_transmitting_enabled) {
mavlink_and_console_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s by command", mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s by command",
_device_name); _device_name);
} }
_transmitting_enabled = false; _transmitting_enabled = false;
src/modules/navigator/mission.cpp
+2 -2
View File
@@ -1468,8 +1468,8 @@ Mission::do_abort_landing()
mission_item_to_position_setpoint(_mission_item, &_navigator->get_position_setpoint_triplet()->current); mission_item_to_position_setpoint(_mission_item, &_navigator->get_position_setpoint_triplet()->current);
_navigator->set_position_setpoint_triplet_updated(); _navigator->set_position_setpoint_triplet_updated();
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "Holding at %d m above landing.", mavlink_log_info(_navigator->get_mavlink_log_pub(), "Holding at %d m above landing.",
(int)(alt_sp - alt_landing)); (int)(alt_sp - alt_landing));
// reset mission index to start of landing // reset mission index to start of landing
if (_land_start_available) { if (_land_start_available) {
src/modules/navigator/navigator_main.cpp
+6 -6
View File
@@ -550,10 +550,10 @@ Navigator::run()
case RTL::RTL_CLOSEST: case RTL::RTL_CLOSEST:
if (rtl_activated) { if (rtl_activated) {
if (rtl_type() == RTL::RTL_LAND) { if (rtl_type() == RTL::RTL_LAND) {
mavlink_and_console_log_info(get_mavlink_log_pub(), "RTL LAND activated"); mavlink_log_info(get_mavlink_log_pub(), "RTL LAND activated");
} else { } else {
mavlink_and_console_log_info(get_mavlink_log_pub(), "RTL Closest landing point activated"); mavlink_log_info(get_mavlink_log_pub(), "RTL Closest landing point activated");
} }
} }
@@ -588,7 +588,7 @@ Navigator::run()
} }
if (rtl_activated) { if (rtl_activated) {
mavlink_and_console_log_info(get_mavlink_log_pub(), "RTL Mission activated, continue mission"); mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, continue mission");
} }
navigation_mode_new = &_mission; navigation_mode_new = &_mission;
@@ -610,14 +610,14 @@ Navigator::run()
} }
if (rtl_activated) { if (rtl_activated) {
mavlink_and_console_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly mission in reverse"); mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly mission in reverse");
} }
navigation_mode_new = &_mission; navigation_mode_new = &_mission;
} else { } else {
if (rtl_activated) { if (rtl_activated) {
mavlink_and_console_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly to home"); mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly to home");
} }
navigation_mode_new = &_rtl; navigation_mode_new = &_rtl;
@@ -628,7 +628,7 @@ Navigator::run()
default: default:
if (rtl_activated) { if (rtl_activated) {
mavlink_and_console_log_info(get_mavlink_log_pub(), "RTL HOME activated"); mavlink_log_info(get_mavlink_log_pub(), "RTL HOME activated");
} }
navigation_mode_new = &_rtl; navigation_mode_new = &_rtl;
src/modules/navigator/rtl.cpp
+14 -14
View File
@@ -180,15 +180,15 @@ RTL::on_activation()
// output the correct message, depending on where the RTL destination is // output the correct message, depending on where the RTL destination is
switch (_destination.type) { switch (_destination.type) {
case RTL_DESTINATION_HOME: case RTL_DESTINATION_HOME:
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at home position."); mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at home position.");
break; break;
case RTL_DESTINATION_MISSION_LANDING: case RTL_DESTINATION_MISSION_LANDING:
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at mission landing."); mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at mission landing.");
break; break;
case RTL_DESTINATION_SAFE_POINT: case RTL_DESTINATION_SAFE_POINT:
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at safe landing point."); mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at safe landing point.");
break; break;
} }
@@ -241,7 +241,7 @@ RTL::set_rtl_item()
if (_destination.type == RTL_DESTINATION_MISSION_LANDING) { if (_destination.type == RTL_DESTINATION_MISSION_LANDING) {
if (_rtl_state > RTL_STATE_CLIMB) { if (_rtl_state > RTL_STATE_CLIMB) {
if (_navigator->start_mission_landing()) { if (_navigator->start_mission_landing()) {
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: using mission landing"); mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: using mission landing");
return; return;
} else { } else {
@@ -277,8 +277,8 @@ RTL::set_rtl_item()
_mission_item.autocontinue = true; _mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD; _mission_item.origin = ORIGIN_ONBOARD;
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above destination)", mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above destination)",
(int)ceilf(_rtl_alt), (int)ceilf(_rtl_alt - _destination.alt)); (int)ceilf(_rtl_alt), (int)ceilf(_rtl_alt - _destination.alt));
break; break;
} }
@@ -306,8 +306,8 @@ RTL::set_rtl_item()
_mission_item.autocontinue = true; _mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD; _mission_item.origin = ORIGIN_ONBOARD;
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above destination)", mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above destination)",
(int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt)); (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt));
break; break;
} }
@@ -342,8 +342,8 @@ RTL::set_rtl_item()
// Disable previous setpoint to prevent drift. // Disable previous setpoint to prevent drift.
pos_sp_triplet->previous.valid = false; pos_sp_triplet->previous.valid = false;
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above destination)", mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above destination)",
(int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt)); (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt));
break; break;
} }
@@ -366,12 +366,12 @@ RTL::set_rtl_item()
if (autoland && (get_time_inside(_mission_item) > FLT_EPSILON)) { if (autoland && (get_time_inside(_mission_item) > FLT_EPSILON)) {
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs", mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs",
(double)get_time_inside(_mission_item)); (double)get_time_inside(_mission_item));
} else { } else {
_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering"); mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering");
} }
break; break;
@@ -390,7 +390,7 @@ RTL::set_rtl_item()
_mission_item.autocontinue = true; _mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD; _mission_item.origin = ORIGIN_ONBOARD;
mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at destination"); mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at destination");
break; break;
} }