fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
src/modules/ekf2/ekf2_main.cpp
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

27
src/modules/ekf2_replay/ekf2_replay_main.cpp
View File

@ -226,7 +226,7 @@ Ekf2Replay::Ekf2Replay(char *logfile) :
_flow{},
_range{},
_airspeed{},
_vehicle_status{},
_vehicle_status{},
_message_counter(0),
_part1_counter_ref(0),
_read_part2(false),
@ -299,6 +299,7 @@ void Ekf2Replay::publishEstimatorInput()
if (_airspeed_pub == nullptr && _read_part6) {
_airspeed_pub = orb_advertise(ORB_ID(airspeed), &_airspeed);
} else if (_airspeed_pub != nullptr) {
orb_publish(ORB_ID(airspeed), _airspeed_pub, &_airspeed);
}
@ -431,7 +432,7 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
_range.current_distance = replay_part4.range_to_ground;
_read_part4 = true;
} else if (type == LOG_RPL6_MSG){
} else if (type == LOG_RPL6_MSG) {
uint8_t *dest_ptr = (uint8_t *)&replay_part6.time_airs_usec;
parseMessage(data, dest_ptr, type);
_airspeed.timestamp = replay_part6.time_airs_usec;
@ -469,17 +470,17 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
}
else if (type == LOG_STAT_MSG) {
uint8_t *dest_ptr = (uint8_t *)&vehicle_status.main_state;
parseMessage(data, dest_ptr, type);
_vehicle_status.is_rotary_wing = vehicle_status.is_rot_wing;
if (_vehicle_status_pub == nullptr) {
_vehicle_status_pub = orb_advertise(ORB_ID(vehicle_status), &_vehicle_status);
} else if (_vehicle_status_pub != nullptr) {
orb_publish(ORB_ID(vehicle_status), _vehicle_status_pub, &_vehicle_status);
}
}
uint8_t *dest_ptr = (uint8_t *)&vehicle_status.main_state;
parseMessage(data, dest_ptr, type);
_vehicle_status.is_rotary_wing = vehicle_status.is_rot_wing;
if (_vehicle_status_pub == nullptr) {
_vehicle_status_pub = orb_advertise(ORB_ID(vehicle_status), &_vehicle_status);
} else if (_vehicle_status_pub != nullptr) {
orb_publish(ORB_ID(vehicle_status), _vehicle_status_pub, &_vehicle_status);
}
}
}
void Ekf2Replay::writeMessage(int &fd, void *data, size_t size)