fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 06:33:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

updated to master (solved merge conflicts)

Browse Source
This commit is contained in:
Youssef Demitri
2015-10-27 09:26:27 +01:00
parent 560efcbc5b 4241e526aa
commit dd59409e31
289 changed files with 10555 additions and 4461 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+60 -33
View File
@@ -79,12 +79,14 @@ using math::Quaternion;
class AttitudeEstimatorQ;
namespace attitude_estimator_q {
namespace attitude_estimator_q
{
AttitudeEstimatorQ *instance;
}
class AttitudeEstimatorQ {
class AttitudeEstimatorQ
{
public:
/**
* Constructor
@@ -160,6 +162,7 @@ private:
/* Low pass filter for attitude rates */
math::LowPassFilter2p _lp_roll_rate;
math::LowPassFilter2p _lp_pitch_rate;
math::LowPassFilter2p _lp_yaw_rate;
hrt_abstime _vel_prev_t = 0;
@@ -187,11 +190,12 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() :
_voter_gyro(3),
_voter_accel(3),
_voter_mag(3),
_lp_roll_rate(250.0f, 20.0f),
_lp_pitch_rate(250.0f, 20.0f)
_lp_roll_rate(250.0f, 18.0f),
_lp_pitch_rate(250.0f, 18.0f),
_lp_yaw_rate(250, 10.0f)
{
_voter_mag.set_timeout(200000);
_params_handles.w_acc = param_find("ATT_W_ACC");
_params_handles.w_mag = param_find("ATT_W_MAG");
_params_handles.w_gyro_bias = param_find("ATT_W_GYRO_BIAS");
@@ -235,11 +239,11 @@ int AttitudeEstimatorQ::start()
/* start the task */
_control_task = px4_task_spawn_cmd("attitude_estimator_q",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2100,
(px4_main_t)&AttitudeEstimatorQ::task_main_trampoline,
nullptr);
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
2100,
(px4_main_t)&AttitudeEstimatorQ::task_main_trampoline,
nullptr);
if (_control_task < 0) {
warn("task start failed");
@@ -289,6 +293,7 @@ void AttitudeEstimatorQ::task_main()
// Poll error, sleep and try again
usleep(10000);
continue;
} else if (ret == 0) {
// Poll timeout, do nothing
continue;
@@ -298,6 +303,7 @@ void AttitudeEstimatorQ::task_main()
// Update sensors
sensor_combined_s sensors;
if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) {
// Feed validator with recent sensor data
@@ -311,6 +317,7 @@ void AttitudeEstimatorQ::task_main()
for (unsigned j = 0; j < 3; j++) {
if (sensors.gyro_integral_dt[i] > 0) {
gyro[j] = (double)sensors.gyro_integral_rad[i * 3 + j] / (sensors.gyro_integral_dt[i] / 1e6);
} else {
/* fall back to angular rate */
gyro[j] = sensors.gyro_rad_s[i * 3 + j];
@@ -319,10 +326,11 @@ void AttitudeEstimatorQ::task_main()
_voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]);
}
_voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3],
sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]);
sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]);
_voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3],
sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]);
sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]);
}
int best_gyro, best_accel, best_mag;
@@ -341,26 +349,28 @@ void AttitudeEstimatorQ::task_main()
_data_good = true;
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;
}
@@ -368,8 +378,10 @@ void AttitudeEstimatorQ::task_main()
bool gpos_updated;
orb_check(_global_pos_sub, &gpos_updated);
if (gpos_updated) {
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_gpos);
if (_mag_decl_auto && _gpos.eph < 20.0f && hrt_elapsed_time(&_gpos.timestamp) < 1000000) {
/* set magnetic declination automatically */
_mag_decl = math::radians(get_mag_declination(_gpos.lat, _gpos.lon));
@@ -387,6 +399,7 @@ void AttitudeEstimatorQ::task_main()
/* calculate acceleration in body frame */
_pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt);
}
_vel_prev_t = _gpos.timestamp;
_vel_prev = vel;
}
@@ -400,7 +413,7 @@ void AttitudeEstimatorQ::task_main()
// Time from previous iteration
hrt_abstime now = hrt_absolute_time();
float dt = (last_time > 0) ? ((now - last_time) / 1000000.0f) : 0.0f;
float dt = (last_time > 0) ? ((now - last_time) / 1000000.0f) : 0.00001f;
last_time = now;
if (dt > _dt_max) {
@@ -420,9 +433,12 @@ void AttitudeEstimatorQ::task_main()
att.pitch = euler(1);
att.yaw = euler(2);
att.rollspeed = _rates(0);
att.pitchspeed = _rates(1);
att.yawspeed = _rates(2);
/* the complimentary filter should reflect the true system
* state, but we need smoother outputs for the control system
*/
att.rollspeed = _lp_roll_rate.apply(_rates(0));
att.pitchspeed = _lp_pitch_rate.apply(_rates(1));
att.yawspeed = _lp_yaw_rate.apply(_rates(2));
for (int i = 0; i < 3; i++) {
att.g_comp[i] = _accel(i) - _pos_acc(i);
@@ -443,6 +459,7 @@ void AttitudeEstimatorQ::task_main()
if (_att_pub == nullptr) {
_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
} else {
orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
}
@@ -472,11 +489,14 @@ void AttitudeEstimatorQ::task_main()
}
}
void AttitudeEstimatorQ::update_parameters(bool force) {
void AttitudeEstimatorQ::update_parameters(bool force)
{
bool updated = force;
if (!updated) {
orb_check(_params_sub, &updated);
}
if (updated) {
parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
@@ -498,7 +518,8 @@ void AttitudeEstimatorQ::update_parameters(bool force) {
}
}
bool AttitudeEstimatorQ::init() {
bool AttitudeEstimatorQ::init()
{
// Rotation matrix can be easily constructed from acceleration and mag field vectors
// 'k' is Earth Z axis (Down) unit vector in body frame
Vector<3> k = -_accel;
@@ -522,9 +543,10 @@ bool AttitudeEstimatorQ::init() {
_q.normalize();
if (PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)) &&
_q.length() > 0.95f && _q.length() < 1.05f) {
PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)) &&
_q.length() > 0.95f && _q.length() < 1.05f) {
_inited = true;
} else {
_inited = false;
}
@@ -532,7 +554,8 @@ bool AttitudeEstimatorQ::init() {
return _inited;
}
bool AttitudeEstimatorQ::update(float dt) {
bool AttitudeEstimatorQ::update(float dt)
{
if (!_inited) {
if (!_data_good) {
@@ -559,18 +582,20 @@ bool AttitudeEstimatorQ::update(float dt) {
// Vector<3> k = _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f));
// Optimized version with dropped zeros
Vector<3> k(
2.0f * (_q(1) * _q(3) - _q(0) * _q(2)),
2.0f * (_q(2) * _q(3) + _q(0) * _q(1)),
(_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3))
2.0f * (_q(1) * _q(3) - _q(0) * _q(2)),
2.0f * (_q(2) * _q(3) + _q(0) * _q(1)),
(_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3))
);
corr += (k % (_accel - _pos_acc).normalized()) * _w_accel;
// Gyro bias estimation
_gyro_bias += corr * (_w_gyro_bias * dt);
for (int i = 0; i < 3; i++) {
_gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max);
}
_rates = _gyro + _gyro_bias;
// Feed forward gyro
@@ -583,7 +608,7 @@ bool AttitudeEstimatorQ::update(float dt) {
_q.normalize();
if (!(PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)))) {
PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)))) {
// Reset quaternion to last good state
_q = q_last;
_rates.zero();
@@ -594,7 +619,9 @@ bool AttitudeEstimatorQ::update(float dt) {
return true;
}
int attitude_estimator_q_main(int argc, char *argv[]) {
int attitude_estimator_q_main(int argc, char *argv[])
{
if (argc < 1) {
warnx("usage: attitude_estimator_q {start|stop|status}");
return 1;
src/modules/attitude_estimator_q/attitude_estimator_q_params.c
+8 -2
View File
@@ -47,6 +47,7 @@
* @group Attitude Q estimator
* @min 0
* @max 1
* @decimal 2
*/
PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f);
@@ -56,6 +57,7 @@ PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f);
* @group Attitude Q estimator
* @min 0
* @max 1
* @decimal 2
*/
PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f);
@@ -65,6 +67,7 @@ PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f);
* @group Attitude Q estimator
* @min 0
* @max 1
* @decimal 2
*/
PARAM_DEFINE_FLOAT(ATT_W_GYRO_BIAS, 0.1f);
@@ -77,6 +80,7 @@ PARAM_DEFINE_FLOAT(ATT_W_GYRO_BIAS, 0.1f);
*
* @group Attitude Q estimator
* @unit degrees
* @decimal 2
*/
PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
@@ -106,6 +110,7 @@ PARAM_DEFINE_INT32(ATT_ACC_COMP, 2);
* @min 0
* @max 2
* @unit rad/s
* @decimal 3
*/
PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f);
@@ -113,7 +118,8 @@ PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f);
* Threshold (of RMS) to warn about high vibration levels
*
* @group Attitude Q estimator
* @min 0.001
* @max 100
* @min 0.01
* @max 10
* @decimal 2
*/
PARAM_DEFINE_FLOAT(ATT_VIBE_THRESH, 0.2f);