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

PX4 style fixes (excluding EKF generated code)

Browse Source
This commit is contained in:
Daniel Agar
2018-05-03 22:25:36 -04:00
parent e5952fadaf
commit c5b2f59f57
22 changed files with 384 additions and 187 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/estimator_interface.cpp
+12 -1
View File
@@ -108,6 +108,7 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u
// Do not retry if allocation has failed previously
if (_drag_buffer.get_length() < _obs_buffer_length) {
_drag_buffer_fail = !_drag_buffer.allocate(_obs_buffer_length);
if (_drag_buffer_fail) {
ECL_ERR("EKF drag buffer allocation failed");
return;
@@ -163,6 +164,7 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float (&data)[3])
// Do not retry if allocation has failed previously
if (_mag_buffer.get_length() < _obs_buffer_length) {
_mag_buffer_fail = !_mag_buffer.allocate(_obs_buffer_length);
if (_mag_buffer_fail) {
ECL_ERR("EKF mag buffer allocation failed");
return;
@@ -194,6 +196,7 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps)
// Do not retry if allocation has failed previously
if (_gps_buffer.get_length() < _obs_buffer_length) {
_gps_buffer_fail = !_gps_buffer.allocate(_obs_buffer_length);
if (_gps_buffer_fail) {
ECL_ERR("EKF GPS buffer allocation failed");
return;
@@ -247,6 +250,7 @@ void EstimatorInterface::setBaroData(uint64_t time_usec, float data)
// Do not retry if allocation has failed previously
if (_baro_buffer.get_length() < _obs_buffer_length) {
_baro_buffer_fail = !_baro_buffer.allocate(_obs_buffer_length);
if (_baro_buffer_fail) {
ECL_ERR("EKF baro buffer allocation failed");
return;
@@ -279,6 +283,7 @@ void EstimatorInterface::setAirspeedData(uint64_t time_usec, float true_airspeed
// Do not retry if allocation has failed previously
if (_airspeed_buffer.get_length() < _obs_buffer_length) {
_airspeed_buffer_fail = !_airspeed_buffer.allocate(_obs_buffer_length);
if (_airspeed_buffer_fail) {
ECL_ERR("EKF airspeed buffer allocation failed");
return;
@@ -308,6 +313,7 @@ void EstimatorInterface::setRangeData(uint64_t time_usec, float data)
// Do not retry if allocation has failed previously
if (_range_buffer.get_length() < _obs_buffer_length) {
_range_buffer_fail = !_range_buffer.allocate(_obs_buffer_length);
if (_range_buffer_fail) {
ECL_ERR("EKF range finder buffer allocation failed");
return;
@@ -336,6 +342,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
// Do not retry if allocation has failed previously
if (_flow_buffer.get_length() < _obs_buffer_length) {
_flow_buffer_fail = !_flow_buffer.allocate(_obs_buffer_length);
if (_flow_buffer_fail) {
ECL_ERR("EKF optical flow buffer allocation failed");
return;
@@ -349,6 +356,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
float delta_time = 1e-6f * (float)flow->dt;
float delta_time_min = 5e-7f * (float)_min_obs_interval_us;
bool delta_time_good = delta_time >= delta_time_min;
if (!delta_time_good) {
// protect against overflow casued by division with very small delta_time
delta_time = delta_time_min;
@@ -415,6 +423,7 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message
// Do not retry if allocation has failed previously
if (_ext_vision_buffer.get_length() < _obs_buffer_length) {
_ev_buffer_fail = !_ext_vision_buffer.allocate(_obs_buffer_length);
if (_ev_buffer_fail) {
ECL_ERR("EKF external vision buffer allocation failed");
return;
@@ -451,6 +460,7 @@ void EstimatorInterface::setAuxVelData(uint64_t time_usec, float (&data)[2], flo
// Do not retry if allocation has failed previously
if (_auxvel_buffer.get_length() < _obs_buffer_length) {
_auxvel_buffer_fail = !_auxvel_buffer.allocate(_obs_buffer_length);
if (_auxvel_buffer_fail) {
ECL_ERR("EKF aux vel buffer allocation failed");
return;
@@ -553,7 +563,8 @@ bool EstimatorInterface::local_position_is_valid()
return !_deadreckon_time_exceeded;
}
void EstimatorInterface::print_status() {
void EstimatorInterface::print_status()
{
ECL_INFO("local position valid: %s", local_position_is_valid() ? "yes" : "no");
ECL_INFO("global position valid: %s", global_position_is_valid() ? "yes" : "no");