zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Plane: move various sensor status flag updates up

master
Peter Barker 6 years ago committed by Randy Mackay
parent
173611e819
commit
d0bf257357
3 changed files with 38 additions and 89 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 3
      ArduPlane/GCS_Mavlink.h
  2. 122
      ArduPlane/GCS_Plane.cpp
  3. 2
      ArduPlane/GCS_Plane.h

3
ArduPlane/GCS_Mavlink.h
Unescape View File

@ -2,9 +2,6 @@ @@ -2,9 +2,6 @@
#include <GCS_MAVLink/GCS.h>
// default sensors are present and healthy: gyro, accelerometer, barometer, rate_control, attitude_stabilization, yaw_position, altitude control, x/y position control, motor_control
#define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS | MAV_SYS_STATUS_AHRS | MAV_SYS_STATUS_SENSOR_RC_RECEIVER | MAV_SYS_STATUS_SENSOR_BATTERY)
class GCS_MAVLINK_Plane : public GCS_MAVLINK
{

122
ArduPlane/GCS_Plane.cpp
Unescape View File

@ -1,18 +1,12 @@ @@ -1,18 +1,12 @@
#include "GCS_Plane.h"
#include "Plane.h"
// update error mask of sensors and subsystems. The mask
// uses the MAV_SYS_STATUS_* values from mavlink. If a bit is set
// then it indicates that the sensor or subsystem is present but
// not functioning correctly.
void GCS_Plane::update_sensor_status_flags(void)
void GCS_Plane::update_vehicle_sensor_status_flags(void)
{
// default sensors present
control_sensors_present = MAVLINK_SENSOR_PRESENT_DEFAULT;
// first what sensors/controllers we have
if (plane.g.compass_enabled) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; // compass present
control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG;
}
const AP_Airspeed *airspeed = AP_Airspeed::get_singleton();
@ -22,11 +16,13 @@ void GCS_Plane::update_sensor_status_flags(void) @@ -22,11 +16,13 @@ void GCS_Plane::update_sensor_status_flags(void)
const AP_GPS &gps = AP::gps();
if (gps.status() > AP_GPS::NO_GPS) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_GPS;
}
#if OPTFLOW == ENABLED
const OpticalFlow *optflow = AP::opticalflow();
if (optflow && optflow->enabled()) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
}
#endif
if (plane.geofence_present()) {
@ -36,13 +32,6 @@ void GCS_Plane::update_sensor_status_flags(void) @@ -36,13 +32,6 @@ void GCS_Plane::update_sensor_status_flags(void)
if (plane.have_reverse_thrust()) {
control_sensors_present |= MAV_SYS_STATUS_REVERSE_MOTOR;
}
const AP_Logger &logger = AP::logger();
if (logger.logging_present()) { // primary logging only (usually File)
control_sensors_present |= MAV_SYS_STATUS_LOGGING;
}
// all present sensors enabled by default except rate control, attitude stabilization, yaw, altitude, position control, geofence, motor, and battery output which we will set individually
control_sensors_enabled = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL & ~MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION & ~MAV_SYS_STATUS_SENSOR_YAW_POSITION & ~MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL & ~MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL & ~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS & ~MAV_SYS_STATUS_GEOFENCE & ~MAV_SYS_STATUS_LOGGING & ~MAV_SYS_STATUS_SENSOR_BATTERY);
if (airspeed && airspeed->enabled() && airspeed->use()) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
@ -52,22 +41,22 @@ void GCS_Plane::update_sensor_status_flags(void) @@ -52,22 +41,22 @@ void GCS_Plane::update_sensor_status_flags(void)
control_sensors_enabled |= MAV_SYS_STATUS_GEOFENCE;
}
if (logger.logging_enabled()) {
control_sensors_enabled |= MAV_SYS_STATUS_LOGGING;
}
const AP_BattMonitor &battery = AP::battery();
if (battery.num_instances() > 0) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_BATTERY;
}
control_sensors_present |=
MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |
MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |
MAV_SYS_STATUS_SENSOR_YAW_POSITION |
MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL |
MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
bool rate_controlled = false;
bool attitude_stabilized = false;
switch (plane.control_mode) {
case MANUAL:
break;
case ACRO:
case QACRO:
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
rate_controlled = true;
break;
case STABILIZE:
@ -78,20 +67,16 @@ void GCS_Plane::update_sensor_status_flags(void) @@ -78,20 +67,16 @@ void GCS_Plane::update_sensor_status_flags(void)
case QLAND:
case QLOITER:
case QAUTOTUNE:
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation
break;
case FLY_BY_WIRE_B:
case CRUISE:
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation
rate_controlled = true;
attitude_stabilized = true;
break;
case TRAINING:
if (!plane.training_manual_roll || !plane.training_manual_pitch) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation
rate_controlled = true;
attitude_stabilized = true;
}
break;
@ -102,44 +87,31 @@ void GCS_Plane::update_sensor_status_flags(void) @@ -102,44 +87,31 @@ void GCS_Plane::update_sensor_status_flags(void)
case GUIDED:
case CIRCLE:
case QRTL:
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_YAW_POSITION; // yaw position
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; // altitude control
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; // X/Y position control
rate_controlled = true;
attitude_stabilized = true;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_YAW_POSITION;
control_sensors_health |= MAV_SYS_STATUS_SENSOR_YAW_POSITION;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
control_sensors_health |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
control_sensors_health |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
break;
case INITIALISING:
break;
}
// set motors outputs as enabled if safety switch is not disarmed (i.e. either NONE or ARMED)
if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS;
if (rate_controlled) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
control_sensors_health |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
}
if (attitude_stabilized) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION;
control_sensors_health |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION;
}
// default: all present sensors healthy except baro, 3D_MAG, GPS, DIFFERNTIAL_PRESSURE. GEOFENCE always defaults to healthy.
control_sensors_health = control_sensors_present & ~(MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE |
MAV_SYS_STATUS_SENSOR_3D_MAG |
MAV_SYS_STATUS_SENSOR_GPS |
MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE);
control_sensors_health |= MAV_SYS_STATUS_GEOFENCE;
AP_AHRS &ahrs = AP::ahrs();
if (ahrs.initialised() && !ahrs.healthy()) {
// AHRS subsystem is unhealthy
control_sensors_health &= ~MAV_SYS_STATUS_AHRS;
}
const AP_InertialSensor &ins = AP::ins();
if (ahrs.have_inertial_nav() && !ins.accel_calibrated_ok_all()) {
// trying to use EKF without properly calibrated accelerometers
control_sensors_health &= ~MAV_SYS_STATUS_AHRS;
}
const AP_Baro &barometer = AP::baro();
if (barometer.all_healthy()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
}
const Compass &compass = AP::compass();
if (plane.g.compass_enabled && compass.healthy() && ahrs.use_compass()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG;
@ -152,29 +124,19 @@ void GCS_Plane::update_sensor_status_flags(void) @@ -152,29 +124,19 @@ void GCS_Plane::update_sensor_status_flags(void)
control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
}
#endif
if (!ins.get_gyro_health_all() || !ins.gyro_calibrated_ok_all()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_GYRO;
}
if (!ins.get_accel_health_all()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_ACCEL;
}
if (airspeed && airspeed->all_healthy()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
}
#if GEOFENCE_ENABLED
if (plane.geofence_breached()) {
control_sensors_health &= ~MAV_SYS_STATUS_GEOFENCE;
if (!plane.geofence_breached()) {
control_sensors_health |= MAV_SYS_STATUS_GEOFENCE;
}
#endif
if (logger.logging_failed()) {
control_sensors_health &= ~MAV_SYS_STATUS_LOGGING;
}
control_sensors_present |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
if (millis() - plane.failsafe.last_valid_rc_ms < 200) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
} else {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
}
#if AP_TERRAIN_AVAILABLE
@ -208,14 +170,4 @@ void GCS_Plane::update_sensor_status_flags(void) @@ -208,14 +170,4 @@ void GCS_Plane::update_sensor_status_flags(void)
control_sensors_enabled |= MAV_SYS_STATUS_REVERSE_MOTOR;
control_sensors_health |= MAV_SYS_STATUS_REVERSE_MOTOR;
}
if (AP_Notify::flags.initialising) {
// while initialising the gyros and accels are not enabled
control_sensors_enabled &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
control_sensors_health &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
}
if (!plane.battery.healthy() || plane.battery.has_failsafed()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_BATTERY;
}
}

2
ArduPlane/GCS_Plane.h
Unescape View File

@ -22,7 +22,7 @@ public: @@ -22,7 +22,7 @@ public:
protected:
void update_sensor_status_flags(void) override;
void update_vehicle_sensor_status_flags(void) override;
uint32_t custom_mode() const override;
MAV_TYPE frame_type() const override;

Write Preview
Loading…
Cancel
Save