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

delete control_state and cleanup vehicle_attitude (#7882)

Browse Source
This commit is contained in:
Daniel Agar
2017-09-21 16:24:53 -04:00
committed by GitHub
parent 5bea264a5f
commit b4755297ec
42 changed files with 776 additions and 1061 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/examples/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+2 -11
View File
@@ -49,7 +49,6 @@
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/actuator_controls.h>
@@ -155,14 +154,12 @@ private:
int _armedSub;
orb_advert_t _att_pub; /**< vehicle attitude */
orb_advert_t _ctrl_state_pub; /**< control state */
orb_advert_t _global_pos_pub; /**< global position */
orb_advert_t _local_pos_pub; /**< position in local frame */
orb_advert_t _estimator_status_pub; /**< status of the estimator */
orb_advert_t _wind_pub; /**< wind estimate */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct control_state_s _ctrl_state; /**< control state */
struct gyro_report _gyro;
struct accel_report _accel;
struct mag_report _mag;
@@ -244,7 +241,7 @@ private:
float magb_pnoise;
float eas_noise;
float pos_stddev_threshold;
int32_t airspeed_mode;
int32_t airspeed_disabled;
} _parameters; /**< local copies of interesting parameters */
struct {
@@ -266,7 +263,7 @@ private:
param_t magb_pnoise;
param_t eas_noise;
param_t pos_stddev_threshold;
param_t airspeed_mode;
param_t airspeed_disabled;
} _parameter_handles; /**< handles for interesting parameters */
AttPosEKF *_ekf;
@@ -323,12 +320,6 @@ private:
**/
void publishAttitude();
/**
* @brief
* Publish the system state for control modules
**/
void publishControlState();
/**
* @brief
* Publish local position relative to reference point where filter was initialized
src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+6 -107
View File
@@ -142,14 +142,12 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
/* publications */
_att_pub(nullptr),
_ctrl_state_pub(nullptr),
_global_pos_pub(nullptr),
_local_pos_pub(nullptr),
_estimator_status_pub(nullptr),
_wind_pub(nullptr),
_att{},
_ctrl_state{},
_gyro{},
_accel{},
_mag{},
@@ -248,7 +246,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_parameter_handles.magb_pnoise = param_find("PE_MAGB_PNOISE");
_parameter_handles.eas_noise = param_find("PE_EAS_NOISE");
_parameter_handles.pos_stddev_threshold = param_find("PE_POSDEV_INIT");
_parameter_handles.airspeed_mode = param_find("FW_AIRSPD_MODE");
_parameter_handles.airspeed_disabled = param_find("FW_AIRSPD_MODE");
/* indicate consumers that the current position data is not valid */
_gps.eph = 10000.0f;
@@ -316,7 +314,7 @@ int AttitudePositionEstimatorEKF::parameters_update()
param_get(_parameter_handles.magb_pnoise, &(_parameters.magb_pnoise));
param_get(_parameter_handles.eas_noise, &(_parameters.eas_noise));
param_get(_parameter_handles.pos_stddev_threshold, &(_parameters.pos_stddev_threshold));
param_get(_parameter_handles.airspeed_mode, &_parameters.airspeed_mode);
param_get(_parameter_handles.airspeed_disabled, &_parameters.airspeed_disabled);
if (_ekf) {
// _ekf->yawVarScale = 1.0f;
@@ -715,9 +713,6 @@ void AttitudePositionEstimatorEKF::task_main()
// Publish attitude estimations
publishAttitude();
// publish control state
publishControlState();
// Publish Local Position estimations
publishLocalPosition();
@@ -841,100 +836,6 @@ void AttitudePositionEstimatorEKF::publishAttitude()
}
}
void AttitudePositionEstimatorEKF::publishControlState()
{
/* Accelerations in Body Frame */
_ctrl_state.x_acc = _ekf->accel.x;
_ctrl_state.y_acc = _ekf->accel.y;
_ctrl_state.z_acc = _ekf->accel.z - _ekf->states[13];
_ctrl_state.horz_acc_mag = _ekf->getAccNavMagHorizontal();
/* Velocity in Body Frame */
Vector3f v_n(_ekf->states[4], _ekf->states[5], _ekf->states[6]);
Vector3f v_n_var(_ekf->P[4][4], _ekf->P[5][5], _ekf->P[6][6]);
Vector3f v_b = _ekf->Tnb * v_n;
Vector3f v_b_var = _ekf->Tnb * v_n_var;
_ctrl_state.x_vel = v_b.x;
_ctrl_state.y_vel = v_b.y;
_ctrl_state.z_vel = v_b.z;
_ctrl_state.vel_variance[0] = v_b_var.x;
_ctrl_state.vel_variance[1] = v_b_var.y;
_ctrl_state.vel_variance[2] = v_b_var.z;
/* Local Position */
_ctrl_state.x_pos = _ekf->states[7];
_ctrl_state.y_pos = _ekf->states[8];
// XXX need to announce change of Z reference somehow elegantly
_ctrl_state.z_pos = _ekf->states[9] - _filter_ref_offset;
_ctrl_state.pos_variance[0] = _ekf->P[7][7];
_ctrl_state.pos_variance[1] = _ekf->P[8][8];
_ctrl_state.pos_variance[2] = _ekf->P[9][9];
/* Attitude */
_ctrl_state.timestamp = _last_sensor_timestamp;
_ctrl_state.q[0] = _ekf->states[0];
_ctrl_state.q[1] = _ekf->states[1];
_ctrl_state.q[2] = _ekf->states[2];
_ctrl_state.q[3] = _ekf->states[3];
// use estimated velocity for airspeed estimate
if (_parameters.airspeed_mode == control_state_s::AIRSPD_MODE_MEAS) {
// use measured airspeed
if (PX4_ISFINITE(_airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - _airspeed.timestamp < 1e6
&& _airspeed.timestamp > 0) {
_ctrl_state.airspeed = _airspeed.indicated_airspeed_m_s;
_ctrl_state.airspeed_valid = true;
}
} else if (_parameters.airspeed_mode == control_state_s::AIRSPD_MODE_EST) {
if (_local_pos.v_xy_valid && _local_pos.v_z_valid) {
_ctrl_state.airspeed = sqrtf(_ekf->states[4] * _ekf->states[4]
+ _ekf->states[5] * _ekf->states[5] + _ekf->states[6] * _ekf->states[6]);
_ctrl_state.airspeed_valid = true;
}
} else if (_parameters.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED) {
// do nothing, airspeed has been declared as non-valid above, controllers
// will handle this assuming always trim airspeed
}
/* Attitude Rates */
_ctrl_state.roll_rate = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt;
_ctrl_state.pitch_rate = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt;
_ctrl_state.yaw_rate = _LP_att_R.apply(_ekf->dAngIMU.z / _ekf->dtIMU) - _ekf->states[12] / _ekf->dtIMUfilt;
/* Gyro bias estimates */
_ctrl_state.roll_rate_bias = _ekf->states[10] / _ekf->dtIMUfilt;
_ctrl_state.pitch_rate_bias = _ekf->states[11] / _ekf->dtIMUfilt;
_ctrl_state.yaw_rate_bias = _ekf->states[12] / _ekf->dtIMUfilt;
/* Guard from bad data */
if (!PX4_ISFINITE(_ctrl_state.x_vel) ||
!PX4_ISFINITE(_ctrl_state.y_vel) ||
!PX4_ISFINITE(_ctrl_state.z_vel) ||
!PX4_ISFINITE(_ctrl_state.x_pos) ||
!PX4_ISFINITE(_ctrl_state.y_pos) ||
!PX4_ISFINITE(_ctrl_state.z_pos)) {
// bad data, abort publication
return;
}
/* lazily publish the control state only once available */
if (_ctrl_state_pub != nullptr) {
/* publish the control state */
orb_publish(ORB_ID(control_state), _ctrl_state_pub, &_ctrl_state);
} else {
/* advertise and publish */
_ctrl_state_pub = orb_advertise(ORB_ID(control_state), &_ctrl_state);
}
}
void AttitudePositionEstimatorEKF::publishLocalPosition()
{
_local_pos.timestamp = _last_sensor_timestamp;
@@ -998,12 +899,10 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon);
_global_pos.lat = est_lat;
_global_pos.lon = est_lon;
_global_pos.time_utc_usec = _gps.time_utc_usec;
} else {
_global_pos.lat = 0.0;
_global_pos.lon = 0.0;
_global_pos.time_utc_usec = 0;
}
if (_local_pos.v_xy_valid) {
@@ -1087,10 +986,10 @@ void AttitudePositionEstimatorEKF::publishWindEstimate()
_wind.windspeed_north = _ekf->states[14];
_wind.windspeed_east = _ekf->states[15];
// XXX we need to do something smart about the covariance here
// but we default to the estimate covariance for now
_wind.covariance_north = _ekf->P[14][14];
_wind.covariance_east = _ekf->P[15][15];
// XXX we need to do something smart about the variance here
// but we default to the estimated variance for now
_wind.variance_north = _ekf->P[14][14];
_wind.variance_east = _ekf->P[15][15];
/* lazily publish the wind estimate only once available */
if (_wind_pub != nullptr) {
src/examples/segway/CMakeLists.txt
+2 -2
View File
@@ -33,8 +33,8 @@
px4_add_module(
MODULE examples__segway
MAIN segway
STACK_MAIN 1300
STACK_MAX 1300
STACK_MAIN 1400
STACK_MAX 1400
SRCS
blocks.cpp
segway_main.cpp
src/examples/uuv_example_app/uuv_example_app.cpp
+6 -6
View File
@@ -64,7 +64,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h> // this topics hold the acceleration data
#include <uORB/topics/actuator_controls.h> // this topic gives the actuators control input
#include <uORB/topics/control_state.h> // this topic holds the orientation of the hippocampus
#include <uORB/topics/vehicle_attitude.h> // this topic holds the orientation of the hippocampus
extern "C" __EXPORT int uuv_example_app_main(int argc, char *argv[]);
@@ -78,9 +78,9 @@ int uuv_example_app_main(int argc, char *argv[])
orb_set_interval(sensor_sub_fd, 200);
/* subscribe to control_state topic */
int ctrl_state_sub_fd = orb_subscribe(ORB_ID(control_state));
int vehicle_attitude_sub_fd = orb_subscribe(ORB_ID(vehicle_attitude));
/* limit the update rate to 5 Hz */
orb_set_interval(ctrl_state_sub_fd, 200);
orb_set_interval(vehicle_attitude_sub_fd, 200);
/* advertise to actuator_control topic */
struct actuator_controls_s act;
@@ -90,7 +90,7 @@ int uuv_example_app_main(int argc, char *argv[])
/* one could wait for multiple topics with this technique, just using one here */
px4_pollfd_struct_t fds[] = {
{ .fd = sensor_sub_fd, .events = POLLIN },
{ .fd = ctrl_state_sub_fd, .events = POLLIN },
{ .fd = vehicle_attitude_sub_fd, .events = POLLIN },
};
int error_counter = 0;
@@ -127,9 +127,9 @@ int uuv_example_app_main(int argc, char *argv[])
(double)raw_sensor.accelerometer_m_s2[2]);
/* obtained data for the third file descriptor */
struct control_state_s raw_ctrl_state;
struct vehicle_attitude_s raw_ctrl_state;
/* copy sensors raw data into local buffer */
orb_copy(ORB_ID(control_state), ctrl_state_sub_fd, &raw_ctrl_state);
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub_fd, &raw_ctrl_state);
// get current rotation matrix from control state quaternions, the quaternions are generated by the
// attitude_estimator_q application using the sensor data