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

Save flash space

Browse Source 
This commit saves roughly 400-500 Bytes in flash space
This commit is contained in:
kamilritz
2020-01-09 19:43:29 +11:00
committed by Paul Riseborough
parent 94484f01ce
commit c7e074276f
2 changed files with 20 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/estimator_interface.cpp
+19 -26
View File
@@ -171,7 +171,7 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float (&data)[3])
_mag_buffer_fail = !_mag_buffer.allocate(_obs_buffer_length); _mag_buffer_fail = !_mag_buffer.allocate(_obs_buffer_length);
if (_mag_buffer_fail) { if (_mag_buffer_fail) {
ECL_ERR_TIMESTAMPED("mag buffer allocation failed"); printBufferAllocationFailed("mag");
return; return;
} }
} }
@@ -203,7 +203,7 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, const gps_message &gps)
_gps_buffer_fail = !_gps_buffer.allocate(_obs_buffer_length); _gps_buffer_fail = !_gps_buffer.allocate(_obs_buffer_length);
if (_gps_buffer_fail) { if (_gps_buffer_fail) {
ECL_ERR_TIMESTAMPED("GPS buffer allocation failed"); printBufferAllocationFailed("GPS");
return; return;
} }
} }
@@ -264,7 +264,7 @@ void EstimatorInterface::setBaroData(uint64_t time_usec, float data)
_baro_buffer_fail = !_baro_buffer.allocate(_obs_buffer_length); _baro_buffer_fail = !_baro_buffer.allocate(_obs_buffer_length);
if (_baro_buffer_fail) { if (_baro_buffer_fail) {
ECL_ERR_TIMESTAMPED("baro buffer allocation failed"); printBufferAllocationFailed("baro");
return; return;
} }
} }
@@ -297,7 +297,7 @@ void EstimatorInterface::setAirspeedData(uint64_t time_usec, float true_airspeed
_airspeed_buffer_fail = !_airspeed_buffer.allocate(_obs_buffer_length); _airspeed_buffer_fail = !_airspeed_buffer.allocate(_obs_buffer_length);
if (_airspeed_buffer_fail) { if (_airspeed_buffer_fail) {
ECL_ERR_TIMESTAMPED("airspeed buffer allocation failed"); printBufferAllocationFailed("airspeed");
return; return;
} }
} }
@@ -327,7 +327,7 @@ void EstimatorInterface::setRangeData(uint64_t time_usec, float data, int8_t qua
_range_buffer_fail = !_range_buffer.allocate(_obs_buffer_length); _range_buffer_fail = !_range_buffer.allocate(_obs_buffer_length);
if (_range_buffer_fail) { if (_range_buffer_fail) {
ECL_ERR_TIMESTAMPED("range finder buffer allocation failed"); printBufferAllocationFailed("range");
return; return;
} }
} }
@@ -357,7 +357,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
_flow_buffer_fail = !_flow_buffer.allocate(_imu_buffer_length); _flow_buffer_fail = !_flow_buffer.allocate(_imu_buffer_length);
if (_flow_buffer_fail) { if (_flow_buffer_fail) {
ECL_ERR_TIMESTAMPED("optical flow buffer allocation failed"); printBufferAllocationFailed("flow");
return; return;
} }
} }
@@ -423,7 +423,7 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message
_ev_buffer_fail = !_ext_vision_buffer.allocate(_obs_buffer_length); _ev_buffer_fail = !_ext_vision_buffer.allocate(_obs_buffer_length);
if (_ev_buffer_fail) { if (_ev_buffer_fail) {
ECL_ERR_TIMESTAMPED("external vision buffer allocation failed"); printBufferAllocationFailed("vision");
return; return;
} }
} }
@@ -459,7 +459,7 @@ void EstimatorInterface::setAuxVelData(uint64_t time_usec, const Vector3f &veloc
_auxvel_buffer_fail = !_auxvel_buffer.allocate(_obs_buffer_length); _auxvel_buffer_fail = !_auxvel_buffer.allocate(_obs_buffer_length);
if (_auxvel_buffer_fail) { if (_auxvel_buffer_fail) {
ECL_ERR_TIMESTAMPED("aux vel buffer allocation failed"); printBufferAllocationFailed("aux vel");
return; return;
} }
} }
@@ -508,30 +508,15 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
_output_buffer.allocate(_imu_buffer_length) && _output_buffer.allocate(_imu_buffer_length) &&
_output_vert_buffer.allocate(_imu_buffer_length))) { _output_vert_buffer.allocate(_imu_buffer_length))) {
ECL_ERR_TIMESTAMPED("buffer allocation failed!"); printBufferAllocationFailed("");
unallocate_buffers(); unallocate_buffers();
return false; return false;
} }
_dt_imu_avg = 0.0f;
_imu_sample_delayed.delta_ang.setZero();
_imu_sample_delayed.delta_vel.setZero();
_imu_sample_delayed.delta_ang_dt = 0.0f;
_imu_sample_delayed.delta_vel_dt = 0.0f;
_imu_sample_delayed.time_us = timestamp; _imu_sample_delayed.time_us = timestamp;
_initialised = false;
_time_last_imu = 0;
_time_last_gps = 0;
_time_last_mag = 0;
_time_last_baro = 0;
_time_last_range = 0;
_time_last_airspeed = 0;
_time_last_optflow = 0;
_fault_status.value = 0; _fault_status.value = 0;
_time_last_ext_vision = 0;
return true; return true;
} }
@@ -558,6 +543,14 @@ bool EstimatorInterface::local_position_is_valid()
return !_deadreckon_time_exceeded; return !_deadreckon_time_exceeded;
} }
void EstimatorInterface::printBufferAllocationFailed(const char * buffer_name)
{
if(buffer_name)
{
ECL_ERR_TIMESTAMPED("%s buffer allocation failed", buffer_name);
}
}
void EstimatorInterface::print_status() void EstimatorInterface::print_status()
{ {
ECL_INFO("local position valid: %s", local_position_is_valid() ? "yes" : "no"); ECL_INFO("local position valid: %s", local_position_is_valid() ? "yes" : "no");
@@ -570,7 +563,7 @@ void EstimatorInterface::print_status()
ECL_INFO("range buffer: %d (%d Bytes)", _range_buffer.get_length(), _range_buffer.get_total_size()); ECL_INFO("range buffer: %d (%d Bytes)", _range_buffer.get_length(), _range_buffer.get_total_size());
ECL_INFO("airspeed buffer: %d (%d Bytes)", _airspeed_buffer.get_length(), _airspeed_buffer.get_total_size()); ECL_INFO("airspeed buffer: %d (%d Bytes)", _airspeed_buffer.get_length(), _airspeed_buffer.get_total_size());
ECL_INFO("flow buffer: %d (%d Bytes)", _flow_buffer.get_length(), _flow_buffer.get_total_size()); ECL_INFO("flow buffer: %d (%d Bytes)", _flow_buffer.get_length(), _flow_buffer.get_total_size());
ECL_INFO("ext vision buffer: %d (%d Bytes)", _ext_vision_buffer.get_length(), _ext_vision_buffer.get_total_size()); ECL_INFO("vision buffer: %d (%d Bytes)", _ext_vision_buffer.get_length(), _ext_vision_buffer.get_total_size());
ECL_INFO("output buffer: %d (%d Bytes)", _output_buffer.get_length(), _output_buffer.get_total_size()); ECL_INFO("output buffer: %d (%d Bytes)", _output_buffer.get_length(), _output_buffer.get_total_size());
ECL_INFO("output vert buffer: %d (%d Bytes)", _output_vert_buffer.get_length(), _output_vert_buffer.get_total_size()); ECL_INFO("output vert buffer: %d (%d Bytes)", _output_vert_buffer.get_length(), _output_vert_buffer.get_total_size());
ECL_INFO("drag buffer: %d (%d Bytes)", _drag_buffer.get_length(), _drag_buffer.get_total_size()); ECL_INFO("drag buffer: %d (%d Bytes)", _drag_buffer.get_length(), _drag_buffer.get_total_size());
EKF/estimator_interface.h
+1 -8
View File
@@ -174,35 +174,27 @@ public:
// accumulate and downsample IMU data to the EKF prediction rate // accumulate and downsample IMU data to the EKF prediction rate
virtual bool collect_imu(const imuSample &imu) = 0; virtual bool collect_imu(const imuSample &imu) = 0;
// set delta angle imu data
void setIMUData(const imuSample &imu_sample); void setIMUData(const imuSample &imu_sample);
// legacy interface for compatibility (2018-09-14) // legacy interface for compatibility (2018-09-14)
void setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float (&delta_ang)[3], float (&delta_vel)[3]); void setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float (&delta_ang)[3], float (&delta_vel)[3]);
// set magnetometer data
void setMagData(uint64_t time_usec, float (&data)[3]); void setMagData(uint64_t time_usec, float (&data)[3]);
// set gps data
void setGpsData(uint64_t time_usec, const gps_message &gps); void setGpsData(uint64_t time_usec, const gps_message &gps);
// set baro data
void setBaroData(uint64_t time_usec, float data); void setBaroData(uint64_t time_usec, float data);
// set airspeed data
void setAirspeedData(uint64_t time_usec, float true_airspeed, float eas2tas); void setAirspeedData(uint64_t time_usec, float true_airspeed, float eas2tas);
// set range data
void setRangeData(uint64_t time_usec, float data, int8_t quality); void setRangeData(uint64_t time_usec, float data, int8_t quality);
// set optical flow data
// if optical flow sensor gyro delta angles are not available, set gyroXYZ vector fields to NaN and the EKF will use its internal delta angle data instead // if optical flow sensor gyro delta angles are not available, set gyroXYZ vector fields to NaN and the EKF will use its internal delta angle data instead
void setOpticalFlowData(uint64_t time_usec, flow_message *flow); void setOpticalFlowData(uint64_t time_usec, flow_message *flow);
// set external vision position and attitude data // set external vision position and attitude data
void setExtVisionData(uint64_t time_usec, ext_vision_message *evdata); void setExtVisionData(uint64_t time_usec, ext_vision_message *evdata);
// set auxiliary velocity data
void setAuxVelData(uint64_t time_usec, const Vector3f &vel, const Vector3f &variance); void setAuxVelData(uint64_t time_usec, const Vector3f &vel, const Vector3f &variance);
// return a address to the parameters struct // return a address to the parameters struct
@@ -574,4 +566,5 @@ protected:
// calculate the inverse rotation matrix from a quaternion rotation // calculate the inverse rotation matrix from a quaternion rotation
Matrix3f quat_to_invrotmat(const Quatf &quat); Matrix3f quat_to_invrotmat(const Quatf &quat);
void printBufferAllocationFailed(const char * buffer_name);
}; };