diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 419b13b63d..47f1667141 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2200,7 +2200,7 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) } while (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 */ return -EINVAL; diff --git a/src/lib/systemlib/mavlink_log.h b/src/lib/systemlib/mavlink_log.h index 0f5b1086bf..bc1ba32965 100644 --- a/src/lib/systemlib/mavlink_log.h +++ b/src/lib/systemlib/mavlink_log.h @@ -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 _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. @@ -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__); \ PX4_WARN(_text, ##__VA_ARGS__); \ } 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; -}; - diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index f229b5abe2..14ce3e057f 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -220,12 +220,12 @@ AirspeedModule::init() _valid_airspeed_index = math::min(_param_airspeed_primary_index.get(), _number_of_airspeed_sensors); if (_number_of_airspeed_sensors == 0) { - mavlink_and_console_log_info(&_mavlink_log_pub, - "No airspeed sensor detected. Switch to non-airspeed mode."); + mavlink_log_info(&_mavlink_log_pub, + "No airspeed sensor detected. Switch to non-airspeed mode."); } else { - mavlink_and_console_log_info(&_mavlink_log_pub, - "Primary airspeed index bigger than number connected sensors. Take last sensor."); + mavlink_log_info(&_mavlink_log_pub, + "Primary airspeed index bigger than number connected sensors. Take last sensor."); } } else { @@ -399,7 +399,7 @@ void AirspeedModule::update_params() -1.0f); // set it to a negative value to start estimation inside wind estimator } 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.commit_no_notification(); } @@ -413,11 +413,11 @@ void AirspeedModule::update_params() _param_west_airspeed_scale.commit_no_notification(); _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", - (double)_airspeed_validator[_valid_airspeed_index - 1].get_EAS_scale()); + mavlink_log_info(&_mavlink_log_pub, "Airspeed: estimated scale (ASPD_ASPD_SCALE): %0.2f", + (double)_airspeed_validator[_valid_airspeed_index - 1].get_EAS_scale()); } 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."); } } diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index f1321cb4c4..408d2dc3a0 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -884,7 +884,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ /* switch to RTL which ends the mission */ if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, &_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; } else { @@ -913,7 +913,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ 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, &_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; } else { @@ -926,7 +926,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_ case vehicle_command_s::VEHICLE_CMD_NAV_PRECLAND: { if (TRANSITION_CHANGED == main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_PRECLAND, 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; } else { @@ -1582,10 +1582,10 @@ Commander::run() if (armed.armed) { if (_was_landed != _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 { - mavlink_and_console_log_info(&mavlink_log_pub, "Takeoff detected"); + mavlink_log_info(&mavlink_log_pub, "Takeoff detected"); _have_taken_off_since_arming = true; // Set all position and velocity test probation durations to takeoff value @@ -1599,7 +1599,7 @@ Commander::run() if (was_falling != _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"); } } } diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h index ff94941eeb..73be0d27e2 100644 --- a/src/modules/commander/calibration_routines.h +++ b/src/modules/commander/calibration_routines.h @@ -139,7 +139,7 @@ bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, ///< uORB handle to w #define calibration_log_info(_pub, _text, ...) \ do { \ - mavlink_and_console_log_info(_pub, _text, ##__VA_ARGS__); \ + mavlink_log_info(_pub, _text, ##__VA_ARGS__); \ px4_usleep(10000); \ } while(0); diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index ea71c41a05..cba8183420 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -207,13 +207,13 @@ void BlockLocalPositionEstimator::Run() s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING && _sub_lidar == nullptr) { _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 && s->get().orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING && _sub_sonar == nullptr) { _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 _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", - double(_param_lpe_lat.get()), double(_param_lpe_lon.get()), double(_altOrigin)); + 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)); } // reinitialize x if necessary @@ -359,7 +359,7 @@ void BlockLocalPositionEstimator::Run() // don't want it to take too long if (!PX4_ISFINITE(_x(i))) { 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; } } @@ -369,7 +369,7 @@ void BlockLocalPositionEstimator::Run() _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 @@ -378,16 +378,16 @@ void BlockLocalPositionEstimator::Run() for (size_t i = 0; i < n_x; i++) { for (size_t j = 0; j <= i; j++) { if (!PX4_ISFINITE(m_P(i, j))) { - mavlink_and_console_log_info(&mavlink_log_pub, - "%sreinit P (%zu, %zu) not finite", msg_label, i, j); + mavlink_log_info(&mavlink_log_pub, + "%sreinit P (%zu, %zu) not finite", msg_label, i, j); reinit_P = true; } if (i == j) { // make sure diagonal elements are positive if (m_P(i, i) <= 0) { - mavlink_and_console_log_info(&mavlink_log_pub, - "%sreinit P (%zu, %zu) negative", msg_label, i, j); + mavlink_log_info(&mavlink_log_pub, + "%sreinit P (%zu, %zu) negative", msg_label, i, j); reinit_P = true; } @@ -1006,7 +1006,7 @@ int BlockLocalPositionEstimator::getDelayPeriods(float delay, uint8_t *periods) *periods = i_hist; 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; } diff --git a/src/modules/local_position_estimator/sensors/baro.cpp b/src/modules/local_position_estimator/sensors/baro.cpp index ecd08be57b..ea61f5cab5 100644 --- a/src/modules/local_position_estimator/sensors/baro.cpp +++ b/src/modules/local_position_estimator/sensors/baro.cpp @@ -22,10 +22,10 @@ void BlockLocalPositionEstimator::baroInit() // if finished if (_baroStats.getCount() > REQ_BARO_INIT_COUNT) { _baroAltOrigin = _baroStats.getMean()(0); - mavlink_and_console_log_info(&mavlink_log_pub, - "[lpe] baro init %d m std %d cm", - (int)_baroStats.getMean()(0), - (int)(100 * _baroStats.getStdDev()(0))); + mavlink_log_info(&mavlink_log_pub, + "[lpe] baro init %d m std %d cm", + (int)_baroStats.getMean()(0), + (int)(100 * _baroStats.getStdDev()(0))); _sensorTimeout &= ~SENSOR_BARO; _sensorFault &= ~SENSOR_BARO; @@ -83,7 +83,7 @@ void BlockLocalPositionEstimator::baroCorrect() } else if (_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 @@ -99,7 +99,7 @@ void BlockLocalPositionEstimator::baroCheckTimeout() if (!(_sensorTimeout & SENSOR_BARO)) { _sensorTimeout |= SENSOR_BARO; _baroStats.reset(); - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] baro timeout "); + mavlink_log_info(&mavlink_log_pub, "[lpe] baro timeout "); } } } diff --git a/src/modules/local_position_estimator/sensors/flow.cpp b/src/modules/local_position_estimator/sensors/flow.cpp index 8cfd66e233..ff32dc5310 100644 --- a/src/modules/local_position_estimator/sensors/flow.cpp +++ b/src/modules/local_position_estimator/sensors/flow.cpp @@ -22,10 +22,10 @@ void BlockLocalPositionEstimator::flowInit() // if finished if (_flowQStats.getCount() > REQ_FLOW_INIT_COUNT) { - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] flow init: " - "quality %d std %d", - int(_flowQStats.getMean()(0)), - int(_flowQStats.getStdDev()(0))); + mavlink_log_info(&mavlink_log_pub, "[lpe] flow init: " + "quality %d std %d", + int(_flowQStats.getMean()(0)), + int(_flowQStats.getStdDev()(0))); _sensorTimeout &= ~SENSOR_FLOW; _sensorFault &= ~SENSOR_FLOW; } @@ -185,13 +185,13 @@ void BlockLocalPositionEstimator::flowCorrect() if (beta > BETA_TABLE[n_y_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; } } else if (_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)) { diff --git a/src/modules/local_position_estimator/sensors/gps.cpp b/src/modules/local_position_estimator/sensors/gps.cpp index 7e49f4c88c..3ef5ffac2e 100644 --- a/src/modules/local_position_estimator/sensors/gps.cpp +++ b/src/modules/local_position_estimator/sensors/gps.cpp @@ -75,8 +75,8 @@ void BlockLocalPositionEstimator::gpsInit() _altOriginInitialized = 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", - gpsLatOrigin, gpsLonOrigin, double(_gpsAltOrigin)); + 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)); } PX4_INFO("[lpe] gps init " @@ -223,7 +223,7 @@ void BlockLocalPositionEstimator::gpsCorrect() } else if (_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 diff --git a/src/modules/local_position_estimator/sensors/land.cpp b/src/modules/local_position_estimator/sensors/land.cpp index 59a6499498..abb2fb3c28 100644 --- a/src/modules/local_position_estimator/sensors/land.cpp +++ b/src/modules/local_position_estimator/sensors/land.cpp @@ -21,7 +21,7 @@ void BlockLocalPositionEstimator::landInit() // if finished 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; _sensorFault &= ~SENSOR_LAND; } @@ -73,7 +73,7 @@ void BlockLocalPositionEstimator::landCorrect() if (beta / BETA_TABLE[n_y_land] > beta_thresh) { if (!(_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 @@ -81,7 +81,7 @@ void BlockLocalPositionEstimator::landCorrect() } else if (_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 @@ -97,7 +97,7 @@ void BlockLocalPositionEstimator::landCheckTimeout() if (!(_sensorTimeout & SENSOR_LAND)) { _sensorTimeout |= SENSOR_LAND; _landCount = 0; - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] land timeout "); + mavlink_log_info(&mavlink_log_pub, "[lpe] land timeout "); } } } diff --git a/src/modules/local_position_estimator/sensors/landing_target.cpp b/src/modules/local_position_estimator/sensors/landing_target.cpp index 37dcb90429..6472fc198f 100644 --- a/src/modules/local_position_estimator/sensors/landing_target.cpp +++ b/src/modules/local_position_estimator/sensors/landing_target.cpp @@ -16,7 +16,7 @@ void BlockLocalPositionEstimator::landingTargetInit() Vector y; 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; _sensorFault &= ~SENSOR_LAND_TARGET; } @@ -89,7 +89,7 @@ void BlockLocalPositionEstimator::landingTargetCorrect() if (beta > BETA_TABLE[n_y_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; } @@ -98,7 +98,7 @@ void BlockLocalPositionEstimator::landingTargetCorrect() } else if (_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 @@ -115,7 +115,7 @@ void BlockLocalPositionEstimator::landingTargetCheckTimeout() if (_timeStamp - _time_last_target > TARGET_TIMEOUT) { if (!(_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"); } } } diff --git a/src/modules/local_position_estimator/sensors/lidar.cpp b/src/modules/local_position_estimator/sensors/lidar.cpp index a621f02361..515b8f5f19 100644 --- a/src/modules/local_position_estimator/sensors/lidar.cpp +++ b/src/modules/local_position_estimator/sensors/lidar.cpp @@ -21,10 +21,10 @@ void BlockLocalPositionEstimator::lidarInit() // if finished if (_lidarStats.getCount() > REQ_LIDAR_INIT_COUNT) { - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar init: " - "mean %d cm stddev %d cm", - int(100 * _lidarStats.getMean()(0)), - int(100 * _lidarStats.getStdDev()(0))); + 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))); _sensorTimeout &= ~SENSOR_LIDAR; _sensorFault &= ~SENSOR_LIDAR; } @@ -103,7 +103,7 @@ void BlockLocalPositionEstimator::lidarCorrect() if (beta > BETA_TABLE[n_y_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; } @@ -112,7 +112,7 @@ void BlockLocalPositionEstimator::lidarCorrect() } else if (_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 @@ -128,7 +128,7 @@ void BlockLocalPositionEstimator::lidarCheckTimeout() if (!(_sensorTimeout & SENSOR_LIDAR)) { _sensorTimeout |= SENSOR_LIDAR; _lidarStats.reset(); - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] lidar timeout "); + mavlink_log_info(&mavlink_log_pub, "[lpe] lidar timeout "); } } } diff --git a/src/modules/local_position_estimator/sensors/mocap.cpp b/src/modules/local_position_estimator/sensors/mocap.cpp index 07e29be854..192350fd45 100644 --- a/src/modules/local_position_estimator/sensors/mocap.cpp +++ b/src/modules/local_position_estimator/sensors/mocap.cpp @@ -25,14 +25,14 @@ void BlockLocalPositionEstimator::mocapInit() // if finished if (_mocapStats.getCount() > REQ_MOCAP_INIT_COUNT) { - mavlink_and_console_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)), - double(_mocapStats.getMean()(2)), - double(_mocapStats.getStdDev()(0)), - double(_mocapStats.getStdDev()(1)), - double(_mocapStats.getStdDev()(2))); + 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)), + double(_mocapStats.getMean()(2)), + double(_mocapStats.getStdDev()(0)), + double(_mocapStats.getStdDev()(1)), + double(_mocapStats.getStdDev()(2))); _sensorTimeout &= ~SENSOR_MOCAP; _sensorFault &= ~SENSOR_MOCAP; @@ -43,8 +43,8 @@ void BlockLocalPositionEstimator::mocapInit() 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) - mavlink_and_console_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)); + 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)); map_projection_init(&_map_ref, _ref_lat, _ref_lon); // set timestamp when origin was set to current time _time_origin = _timeStamp; @@ -106,8 +106,8 @@ void BlockLocalPositionEstimator::mocapCorrect() Vector y; if (mocapMeasure(y) != OK) { - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap data invalid. eph: %f epv: %f", (double)_mocap_eph, - (double)_mocap_epv); + mavlink_log_info(&mavlink_log_pub, "[lpe] mocap data invalid. eph: %f epv: %f", (double)_mocap_eph, + (double)_mocap_epv); return; } @@ -168,13 +168,13 @@ void BlockLocalPositionEstimator::mocapCorrect() if (beta > BETA_TABLE[n_y_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; } } else if (_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 @@ -190,7 +190,7 @@ void BlockLocalPositionEstimator::mocapCheckTimeout() if (!(_sensorTimeout & SENSOR_MOCAP)) { _sensorTimeout |= SENSOR_MOCAP; _mocapStats.reset(); - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] mocap timeout "); + mavlink_log_info(&mavlink_log_pub, "[lpe] mocap timeout "); } } } diff --git a/src/modules/local_position_estimator/sensors/sonar.cpp b/src/modules/local_position_estimator/sensors/sonar.cpp index 3caaf32825..c39e3f1f46 100644 --- a/src/modules/local_position_estimator/sensors/sonar.cpp +++ b/src/modules/local_position_estimator/sensors/sonar.cpp @@ -26,11 +26,11 @@ void BlockLocalPositionEstimator::sonarInit() // if finished if (_sonarStats.getCount() > REQ_SONAR_INIT_COUNT) { 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(); } 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(); } else { @@ -124,7 +124,7 @@ void BlockLocalPositionEstimator::sonarCorrect() if (beta > BETA_TABLE[n_y_sonar]) { if (!(_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 @@ -132,7 +132,7 @@ void BlockLocalPositionEstimator::sonarCorrect() } else if (_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 @@ -151,7 +151,7 @@ void BlockLocalPositionEstimator::sonarCheckTimeout() if (!(_sensorTimeout & SENSOR_SONAR)) { _sensorTimeout |= SENSOR_SONAR; _sonarStats.reset(); - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] sonar timeout "); + mavlink_log_info(&mavlink_log_pub, "[lpe] sonar timeout "); } } } diff --git a/src/modules/local_position_estimator/sensors/vision.cpp b/src/modules/local_position_estimator/sensors/vision.cpp index 4aea91c4f2..35b8a261ad 100644 --- a/src/modules/local_position_estimator/sensors/vision.cpp +++ b/src/modules/local_position_estimator/sensors/vision.cpp @@ -30,14 +30,14 @@ void BlockLocalPositionEstimator::visionInit() // increament sums for mean if (_visionStats.getCount() > REQ_VISION_INIT_COUNT) { - mavlink_and_console_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)), - double(_visionStats.getMean()(2)), - double(_visionStats.getStdDev()(0)), - double(_visionStats.getStdDev()(1)), - double(_visionStats.getStdDev()(2))); + 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)), + double(_visionStats.getMean()(2)), + double(_visionStats.getStdDev()(0)), + double(_visionStats.getStdDev()(1)), + double(_visionStats.getStdDev()(2))); _sensorTimeout &= ~SENSOR_VISION; _sensorFault &= ~SENSOR_VISION; @@ -48,8 +48,8 @@ void BlockLocalPositionEstimator::visionInit() if (!_map_ref.init_done && _is_global_cov_init) { // 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", - double(_ref_lat), double(_ref_lon), double(_ref_alt)); + 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)); map_projection_init(&_map_ref, _ref_lat, _ref_lon); // set timestamp when origin was set to current time _time_origin = _timeStamp; @@ -111,8 +111,8 @@ void BlockLocalPositionEstimator::visionCorrect() Vector y; if (visionMeasure(y) != OK) { - mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision data invalid. eph: %f epv: %f", (double)_vision_eph, - (double)_vision_epv); + mavlink_log_info(&mavlink_log_pub, "[lpe] vision data invalid. eph: %f epv: %f", (double)_vision_eph, + (double)_vision_epv); return; } @@ -185,13 +185,13 @@ void BlockLocalPositionEstimator::visionCorrect() if (beta > BETA_TABLE[n_y_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; } } else if (_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 diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 7d2796fd0c..aad9e1ff64 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2253,11 +2253,11 @@ Mavlink::task_main(int argc, char *argv[]) (_first_heartbeat_sent)) { _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) { _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)) { if (vehicle_cmd.param1 > 0.5f) { if (!_transmitting_enabled) { - mavlink_and_console_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s by command", - _device_name); + mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s by command", + _device_name); } _transmitting_enabled = true; @@ -2278,8 +2278,8 @@ Mavlink::task_main(int argc, char *argv[]) } else { if (_transmitting_enabled) { - mavlink_and_console_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s by command", - _device_name); + mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s by command", + _device_name); } _transmitting_enabled = false; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 47b50ec140..3cf11a6c6f 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -1468,8 +1468,8 @@ Mission::do_abort_landing() mission_item_to_position_setpoint(_mission_item, &_navigator->get_position_setpoint_triplet()->current); _navigator->set_position_setpoint_triplet_updated(); - mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "Holding at %d m above landing.", - (int)(alt_sp - alt_landing)); + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Holding at %d m above landing.", + (int)(alt_sp - alt_landing)); // reset mission index to start of landing if (_land_start_available) { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e580cff508..b73f23a43c 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -550,10 +550,10 @@ Navigator::run() case RTL::RTL_CLOSEST: if (rtl_activated) { 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 { - 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) { - 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; @@ -610,14 +610,14 @@ Navigator::run() } 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; } else { 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; @@ -628,7 +628,7 @@ Navigator::run() default: 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; diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index c58111d08c..1d7e1613a4 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -180,15 +180,15 @@ RTL::on_activation() // output the correct message, depending on where the RTL destination is switch (_destination.type) { 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; 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; 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; } @@ -241,7 +241,7 @@ RTL::set_rtl_item() if (_destination.type == RTL_DESTINATION_MISSION_LANDING) { if (_rtl_state > RTL_STATE_CLIMB) { 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; } else { @@ -277,8 +277,8 @@ RTL::set_rtl_item() _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - mavlink_and_console_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)); + 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)); break; } @@ -306,8 +306,8 @@ RTL::set_rtl_item() _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - mavlink_and_console_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)); + 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)); break; } @@ -342,8 +342,8 @@ RTL::set_rtl_item() // Disable previous setpoint to prevent drift. 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)", - (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt)); + 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)); break; } @@ -366,12 +366,12 @@ RTL::set_rtl_item() if (autoland && (get_time_inside(_mission_item) > FLT_EPSILON)) { _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; - mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs", - (double)get_time_inside(_mission_item)); + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs", + (double)get_time_inside(_mission_item)); } else { _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; @@ -390,7 +390,7 @@ RTL::set_rtl_item() _mission_item.autocontinue = true; _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; }