Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: move update_sensor_status_flags into GCS subclasses

Browse Source
This commit is contained in:
Peter Barker 2019-02-14 13:25:10 +11:00 committed by Peter Barker
parent 323d6850d5
commit 34853fb1bf
5 changed files with 39 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/ArduPlane.cpp
View File

@ -305,7 +305,7 @@ void Plane::one_second_loop()
// 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
update_sensor_status_flags();
gcs().update_sensor_status_flags();
}
void Plane::compass_save()

11
ArduPlane/GCS_Mavlink.cpp
View File

@ -153,17 +153,6 @@ void Plane::send_fence_status(mavlink_channel_t chan)
#endif
void GCS_Plane::get_sensor_status_flags(uint32_t &present,
uint32_t &enabled,
uint32_t &health)
{
plane.update_sensor_status_flags();
present = plane.control_sensors_present;
enabled = plane.control_sensors_enabled;
health = plane.control_sensors_health;
}
void GCS_MAVLINK_Plane::send_nav_controller_output() const
{
if (plane.control_mode == MANUAL) {

4
ArduPlane/GCS_Plane.h
View File

@ -22,7 +22,9 @@ public:
void send_airspeed_calibration(const Vector3f &vg);
void get_sensor_status_flags(uint32_t &present, uint32_t &enabled, uint32_t &health);
protected:
void update_sensor_status_flags(void) override;
private:

6
ArduPlane/Plane.h
View File

@ -417,11 +417,6 @@ private:
AP_DEVO_Telem devo_telemetry;
#endif
// Variables for extended status MAVLink messages
uint32_t control_sensors_present;
uint32_t control_sensors_enabled;
uint32_t control_sensors_health;
// Airspeed Sensors
AP_Airspeed airspeed;
@ -798,7 +793,6 @@ private:
void adjust_nav_pitch_throttle(void);
void update_load_factor(void);
void send_fence_status(mavlink_channel_t chan);
void update_sensor_status_flags(void);
void send_servo_out(mavlink_channel_t chan);
void send_wind(mavlink_channel_t chan);
void send_pid_info(const mavlink_channel_t chan, const AP_Logger::PID_Info *pid_info, const uint8_t axis, const float achieved);

59
ArduPlane/sensors.cpp
View File

@ -1,5 +1,6 @@
#include "Plane.h"
#include <AP_RSSI/AP_RSSI.h>
#include <AP_OpticalFlow/AP_OpticalFlow.h>
/*
read the rangefinder and update height estimate
@ -96,58 +97,63 @@ void Plane::rpm_update(void)
// 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 Plane::update_sensor_status_flags(void)
void GCS_Plane::update_sensor_status_flags(void)
{
// default sensors present
control_sensors_present = MAVLINK_SENSOR_PRESENT_DEFAULT;
// first what sensors/controllers we have
if (g.compass_enabled) {
if (plane.g.compass_enabled) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; // compass present
}
if (airspeed.enabled()) {
const AP_Airspeed *airspeed = AP_Airspeed::get_singleton();
if (airspeed && airspeed->enabled()) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
}
const AP_GPS &gps = AP::gps();
if (gps.status() > AP_GPS::NO_GPS) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS;
}
#if OPTFLOW == ENABLED
if (optflow.enabled()) {
const OpticalFlow *optflow = AP::opticalflow();
if (optflow && optflow->enabled()) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
}
#endif
if (geofence_present()) {
if (plane.geofence_present()) {
control_sensors_present |= MAV_SYS_STATUS_GEOFENCE;
}
if (have_reverse_thrust()) {
if (plane.have_reverse_thrust()) {
control_sensors_present |= MAV_SYS_STATUS_REVERSE_MOTOR;
}
if (plane.logger.logging_present()) { // primary logging only (usually File)
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.enabled() && airspeed.use()) {
if (airspeed && airspeed->enabled() && airspeed->use()) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
}
if (geofence_enabled()) {
if (plane.geofence_enabled()) {
control_sensors_enabled |= MAV_SYS_STATUS_GEOFENCE;
}
if (plane.logger.logging_enabled()) {
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;
}
switch (control_mode) {
switch (plane.control_mode) {
case MANUAL:
break;
@ -174,7 +180,7 @@ void Plane::update_sensor_status_flags(void)
break;
case TRAINING:
if (!training_manual_roll || !training_manual_pitch) {
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
}
@ -210,26 +216,30 @@ void Plane::update_sensor_status_flags(void)
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;
}
if (g.compass_enabled && compass.healthy() && ahrs.use_compass()) {
const Compass &compass = AP::compass();
if (plane.g.compass_enabled && compass.healthy() && ahrs.use_compass()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG;
}
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.is_healthy()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
}
#if OPTFLOW == ENABLED
if (optflow.healthy()) {
if (optflow && optflow->healthy()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
}
#endif
@ -239,27 +249,27 @@ void Plane::update_sensor_status_flags(void)
if (!ins.get_accel_health_all()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_ACCEL;
}
if (airspeed.all_healthy()) {
if (airspeed && airspeed->all_healthy()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
}
#if GEOFENCE_ENABLED
if (geofence_breached()) {
if (plane.geofence_breached()) {
control_sensors_health &= ~MAV_SYS_STATUS_GEOFENCE;
}
#endif
if (plane.logger.logging_failed()) {
if (logger.logging_failed()) {
control_sensors_health &= ~MAV_SYS_STATUS_LOGGING;
}
if (millis() - failsafe.last_valid_rc_ms < 200) {
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
switch (terrain.status()) {
switch (plane.terrain.status()) {
case AP_Terrain::TerrainStatusDisabled:
break;
case AP_Terrain::TerrainStatusUnhealthy:
@ -274,17 +284,18 @@ void Plane::update_sensor_status_flags(void)
}
#endif
if (rangefinder.has_orientation(ROTATION_PITCH_270)) {
const RangeFinder *rangefinder = RangeFinder::get_singleton();
if (rangefinder && rangefinder->has_orientation(ROTATION_PITCH_270)) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
if (g.rangefinder_landing) {
if (plane.g.rangefinder_landing) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
if (rangefinder.has_data_orient(ROTATION_PITCH_270)) {
if (rangefinder->has_data_orient(ROTATION_PITCH_270)) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
}
if (have_reverse_thrust() && SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < 0) {
if (plane.have_reverse_thrust() && SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < 0) {
control_sensors_enabled |= MAV_SYS_STATUS_REVERSE_MOTOR;
control_sensors_health |= MAV_SYS_STATUS_REVERSE_MOTOR;
}
@ -301,6 +312,6 @@ void Plane::update_sensor_status_flags(void)
#if FRSKY_TELEM_ENABLED == ENABLED
// give mask of error flags to Frsky_Telemetry
frsky_telemetry.update_sensor_status_flags(~control_sensors_health & control_sensors_enabled & control_sensors_present);
plane.frsky_telemetry.update_sensor_status_flags(~control_sensors_health & control_sensors_enabled & control_sensors_present);
#endif
}