mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-01 08:51:35 +08:00
ekf2_main: add {} to limit variable scope and save stack
Reformatting makes it look worse than it is, but it really only adds {}
This commit is contained in:
Beat Küng
+105
-99
@@ -627,105 +627,109 @@ void Ekf2::task_main()
|
|||||||
|
|
||||||
float gyro_rad[3];
|
float gyro_rad[3];
|
||||||
|
|
||||||
// generate control state data
|
{
|
||||||
control_state_s ctrl_state = {};
|
// generate control state data
|
||||||
float gyro_bias[3] = {};
|
control_state_s ctrl_state = {};
|
||||||
_ekf.get_gyro_bias(gyro_bias);
|
float gyro_bias[3] = {};
|
||||||
ctrl_state.timestamp = hrt_absolute_time();
|
_ekf.get_gyro_bias(gyro_bias);
|
||||||
gyro_rad[0] = sensors.gyro_rad[0] - gyro_bias[0];
|
ctrl_state.timestamp = hrt_absolute_time();
|
||||||
gyro_rad[1] = sensors.gyro_rad[1] - gyro_bias[1];
|
gyro_rad[0] = sensors.gyro_rad[0] - gyro_bias[0];
|
||||||
gyro_rad[2] = sensors.gyro_rad[2] - gyro_bias[2];
|
gyro_rad[1] = sensors.gyro_rad[1] - gyro_bias[1];
|
||||||
ctrl_state.roll_rate = _lp_roll_rate.apply(gyro_rad[0]);
|
gyro_rad[2] = sensors.gyro_rad[2] - gyro_bias[2];
|
||||||
ctrl_state.pitch_rate = _lp_pitch_rate.apply(gyro_rad[1]);
|
ctrl_state.roll_rate = _lp_roll_rate.apply(gyro_rad[0]);
|
||||||
ctrl_state.yaw_rate = _lp_yaw_rate.apply(gyro_rad[2]);
|
ctrl_state.pitch_rate = _lp_pitch_rate.apply(gyro_rad[1]);
|
||||||
|
ctrl_state.yaw_rate = _lp_yaw_rate.apply(gyro_rad[2]);
|
||||||
|
|
||||||
// Velocity in body frame
|
// Velocity in body frame
|
||||||
Vector3f v_n(velocity);
|
Vector3f v_n(velocity);
|
||||||
matrix::Dcm<float> R_to_body(q.inversed());
|
matrix::Dcm<float> R_to_body(q.inversed());
|
||||||
Vector3f v_b = R_to_body * v_n;
|
Vector3f v_b = R_to_body * v_n;
|
||||||
ctrl_state.x_vel = v_b(0);
|
ctrl_state.x_vel = v_b(0);
|
||||||
ctrl_state.y_vel = v_b(1);
|
ctrl_state.y_vel = v_b(1);
|
||||||
ctrl_state.z_vel = v_b(2);
|
ctrl_state.z_vel = v_b(2);
|
||||||
|
|
||||||
|
|
||||||
// Local Position NED
|
// Local Position NED
|
||||||
float position[3];
|
float position[3];
|
||||||
_ekf.get_position(position);
|
_ekf.get_position(position);
|
||||||
ctrl_state.x_pos = position[0];
|
ctrl_state.x_pos = position[0];
|
||||||
ctrl_state.y_pos = position[1];
|
ctrl_state.y_pos = position[1];
|
||||||
ctrl_state.z_pos = position[2];
|
ctrl_state.z_pos = position[2];
|
||||||
|
|
||||||
// Attitude quaternion
|
// Attitude quaternion
|
||||||
ctrl_state.q[0] = q(0);
|
ctrl_state.q[0] = q(0);
|
||||||
ctrl_state.q[1] = q(1);
|
ctrl_state.q[1] = q(1);
|
||||||
ctrl_state.q[2] = q(2);
|
ctrl_state.q[2] = q(2);
|
||||||
ctrl_state.q[3] = q(3);
|
ctrl_state.q[3] = q(3);
|
||||||
|
|
||||||
// Acceleration data
|
// Acceleration data
|
||||||
matrix::Vector<float, 3> acceleration(sensors.accelerometer_m_s2);
|
matrix::Vector<float, 3> acceleration(sensors.accelerometer_m_s2);
|
||||||
|
|
||||||
float accel_bias[3];
|
float accel_bias[3];
|
||||||
_ekf.get_accel_bias(accel_bias);
|
_ekf.get_accel_bias(accel_bias);
|
||||||
ctrl_state.x_acc = acceleration(0) - accel_bias[0];
|
ctrl_state.x_acc = acceleration(0) - accel_bias[0];
|
||||||
ctrl_state.y_acc = acceleration(1) - accel_bias[1];
|
ctrl_state.y_acc = acceleration(1) - accel_bias[1];
|
||||||
ctrl_state.z_acc = acceleration(2) - accel_bias[2];
|
ctrl_state.z_acc = acceleration(2) - accel_bias[2];
|
||||||
|
|
||||||
// compute lowpass filtered horizontal acceleration
|
// compute lowpass filtered horizontal acceleration
|
||||||
acceleration = R_to_body.transpose() * acceleration;
|
acceleration = R_to_body.transpose() * acceleration;
|
||||||
_acc_hor_filt = 0.95f * _acc_hor_filt + 0.05f * sqrtf(acceleration(0) * acceleration(0) +
|
_acc_hor_filt = 0.95f * _acc_hor_filt + 0.05f * sqrtf(acceleration(0) * acceleration(0) +
|
||||||
acceleration(1) * acceleration(1));
|
acceleration(1) * acceleration(1));
|
||||||
ctrl_state.horz_acc_mag = _acc_hor_filt;
|
ctrl_state.horz_acc_mag = _acc_hor_filt;
|
||||||
|
|
||||||
ctrl_state.airspeed_valid = false;
|
ctrl_state.airspeed_valid = false;
|
||||||
|
|
||||||
// use estimated velocity for airspeed estimate
|
// use estimated velocity for airspeed estimate
|
||||||
if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_MEAS) {
|
if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_MEAS) {
|
||||||
// use measured airspeed
|
// use measured airspeed
|
||||||
if (PX4_ISFINITE(airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - airspeed.timestamp < 1e6
|
if (PX4_ISFINITE(airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - airspeed.timestamp < 1e6
|
||||||
&& airspeed.timestamp > 0) {
|
&& airspeed.timestamp > 0) {
|
||||||
ctrl_state.airspeed = airspeed.indicated_airspeed_m_s;
|
ctrl_state.airspeed = airspeed.indicated_airspeed_m_s;
|
||||||
ctrl_state.airspeed_valid = true;
|
ctrl_state.airspeed_valid = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
} else if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_EST) {
|
||||||
|
if (_ekf.local_position_is_valid()) {
|
||||||
|
ctrl_state.airspeed = sqrtf(velocity[0] * velocity[0] + velocity[1] * velocity[1] + velocity[2] * velocity[2]);
|
||||||
|
ctrl_state.airspeed_valid = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
} else if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_DISABLED) {
|
||||||
|
// do nothing, airspeed has been declared as non-valid above, controllers
|
||||||
|
// will handle this assuming always trim airspeed
|
||||||
}
|
}
|
||||||
|
|
||||||
} else if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_EST) {
|
// publish control state data
|
||||||
if (_ekf.local_position_is_valid()) {
|
if (_control_state_pub == nullptr) {
|
||||||
ctrl_state.airspeed = sqrtf(velocity[0] * velocity[0] + velocity[1] * velocity[1] + velocity[2] * velocity[2]);
|
_control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);
|
||||||
ctrl_state.airspeed_valid = true;
|
|
||||||
|
} else {
|
||||||
|
orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state);
|
||||||
}
|
}
|
||||||
|
|
||||||
} else if (_airspeed_mode.get() == control_state_s::AIRSPD_MODE_DISABLED) {
|
|
||||||
// do nothing, airspeed has been declared as non-valid above, controllers
|
|
||||||
// will handle this assuming always trim airspeed
|
|
||||||
}
|
|
||||||
|
|
||||||
// publish control state data
|
|
||||||
if (_control_state_pub == nullptr) {
|
|
||||||
_control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);
|
|
||||||
|
|
||||||
} else {
|
|
||||||
orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
// generate vehicle attitude quaternion data
|
{
|
||||||
struct vehicle_attitude_s att = {};
|
// generate vehicle attitude quaternion data
|
||||||
att.timestamp = hrt_absolute_time();
|
struct vehicle_attitude_s att = {};
|
||||||
|
att.timestamp = hrt_absolute_time();
|
||||||
|
|
||||||
att.q[0] = q(0);
|
att.q[0] = q(0);
|
||||||
att.q[1] = q(1);
|
att.q[1] = q(1);
|
||||||
att.q[2] = q(2);
|
att.q[2] = q(2);
|
||||||
att.q[3] = q(3);
|
att.q[3] = q(3);
|
||||||
|
|
||||||
att.rollspeed = gyro_rad[0];
|
att.rollspeed = gyro_rad[0];
|
||||||
att.pitchspeed = gyro_rad[1];
|
att.pitchspeed = gyro_rad[1];
|
||||||
att.yawspeed = gyro_rad[2];
|
att.yawspeed = gyro_rad[2];
|
||||||
|
|
||||||
// publish vehicle attitude data
|
// publish vehicle attitude data
|
||||||
if (_att_pub == nullptr) {
|
if (_att_pub == nullptr) {
|
||||||
_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
|
_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
|
orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// generate vehicle local position data
|
// generate vehicle local position data
|
||||||
@@ -884,29 +888,31 @@ void Ekf2::task_main()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// publish estimator innovation data
|
// publish estimator innovation data
|
||||||
struct ekf2_innovations_s innovations = {};
|
{
|
||||||
innovations.timestamp = hrt_absolute_time();
|
struct ekf2_innovations_s innovations = {};
|
||||||
_ekf.get_vel_pos_innov(&innovations.vel_pos_innov[0]);
|
innovations.timestamp = hrt_absolute_time();
|
||||||
_ekf.get_mag_innov(&innovations.mag_innov[0]);
|
_ekf.get_vel_pos_innov(&innovations.vel_pos_innov[0]);
|
||||||
_ekf.get_heading_innov(&innovations.heading_innov);
|
_ekf.get_mag_innov(&innovations.mag_innov[0]);
|
||||||
_ekf.get_airspeed_innov(&innovations.airspeed_innov);
|
_ekf.get_heading_innov(&innovations.heading_innov);
|
||||||
_ekf.get_flow_innov(&innovations.flow_innov[0]);
|
_ekf.get_airspeed_innov(&innovations.airspeed_innov);
|
||||||
_ekf.get_hagl_innov(&innovations.hagl_innov);
|
_ekf.get_flow_innov(&innovations.flow_innov[0]);
|
||||||
|
_ekf.get_hagl_innov(&innovations.hagl_innov);
|
||||||
|
|
||||||
_ekf.get_vel_pos_innov_var(&innovations.vel_pos_innov_var[0]);
|
_ekf.get_vel_pos_innov_var(&innovations.vel_pos_innov_var[0]);
|
||||||
_ekf.get_mag_innov_var(&innovations.mag_innov_var[0]);
|
_ekf.get_mag_innov_var(&innovations.mag_innov_var[0]);
|
||||||
_ekf.get_heading_innov_var(&innovations.heading_innov_var);
|
_ekf.get_heading_innov_var(&innovations.heading_innov_var);
|
||||||
_ekf.get_airspeed_innov_var(&innovations.airspeed_innov_var);
|
_ekf.get_airspeed_innov_var(&innovations.airspeed_innov_var);
|
||||||
_ekf.get_flow_innov_var(&innovations.flow_innov_var[0]);
|
_ekf.get_flow_innov_var(&innovations.flow_innov_var[0]);
|
||||||
_ekf.get_hagl_innov_var(&innovations.hagl_innov_var);
|
_ekf.get_hagl_innov_var(&innovations.hagl_innov_var);
|
||||||
|
|
||||||
_ekf.get_output_tracking_error(&innovations.output_tracking_error[0]);
|
_ekf.get_output_tracking_error(&innovations.output_tracking_error[0]);
|
||||||
|
|
||||||
if (_estimator_innovations_pub == nullptr) {
|
if (_estimator_innovations_pub == nullptr) {
|
||||||
_estimator_innovations_pub = orb_advertise(ORB_ID(ekf2_innovations), &innovations);
|
_estimator_innovations_pub = orb_advertise(ORB_ID(ekf2_innovations), &innovations);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
orb_publish(ORB_ID(ekf2_innovations), _estimator_innovations_pub, &innovations);
|
orb_publish(ORB_ID(ekf2_innovations), _estimator_innovations_pub, &innovations);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// save the declination to the EKF2_MAG_DECL parameter when a land event is detected
|
// save the declination to the EKF2_MAG_DECL parameter when a land event is detected
|
||||||
|
|||||||
Reference in New Issue
Block a user