Skip to content
Snippets Groups Projects
Commit b9e9f62b authored by tumbili's avatar tumbili
Browse files

code style formatting

parent acea2f98
No related branches found
No related tags found
No related merge requests found
......@@ -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
......
......@@ -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)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment