bool RangeFinder::has_data_orient(enum Rotation orientation) const { AP_RangeFinder_Backend *backend = find_instance(orientation); if (backend == nullptr) { return false; } return backend->has_data(); }
// 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 Rover::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) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; // compass present } if (gps.status() > AP_GPS::NO_GPS) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS; } if (g2.visual_odom.enabled()) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION; } if (rover.DataFlash.logging_present()) { // primary logging only (usually File) control_sensors_present |= MAV_SYS_STATUS_LOGGING; } if (rover.g2.proximity.get_status() > AP_Proximity::Proximity_NotConnected) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; } // all present sensors enabled by default except rate control, attitude stabilization, yaw, altitude, position control and motor 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_XY_POSITION_CONTROL & ~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS & ~MAV_SYS_STATUS_LOGGING & ~MAV_SYS_STATUS_SENSOR_BATTERY); if (control_mode->attitude_stabilized()) { control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // 3D angular rate control } if (control_mode->is_autopilot_mode()) { control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_YAW_POSITION; // yaw position control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; // X/Y position control } if (rover.DataFlash.logging_enabled()) { control_sensors_enabled |= MAV_SYS_STATUS_LOGGING; } // 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 (battery.num_instances() > 0) { control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_BATTERY; } // default to all healthy except compass and gps which we set individually control_sensors_health = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_3D_MAG & ~MAV_SYS_STATUS_SENSOR_GPS); if (g.compass_enabled && compass.healthy(0) && 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 (g2.visual_odom.enabled() && !g2.visual_odom.healthy()) { control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_VISION_POSITION; } 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 (rangefinder.num_sensors() > 0) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; if (g.rangefinder_trigger_cm > 0) { control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; } AP_RangeFinder_Backend *s = rangefinder.get_backend(0); if (s != nullptr && s->has_data()) { control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; } } if (rover.g2.proximity.get_status() < AP_Proximity::Proximity_Good) { control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_LASER_POSITION; } if (rover.DataFlash.logging_failed()) { control_sensors_health &= ~MAV_SYS_STATUS_LOGGING; } if (!battery.healthy() || battery.has_failsafed()) { control_sensors_enabled &= ~MAV_SYS_STATUS_SENSOR_BATTERY; } if (!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 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); #endif }