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

code style formatting

Browse Source
This commit is contained in:
tumbili
2016-06-08 17:14:33 +02:00
parent acea2f98d5
commit b9e9f62bee
2 changed files with 33 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+19 -10
View File
@@ -258,9 +258,10 @@ private:
control::BlockParamFloat _ev_pos_y; // Y position of VI sensor focal point in body frame (m)
control::BlockParamFloat _ev_pos_z; // Z position of VI sensor focal point in body frame (m)
// control of airspeed and sideslip fusion
control::BlockParamFloat _arspFusionThreshold; // a value of zero will disabled airspeed fusion. Any another positive value will determine
// the minimum airspeed which will still be fused
control::BlockParamFloat
_arspFusionThreshold; // a value of zero will disabled airspeed fusion. Any another positive value will determine
// the minimum airspeed which will still be fused
// output predictor filter time constants
control::BlockParamFloat _tau_vel; // time constant used by the output velocity complementary filter (s)
control::BlockParamFloat _tau_pos; // time constant used by the output position complementary filter (s)
@@ -453,11 +454,11 @@ void Ekf2::task_main()
orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors);
// update all other topics if they have new data
orb_check(_status_sub, &vehicle_status_updated);
orb_check(_status_sub, &vehicle_status_updated);
if (vehicle_status_updated) {
orb_copy(ORB_ID(vehicle_status), _status_sub, &_vehicle_status);
}
orb_copy(ORB_ID(vehicle_status), _status_sub, &_vehicle_status);
}
orb_check(_gps_sub, &gps_updated);
@@ -534,11 +535,13 @@ void Ekf2::task_main()
}
// only set airspeed data if condition for airspeed fusion are met
bool fuse_airspeed = airspeed_updated && !_vehicle_status.is_rotary_wing && _arspFusionThreshold.get() <= airspeed.true_airspeed_m_s;
if (fuse_airspeed) {
float eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s;
bool fuse_airspeed = airspeed_updated && !_vehicle_status.is_rotary_wing
&& _arspFusionThreshold.get() <= airspeed.true_airspeed_m_s;
if (fuse_airspeed) {
float eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s;
_ekf.setAirspeedData(airspeed.timestamp, &airspeed.true_airspeed_m_s, &eas2tas);
}
}
if (optical_flow_updated) {
flow_message flow;
@@ -569,18 +572,23 @@ void Ekf2::task_main()
ev_data.posNED(2) = ev.z;
Quaternion q(ev.q);
ev_data.quat = q;
// position measurement error
if (ev.pos_err >= 0.001f) {
ev_data.posErr = ev.pos_err;
} else {
ev_data.posErr = _default_ev_pos_noise;
}
// angle measurement error
if (ev.ang_err >= 0.001f) {
ev_data.angErr = ev.ang_err;
} else {
ev_data.angErr = _default_ev_ang_noise;
}
// use timestamp from external computer - requires clocks to be synchronised so may not be a good idea
_ekf.setExtVisionData(ev.timestamp_computer, &ev_data);
}
@@ -861,6 +869,7 @@ void Ekf2::task_main()
_ekf.copy_mag_decl_deg(&decl_deg);
_mag_declination_deg.set(decl_deg);
}
_prev_landed = vehicle_land_detected.landed;
// publish replay message if in replay mode