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

code style

This commit is contained in:
Roman 2015-12-11 08:51:55 +01:00 committed by Lorenz Meier
parent 27e1aaeea5
commit 4df4d2ff93
3 changed files with 486 additions and 349 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -74,9 +74,12 @@ static uint64_t IMUusec = 0;
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 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)
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)
static constexpr float EPH_LARGE_VALUE = 1000.0f;
static constexpr float EPV_LARGE_VALUE = 1000.0f;
@ -118,7 +121,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_task_running(false),
_estimator_task(-1),
/* subscriptions */
/* subscriptions */
_sensor_combined_sub(-1),
_distance_sub(-1),
_airspeed_sub(-1),
@ -163,7 +166,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_sensor_combined{},
_pos_ref{},
_pos_ref{},
_filter_ref_offset(0.0f),
_baro_gps_offset(0.0f),
@ -177,7 +180,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")),
_perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")),
/* states */
/* states */
_gps_alt_filt(0.0f),
_baro_alt_filt(0.0f),
_covariancePredictionDt(0.0f),
@ -270,6 +273,7 @@ AttitudePositionEstimatorEKF::~AttitudePositionEstimatorEKF()
}
} while (_estimator_task != -1);
}
delete _terrain_estimator;
delete _ekf;
@ -324,13 +328,13 @@ int AttitudePositionEstimatorEKF::parameters_update()
_ekf->accelProcessNoise = _parameters.acc_pnoise;
_ekf->airspeedMeasurementSigma = _parameters.eas_noise;
_ekf->rngFinderPitch = 0.0f; // XXX base on SENS_BOARD_Y_OFF
#if 0
#if 0
// Initially disable loading until
// convergence is flight-test proven
_ekf->magBias.x = _mag_offset_x.get();
_ekf->magBias.y = _mag_offset_y.get();
_ekf->magBias.z = _mag_offset_z.get();
#endif
#endif
}
return OK;
@ -453,18 +457,18 @@ int AttitudePositionEstimatorEKF::check_filter_state()
if (rep.health_flags < ((1 << 0) | (1 << 1) | (1 << 2) | (1 << 3))) {
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"),
((rep.health_flags & (1 << 3)) ? "OK" : "ERR"));
((rep.health_flags & (1 << 0)) ? "OK" : "ERR"),
((rep.health_flags & (1 << 1)) ? "OK" : "ERR"),
((rep.health_flags & (1 << 2)) ? "OK" : "ERR"),
((rep.health_flags & (1 << 3)) ? "OK" : "ERR"));
}
if (rep.timeout_flags) {
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 " : ""),
((rep.timeout_flags & (1 << 3)) ? "IMU " : ""));
((rep.timeout_flags & (1 << 0)) ? "VEL " : ""),
((rep.timeout_flags & (1 << 1)) ? "POS " : ""),
((rep.timeout_flags & (1 << 2)) ? "HGT " : ""),
((rep.timeout_flags & (1 << 3)) ? "IMU " : ""));
}
}
@ -476,7 +480,7 @@ int AttitudePositionEstimatorEKF::check_filter_state()
// Copy diagonal elemnts of covariance matrix
float covariances[28];
_ekf->get_covariance(covariances);
for (size_t i = 0; i < rep.n_states; i++) {
rep.states[i] = ekf_report.states[i];
rep.covariances[i] = covariances[i];
@ -645,6 +649,7 @@ void AttitudePositionEstimatorEKF::task_main()
if (_gpsIsGood) {
_baro_gps_offset = _baro_alt_filt - _gps_alt_filt;
}
// if (hrt_elapsed_time(&_last_debug_print) >= 5e6) {
// _last_debug_print = hrt_absolute_time();
// perf_print_counter(_perf_baro);
@ -733,13 +738,15 @@ void AttitudePositionEstimatorEKF::task_main()
}
void AttitudePositionEstimatorEKF::initReferencePosition(hrt_abstime timestamp,
bool gps_valid, double lat, double lon, float gps_alt, float baro_alt)
bool gps_valid, double lat, double lon, float gps_alt, float baro_alt)
{
// Reference altitude
if (PX4_ISFINITE(_ekf->states[9])) {
_filter_ref_offset = _ekf->states[9];
} else if (PX4_ISFINITE(-_ekf->hgtMea)) {
_filter_ref_offset = -_ekf->hgtMea;
} else {
_filter_ref_offset = -_baro.altitude;
}
@ -795,11 +802,11 @@ void AttitudePositionEstimatorEKF::initializeGPS()
#if 0
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]);
(double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]);
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);
(double)_filter_ref_offset);
PX4_INFO("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv,
(double)math::degrees(declination));
(double)math::degrees(declination));
#endif
_gps_initialized = true;
@ -904,6 +911,7 @@ void AttitudePositionEstimatorEKF::publishControlState()
} else {
_ctrl_state.airspeed_valid = false;
}
/* Attitude Rates */
_ctrl_state.roll_rate = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt;
_ctrl_state.pitch_rate = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt;
@ -911,12 +919,11 @@ void AttitudePositionEstimatorEKF::publishControlState()
/* Guard from bad data */
if (!PX4_ISFINITE(_ctrl_state.x_vel) ||
!PX4_ISFINITE(_ctrl_state.y_vel) ||
!PX4_ISFINITE(_ctrl_state.z_vel) ||
!PX4_ISFINITE(_ctrl_state.x_pos) ||
!PX4_ISFINITE(_ctrl_state.y_pos) ||
!PX4_ISFINITE(_ctrl_state.z_pos))
{
!PX4_ISFINITE(_ctrl_state.y_vel) ||
!PX4_ISFINITE(_ctrl_state.z_vel) ||
!PX4_ISFINITE(_ctrl_state.x_pos) ||
!PX4_ISFINITE(_ctrl_state.y_pos) ||
!PX4_ISFINITE(_ctrl_state.z_pos)) {
// bad data, abort publication
return;
}
@ -956,12 +963,11 @@ void AttitudePositionEstimatorEKF::publishLocalPosition()
_local_pos.yaw = _att.yaw;
if (!PX4_ISFINITE(_local_pos.x) ||
!PX4_ISFINITE(_local_pos.y) ||
!PX4_ISFINITE(_local_pos.z) ||
!PX4_ISFINITE(_local_pos.vx) ||
!PX4_ISFINITE(_local_pos.vy) ||
!PX4_ISFINITE(_local_pos.vz))
{
!PX4_ISFINITE(_local_pos.y) ||
!PX4_ISFINITE(_local_pos.z) ||
!PX4_ISFINITE(_local_pos.vx) ||
!PX4_ISFINITE(_local_pos.vy) ||
!PX4_ISFINITE(_local_pos.vz)) {
// bad data, abort publication
return;
}
@ -987,6 +993,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
_global_pos.lat = est_lat;
_global_pos.lon = est_lon;
_global_pos.time_utc_usec = _gps.time_utc_usec;
} else {
_global_pos.lat = 0.0;
_global_pos.lon = 0.0;
@ -1007,6 +1014,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
if (_local_pos.v_z_valid) {
_global_pos.vel_d = _local_pos.vz;
} else {
_global_pos.vel_d = 0.0f;
}
@ -1015,6 +1023,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
if (_terrain_estimator->is_valid()) {
_global_pos.terrain_alt = _global_pos.alt - _terrain_estimator->get_distance_to_ground();
_global_pos.terrain_alt_valid = true;
} else {
_global_pos.terrain_alt_valid = false;
}
@ -1028,8 +1037,8 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
if (!_local_pos.xy_global ||
!_local_pos.v_xy_valid ||
_gps.timestamp_position == 0 ||
(dtLastGoodGPS >= POS_RESET_THRESHOLD)) {
_gps.timestamp_position == 0 ||
(dtLastGoodGPS >= POS_RESET_THRESHOLD)) {
_global_pos.eph = EPH_LARGE_VALUE;
_global_pos.epv = EPV_LARGE_VALUE;
@ -1040,12 +1049,11 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
}
if (!PX4_ISFINITE(_global_pos.lat) ||
!PX4_ISFINITE(_global_pos.lon) ||
!PX4_ISFINITE(_global_pos.alt) ||
!PX4_ISFINITE(_global_pos.vel_n) ||
!PX4_ISFINITE(_global_pos.vel_e) ||
!PX4_ISFINITE(_global_pos.vel_d))
{
!PX4_ISFINITE(_global_pos.lon) ||
!PX4_ISFINITE(_global_pos.alt) ||
!PX4_ISFINITE(_global_pos.vel_n) ||
!PX4_ISFINITE(_global_pos.vel_e) ||
!PX4_ISFINITE(_global_pos.vel_d)) {
// bad data, abort publication
return;
}
@ -1102,6 +1110,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
if (_prediction_steps < MAX_PREDICITION_STEPS && ((hrt_absolute_time() - _prediction_last) < 20 * 1000)) {
_prediction_steps++;
return;
} else {
_prediction_steps = 0;
_prediction_last = hrt_absolute_time();
@ -1221,11 +1230,11 @@ int AttitudePositionEstimatorEKF::start()
/* start the task */
_estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
4600,
(px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
nullptr);
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 20,
4600,
(px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
nullptr);
if (_estimator_task < 0) {
warn("task start failed");
@ -1242,7 +1251,7 @@ void AttitudePositionEstimatorEKF::print_status()
math::Vector<3> euler = R.to_euler();
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)));
(double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2)));
// State vector:
// 0-3: quaternions (q0, q1, q2, q3)
@ -1258,51 +1267,51 @@ void AttitudePositionEstimatorEKF::print_status()
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);
(double)_baro_gps_offset);
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);
(double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z);
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);
(double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y,
(double)_ekf->correctedDelAng.z);
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]);
(double)_ekf->states[2], (double)_ekf->states[3]);
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]);
(double)_ekf->states[6]);
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]);
(double)_ekf->states[9]);
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]);
(double)_ekf->states[12]);
if (EKF_STATE_ESTIMATES == 23) {
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]);
(double)_ekf->states[18]);
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]);
(double)_ekf->states[21]);
PX4_INFO("states (terrain) [22]: %8.4f", (double)_ekf->states[22]);
} else {
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]);
(double)_ekf->states[18]);
PX4_INFO("states (mag bias) [19-21]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[19], (double)_ekf->states[20],
(double)_ekf->states[21]);
(double)_ekf->states[21]);
}
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",
(_ekf->fusePosData) ? "FUSE_POS" : "INH_POS",
(_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT",
(_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG",
(_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS",
(_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
(_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
(_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
(_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT",
(_landDetector.landed) ? "ON_GROUND" : "AIRBORNE",
(_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL",
(_ekf->fusePosData) ? "FUSE_POS" : "INH_POS",
(_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT",
(_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG",
(_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS",
(_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
(_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
(_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
PX4_INFO("gyro status:");
_voter_gyro.print();
@ -1333,6 +1342,7 @@ void AttitudePositionEstimatorEKF::pollData()
if (PX4_ISFINITE(_ekf->dtIMUfilt) && _ekf->dtIMUfilt < 0.5f && _ekf->dtIMUfilt > 0.0001f) {
deltaT = _ekf->dtIMUfilt;
} else {
deltaT = 0.01f;
}
@ -1345,16 +1355,17 @@ void AttitudePositionEstimatorEKF::pollData()
for (unsigned i = 0; i < (sizeof(_sensor_combined.gyro_timestamp) / sizeof(_sensor_combined.gyro_timestamp[0])); i++) {
_voter_gyro.put(i, _sensor_combined.gyro_timestamp[i], &_sensor_combined.gyro_rad_s[i * 3],
_sensor_combined.gyro_errcount[i], _sensor_combined.gyro_priority[i]);
_sensor_combined.gyro_errcount[i], _sensor_combined.gyro_priority[i]);
_voter_accel.put(i, _sensor_combined.accelerometer_timestamp[i], &_sensor_combined.accelerometer_m_s2[i * 3],
_sensor_combined.accelerometer_errcount[i], _sensor_combined.accelerometer_priority[i]);
_sensor_combined.accelerometer_errcount[i], _sensor_combined.accelerometer_priority[i]);
_voter_mag.put(i, _sensor_combined.magnetometer_timestamp[i], &_sensor_combined.magnetometer_ga[i * 3],
_sensor_combined.magnetometer_errcount[i], _sensor_combined.magnetometer_priority[i]);
_sensor_combined.magnetometer_errcount[i], _sensor_combined.magnetometer_priority[i]);
}
// Get best measurement values
hrt_abstime curr_time = hrt_absolute_time();
(void)_voter_gyro.get_best(curr_time, &_gyro_main);
if (_gyro_main >= 0) {
// Use pre-integrated values if possible
@ -1362,12 +1373,15 @@ void AttitudePositionEstimatorEKF::pollData()
_ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 0];
_ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1];
_ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2];
} else {
float dt_gyro = _sensor_combined.gyro_integral_dt[_gyro_main] / 1e6f;
if (PX4_ISFINITE(dt_gyro) && (dt_gyro < 0.5f) && (dt_gyro > 0.00001f)) {
deltaT = dt_gyro;
_ekf->dtIMU = deltaT;
}
_ekf->dAngIMU.x = 0.5f * (_ekf->angRate.x + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]) * _ekf->dtIMU;
_ekf->dAngIMU.y = 0.5f * (_ekf->angRate.y + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]) * _ekf->dtIMU;
_ekf->dAngIMU.z = 0.5f * (_ekf->angRate.z + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]) * _ekf->dtIMU;
@ -1380,6 +1394,7 @@ void AttitudePositionEstimatorEKF::pollData()
}
(void)_voter_accel.get_best(curr_time, &_accel_main);
if (_accel_main >= 0 && (_last_accel != _sensor_combined.accelerometer_timestamp[_accel_main])) {
// Use pre-integrated values if possible
@ -1387,6 +1402,7 @@ void AttitudePositionEstimatorEKF::pollData()
_ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 0];
_ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 1];
_ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 2];
} else {
_ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]) * _ekf->dtIMU;
_ekf->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]) * _ekf->dtIMU;
@ -1400,9 +1416,10 @@ void AttitudePositionEstimatorEKF::pollData()
}
(void)_voter_mag.get_best(curr_time, &_mag_main);
if (_mag_main >= 0) {
Vector3f mag(_sensor_combined.magnetometer_ga[_mag_main * 3 + 0], _sensor_combined.magnetometer_ga[_mag_main * 3 + 1],
_sensor_combined.magnetometer_ga[_mag_main * 3 + 2]);
_sensor_combined.magnetometer_ga[_mag_main * 3 + 2]);
/* fail over to the 2nd mag if we know the first is down */
if (mag.length() > 0.1f && (_last_mag != _sensor_combined.magnetometer_timestamp[_mag_main])) {
@ -1416,26 +1433,28 @@ void AttitudePositionEstimatorEKF::pollData()
}
if (!_failsafe && (_voter_gyro.failover_count() > 0 ||
_voter_accel.failover_count() > 0 ||
_voter_mag.failover_count() > 0)) {
_voter_accel.failover_count() > 0 ||
_voter_mag.failover_count() > 0)) {
_failsafe = true;
mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
}
if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
_voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
_voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) {
_voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold ||
_voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) {
if (_vibration_warning_timestamp == 0) {
_vibration_warning_timestamp = curr_time;
} else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) {
_vibration_warning = true;
mavlink_and_console_log_critical(_mavlink_fd, "HIGH VIBRATION! g: %d a: %d m: %d",
(int)(100 * _voter_gyro.get_vibration_factor(curr_time)),
(int)(100 * _voter_accel.get_vibration_factor(curr_time)),
(int)(100 * _voter_mag.get_vibration_factor(curr_time)));
(int)(100 * _voter_gyro.get_vibration_factor(curr_time)),
(int)(100 * _voter_accel.get_vibration_factor(curr_time)),
(int)(100 * _voter_mag.get_vibration_factor(curr_time)));
}
} else {
_vibration_warning_timestamp = 0;
}
@ -1444,7 +1463,7 @@ void AttitudePositionEstimatorEKF::pollData()
// XXX this is for assessing the filter performance
// leave this in as long as larger improvements are still being made.
#if 0
#if 0
float deltaTIntegral = (_sensor_combined.gyro_integral_dt[0]) / 1e6f;
float deltaTIntAcc = (_sensor_combined.accelerometer_integral_dt[0]) / 1e6f;
@ -1455,32 +1474,32 @@ void AttitudePositionEstimatorEKF::pollData()
if (hrt_elapsed_time(&lastprint) > 1000000 || _sensor_combined.gyro_rad_s[0] > 4.0f) {
PX4_WARN("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u",
(double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc,
dtoverflow5, dtoverflow10);
(double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc,
dtoverflow5, dtoverflow10);
PX4_WARN("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
(double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.z,
(double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z);
(double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.z,
(double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z);
PX4_WARN("INT: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
(double)(_sensor_combined.gyro_integral_rad[0]), (double)(_sensor_combined.gyro_integral_rad[2]),
(double)(_sensor_combined.accelerometer_integral_m_s[0]),
(double)(_sensor_combined.accelerometer_integral_m_s[1]),
(double)(_sensor_combined.accelerometer_integral_m_s[2]));
(double)(_sensor_combined.gyro_integral_rad[0]), (double)(_sensor_combined.gyro_integral_rad[2]),
(double)(_sensor_combined.accelerometer_integral_m_s[0]),
(double)(_sensor_combined.accelerometer_integral_m_s[1]),
(double)(_sensor_combined.accelerometer_integral_m_s[2]));
PX4_WARN("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
(double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT),
(double)(_sensor_combined.accelerometer_m_s2[0] * deltaT),
(double)(_sensor_combined.accelerometer_m_s2[1] * deltaT),
(double)(_sensor_combined.accelerometer_m_s2[2] * deltaT));
(double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT),
(double)(_sensor_combined.accelerometer_m_s2[0] * deltaT),
(double)(_sensor_combined.accelerometer_m_s2[1] * deltaT),
(double)(_sensor_combined.accelerometer_m_s2[2] * deltaT));
PX4_WARN("EKF rate: %8.4f, %8.4f, %8.4f",
(double)_att.rollspeed, (double)_att.pitchspeed, (double)_att.yawspeed);
(double)_att.rollspeed, (double)_att.pitchspeed, (double)_att.yawspeed);
PX4_WARN("DRV rate: %8.4f, %8.4f, %8.4f",
(double)_sensor_combined.gyro_rad_s[0],
(double)_sensor_combined.gyro_rad_s[1],
(double)_sensor_combined.gyro_rad_s[2]);
(double)_sensor_combined.gyro_rad_s[0],
(double)_sensor_combined.gyro_rad_s[1],
(double)_sensor_combined.gyro_rad_s[2]);
lastprint = hrt_absolute_time();
}
@ -1492,7 +1511,8 @@ void AttitudePositionEstimatorEKF::pollData()
if (deltaT > 0.01f) {
dtoverflow10++;
}
#endif
#endif
_data_good = true;
@ -1652,6 +1672,7 @@ void AttitudePositionEstimatorEKF::pollData()
}
baro_last = _baro.timestamp;
if (!_baro_init) {
_baro_init = true;
_baro_alt_filt = _baro.altitude;
@ -1674,8 +1695,9 @@ void AttitudePositionEstimatorEKF::pollData()
if (_newRangeData) {
orb_copy(ORB_ID(distance_sensor), _distance_sub, &_distance);
if ((_distance.current_distance > _distance.min_distance)
&& (_distance.current_distance < _distance.max_distance)) {
&& (_distance.current_distance < _distance.max_distance)) {
_ekf->rngMea = _distance.current_distance;
_distance_last_valid = _distance.timestamp;

109
src/modules/fw_att_control/fw_att_control_main.cpp
View File

@ -44,7 +44,7 @@
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <px4_posix.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
@ -347,7 +347,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_task_running(false),
_control_task(-1),
/* subscriptions */
/* subscriptions */
_ctrl_state_sub(-1),
_accel_sub(-1),
_vcontrol_mode_sub(-1),
@ -356,7 +356,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_global_pos_sub(-1),
_vehicle_status_sub(-1),
/* publications */
/* publications */
_rate_sp_pub(nullptr),
_attitude_sp_pub(nullptr),
_actuators_0_pub(nullptr),
@ -366,11 +366,11 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_actuators_id(0),
_attitude_setpoint_id(0),
/* performance counters */
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")),
_nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")),
/* states */
/* states */
_setpoint_valid(false),
_debug(false),
_flaps_cmd_last(0),
@ -635,12 +635,14 @@ FixedwingAttitudeControl::vehicle_status_poll()
if (vehicle_status_updated) {
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
/* set correct uORB ID, depending on if vehicle is VTOL or not */
if (!_rates_sp_id) {
if (_vehicle_status.is_vtol) {
_rates_sp_id = ORB_ID(fw_virtual_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_virtual_fw);
_attitude_setpoint_id = ORB_ID(fw_virtual_attitude_setpoint);
} else {
_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
_actuators_id = ORB_ID(actuator_controls_0);
@ -727,8 +729,9 @@ FixedwingAttitudeControl::task_main()
last_run = hrt_absolute_time();
/* guard against too large deltaT's */
if (deltaT > 1.0f)
if (deltaT > 1.0f) {
deltaT = 0.01f;
}
/* load local copies */
orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
@ -821,6 +824,7 @@ FixedwingAttitudeControl::task_main()
if (_vcontrol_mode.flag_control_termination_enabled) {
_actuators_airframe.control[7] = 1.0f;
//warnx("_actuators_airframe.control[1] = 1.0f;");
} else {
_actuators_airframe.control[7] = 0.0f;
//warnx("_actuators_airframe.control[1] = -1.0f;");
@ -838,13 +842,15 @@ FixedwingAttitudeControl::task_main()
/* map flaps by default to manual if valid */
if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled) {
flaps_control = 0.5f * (_manual.flaps + 1.0f ) * _parameters.flaps_scale;
flaps_control = 0.5f * (_manual.flaps + 1.0f) * _parameters.flaps_scale;
} else if (_vcontrol_mode.flag_control_auto_enabled) {
flaps_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaps_scale : 0.0f;
}
// move the actual control value continuous with time
static hrt_abstime t_flaps_changed = 0;
if (fabsf(flaps_control - _flaps_cmd_last) > 0.01f) {
t_flaps_changed = hrt_absolute_time();
delta_flaps = flaps_control - _flaps_cmd_last;
@ -865,12 +871,14 @@ FixedwingAttitudeControl::task_main()
/* map flaperons by default to manual if valid */
if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled) {
flaperon = 0.5f * (_manual.aux2 + 1.0f) * _parameters.flaperon_scale;
} else if (_vcontrol_mode.flag_control_auto_enabled) {
flaperon = _att_sp.apply_flaps ? 1.0f * _parameters.flaperon_scale : 0.0f;
}
// move the actual control value continuous with time
static hrt_abstime t_flaperons_changed = 0;
if (fabsf(flaperon - _flaperons_cmd_last) > 0.01f) {
t_flaperons_changed = hrt_absolute_time();
delta_flaperon = flaperon - _flaperons_cmd_last;
@ -880,7 +888,8 @@ FixedwingAttitudeControl::task_main()
static float flaperon_applied = 0.0f;
if (fabsf(flaperon_applied - flaperon) > 0.01f) {
flaperon_applied = (flaperon - delta_flaperon) + (float)hrt_elapsed_time(&t_flaperons_changed) * (delta_flaperon) / 1000000;
flaperon_applied = (flaperon - delta_flaperon) + (float)hrt_elapsed_time(&t_flaperons_changed) *
(delta_flaperon) / 1000000;
}
/* decide if in stabilized or full manual control */
@ -908,13 +917,14 @@ FixedwingAttitudeControl::task_main()
*
* Forcing the scaling to this value allows reasonable handheld tests.
*/
float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed);
float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min :
airspeed);
/* Use min airspeed to calculate ground speed scaling region.
* Don't scale below gspd_scaling_trim
*/
float groundspeed = sqrtf(_global_pos.vel_n * _global_pos.vel_n +
_global_pos.vel_e * _global_pos.vel_e);
_global_pos.vel_e * _global_pos.vel_e);
float gspd_scaling_trim = (_parameters.airspeed_min * 0.6f);
float groundspeed_scaler = gspd_scaling_trim / ((groundspeed < gspd_scaling_trim) ? gspd_scaling_trim : groundspeed);
@ -930,7 +940,7 @@ FixedwingAttitudeControl::task_main()
* for sure not be set from the remote control values)
*/
if (_vcontrol_mode.flag_control_auto_enabled ||
!_vcontrol_mode.flag_control_manual_enabled) {
!_vcontrol_mode.flag_control_manual_enabled) {
/* read in attitude setpoint from attitude setpoint uorb topic */
roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
@ -941,13 +951,16 @@ FixedwingAttitudeControl::task_main()
if (_att_sp.roll_reset_integral) {
_roll_ctrl.reset_integrator();
}
if (_att_sp.pitch_reset_integral) {
_pitch_ctrl.reset_integrator();
}
if (_att_sp.yaw_reset_integral) {
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
}
} else if (_vcontrol_mode.flag_control_velocity_enabled) {
/* the pilot does not want to change direction,
@ -955,9 +968,10 @@ FixedwingAttitudeControl::task_main()
*/
if (fabsf(_manual.y) < 0.01f && fabsf(_roll) < 0.2f) {
roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
} else {
roll_sp = (_manual.y * _parameters.man_roll_max)
+ _parameters.rollsp_offset_rad;
+ _parameters.rollsp_offset_rad;
}
pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
@ -967,16 +981,18 @@ FixedwingAttitudeControl::task_main()
if (_att_sp.roll_reset_integral) {
_roll_ctrl.reset_integrator();
}
if (_att_sp.pitch_reset_integral) {
_pitch_ctrl.reset_integrator();
}
if (_att_sp.yaw_reset_integral) {
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
}
} else if (_vcontrol_mode.flag_control_altitude_enabled) {
/*
/*
* Velocity should be controlled and manual is enabled.
*/
roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad;
@ -987,13 +1003,16 @@ FixedwingAttitudeControl::task_main()
if (_att_sp.roll_reset_integral) {
_roll_ctrl.reset_integrator();
}
if (_att_sp.pitch_reset_integral) {
_pitch_ctrl.reset_integrator();
}
if (_att_sp.yaw_reset_integral) {
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
}
} else {
/*
* Scale down roll and pitch as the setpoints are radians
@ -1029,6 +1048,7 @@ FixedwingAttitudeControl::task_main()
if (_attitude_sp_pub != nullptr) {
/* publish the attitude setpoint */
orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &att_sp);
} else if (_attitude_setpoint_id) {
/* advertise and publish */
_attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &att_sp);
@ -1090,6 +1110,7 @@ FixedwingAttitudeControl::task_main()
/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
float roll_u = _roll_ctrl.control_bodyrate(control_input);
_actuators.control[0] = (PX4_ISFINITE(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;
if (!PX4_ISFINITE(roll_u)) {
_roll_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
@ -1101,26 +1122,29 @@ FixedwingAttitudeControl::task_main()
float pitch_u = _pitch_ctrl.control_bodyrate(control_input);
_actuators.control[1] = (PX4_ISFINITE(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
if (!PX4_ISFINITE(pitch_u)) {
_pitch_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (_debug && loop_counter % 10 == 0) {
warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
" airspeed %.4f, airspeed_scaling %.4f,"
" roll_sp %.4f, pitch_sp %.4f,"
" _roll_ctrl.get_desired_rate() %.4f,"
" _pitch_ctrl.get_desired_rate() %.4f"
" att_sp.roll_body %.4f",
(double)pitch_u, (double)_yaw_ctrl.get_desired_rate(),
(double)airspeed, (double)airspeed_scaling,
(double)roll_sp, (double)pitch_sp,
(double)_roll_ctrl.get_desired_rate(),
(double)_pitch_ctrl.get_desired_rate(),
(double)_att_sp.roll_body);
" airspeed %.4f, airspeed_scaling %.4f,"
" roll_sp %.4f, pitch_sp %.4f,"
" _roll_ctrl.get_desired_rate() %.4f,"
" _pitch_ctrl.get_desired_rate() %.4f"
" att_sp.roll_body %.4f",
(double)pitch_u, (double)_yaw_ctrl.get_desired_rate(),
(double)airspeed, (double)airspeed_scaling,
(double)roll_sp, (double)pitch_sp,
(double)_roll_ctrl.get_desired_rate(),
(double)_pitch_ctrl.get_desired_rate(),
(double)_att_sp.roll_body);
}
}
float yaw_u = 0.0f;
if (_att_sp.fw_control_yaw == true) {
yaw_u = _wheel_ctrl.control_bodyrate(control_input);
}
@ -1128,14 +1152,17 @@ FixedwingAttitudeControl::task_main()
else {
yaw_u = _yaw_ctrl.control_bodyrate(control_input);
}
_actuators.control[2] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
/* add in manual rudder control */
_actuators.control[2] += yaw_manual;
if (!PX4_ISFINITE(yaw_u)) {
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (_debug && loop_counter % 10 == 0) {
warnx("yaw_u %.4f", (double)yaw_u);
}
@ -1144,16 +1171,19 @@ FixedwingAttitudeControl::task_main()
/* throttle passed through if it is finite and if no engine failure was
* detected */
_actuators.control[3] = (PX4_ISFINITE(throttle_sp) &&
!(_vehicle_status.engine_failure ||
_vehicle_status.engine_failure_cmd)) ?
throttle_sp : 0.0f;
!(_vehicle_status.engine_failure ||
_vehicle_status.engine_failure_cmd)) ?
throttle_sp : 0.0f;
if (!PX4_ISFINITE(throttle_sp)) {
if (_debug && loop_counter % 10 == 0) {
warnx("throttle_sp %.4f", (double)throttle_sp);
}
}
} else {
perf_count(_nonfinite_input_perf);
if (_debug && loop_counter % 10 == 0) {
warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
}
@ -1172,6 +1202,7 @@ FixedwingAttitudeControl::task_main()
if (_rate_sp_pub != nullptr) {
/* publish the attitude rates setpoint */
orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp);
} else if (_rates_sp_id) {
/* advertise the attitude rates setpoint */
_rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp);
@ -1197,15 +1228,15 @@ FixedwingAttitudeControl::task_main()
_actuators_airframe.timestamp_sample = _ctrl_state.timestamp;
/* Only publish if any of the proper modes are enabled */
if(_vcontrol_mode.flag_control_rates_enabled ||
_vcontrol_mode.flag_control_attitude_enabled ||
_vcontrol_mode.flag_control_manual_enabled)
{
if (_vcontrol_mode.flag_control_rates_enabled ||
_vcontrol_mode.flag_control_attitude_enabled ||
_vcontrol_mode.flag_control_manual_enabled) {
/* publish the actuator controls */
if (_actuators_0_pub != nullptr) {
orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
} else if (_actuators_id) {
_actuators_0_pub= orb_advertise(_actuators_id, &_actuators);
_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
}
if (_actuators_2_pub != nullptr) {
@ -1236,11 +1267,11 @@ FixedwingAttitudeControl::start()
/* start the task */
_control_task = px4_task_spawn_cmd("fw_att_control",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
1300,
(px4_main_t)&FixedwingAttitudeControl::task_main_trampoline,
nullptr);
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
1300,
(px4_main_t)&FixedwingAttitudeControl::task_main_trampoline,
nullptr);
if (_control_task < 0) {
warn("task start failed");
@ -1287,13 +1318,15 @@ int fw_att_control_main(int argc, char *argv[])
printf(".");
fflush(stdout);
}
printf("\n");
}
return 0;
}
if (!strcmp(argv[1], "stop")) {
if (att_control::g_control == nullptr){
if (att_control::g_control == nullptr) {
warnx("not running");
return 1;
}

506
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

File diff suppressed because it is too large Load Diff