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

fixed typos (#147)

Browse Source
This commit is contained in:
Carl Olsson
2016-05-22 17:40:43 +02:00
committed by Lorenz Meier
parent dbe2513d48
commit 0fafc49a49
8 changed files with 28 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+5 -5
View File
@@ -62,7 +62,7 @@ void Ekf::controlFusionModes()
}
}
// control use of various external souces for position and velocity aiding
// control use of various external sources for position and velocity aiding
controlExternalVisionAiding();
controlOpticalFlowAiding();
controlGpsAiding();
@@ -151,7 +151,7 @@ void Ekf::controlOpticalFlowAiding()
float range = heightAboveGndEst / _R_to_earth(2, 2);
if ((range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) {
// we should ahve reliable OF measurements so
// we should have reliable OF measurements so
// calculate X and Y body relative velocities from OF measurements
Vector3f vel_optflow_body;
vel_optflow_body(0) = - range * _flow_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt;
@@ -183,7 +183,7 @@ void Ekf::controlOpticalFlowAiding()
_state.pos(0) = 0.0f;
_state.pos(1) = 0.0f;
// reset the coresponding covariances
// reset the corresponding covariances
// we are by definition at the origin at commencement so variances are also zeroed
zeroRows(P,7,8);
zeroCols(P,7,8);
@@ -246,7 +246,7 @@ void Ekf::controlGpsAiding()
// We are relying on GPS aiding to constrain attitude drift so after 10 seconds without aiding we need to do something
if ((_time_last_imu - _time_last_pos_fuse > 10e6) && (_time_last_imu - _time_last_vel_fuse > 10e6)) {
if (_time_last_imu - _time_last_gps > 5e5) {
// if we don't have gps then we need to switch to the non-aiding mode, zero the veloity states
// if we don't have gps then we need to switch to the non-aiding mode, zero the velocity states
// and set the synthetic GPS position to the current estimate
_control_status.flags.gps = false;
_last_known_posNE(0) = _state.pos(0);
@@ -361,7 +361,7 @@ void Ekf::controlHeightSensorTimeouts()
// if GPS height is unavailable and baro data is available, reset height to baro
reset_to_baro = reset_to_baro || (!gps_hgt_available && baro_data_fresh);
// if we cannot switch to baro and GPs data is available, reset height to GPS
// if we cannot switch to baro and GPS data is available, reset height to GPS
bool reset_to_gps = !reset_to_baro && gps_hgt_available;
if (reset_to_baro) {
EKF/covariance.cpp
+1 -1
View File
@@ -53,7 +53,7 @@ void Ekf::initialiseCovariance()
}
// calculate average prediction time step in sec
float dt = 0.001f * (float)FILTER_UPDATE_PERRIOD_MS;
float dt = 0.001f * (float)FILTER_UPDATE_PERIOD_MS;
// define the initial angle uncertainty as variances for a rotation vector
Vector3f rot_vec_var;
EKF/ekf.cpp
+12 -12
View File
@@ -178,7 +178,7 @@ bool Ekf::init(uint64_t timestamp)
_control_status.value = 0;
_control_status_prev.value = 0;
_dt_ekf_avg = 0.001f * (float)(FILTER_UPDATE_PERRIOD_MS);
_dt_ekf_avg = 0.001f * (float)(FILTER_UPDATE_PERIOD_MS);
return ret;
}
@@ -214,7 +214,7 @@ bool Ekf::update()
// measurement updates
// Fuse magnetometer data using the selected fuson method and only if angular alignment is complete
// Fuse magnetometer data using the selected fusion method and only if angular alignment is complete
if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
if (_control_status.flags.mag_3D && _control_status.flags.yaw_align) {
fuseMag();
@@ -224,7 +224,7 @@ bool Ekf::update()
}
} else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) {
// fusion of a Euler yaw angle from either a 321 or 312 rotation sequence
// fusion of an Euler yaw angle from either a 321 or 312 rotation sequence
fuseHeading();
} else {
@@ -232,7 +232,7 @@ bool Ekf::update()
}
}
// determine if range finder data has fallen behind the fusin time horizon fuse it if we are
// determine if range finder data has fallen behind the fusion time horizon fuse it if we are
// not tilted too much to use it
if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)
&& (_R_to_earth(2, 2) > 0.7071f)) {
@@ -261,14 +261,14 @@ bool Ekf::update()
_fuse_height = true;
}
// determine if baro data has fallen behind the fuson time horizon and fuse it in the main filter if enabled
// determine if baro data has fallen behind the fusion time horizon and fuse it in the main filter if enabled
uint64_t last_baro_time_us = _baro_sample_delayed.time_us;
if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
if (_control_status.flags.baro_hgt) {
_fuse_height = true;
} else {
// calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference
// calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference
float local_time_step = 1e-6f*(float)(_baro_sample_delayed.time_us - last_baro_time_us);
local_time_step = math::constrain(local_time_step,0.0f,1.0f);
last_baro_time_us = _baro_sample_delayed.time_us;
@@ -589,13 +589,13 @@ void Ekf::predictState()
float input = 0.5f*(_imu_sample_delayed.delta_vel_dt + _imu_sample_delayed.delta_ang_dt);
// filter and limit input between -50% and +100% of nominal value
input = math::constrain(input,0.0005f * (float)(FILTER_UPDATE_PERRIOD_MS),0.002f * (float)(FILTER_UPDATE_PERRIOD_MS));
input = math::constrain(input,0.0005f * (float)(FILTER_UPDATE_PERIOD_MS),0.002f * (float)(FILTER_UPDATE_PERIOD_MS));
_dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * input;
}
bool Ekf::collect_imu(imuSample &imu)
{
// accumulate and downsample IMU data across a period FILTER_UPDATE_PERRIOD_MS long
// accumulate and downsample IMU data across a period FILTER_UPDATE_PERIOD_MS long
// copy imu data to local variables
_imu_sample_new.delta_ang = imu.delta_ang;
@@ -625,7 +625,7 @@ bool Ekf::collect_imu(imuSample &imu)
// if the target time delta between filter prediction steps has been exceeded
// write the accumulated IMU data to the ring buffer
float target_dt = (float)(FILTER_UPDATE_PERRIOD_MS) / 1000;
float target_dt = (float)(FILTER_UPDATE_PERIOD_MS) / 1000;
if (_imu_down_sampled.delta_ang_dt >= target_dt - _last_dt_overrun) {
// store the amount we have over-run the target update rate by
@@ -734,7 +734,7 @@ void Ekf::calculateOutputStates()
delta_ang_error(2) = scalar * q_error(3);
// calculate a gain that provides tight tracking of the estimator attitude states and
// adjust for changes in time delay to mantain consistent damping ratio of ~0.7
// adjust for changes in time delay to maintain consistent damping ratio of ~0.7
float time_delay = 1e-6f * (float)(_imu_sample_new.time_us - _imu_sample_delayed.time_us);
time_delay = fmaxf(time_delay, _dt_imu_avg);
float att_gain = 0.5f * _dt_imu_avg / time_delay;
@@ -747,13 +747,13 @@ void Ekf::calculateOutputStates()
float vel_gain = _dt_ekf_avg / math::constrain(_params.vel_Tau, _dt_ekf_avg, 10.0f);
float pos_gain = _dt_ekf_avg / math::constrain(_params.pos_Tau, _dt_ekf_avg, 10.0f);
// calculate velocity and position corrections at the EKF fusion time horizon
// calculate velocity and position corrections at the EKF fusion time horizon
Vector3f vel_delta = (_state.vel - _output_sample_delayed.vel) * vel_gain;
Vector3f pos_delta = (_state.pos - _output_sample_delayed.pos) * pos_gain;
// loop through the output filter state history and apply the corrections to the translational states
// this method is too expensive to use for the attitude states due to the quaternion operations required
// but does not introudce a time dela in the 'correction loop' and allows smaller trackin gtime constants
// but does not introudce a time dela in the 'correction loop' and allows smaller tracking time constants
// to be used
outputSample output_states;
unsigned output_length = _output_buffer.get_length();
EKF/ekf.h
+1 -1
View File
@@ -159,7 +159,7 @@ private:
uint64_t _time_last_arsp_fuse; // time the last fusion of airspeed measurements were performed (usec)
Vector2f _last_known_posNE; // last known local NE position vector (m)
float _last_disarmed_posD; // vertical position recorded at arming (m)
float _last_dt_overrun; // the amount of time the last IMU collection over-ran the target set by FILTER_UPDATE_PERRIOD_MS (sec)
float _last_dt_overrun; // the amount of time the last IMU collection over-ran the target set by FILTER_UPDATE_PERIOD_MS (sec)
Vector3f _earth_rate_NED; // earth rotation vector (NED) in rad/s
EKF/estimator_interface.cpp
+4 -4
View File
@@ -131,7 +131,7 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float *data)
magSample mag_sample_new = {};
mag_sample_new.time_us = time_usec - _params.mag_delay_ms * 1000;
mag_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2;
mag_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
_time_last_mag = time_usec;
@@ -148,7 +148,7 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps)
gpsSample gps_sample_new = {};
gps_sample_new.time_us = gps->time_usec - _params.gps_delay_ms * 1000;
gps_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2;
gps_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
_time_last_gps = time_usec;
gps_sample_new.time_us = math::max(gps_sample_new.time_us, _imu_sample_delayed.time_us);
@@ -191,7 +191,7 @@ void EstimatorInterface::setBaroData(uint64_t time_usec, float *data)
baro_sample_new.hgt = *data;
baro_sample_new.time_us = time_usec - _params.baro_delay_ms * 1000;
baro_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2;
baro_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
_time_last_baro = time_usec;
baro_sample_new.time_us = math::max(baro_sample_new.time_us, _imu_sample_delayed.time_us);
@@ -211,7 +211,7 @@ void EstimatorInterface::setAirspeedData(uint64_t time_usec, float *true_airspee
airspeed_sample_new.true_airspeed = *true_airspeed;
airspeed_sample_new.eas2tas = *eas2tas;
airspeed_sample_new.time_us = time_usec - _params.airspeed_delay_ms * 1000;
airspeed_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2; //typo PeRRiod
airspeed_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2; //typo PeRRiod
_time_last_airspeed = time_usec;
_airspeed_buffer.push(airspeed_sample_new);
EKF/estimator_interface.h
+1 -1
View File
@@ -235,7 +235,7 @@ protected:
static const uint8_t OBS_BUFFER_LENGTH = 10; // defines how many measurement samples we can buffer
static const uint8_t IMU_BUFFER_LENGTH = 30; // defines how many imu samples we can buffer
static const unsigned FILTER_UPDATE_PERRIOD_MS = 10; // ekf prediction period in milliseconds
static const unsigned FILTER_UPDATE_PERIOD_MS = 10; // ekf prediction period in milliseconds
float _dt_imu_avg; // average imu update period in s
EKF/mag_fusion.cpp
+3 -3
View File
@@ -86,7 +86,7 @@ void Ekf::fuseMag()
float H_MAG[3][24] = {};
float Kfusion[24] = {};
// Calculate observation Jacobians and kalman gains for each magentoemter axis
// Calculate observation Jacobians and kalman gains for each magnetometer axis
// X Axis
H_MAG[0][0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
H_MAG[0][1] = SH_MAG[0];
@@ -108,7 +108,7 @@ void Ekf::fuseMag()
_fault_status.flags.bad_mag_x = false;
} else {
// the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
_fault_status.flags.bad_mag_x = true;
// we need to reinitialise the covariance matrix and abort this fusion step
initialiseCovariance();
@@ -190,7 +190,7 @@ void Ekf::fuseMag()
return;
}
// update the states and covariance usinng sequential fusion of the magnetometer components
// update the states and covariance using sequential fusion of the magnetometer components
for (uint8_t index = 0; index <= 2; index++) {
// Calculate Kalman gains
if (index == 0) {
EKF/vel_pos_fusion.cpp
+1 -1
View File
@@ -46,7 +46,7 @@
void Ekf::fuseVelPosHeight()
{
bool fuse_map[6] = {}; // map of booelans true when [VN,VE,VD,PN,PE,PD] observations are available
bool fuse_map[6] = {}; // map of booleans true when [VN,VE,VD,PN,PE,PD] observations are available
bool innov_check_pass_map[6] = {}; // true when innovations consistency checks pass for [VN,VE,VD,PN,PE,PD] observations
float R[6] = {}; // observation variances for [VN,VE,VD,PN,PE,PD]
float gate_size[6] = {}; // innovation consistency check gate sizes for [VN,VE,VD,PN,PE,PD] observations