Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: move various sensor status flag updates up

Browse Source
This commit is contained in:
Peter Barker 2019-02-20 10:01:16 +11:00 committed by Randy Mackay
parent b58ded8e0c
commit 2796beac8d
3 changed files with 69 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
libraries/GCS_MAVLink/GCS.cpp
View File

@ -69,3 +69,70 @@ bool GCS::install_alternative_protocol(mavlink_channel_t c, GCS_MAVLINK::protoco
}
#undef FOR_EACH_ACTIVE_CHANNEL
void GCS::update_sensor_status_flags()
{
control_sensors_present = 0;
control_sensors_enabled = 0;
control_sensors_health = 0;
AP_AHRS &ahrs = AP::ahrs();
const AP_InertialSensor &ins = AP::ins();
control_sensors_present |= MAV_SYS_STATUS_AHRS;
control_sensors_enabled |= MAV_SYS_STATUS_AHRS;
if (!ahrs.initialised() || ahrs.healthy()) {
if (!ahrs.have_inertial_nav() || ins.accel_calibrated_ok_all()) {
control_sensors_health |= MAV_SYS_STATUS_AHRS;
}
}
const AP_Baro &barometer = AP::baro();
control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
if (barometer.all_healthy()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
}
const AP_BattMonitor &battery = AP::battery();
control_sensors_present |= MAV_SYS_STATUS_SENSOR_BATTERY;
if (battery.num_instances() > 0) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_BATTERY;
}
if (battery.healthy() && !battery.has_failsafed()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_BATTERY;
}
control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_GYRO;
control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_ACCEL;
if (!ins.calibrating()) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_ACCEL;
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_GYRO;
if (ins.get_accel_health_all()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_ACCEL;
}
if (ins.get_gyro_health_all() && ins.gyro_calibrated_ok_all()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_GYRO;
}
}
const AP_Logger &logger = AP::logger();
if (logger.logging_present()) { // primary logging only (usually File)
control_sensors_present |= MAV_SYS_STATUS_LOGGING;
}
if (logger.logging_enabled()) {
control_sensors_enabled |= MAV_SYS_STATUS_LOGGING;
}
if (!logger.logging_failed()) {
control_sensors_health |= MAV_SYS_STATUS_LOGGING;
}
// set motors outputs as enabled if safety switch is not disarmed (i.e. either NONE or ARMED)
control_sensors_present |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS;
if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS;
}
control_sensors_health |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS;
update_vehicle_sensor_status_flags();
}

3
libraries/GCS_MAVLink/GCS.h
View File

@ -793,7 +793,8 @@ protected:
uint32_t control_sensors_present;
uint32_t control_sensors_enabled;
uint32_t control_sensors_health;
virtual void update_sensor_status_flags(void) = 0;
void update_sensor_status_flags();
virtual void update_vehicle_sensor_status_flags() {}
private:

2
libraries/GCS_MAVLink/GCS_Dummy.h
View File

@ -53,8 +53,6 @@ class GCS_Dummy : public GCS
void send_statustext(MAV_SEVERITY severity, uint8_t dest_bitmask, const char *text) { hal.console->printf("TOGCS: %s\n", text); }
void update_sensor_status_flags(void) override {};
MAV_TYPE frame_type() const override { return MAV_TYPE_FIXED_WING; }
uint32_t custom_mode() const override { return 3; } // magic number
};