dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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 <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois 2015-07-01 11:05:45 -07:00 committed by Lorenz Meier
parent af4cc8ec91
commit 28dd9759a6
8 changed files with 94 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/commander/PreflightCheck.cpp
View File

@ -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;
}

2
src/modules/commander/calibration_routines.cpp
View File

@ -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;

41
src/modules/commander/commander.cpp
View File

@ -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;
}

8
src/modules/commander/mag_calibration.cpp
View File

@ -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;

2
src/modules/commander/state_machine_helper_posix.cpp
View File

@ -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;
}

125
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -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;
}

2
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -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;
}

1
src/systemcmds/tests/test_int.c
View File

@ -57,6 +57,7 @@
#include <math.h>
#include <float.h>
#include <inttypes.h>
/****************************************************************************