mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
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
Lorenz Meier
parent
af4cc8ec91
commit
28dd9759a6
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -57,6 +57,7 @@
|
||||
|
||||
#include <math.h>
|
||||
#include <float.h>
|
||||
#include <inttypes.h>
|
||||
|
||||
|
||||
/****************************************************************************
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user