From 39db4eb871bdf5cc19199fdb82f21c54c315279a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 24 Mar 2019 14:26:06 +1100 Subject: [PATCH] Tracker: move enabled parameter into compass library --- AntennaTracker/GCS_Tracker.cpp | 4 ++-- AntennaTracker/Parameters.cpp | 7 ------- AntennaTracker/Parameters.h | 4 +--- AntennaTracker/Tracker.h | 1 - AntennaTracker/sensors.cpp | 22 +++------------------- AntennaTracker/system.cpp | 2 +- 6 files changed, 7 insertions(+), 33 deletions(-) diff --git a/AntennaTracker/GCS_Tracker.cpp b/AntennaTracker/GCS_Tracker.cpp index adad5f12ab..c28f6d39a7 100644 --- a/AntennaTracker/GCS_Tracker.cpp +++ b/AntennaTracker/GCS_Tracker.cpp @@ -57,7 +57,7 @@ void GCS_Tracker::update_vehicle_sensor_status_flags() MAV_SYS_STATUS_SENSOR_YAW_POSITION; // first what sensors/controllers we have - if (tracker.g.compass_enabled) { + if (AP::compass().enabled()) { control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG; } @@ -70,7 +70,7 @@ void GCS_Tracker::update_vehicle_sensor_status_flags() AP_AHRS &ahrs = AP::ahrs(); const Compass &compass = AP::compass(); - if (tracker.g.compass_enabled && compass.healthy(0) && ahrs.use_compass()) { + if (AP::compass().enabled() && compass.healthy(0) && ahrs.use_compass()) { control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG; } if (gps.is_healthy()) { diff --git a/AntennaTracker/Parameters.cpp b/AntennaTracker/Parameters.cpp index 2a64482f23..808c08f2f7 100644 --- a/AntennaTracker/Parameters.cpp +++ b/AntennaTracker/Parameters.cpp @@ -39,13 +39,6 @@ const AP_Param::Info Tracker::var_info[] = { // @User: Advanced GSCALAR(sysid_target, "SYSID_TARGET", 0), - // @Param: MAG_ENABLE - // @DisplayName: Enable Compass - // @Description: Setting this to Enabled(1) will enable the compass. Setting this to Disabled(0) will disable the compass. Note that this is separate from COMPASS_USE. This will enable the low level senor, and will enable logging of magnetometer data. To use the compass for navigation you must also set COMPASS_USE to 1. - // @Values: 0:Disabled,1:Enabled - // @User: Standard - GSCALAR(compass_enabled, "MAG_ENABLE", 1), - // @Param: YAW_SLEW_TIME // @DisplayName: Time for yaw to slew through its full range // @Description: This controls how rapidly the tracker will change the servo output for yaw. It is set as the number of seconds to do a full rotation. You can use this parameter to slow the trackers movements, which may help with some types of trackers. A value of zero will allow for unlimited servo movement per update. diff --git a/AntennaTracker/Parameters.h b/AntennaTracker/Parameters.h index c8ef320703..95feeb4035 100644 --- a/AntennaTracker/Parameters.h +++ b/AntennaTracker/Parameters.h @@ -48,7 +48,7 @@ public: k_param_serial0_baud, // deprecated k_param_serial1_baud, // deprecated k_param_imu, - k_param_compass_enabled, + k_param_compass_enabled_deprecated, k_param_compass, k_param_ahrs, // AHRS group k_param_barometer, @@ -130,8 +130,6 @@ public: AP_Int16 sysid_my_gcs; AP_Int16 sysid_target; - AP_Int8 compass_enabled; - AP_Float yaw_slew_time; AP_Float pitch_slew_time; AP_Float min_reverse_time; diff --git a/AntennaTracker/Tracker.h b/AntennaTracker/Tracker.h index 32f446ea2a..c7c0b67f79 100644 --- a/AntennaTracker/Tracker.h +++ b/AntennaTracker/Tracker.h @@ -243,7 +243,6 @@ private: // sensors.cpp void update_ahrs(); - void init_compass(); void compass_save(); void init_compass_location(); void update_compass(void); diff --git a/AntennaTracker/sensors.cpp b/AntennaTracker/sensors.cpp index 74acfef480..1da7b72b80 100644 --- a/AntennaTracker/sensors.cpp +++ b/AntennaTracker/sensors.cpp @@ -8,22 +8,6 @@ void Tracker::update_ahrs() ahrs.update(); } -// initialise compass -void Tracker::init_compass() -{ - if (!g.compass_enabled) { - return; - } - - compass.init(); - if (!compass.read()) { - hal.console->printf("Compass initialisation failed!\n"); - g.compass_enabled = false; - } else { - ahrs.set_compass(&compass); - } -} - /* initialise compass's location used for declination */ @@ -44,7 +28,7 @@ void Tracker::init_compass_location(void) */ void Tracker::update_compass(void) { - if (g.compass_enabled && compass.read()) { + if (AP::compass().enabled() && compass.read()) { ahrs.set_compass(&compass); if (should_log(MASK_LOG_COMPASS)) { logger.Write_Compass(); @@ -63,7 +47,7 @@ void Tracker::compass_cal_update() { // Save compass offsets void Tracker::compass_save() { - if (g.compass_enabled && + if (AP::compass().enabled() && compass.get_learn_type() >= Compass::LEARN_INTERNAL && !hal.util->get_soft_armed()) { compass.save_offsets(); @@ -114,7 +98,7 @@ void Tracker::update_GPS(void) // silently ignored } - if (g.compass_enabled) { + if (AP::compass().enabled()) { // Set compass declination automatically compass.set_initial_location(gps.location().lat, gps.location().lng); } diff --git a/AntennaTracker/system.cpp b/AntennaTracker/system.cpp index 8a05b29e9a..c863c85cbb 100644 --- a/AntennaTracker/system.cpp +++ b/AntennaTracker/system.cpp @@ -60,7 +60,7 @@ void Tracker::init_tracker() #endif // ENABLE_SCRIPTING // initialise compass - init_compass(); + AP::compass().init(); // GPS Initialization gps.set_log_gps_bit(MASK_LOG_GPS);