diff --git a/APMrover2/GCS_Rover.cpp b/APMrover2/GCS_Rover.cpp index 5ea848ff33..3edb481073 100644 --- a/APMrover2/GCS_Rover.cpp +++ b/APMrover2/GCS_Rover.cpp @@ -23,7 +23,7 @@ bool GCS_Rover::supersimple_input_active() const void GCS_Rover::update_vehicle_sensor_status_flags(void) { // first what sensors/controllers we have - if (rover.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; } @@ -65,7 +65,7 @@ void GCS_Rover::update_vehicle_sensor_status_flags(void) AP_AHRS &ahrs = AP::ahrs(); const Compass &compass = AP::compass(); - if (rover.g.compass_enabled && compass.healthy(0) && ahrs.use_compass()) { + if (AP::compass().enabled() && AP::compass().healthy(0) && ahrs.use_compass()) { control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG; } if (gps.is_healthy()) { diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index e1bed1fb11..77bf37bb36 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -68,13 +68,6 @@ const AP_Param::Info Rover::var_info[] = { // @Bitmask: 0:Steering,1:Throttle,2:Pitch,3:Left Wheel,4:Right Wheel,5:Sailboat Heel GSCALAR(gcs_pid_mask, "GCS_PID_MASK", 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. - // @User: Standard - // @Values: 0:Disabled,1:Enabled - GSCALAR(compass_enabled, "MAG_ENABLE", MAGNETOMETER), - // @Param: AUTO_TRIGGER_PIN // @DisplayName: Auto mode trigger pin // @Description: pin number to use to enable the throttle in auto mode. If set to -1 then don't use a trigger, otherwise this is a pin number which if held low in auto mode will enable the motor to run. If the switch is released while in AUTO then the motor will stop again. This can be used in combination with INITIAL_MODE to give a 'press button to start' rover with no receiver. diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 2de785b798..ec72374b2a 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -84,7 +84,7 @@ public: // // 130: Sensor parameters // - k_param_compass_enabled = 130, + k_param_compass_enabled_deprecated = 130, k_param_steering_learn, // unused k_param_NavEKF, // deprecated - remove k_param_mission, // mission library @@ -227,9 +227,6 @@ public: AP_Int16 sysid_my_gcs; AP_Int8 telem_delay; - // sensor parameters - AP_Int8 compass_enabled; - // navigation parameters // AP_Float speed_cruise; diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 5d440470c3..7333552f77 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -461,7 +461,6 @@ private: float sailboat_calc_heading(float desired_heading_cd); // sensors.cpp - void init_compass(void); void init_compass_location(void); void update_compass(void); void compass_cal_update(void); diff --git a/APMrover2/config.h b/APMrover2/config.h index 876dcef228..62e2d98a52 100644 --- a/APMrover2/config.h +++ b/APMrover2/config.h @@ -63,12 +63,6 @@ #define CH7_OPTION CH7_SAVE_WP #endif -////////////////////////////////////////////////////////////////////////////// -// MAGNETOMETER -#ifndef MAGNETOMETER - #define MAGNETOMETER ENABLED -#endif - ////////////////////////////////////////////////////////////////////////////// // MODE // MODE_CHANNEL diff --git a/APMrover2/sensors.cpp b/APMrover2/sensors.cpp index 126b99e223..fce932460f 100644 --- a/APMrover2/sensors.cpp +++ b/APMrover2/sensors.cpp @@ -3,22 +3,6 @@ #include #include -// initialise compass -void Rover::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 */ @@ -37,7 +21,7 @@ void Rover::init_compass_location(void) // check for new compass data - 10Hz void Rover::update_compass(void) { - if (g.compass_enabled && compass.read()) { + if (AP::compass().enabled() && compass.read()) { ahrs.set_compass(&compass); // update offsets if (should_log(MASK_LOG_COMPASS)) { @@ -55,7 +39,7 @@ void Rover::compass_cal_update() { // Save compass offsets void Rover::compass_save() { - if (g.compass_enabled && + if (AP::compass().enabled() && compass.get_learn_type() >= Compass::LEARN_INTERNAL && !arming.is_armed()) { compass.save_offsets(); diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index b50c3b7b3a..c55e25bbc4 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -89,7 +89,7 @@ void Rover::init_ardupilot() #endif // initialise compass - init_compass(); + AP::compass().init(); // initialise rangefinder rangefinder.init();