dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

AttPosEKF: Subscribe to vehicle armed status

This commit is contained in:
Johan Jansen 2015-03-03 13:28:21 +01:00
parent c434a6e097
commit 74a5dcdb1b
2 changed files with 15 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
View File

@ -143,6 +143,7 @@ private:
int _mission_sub; int _mission_sub;
int _home_sub; /**< home position as defined by commander / user */ int _home_sub; /**< home position as defined by commander / user */
int _landDetectorSub; int _landDetectorSub;
int _armedSub;
orb_advert_t _att_pub; /**< vehicle attitude */ orb_advert_t _att_pub; /**< vehicle attitude */
orb_advert_t _global_pos_pub; /**< global position */ orb_advert_t _global_pos_pub; /**< global position */
@ -163,6 +164,7 @@ private:
struct wind_estimate_s _wind; /**< wind estimate */ struct wind_estimate_s _wind; /**< wind estimate */
struct range_finder_report _distance; /**< distance estimate */ struct range_finder_report _distance; /**< distance estimate */
struct vehicle_land_detected_s _landDetector; struct vehicle_land_detected_s _landDetector;
struct actuator_armed_s _armed;
struct gyro_scale _gyro_offsets[3]; struct gyro_scale _gyro_offsets[3];
struct accel_scale _accel_offsets[3]; struct accel_scale _accel_offsets[3];

22
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -122,6 +122,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_mission_sub(-1), _mission_sub(-1),
_home_sub(-1), _home_sub(-1),
_landDetectorSub(-1), _landDetectorSub(-1),
_armedSub(-1),
/* publications */ /* publications */
_att_pub(-1), _att_pub(-1),
@ -143,6 +144,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_wind({}), _wind({}),
_distance{}, _distance{},
_landDetector{}, _landDetector{},
_armed{},
_gyro_offsets({}), _gyro_offsets({}),
_accel_offsets({}), _accel_offsets({}),
@ -334,7 +336,7 @@ int AttitudePositionEstimatorEKF::parameters_update()
_ekf->posDSigma = _parameters.posd_noise; _ekf->posDSigma = _parameters.posd_noise;
_ekf->magMeasurementSigma = _parameters.mag_noise; _ekf->magMeasurementSigma = _parameters.mag_noise;
_ekf->gyroProcessNoise = _parameters.gyro_pnoise; _ekf->gyroProcessNoise = _parameters.gyro_pnoise;
_ekf->accelProcessNoise = _parameters.acc_pnoise; _ekf->accelProcessNoise = _parameters.acc_pnoise;
_ekf->airspeedMeasurementSigma = _parameters.eas_noise; _ekf->airspeedMeasurementSigma = _parameters.eas_noise;
_ekf->rngFinderPitch = 0.0f; // XXX base on SENS_BOARD_Y_OFF _ekf->rngFinderPitch = 0.0f; // XXX base on SENS_BOARD_Y_OFF
} }
@ -497,6 +499,7 @@ void AttitudePositionEstimatorEKF::task_main()
_params_sub = orb_subscribe(ORB_ID(parameter_update)); _params_sub = orb_subscribe(ORB_ID(parameter_update));
_home_sub = orb_subscribe(ORB_ID(home_position)); _home_sub = orb_subscribe(ORB_ID(home_position));
_landDetectorSub = orb_subscribe(ORB_ID(vehicle_land_detected)); _landDetectorSub = orb_subscribe(ORB_ID(vehicle_land_detected));
_armedSub = orb_subscribe(ORB_ID(actuator_armed));
/* rate limit vehicle status updates to 5Hz */ /* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vstatus_sub, 200); orb_set_interval(_vstatus_sub, 200);
@ -1096,11 +1099,18 @@ void AttitudePositionEstimatorEKF::print_status()
void AttitudePositionEstimatorEKF::pollData() void AttitudePositionEstimatorEKF::pollData()
{ {
//Update arming status
bool armedUpdate;
orb_check(_armedSub, &armedUpdate);
if(armedUpdate) {
orb_copy(ORB_ID(actuator_armed), _armedSub, &_armed);
}
//Update Gyro and Accelerometer
static Vector3f lastAngRate; static Vector3f lastAngRate;
static Vector3f lastAccel; static Vector3f lastAccel;
bool accel_updated = false; bool accel_updated = false;
//Update Gyro and Accelerometer
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
static hrt_abstime last_accel = 0; static hrt_abstime last_accel = 0;
@ -1413,12 +1423,7 @@ int AttitudePositionEstimatorEKF::trip_nan() {
int ret = 0; int ret = 0;
// If system is not armed, inject a NaN value into the filter // If system is not armed, inject a NaN value into the filter
int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); if (_armed.armed) {
struct actuator_armed_s armed;
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
if (armed.armed) {
warnx("ACTUATORS ARMED! NOT TRIPPING SYSTEM"); warnx("ACTUATORS ARMED! NOT TRIPPING SYSTEM");
ret = 1; ret = 1;
} else { } else {
@ -1453,7 +1458,6 @@ int AttitudePositionEstimatorEKF::trip_nan() {
print_status(); print_status();
} }
close(armed_sub);
return ret; return ret;
} }