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

Copter: move various sensor status flag updates up

master
Peter Barker 6 years ago committed by Randy Mackay
parent
2796beac8d
commit
9a2d5ac5a6
3 changed files with 42 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. 126
      ArduCopter/GCS_Copter.cpp
  2. 2
      ArduCopter/GCS_Copter.h
  3. 3
      ArduCopter/GCS_Mavlink.h

126
ArduCopter/GCS_Copter.cpp
Unescape View File

@ -17,48 +17,57 @@ bool GCS_Copter::supersimple_input_active() const @@ -17,48 +17,57 @@ bool GCS_Copter::supersimple_input_active() const
return copter.ap.simple_mode == 2;
}
// 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_Copter::update_sensor_status_flags(void)
void GCS_Copter::update_vehicle_sensor_status_flags(void)
{
// default sensors present
control_sensors_present = MAVLINK_SENSOR_PRESENT_DEFAULT;
control_sensors_present |=
MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |
MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |
MAV_SYS_STATUS_SENSOR_YAW_POSITION;
control_sensors_enabled |=
MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |
MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |
MAV_SYS_STATUS_SENSOR_YAW_POSITION;
control_sensors_health |=
MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL |
MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION |
MAV_SYS_STATUS_SENSOR_YAW_POSITION;
// first what sensors/controllers we have
if (copter.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_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 PRECISION_LANDING == ENABLED
if (copter.precland.enabled()) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
}
#endif
#if VISUAL_ODOMETRY_ENABLED == ENABLED
const AP_VisualOdom *visual_odom = AP::visualodom();
if (visual_odom && visual_odom->enabled()) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
}
#endif
const Copter::ap_t &ap = copter.ap;
if (ap.rc_receiver_present) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
}
const AP_Logger &logger = AP::logger();
if (logger.logging_present()) { // primary logging only (usually File)
control_sensors_present |= MAV_SYS_STATUS_LOGGING;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
}
#if PROXIMITY_ENABLED == ENABLED
if (copter.g2.proximity.sensor_present()) {
@ -77,15 +86,8 @@ void GCS_Copter::update_sensor_status_flags(void) @@ -77,15 +86,8 @@ void GCS_Copter::update_sensor_status_flags(void)
}
#endif
// all sensors are present except these, which may be set as enabled below:
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_LOGGING &
~MAV_SYS_STATUS_SENSOR_BATTERY &
~MAV_SYS_STATUS_GEOFENCE &
~MAV_SYS_STATUS_SENSOR_LASER_POSITION &
~MAV_SYS_STATUS_SENSOR_PROXIMITY);
control_sensors_present |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL;
control_sensors_present |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL;
switch (copter.control_mode) {
case AUTO:
@ -114,19 +116,6 @@ void GCS_Copter::update_sensor_status_flags(void) @@ -114,19 +116,6 @@ void GCS_Copter::update_sensor_status_flags(void)
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 (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;
}
#if AC_FENCE == ENABLED
if (copter.fence.sys_status_enabled()) {
control_sensors_enabled |= MAV_SYS_STATUS_GEOFENCE;
@ -138,60 +127,36 @@ void GCS_Copter::update_sensor_status_flags(void) @@ -138,60 +127,36 @@ void GCS_Copter::update_sensor_status_flags(void)
}
#endif
// default to all healthy
control_sensors_health = control_sensors_present;
const AP_Baro &barometer = AP::baro();
if (!barometer.all_healthy()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
}
AP_AHRS &ahrs = AP::ahrs();
const Compass &compass = AP::compass();
if (!copter.g.compass_enabled || !compass.healthy() || !ahrs.use_compass()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_MAG;
if (copter.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 (gps.is_healthy()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
}
if (!ap.rc_receiver_present || copter.failsafe.radio) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
if (ap.rc_receiver_present && !copter.failsafe.radio) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
}
#if OPTFLOW == ENABLED
if (!optflow || !optflow->healthy()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
if (optflow && optflow->healthy()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
}
#endif
#if PRECISION_LANDING == ENABLED
if (copter.precland.enabled() && !copter.precland.healthy()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_VISION_POSITION;
if (!copter.precland.enabled() || copter.precland.healthy()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
}
#endif
#if VISUAL_ODOMETRY_ENABLED == ENABLED
if (visual_odom && visual_odom->enabled() && visual_odom->healthy()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_VISION_POSITION;
control_sensors_health |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
}
#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 (logger.logging_failed()) {
control_sensors_health &= ~MAV_SYS_STATUS_LOGGING;
}
#if PROXIMITY_ENABLED == ENABLED
if (copter.g2.proximity.sensor_failed()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_PROXIMITY;
if (!copter.g2.proximity.sensor_failed()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_PROXIMITY;
}
#endif
@ -215,24 +180,15 @@ void GCS_Copter::update_sensor_status_flags(void) @@ -215,24 +180,15 @@ void GCS_Copter::update_sensor_status_flags(void)
#if RANGEFINDER_ENABLED == ENABLED
if (copter.rangefinder_state.enabled) {
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;
if (rangefinder && rangefinder->has_data_orient(ROTATION_PITCH_270)) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
}
#endif
if (!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);
}
if (!copter.battery.healthy() || copter.battery.has_failsafed()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_BATTERY;
}
#if AC_FENCE == ENABLED
if (copter.fence.sys_status_failed()) {
control_sensors_health &= ~MAV_SYS_STATUS_GEOFENCE;
if (!copter.fence.sys_status_failed()) {
control_sensors_health |= MAV_SYS_STATUS_GEOFENCE;
}
#endif
}

2
ArduCopter/GCS_Copter.h
Unescape View File

@ -20,7 +20,7 @@ public: @@ -20,7 +20,7 @@ public:
return _chan[ofs];
};
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;

3
ArduCopter/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_BATTERY)
class GCS_MAVLINK_Copter : public GCS_MAVLINK
{

Write Preview
Loading…
Cancel
Save