From 28dd9759a629cd7a3979132faa9297dbf57f6ef1 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 1 Jul 2015 11:05:45 -0700 Subject: [PATCH] POSIX: fixes for use of open vs px4_open, etc Fixes for the posix build when virtual devices are used. Signed-off-by: Mark Charlebois --- src/modules/commander/PreflightCheck.cpp | 8 +- .../commander/calibration_routines.cpp | 2 +- src/modules/commander/commander.cpp | 41 +++--- src/modules/commander/mag_calibration.cpp | 8 +- .../commander/state_machine_helper_posix.cpp | 2 +- .../ekf_att_pos_estimator_main.cpp | 125 +++++++++--------- .../position_estimator_inav_main.c | 2 +- src/systemcmds/tests/test_int.c | 1 + 8 files changed, 94 insertions(+), 95 deletions(-) diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 9d38e8179a..1a9d0ad57b 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -268,7 +268,7 @@ static bool airspeedCheck(int mavlink_fd, bool optional) } out: - close(fd); + px4_close(fd); return success; } @@ -279,10 +279,10 @@ static bool gnssCheck(int mavlink_fd) int gpsSub = orb_subscribe(ORB_ID(vehicle_gps_position)); //Wait up to 2000ms to allow the driver to detect a GNSS receiver module - struct pollfd fds[1]; + px4_pollfd_struct_t fds[1]; fds[0].fd = gpsSub; fds[0].events = POLLIN; - if(poll(fds, 1, 2000) <= 0) { + if(px4_poll(fds, 1, 2000) <= 0) { success = false; } else { @@ -298,7 +298,7 @@ static bool gnssCheck(int mavlink_fd) mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING"); } - close(gpsSub); + px4_close(gpsSub); return success; } diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 2afc27c14b..fedafac6cd 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -493,7 +493,7 @@ calibrate_return calibrate_from_orientation(int mavlink_fd, } if (sub_accel >= 0) { - close(sub_accel); + px4_close(sub_accel); } return result; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a794d203be..db6e8aeb1d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -362,7 +362,7 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "check")) { int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); int checkres = prearm_check(&status, mavlink_fd_local); - close(mavlink_fd_local); + px4_close(mavlink_fd_local); warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED"); return 0; } @@ -371,14 +371,14 @@ int commander_main(int argc, char *argv[]) int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); arm_disarm(true, mavlink_fd_local, "command line"); warnx("note: not updating home position on commandline arming!"); - close(mavlink_fd_local); + px4_close(mavlink_fd_local); return 0; } if (!strcmp(argv[1], "disarm")) { int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0); arm_disarm(false, mavlink_fd_local, "command line"); - close(mavlink_fd_local); + px4_close(mavlink_fd_local); return 0; } @@ -389,10 +389,10 @@ int commander_main(int argc, char *argv[]) void usage(const char *reason) { if (reason) { - fprintf(stderr, "%s\n", reason); + PX4_INFO("%s\n", reason); } - fprintf(stderr, "usage: commander {start|stop|status|calibrate|check|arm|disarm}\n\n"); + PX4_INFO("usage: commander {start|stop|status|calibrate|check|arm|disarm}\n\n"); } void print_status() @@ -444,7 +444,7 @@ void print_status() break; } - close(state_sub); + px4_close(state_sub); warnx("arming: %s", armed_str); @@ -931,7 +931,6 @@ int commander_thread_main(int argc, char *argv[]) if (battery_init() != OK) { mavlink_and_console_log_critical(mavlink_fd, "ERROR: BATTERY INIT FAIL"); } - mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0); /* vehicle status topic */ @@ -2166,7 +2165,7 @@ int commander_thread_main(int argc, char *argv[]) arm_tune_played = false; } - fflush(stdout); + //fflush(stdout); counter++; int blink_state = blink_msg_state(); @@ -2200,18 +2199,18 @@ int commander_thread_main(int argc, char *argv[]) /* close fds */ led_deinit(); buzzer_deinit(); - close(sp_man_sub); - close(offboard_control_mode_sub); - close(local_position_sub); - close(global_position_sub); - close(gps_sub); - close(sensor_sub); - close(safety_sub); - close(cmd_sub); - close(subsys_sub); - close(diff_pres_sub); - close(param_changed_sub); - close(battery_sub); + px4_close(sp_man_sub); + px4_close(offboard_control_mode_sub); + px4_close(local_position_sub); + px4_close(global_position_sub); + px4_close(gps_sub); + px4_close(sensor_sub); + px4_close(safety_sub); + px4_close(cmd_sub); + px4_close(subsys_sub); + px4_close(diff_pres_sub); + px4_close(param_changed_sub); + px4_close(battery_sub); thread_running = false; @@ -2977,7 +2976,7 @@ void *commander_low_prio_loop(void *arg) } } - close(cmd_sub); + px4_close(cmd_sub); return NULL; } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 8e11f3e65e..1a6977eb1e 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -243,7 +243,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta /* abort on request */ if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) { result = calibrate_return_cancelled; - close(sub_gyro); + px4_close(sub_gyro); return result; } @@ -256,12 +256,12 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta } /* Wait clocking for new data on all gyro */ - struct pollfd fds[1]; + px4_pollfd_struct_t fds[1]; fds[0].fd = sub_gyro; fds[0].events = POLLIN; size_t fd_count = 1; - int poll_ret = poll(fds, fd_count, 1000); + int poll_ret = px4_poll(fds, fd_count, 1000); if (poll_ret > 0) { struct gyro_report gyro; @@ -281,7 +281,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta } } - close(sub_gyro); + px4_close(sub_gyro); uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds; unsigned poll_errcount = 0; diff --git a/src/modules/commander/state_machine_helper_posix.cpp b/src/modules/commander/state_machine_helper_posix.cpp index 2d3b78ddaf..000cbf366a 100644 --- a/src/modules/commander/state_machine_helper_posix.cpp +++ b/src/modules/commander/state_machine_helper_posix.cpp @@ -371,7 +371,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, break; /* skip mavlink */ - if (!strcmp("/dev/mavlink", devname)) { + if (!strcmp(MAVLINK_LOG_DEVICE, devname)) { continue; } diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 5aba1d014e..6a8ec46053 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -73,8 +73,8 @@ static uint64_t IMUusec = 0; //Constants static constexpr float rc = 10.0f; // RC time constant of 1st order LPF in seconds -static constexpr uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; ///< units: microseconds -static constexpr float POS_RESET_THRESHOLD = 5.0f; ///< Seconds before we signal a total GPS failure +static constexpr uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; ///< units: microseconds +static constexpr float POS_RESET_THRESHOLD = 5.0f; ///< Seconds before we signal a total GPS failure static constexpr unsigned MAG_SWITCH_HYSTERESIS = 10; ///< Ignore the first few mag failures (which amounts to a few milliseconds) static constexpr unsigned GYRO_SWITCH_HYSTERESIS = 5; ///< Ignore the first few gyro failures (which amounts to a few milliseconds) static constexpr unsigned ACCEL_SWITCH_HYSTERESIS = 5; ///< Ignore the first few accel failures (which amounts to a few milliseconds) @@ -246,10 +246,10 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : if (fd >= 0) { res = px4_ioctl(fd, GYROIOCGSCALE, (long unsigned int)&_gyro_offsets[s]); - close(fd); + px4_close(fd); if (res) { - warnx("G%u SCALE FAIL", s); + PX4_WARN("G%u SCALE FAIL", s); } } @@ -261,7 +261,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : px4_close(fd); if (res) { - warnx("A%u SCALE FAIL", s); + PX4_WARN("A%u SCALE FAIL", s); } } @@ -273,7 +273,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : px4_close(fd); if (res) { - warnx("M%u SCALE FAIL", s); + PX4_WARN("M%u SCALE FAIL", s); } } } @@ -404,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)) { - warnx("reset: %s", feedback[warn_index]); + PX4_WARN("reset: %s", feedback[warn_index]); mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]); } } @@ -461,7 +461,7 @@ int AttitudePositionEstimatorEKF::check_filter_state() if (_debug > 10) { if (rep.health_flags < ((1 << 0) | (1 << 1) | (1 << 2) | (1 << 3))) { - warnx("health: VEL:%s POS:%s HGT:%s OFFS:%s", + PX4_INFO("health: VEL:%s POS:%s HGT:%s OFFS:%s", ((rep.health_flags & (1 << 0)) ? "OK" : "ERR"), ((rep.health_flags & (1 << 1)) ? "OK" : "ERR"), ((rep.health_flags & (1 << 2)) ? "OK" : "ERR"), @@ -469,7 +469,7 @@ int AttitudePositionEstimatorEKF::check_filter_state() } if (rep.timeout_flags) { - warnx("timeout: %s%s%s%s", + PX4_INFO("timeout: %s%s%s%s", ((rep.timeout_flags & (1 << 0)) ? "VEL " : ""), ((rep.timeout_flags & (1 << 1)) ? "POS " : ""), ((rep.timeout_flags & (1 << 2)) ? "HGT " : ""), @@ -515,13 +515,13 @@ void AttitudePositionEstimatorEKF::task_main() _ekf = new AttPosEKF(); - _filter_start_time = hrt_absolute_time(); - if (!_ekf) { - warnx("OUT OF MEM!"); + PX4_ERR("OUT OF MEM!"); return; } + _filter_start_time = hrt_absolute_time(); + /* * do subscriptions */ @@ -659,7 +659,7 @@ void AttitudePositionEstimatorEKF::task_main() // _last_debug_print = hrt_absolute_time(); // perf_print_counter(_perf_baro); // perf_reset(_perf_baro); -// warnx("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f", +// PX4_INFO("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f", // (double)_baro_gps_offset, // (double)_baro_alt_filt, // (double)_gps_alt_filt, @@ -683,7 +683,7 @@ void AttitudePositionEstimatorEKF::task_main() _filter_ref_offset = -_baro.altitude; - warnx("filter ref off: baro_alt: %8.4f", (double)_filter_ref_offset); + PX4_INFO("filter ref off: baro_alt: %8.4f", (double)_filter_ref_offset); } else { @@ -794,11 +794,11 @@ void AttitudePositionEstimatorEKF::initializeGPS() initReferencePosition(_gps.timestamp_position, lat, lon, gps_alt, _baro.altitude); #if 0 - warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, + PX4_INFO("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); - warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, + PX4_INFO("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_filter_ref_offset); - warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, + PX4_INFO("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination)); #endif @@ -1132,7 +1132,7 @@ void AttitudePositionEstimatorEKF::print_status() math::Matrix<3, 3> R = q.to_dcm(); math::Vector<3> euler = R.to_euler(); - printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n", + PX4_INFO("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n", (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2))); // State vector: @@ -1145,43 +1145,43 @@ void AttitudePositionEstimatorEKF::print_status() // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) - printf("dtIMU: %8.6f filt: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)IMUmsec); - printf("alt RAW: baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)_ekf->gpsHgt); - printf("alt EST: local alt: %8.4f (NED), AMSL alt: %8.4f (ENU)\n", (double)(_local_pos.z), (double)_global_pos.alt); - printf("filter ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_filter_ref_offset, + PX4_INFO("dtIMU: %8.6f filt: %8.6f IMUmsec: %d", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)IMUmsec); + PX4_INFO("alt RAW: baro alt: %8.4f GPS alt: %8.4f", (double)_baro.altitude, (double)_ekf->gpsHgt); + PX4_INFO("alt EST: local alt: %8.4f (NED), AMSL alt: %8.4f (ENU)", (double)(_local_pos.z), (double)_global_pos.alt); + PX4_INFO("filter ref offset: %8.4f baro GPS offset: %8.4f", (double)_filter_ref_offset, (double)_baro_gps_offset); - printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, + PX4_INFO("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); - printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, + PX4_INFO("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); - printf("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], + PX4_INFO("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); - printf("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], + PX4_INFO("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]); - printf("states (pos m) [7-9]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], + PX4_INFO("states (pos m) [7-9]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]); - printf("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], + PX4_INFO("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]); if (EKF_STATE_ESTIMATES == 23) { - printf("states (accel offs) [13]: %8.4f\n", (double)_ekf->states[13]); - printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]); - printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17], + PX4_INFO("states (accel offs) [13]: %8.4f", (double)_ekf->states[13]); + PX4_INFO("states (wind) [14-15]: %8.4f, %8.4f", (double)_ekf->states[14], (double)_ekf->states[15]); + PX4_INFO("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[16], (double)_ekf->states[17], (double)_ekf->states[18]); - printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20], + PX4_INFO("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[19], (double)_ekf->states[20], (double)_ekf->states[21]); - printf("states (terrain) [22]: %8.4f\n", (double)_ekf->states[22]); + PX4_INFO("states (terrain) [22]: %8.4f", (double)_ekf->states[22]); } else { - printf("states (wind) [13-14]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]); - printf("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], + PX4_INFO("states (wind) [13-14]: %8.4f, %8.4f", (double)_ekf->states[13], (double)_ekf->states[14]); + PX4_INFO("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]); - printf("states (body mag) [18-20]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], + PX4_INFO("states (body mag) [18-20]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]); } - printf("states: %s %s %s %s %s %s %s %s %s %s\n", + PX4_INFO("states: %s %s %s %s %s %s %s %s %s %s", (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", (_landDetector.landed) ? "ON_GROUND" : "AIRBORNE", (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", @@ -1324,7 +1324,7 @@ void AttitudePositionEstimatorEKF::pollData() last_mag = _sensor_combined.magnetometer_timestamp; - //warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z); + //PX4_INFO("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z); //Update Land Detector bool newLandData; @@ -1413,21 +1413,21 @@ void AttitudePositionEstimatorEKF::pollData() } } - //warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)dtGoodGPS); + //PX4_INFO("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)dtGoodGPS); // if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) { - // _ekf->vneSigma = sqrtf(_gps.s_variance_m_s); + // _ekf->vneSigma = sqrtf(_gps.s_variance_m_s); // } else { - // _ekf->vneSigma = _parameters.velne_noise; + // _ekf->vneSigma = _parameters.velne_noise; // } // if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) { - // _ekf->posNeSigma = sqrtf(_gps.p_variance_m); + // _ekf->posNeSigma = sqrtf(_gps.p_variance_m); // } else { - // _ekf->posNeSigma = _parameters.posne_noise; + // _ekf->posNeSigma = _parameters.posne_noise; // } - // warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); + // PX4_INFO("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); _previousGPSTimestamp = _gps.timestamp_position; @@ -1572,42 +1572,42 @@ int AttitudePositionEstimatorEKF::trip_nan() // If system is not armed, inject a NaN value into the filter if (_armed.armed) { - warnx("ACTUATORS ARMED! NOT TRIPPING SYSTEM"); + PX4_INFO("ACTUATORS ARMED! NOT TRIPPING SYSTEM"); ret = 1; } else { float nan_val = 0.0f / 0.0f; - warnx("system not armed, tripping state vector with NaN"); + PX4_INFO("system not armed, tripping state vector with NaN"); _ekf->states[5] = nan_val; usleep(100000); - warnx("tripping covariance #1 with NaN"); + PX4_INFO("tripping covariance #1 with NaN"); _ekf->KH[2][2] = nan_val; // intermediate result used for covariance updates usleep(100000); - warnx("tripping covariance #2 with NaN"); + PX4_INFO("tripping covariance #2 with NaN"); _ekf->KHP[5][5] = nan_val; // intermediate result used for covariance updates usleep(100000); - warnx("tripping covariance #3 with NaN"); + PX4_INFO("tripping covariance #3 with NaN"); _ekf->P[3][3] = nan_val; // covariance matrix usleep(100000); - warnx("tripping Kalman gains with NaN"); + PX4_INFO("tripping Kalman gains with NaN"); _ekf->Kfusion[0] = nan_val; // Kalman gains usleep(100000); - warnx("tripping stored states[0] with NaN"); + PX4_INFO("tripping stored states[0] with NaN"); _ekf->storedStates[0][0] = nan_val; usleep(100000); - warnx("tripping states[9] with NaN"); + PX4_INFO("tripping states[9] with NaN"); _ekf->states[9] = nan_val; usleep(100000); - warnx("\nDONE - FILTER STATE:"); + PX4_INFO("DONE - FILTER STATE:"); print_status(); } @@ -1617,45 +1617,44 @@ int AttitudePositionEstimatorEKF::trip_nan() int ekf_att_pos_estimator_main(int argc, char *argv[]) { if (argc < 2) { - warnx("usage: ekf_att_pos_estimator {start|stop|status|logging}"); + PX4_ERR("usage: ekf_att_pos_estimator {start|stop|status|logging}"); return 1; } if (!strcmp(argv[1], "start")) { if (estimator::g_estimator != nullptr) { - warnx("already running"); + PX4_ERR("already running"); return 1; } estimator::g_estimator = new AttitudePositionEstimatorEKF(); if (estimator::g_estimator == nullptr) { - warnx("alloc failed"); + PX4_ERR("alloc failed"); return 1; } if (OK != estimator::g_estimator->start()) { delete estimator::g_estimator; estimator::g_estimator = nullptr; - warnx("start failed"); + PX4_ERR("start failed"); return 1; } /* avoid memory fragmentation by not exiting start handler until the task has fully started */ while (estimator::g_estimator == nullptr || !estimator::g_estimator->task_running()) { usleep(50000); - printf("."); - fflush(stdout); + PX4_INFO("."); } - printf("\n"); + PX4_INFO(" "); return 0; } if (estimator::g_estimator == nullptr) { - warnx("not running"); + PX4_ERR("not running"); return 1; } @@ -1667,7 +1666,7 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) } if (!strcmp(argv[1], "status")) { - warnx("running"); + PX4_INFO("running"); estimator::g_estimator->print_status(); @@ -1693,6 +1692,6 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) return ret; } - warnx("unrecognized command"); + PX4_ERR("unrecognized command"); return 1; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 7683daed2c..158d2ef27d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -148,7 +148,7 @@ int position_estimator_inav_main(int argc, char *argv[]) inav_verbose_mode = false; - if (argc > 2 && !strcmp(argv[2], "-v")) { + if ((argc > 2) && (!strcmp(argv[2], "-v"))) { inav_verbose_mode = true; } diff --git a/src/systemcmds/tests/test_int.c b/src/systemcmds/tests/test_int.c index 01092aa2d4..051a48e6c2 100644 --- a/src/systemcmds/tests/test_int.c +++ b/src/systemcmds/tests/test_int.c @@ -57,6 +57,7 @@ #include #include +#include /****************************************************************************