16 #if RANGEFINDER_ENABLED == ENABLED 26 #if RANGEFINDER_ENABLED == ENABLED 38 #if RANGEFINDER_TILT_CORRECTION == ENABLED 79 #if RPM_ENABLED == ENABLED 118 if (!
ap.compass_init_location) {
122 ap.compass_init_location =
true;
130 #if OPTFLOW == ENABLED 133 #endif // OPTFLOW == ENABLED 137 #if OPTFLOW == ENABLED 140 static uint32_t last_of_update = 0;
163 #endif // OPTFLOW == ENABLED 167 static uint32_t compass_cal_stick_gesture_begin = 0;
181 if (!stick_gesture_detected) {
182 compass_cal_stick_gesture_begin = tnow;
184 #ifdef CAL_ALWAYS_REBOOT 200 float trim_roll, trim_pitch;
205 #ifdef CAL_ALWAYS_REBOOT 216 #if PROXIMITY_ENABLED == ENABLED 238 #if OPTFLOW == ENABLED 243 #if PRECISION_LANDING == ENABLED 248 #if VISUAL_ODOMETRY_ENABLED == ENABLED 253 if (
ap.rc_receiver_present) {
256 if (
copter.DataFlash.logging_present()) {
259 #if PROXIMITY_ENABLED == ENABLED 264 #if AC_FENCE == ENABLED 265 if (
copter.fence.sys_status_present()) {
272 ~MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL &
273 ~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS &
274 ~MAV_SYS_STATUS_LOGGING &
275 ~MAV_SYS_STATUS_SENSOR_BATTERY &
276 ~MAV_SYS_STATUS_GEOFENCE);
310 if (
copter.DataFlash.logging_enabled()) {
317 #if AC_FENCE == ENABLED 318 if (
copter.fence.sys_status_enabled()) {
336 if (!
ap.rc_receiver_present ||
failsafe.radio) {
339 #if OPTFLOW == ENABLED 344 #if PRECISION_LANDING == ENABLED 349 #if VISUAL_ODOMETRY_ENABLED == ENABLED 366 if (
copter.DataFlash.logging_failed()) {
370 #if PROXIMITY_ENABLED == ENABLED 376 #if AP_TERRAIN_AVAILABLE && AC_TERRAIN 378 case AP_Terrain::TerrainStatusDisabled:
380 case AP_Terrain::TerrainStatusUnhealthy:
385 case AP_Terrain::TerrainStatusOK:
393 #if RANGEFINDER_ENABLED == ENABLED 409 if (!
copter.battery.healthy() ||
copter.battery.has_failsafed()) {
412 #if AC_FENCE == ENABLED 413 if (
copter.fence.sys_status_failed()) {
418 #if FRSKY_TELEM_ENABLED == ENABLED 427 #if VISUAL_ODOMETRY_ENABLED == ENABLED 435 #if VISUAL_ODOMETRY_ENABLED == ENABLED 458 #if WINCH_ENABLED == ENABLED 467 #if WINCH_ENABLED == ENABLED bool accel_cal_requires_reboot() const
bool get_soft_armed() const
float get_climb_rate(void)
bool has_data_orient(enum Rotation orientation) const
void read_barometer(void)
bool enabled(uint8_t instance) const
bool initialised() const override
bool use_compass() override
void set_compass(Compass *compass)
bool rangefinder_alt_ok()
void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, const Vector3f &posOffset)
AP_VisualOdom visual_odom
#define RANGEFINDER_TIMEOUT_MS
bool healthy() const override
AP_HAL::UARTDriver * console
void init_rangefinder(void)
void start_calibration_all(bool retry=false, bool autosave=false, float delay_sec=0.0f, bool autoreboot=false)
#define RANGEFINDER_HEALTH_MAX
AP_WheelEncoder wheel_encoder
bool has_orientation(enum Rotation orientation) const
uint8_t range_valid_count_orient(enum Rotation orientation) const
void cancel_calibration_all()
struct Copter::@0 rangefinder_state
#define ERROR_SUBSYSTEM_COMPASS
const Vector3f & get_angle_delta() const
uint32_t control_sensors_health
AP_Frsky_Telem frsky_telemetry
#define MAVLINK_SENSOR_PRESENT_DEFAULT
bool is_healthy(uint8_t instance) const
bool should_log(uint32_t mask)
uint8_t num_sensors(void) const
uint64_t get_time_delta_usec() const
const Vector2f & flowRate() const
void update_visual_odom()
float get_air_density_ratio(void)
virtual void delay(uint16_t ms)=0
void update_sensor_status_flags(uint32_t error_mask)
void Log_Write_Error(uint8_t sub_system, uint8_t error_code)
virtual void printf(const char *,...) FMT_PRINTF(2
virtual void set_trim(Vector3f new_trim)
RC_Channel * channel_throttle
const Vector3f & get_position_delta() const
void init(const AP_WheelEncoder *wheel_encoder=nullptr)
void Log_Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence)
int16_t get_control_in() const
uint32_t get_last_update_ms() const
bool get_accel_health_all(void) const
#define COMPASS_CAL_STICK_DELAY
#define ERROR_CODE_FAILED_TO_INITIALISE
void update_sensor_status_flags(void)
uint16_t distance_cm_orient(enum Rotation orientation) const
virtual enum safety_state safety_switch_state(void)
void writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset)
uint32_t last_update() const
bool healthy[COMPASS_MAX_INSTANCES]
bool get_position(struct Location &loc) const override
const Vector2f & bodyRate() const
GPS_Status status(uint8_t instance) const
DataFlash_Class DataFlash
void Log_Write_RPM(const AP_RPM &rpm_sensor)
uint32_t control_sensors_enabled
float get_altitude(void) const
virtual void reboot(bool hold_in_bootloader)=0
RangeFinder_Status status_orient(enum Rotation orientation) const
void compass_cal_update()
AP_HAL_MAIN_CALLBACKS & copter
const Matrix3f & get_rotation_body_to_ned(void) const override
struct Copter::@2 failsafe
void update_optical_flow(void)
void compass_accumulate(void)
bool gyro_calibrated_ok_all() const
bool get_new_trim(float &trim_roll, float &trim_pitch)
bool is_calibrating() const
uint32_t control_sensors_present
#define RANGEFINDER_WPNAV_FILT_HZ
void Log_Write_RFND(const RangeFinder &rangefinder)
uint8_t num_instances(void) const
void accel_cal_update(void)
control_mode_t control_mode
void compass_cal_update(void)
uint32_t visual_odom_last_update_ms
void set_rangefinder_alt(bool use, bool healthy, float alt_cm)
float get_confidence() const
bool get_gyro_health_all(void) const
void set_initial_location(int32_t latitude, int32_t longitude)
bool all_healthy(void) const
const Vector3f & get_pos_offset(void) const
AP_HAL::Scheduler * scheduler
#define COMPASS_CAL_STICK_GESTURE_TIME
void read_rangefinder(void)
const Vector3f & get_pos_offset(void) const
void set_rangefinder(const RangeFinder *rangefinder)