forked from Archive/PX4-Autopilot
Save flash space
This commit saves roughly 400-500 Bytes in flash space
This commit is contained in:
kamilritz
Paul Riseborough
parent
94484f01ce
commit
c7e074276f
|
|
@ -171,7 +171,7 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float (&data)[3])
|
|||
_mag_buffer_fail = !_mag_buffer.allocate(_obs_buffer_length);
|
||||
|
||||
if (_mag_buffer_fail) {
|
||||
ECL_ERR_TIMESTAMPED("mag buffer allocation failed");
|
||||
printBufferAllocationFailed("mag");
|
||||
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);
|
||||
|
||||
if (_gps_buffer_fail) {
|
||||
ECL_ERR_TIMESTAMPED("GPS buffer allocation failed");
|
||||
printBufferAllocationFailed("GPS");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
|
@ -264,7 +264,7 @@ void EstimatorInterface::setBaroData(uint64_t time_usec, float data)
|
|||
_baro_buffer_fail = !_baro_buffer.allocate(_obs_buffer_length);
|
||||
|
||||
if (_baro_buffer_fail) {
|
||||
ECL_ERR_TIMESTAMPED("baro buffer allocation failed");
|
||||
printBufferAllocationFailed("baro");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
|
@ -297,7 +297,7 @@ void EstimatorInterface::setAirspeedData(uint64_t time_usec, float true_airspeed
|
|||
_airspeed_buffer_fail = !_airspeed_buffer.allocate(_obs_buffer_length);
|
||||
|
||||
if (_airspeed_buffer_fail) {
|
||||
ECL_ERR_TIMESTAMPED("airspeed buffer allocation failed");
|
||||
printBufferAllocationFailed("airspeed");
|
||||
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);
|
||||
|
||||
if (_range_buffer_fail) {
|
||||
ECL_ERR_TIMESTAMPED("range finder buffer allocation failed");
|
||||
printBufferAllocationFailed("range");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
|
@ -357,7 +357,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
|
|||
_flow_buffer_fail = !_flow_buffer.allocate(_imu_buffer_length);
|
||||
|
||||
if (_flow_buffer_fail) {
|
||||
ECL_ERR_TIMESTAMPED("optical flow buffer allocation failed");
|
||||
printBufferAllocationFailed("flow");
|
||||
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);
|
||||
|
||||
if (_ev_buffer_fail) {
|
||||
ECL_ERR_TIMESTAMPED("external vision buffer allocation failed");
|
||||
printBufferAllocationFailed("vision");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
|
@ -459,7 +459,7 @@ void EstimatorInterface::setAuxVelData(uint64_t time_usec, const Vector3f &veloc
|
|||
_auxvel_buffer_fail = !_auxvel_buffer.allocate(_obs_buffer_length);
|
||||
|
||||
if (_auxvel_buffer_fail) {
|
||||
ECL_ERR_TIMESTAMPED("aux vel buffer allocation failed");
|
||||
printBufferAllocationFailed("aux vel");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
|
@ -508,30 +508,15 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp)
|
|||
_output_buffer.allocate(_imu_buffer_length) &&
|
||||
_output_vert_buffer.allocate(_imu_buffer_length))) {
|
||||
|
||||
ECL_ERR_TIMESTAMPED("buffer allocation failed!");
|
||||
printBufferAllocationFailed("");
|
||||
unallocate_buffers();
|
||||
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;
|
||||
|
||||
_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;
|
||||
_time_last_ext_vision = 0;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
@ -558,6 +543,14 @@ bool EstimatorInterface::local_position_is_valid()
|
|||
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()
|
||||
{
|
||||
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("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("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 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());
|
||||
|
|
|
|||
|
|
@ -174,35 +174,27 @@ public:
|
|||
// accumulate and downsample IMU data to the EKF prediction rate
|
||||
virtual bool collect_imu(const imuSample &imu) = 0;
|
||||
|
||||
// set delta angle imu data
|
||||
void setIMUData(const imuSample &imu_sample);
|
||||
|
||||
// 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]);
|
||||
|
||||
// set magnetometer data
|
||||
void setMagData(uint64_t time_usec, float (&data)[3]);
|
||||
|
||||
// set gps data
|
||||
void setGpsData(uint64_t time_usec, const gps_message &gps);
|
||||
|
||||
// set baro data
|
||||
void setBaroData(uint64_t time_usec, float data);
|
||||
|
||||
// set airspeed data
|
||||
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);
|
||||
|
||||
// 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
|
||||
void setOpticalFlowData(uint64_t time_usec, flow_message *flow);
|
||||
|
||||
// set external vision position and attitude data
|
||||
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);
|
||||
|
||||
// return a address to the parameters struct
|
||||
|
|
@ -574,4 +566,5 @@ protected:
|
|||
// calculate the inverse rotation matrix from a quaternion rotation
|
||||
Matrix3f quat_to_invrotmat(const Quatf &quat);
|
||||
|
||||
void printBufferAllocationFailed(const char * buffer_name);
|
||||
};
|
||||
|
|
|
|||
Loading…
Reference in New Issue