Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Tracker: move update_sensor_status_flags into GCS subclasses

This commit is contained in:
Peter Barker 2019-02-14 13:33:49 +11:00 committed by Peter Barker
parent 34853fb1bf
commit 6ef1c64652
4 changed files with 11 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
AntennaTracker/GCS_Mavlink.cpp
View File

@ -71,17 +71,6 @@ MAV_STATE GCS_MAVLINK_Tracker::system_status() const
return MAV_STATE_ACTIVE;
}
void GCS_Tracker::get_sensor_status_flags(uint32_t &present,
uint32_t &enabled,
uint32_t &health)
{
tracker.update_sensor_status_flags();
present = tracker.control_sensors_present;
enabled = tracker.control_sensors_enabled;
health = tracker.control_sensors_health;
}
void GCS_MAVLINK_Tracker::send_nav_controller_output() const
{
float alt_diff = (tracker.g.alt_source == ALT_SOURCE_BARO) ? tracker.nav_status.alt_difference_baro : tracker.nav_status.alt_difference_gps;

2
AntennaTracker/GCS_Tracker.h
View File

@ -17,7 +17,7 @@ public:
GCS_MAVLINK_Tracker &chan(const uint8_t ofs) override { return _chan[ofs]; };
const GCS_MAVLINK_Tracker &chan(const uint8_t ofs) const override { return _chan[ofs]; };
void get_sensor_status_flags(uint32_t &present, uint32_t &enabled, uint32_t &health);
void update_sensor_status_flags() override;
private:

6
AntennaTracker/Tracker.h
View File

@ -139,11 +139,6 @@ private:
GCS_Tracker _gcs; // avoid using this; use gcs()
GCS_Tracker &gcs() { return _gcs; }
// variables for extended status MAVLink messages
uint32_t control_sensors_present;
uint32_t control_sensors_enabled;
uint32_t control_sensors_health;
AP_BoardConfig BoardConfig;
#if HAL_WITH_UAVCAN
@ -241,7 +236,6 @@ private:
void accel_cal_update(void);
void update_GPS(void);
void handle_battery_failsafe(const char* type_str, const int8_t action);
void update_sensor_status_flags();
// servos.cpp
void init_servos();

13
AntennaTracker/sensors.cpp
View File

@ -91,18 +91,20 @@ void Tracker::handle_battery_failsafe(const char* type_str, const int8_t action)
}
// update sensors and subsystems present, enabled and healthy flags for reporting to GCS
void Tracker::update_sensor_status_flags()
void GCS_Tracker::update_sensor_status_flags()
{
// default sensors present
control_sensors_present = MAVLINK_SENSOR_PRESENT_DEFAULT;
// first what sensors/controllers we have
if (g.compass_enabled) {
if (tracker.g.compass_enabled) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; // compass present
}
const AP_GPS &gps = AP::gps();
if (gps.status() > AP_GPS::NO_GPS) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS;
}
const AP_Logger &logger = AP::logger();
if (logger.logging_present()) { // primary logging only (usually File)
control_sensors_present |= MAV_SYS_STATUS_LOGGING;
}
@ -121,18 +123,23 @@ void Tracker::update_sensor_status_flags()
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;
}
AP_AHRS &ahrs = AP::ahrs();
// 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()) {
const Compass &compass = AP::compass();
if (tracker.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;
}
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;
}