From 57e8528a9645b29bb807a3d9bce72df34eb5aa90 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 20 Feb 2019 08:32:22 +1100 Subject: [PATCH] Sub: move update_sensor_flags to GCS_Sub.cpp --- ArduSub/GCS_Mavlink.cpp | 130 -------------------------------------- ArduSub/GCS_Sub.cpp | 134 ++++++++++++++++++++++++++++++++++++++++ 2 files changed, 134 insertions(+), 130 deletions(-) create mode 100644 ArduSub/GCS_Sub.cpp diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 566f836df2..95d69eeb60 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -2,9 +2,6 @@ #include "GCS_Mavlink.h" -// default sensors are present and healthy: gyro, accelerometer, 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_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_BATTERY) - void Sub::gcs_send_heartbeat() { gcs().send_message(MSG_HEARTBEAT); @@ -84,133 +81,6 @@ MAV_STATE GCS_MAVLINK_Sub::system_status() const return MAV_STATE_STANDBY; } -void GCS_Sub::update_sensor_status_flags() -{ - // default sensors present - control_sensors_present = MAVLINK_SENSOR_PRESENT_DEFAULT; - - // first what sensors/controllers we have - if (sub.g.compass_enabled) { - control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; // compass present - } - if (sub.ap.depth_sensor_present) { - control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_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 - const OpticalFlow *optflow = AP::opticalflow(); - if (optflow && optflow->enabled()) { - control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; - } -#endif - - // all present sensors enabled by default except altitude and position control and motors which we will set individually - control_sensors_enabled = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL & - ~MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL & - ~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS & ~MAV_SYS_STATUS_SENSOR_BATTERY); - - switch (sub.control_mode) { - case ALT_HOLD: - case AUTO: - case GUIDED: - case CIRCLE: - case SURFACE: - case POSHOLD: - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; - break; - default: - 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; - } - - const AP_BattMonitor &battery = AP::battery(); - if (battery.num_instances() > 0) { - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_BATTERY; - } - - // default to all healthy except baro, compass, gps and receiver which we set individually - 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_RC_RECEIVER); - - if (sub.sensor_health.depth) { // check the internal barometer only - control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; - } - AP_AHRS &ahrs = AP::ahrs(); - const Compass &compass = AP::compass(); - if (sub.g.compass_enabled && compass.healthy() && ahrs.use_compass()) { - control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG; - } - if (gps.is_healthy()) { - control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS; - } -#if OPTFLOW == ENABLED - if (optflow && optflow->healthy()) { - control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; - } -#endif - - const AP_InertialSensor &ins = AP::ins(); - 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 (ahrs.initialised() && !ahrs.healthy()) { - // AHRS subsystem is unhealthy - control_sensors_health &= ~MAV_SYS_STATUS_AHRS; - } - - if (!battery.healthy() || battery.has_failsafed()) { - control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_BATTERY; - } - -#if AP_TERRAIN_AVAILABLE && AC_TERRAIN - switch (terrain.status()) { - case AP_Terrain::TerrainStatusDisabled: - break; - case AP_Terrain::TerrainStatusUnhealthy: - // To-Do: restore unhealthy terrain status reporting once terrain is used in Sub - //control_sensors_present |= MAV_SYS_STATUS_TERRAIN; - //control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN; - //break; - case AP_Terrain::TerrainStatusOK: - control_sensors_present |= MAV_SYS_STATUS_TERRAIN; - control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN; - control_sensors_health |= MAV_SYS_STATUS_TERRAIN; - break; - } -#endif - -#if RANGEFINDER_ENABLED == ENABLED - const RangeFinder *rangefinder = RangeFinder::get_singleton(); - if (sub.rangefinder_state.enabled) { - control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - if (rangefinder && rangefinder->has_data_orient(ROTATION_PITCH_270)) { - control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - } - } -#endif - - if (!sub.ap.initialised || ins.calibrating()) { - // 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); - } -} - void GCS_MAVLINK_Sub::send_nav_controller_output() const { const Vector3f &targets = sub.attitude_control.get_att_target_euler_cd(); diff --git a/ArduSub/GCS_Sub.cpp b/ArduSub/GCS_Sub.cpp new file mode 100644 index 0000000000..5821ed3d70 --- /dev/null +++ b/ArduSub/GCS_Sub.cpp @@ -0,0 +1,134 @@ +#include "GCS_Sub.h" + +#include "Sub.h" + +// default sensors are present and healthy: gyro, accelerometer, 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_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_BATTERY) + +void GCS_Sub::update_sensor_status_flags() +{ + // default sensors present + control_sensors_present = MAVLINK_SENSOR_PRESENT_DEFAULT; + + // first what sensors/controllers we have + if (sub.g.compass_enabled) { + control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; // compass present + } + if (sub.ap.depth_sensor_present) { + control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_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 + const OpticalFlow *optflow = AP::opticalflow(); + if (optflow && optflow->enabled()) { + control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; + } +#endif + + // all present sensors enabled by default except altitude and position control and motors which we will set individually + control_sensors_enabled = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL & + ~MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL & + ~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS & ~MAV_SYS_STATUS_SENSOR_BATTERY); + + switch (sub.control_mode) { + case ALT_HOLD: + case AUTO: + case GUIDED: + case CIRCLE: + case SURFACE: + case POSHOLD: + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; + break; + default: + 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; + } + + const AP_BattMonitor &battery = AP::battery(); + if (battery.num_instances() > 0) { + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_BATTERY; + } + + // default to all healthy except baro, compass, gps and receiver which we set individually + 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_RC_RECEIVER); + + if (sub.sensor_health.depth) { // check the internal barometer only + control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; + } + AP_AHRS &ahrs = AP::ahrs(); + const Compass &compass = AP::compass(); + if (sub.g.compass_enabled && compass.healthy() && ahrs.use_compass()) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG; + } + if (gps.is_healthy()) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS; + } +#if OPTFLOW == ENABLED + if (optflow && optflow->healthy()) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; + } +#endif + + const AP_InertialSensor &ins = AP::ins(); + 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 (ahrs.initialised() && !ahrs.healthy()) { + // AHRS subsystem is unhealthy + control_sensors_health &= ~MAV_SYS_STATUS_AHRS; + } + + if (!battery.healthy() || battery.has_failsafed()) { + control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_BATTERY; + } + +#if AP_TERRAIN_AVAILABLE && AC_TERRAIN + switch (terrain.status()) { + case AP_Terrain::TerrainStatusDisabled: + break; + case AP_Terrain::TerrainStatusUnhealthy: + // To-Do: restore unhealthy terrain status reporting once terrain is used in Sub + //control_sensors_present |= MAV_SYS_STATUS_TERRAIN; + //control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN; + //break; + case AP_Terrain::TerrainStatusOK: + control_sensors_present |= MAV_SYS_STATUS_TERRAIN; + control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN; + control_sensors_health |= MAV_SYS_STATUS_TERRAIN; + break; + } +#endif + +#if RANGEFINDER_ENABLED == ENABLED + const RangeFinder *rangefinder = RangeFinder::get_singleton(); + if (sub.rangefinder_state.enabled) { + control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + if (rangefinder && rangefinder->has_data_orient(ROTATION_PITCH_270)) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + } + } +#endif + + if (!sub.ap.initialised || ins.calibrating()) { + // 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); + } +} +