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

commander: internalize system status bools

Browse Source 
Most condition bools in the commander are not used anywhere but in the
commander. It therefore makes sense to move them to a different internal
struct and remove them from the vehicle_status message.

Also, the land_detected should be used by all the modules instead of
getting it through the commander and system_status.
This commit is contained in:
Julian Oes
2016-04-11 18:01:47 +02:00
parent 705979e3c7
commit 1f44fb1efd
18 changed files with 450 additions and 332 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+11 -5
View File
@@ -50,9 +50,9 @@
#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>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/home_position.h>
@@ -147,7 +147,8 @@ private:
int _airspeed_sub; /**< airspeed subscription */
int _baro_sub; /**< barometer subscription */
int _gps_sub; /**< GPS subscription */
int _vstatus_sub; /**< vehicle status subscription */
int _vehicle_status_sub;
int _vehicle_land_detected_sub;
int _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control updates */
int _mission_sub;
@@ -169,13 +170,13 @@ private:
struct mag_report _mag;
struct airspeed_s _airspeed; /**< airspeed */
struct baro_report _baro; /**< baro readings */
struct vehicle_status_s _vstatus; /**< vehicle status */
struct vehicle_status_s _vehicle_status;
struct vehicle_land_detected_s _vehicle_land_detected;
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
struct vehicle_local_position_s _local_pos; /**< local vehicle position */
struct vehicle_gps_position_s _gps; /**< GPS position */
struct wind_estimate_s _wind; /**< wind estimate */
struct distance_sensor_s _distance; /**< distance estimate */
struct vehicle_land_detected_s _landDetector;
struct actuator_armed_s _armed;
hrt_abstime _last_accel;
@@ -299,10 +300,15 @@ private:
void control_update();
/**
* Check for changes in vehicle status.
* Check for changes in land detected.
*/
void vehicle_status_poll();
/**
* Check for changes in land detected.
*/
void vehicle_land_detected_poll();
/**
* Shim for calling task_main from task_create.
*/
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+29 -27
View File
@@ -132,12 +132,12 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_airspeed_sub(-1),
_baro_sub(-1),
_gps_sub(-1),
_vstatus_sub(-1),
_vehicle_status_sub(-1),
_vehicle_land_detected_sub(-1),
_params_sub(-1),
_manual_control_sub(-1),
_mission_sub(-1),
_home_sub(-1),
_landDetectorSub(-1),
_armedSub(-1),
/* publications */
@@ -155,13 +155,13 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_mag{},
_airspeed{},
_baro{},
_vstatus{},
_vehicle_status{},
_vehicle_land_detected{},
_global_pos{},
_local_pos{},
_gps{},
_wind{},
_distance{},
_landDetector{},
_armed{},
_last_accel(0),
@@ -346,22 +346,31 @@ int AttitudePositionEstimatorEKF::parameters_update()
void AttitudePositionEstimatorEKF::vehicle_status_poll()
{
bool vstatus_updated;
bool updated;
/* Check HIL state if vehicle status has changed */
orb_check(_vstatus_sub, &vstatus_updated);
orb_check(_vehicle_status_sub, &updated);
bool landed = _vstatus.condition_landed;
if (updated) {
if (vstatus_updated) {
orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus);
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
// Tell EKF that the vehicle is a fixed wing or multi-rotor
_ekf->setIsFixedWing(!_vstatus.is_rotary_wing);
_ekf->setIsFixedWing(!_vehicle_status.is_rotary_wing);
}
}
void AttitudePositionEstimatorEKF::vehicle_land_detected_poll()
{
bool updated;
orb_check(_vehicle_land_detected_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &_vehicle_land_detected);
// Save params on landed
if (!landed && _vstatus.condition_landed) {
if (!_vehicle_land_detected.landed) {
_mag_offset_x.set(_ekf->magBias.x);
_mag_offset_x.commit();
_mag_offset_y.set(_ekf->magBias.y);
@@ -526,10 +535,10 @@ void AttitudePositionEstimatorEKF::task_main()
_baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0);
_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_home_sub = orb_subscribe(ORB_ID(home_position));
_landDetectorSub = orb_subscribe(ORB_ID(vehicle_land_detected));
_armedSub = orb_subscribe(ORB_ID(actuator_armed));
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
@@ -586,13 +595,14 @@ void AttitudePositionEstimatorEKF::task_main()
if (fds[1].revents & POLLIN) {
/* check vehicle status for changes to publication state */
bool prev_hil = (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON);
bool prev_hil = (_vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON);
vehicle_status_poll();
vehicle_land_detected_poll();
perf_count(_perf_gyro);
/* Reset baro reference if switching to HIL, reset sensor states */
if (!prev_hil && (_vstatus.hil_state == vehicle_status_s::HIL_STATE_ON)) {
if (!prev_hil && (_vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON)) {
/* system is in HIL now, wait for measurements to come in one last round */
usleep(60000);
@@ -690,7 +700,7 @@ void AttitudePositionEstimatorEKF::task_main()
}
// Check if on ground - status is used by covariance prediction
_ekf->setOnGround(_landDetector.landed);
_ekf->setOnGround(_vehicle_land_detected.landed);
// We're apparently initialized in this case now
// check (and reset the filter as needed)
@@ -1305,7 +1315,7 @@ void AttitudePositionEstimatorEKF::print_status()
PX4_INFO("states: %s %s %s %s %s %s %s %s %s %s",
(_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT",
(_landDetector.landed) ? "ON_GROUND" : "AIRBORNE",
(_vehicle_land_detected.landed) ? "ON_GROUND" : "AIRBORNE",
(_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL",
(_ekf->fusePosData) ? "FUSE_POS" : "INH_POS",
(_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT",
@@ -1520,14 +1530,6 @@ void AttitudePositionEstimatorEKF::pollData()
//PX4_INFO("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z);
//Update Land Detector
bool newLandData;
orb_check(_landDetectorSub, &newLandData);
if (newLandData) {
orb_copy(ORB_ID(vehicle_land_detected), _landDetectorSub, &_landDetector);
}
//Update AirSpeed
orb_check(_airspeed_sub, &_newAdsData);