diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index f1e12f57ae..9e1c7ce1ad 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -305,7 +305,7 @@ void Copter::update_batt_compass(void) // read battery before compass because it may be used for motor interference compensation battery.read(); - if(g.compass_enabled) { + if(AP::compass().enabled()) { // update compass with throttle value - used for compassmot compass.set_throttle(motors->get_throttle()); compass.set_voltage(battery.voltage()); diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 960bc96489..4fd039c9d2 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -786,7 +786,6 @@ private: void read_rangefinder(void); bool rangefinder_alt_ok(); void rpm_update(); - void init_compass(); void init_compass_location(); void init_optflow(); void update_optical_flow(void); diff --git a/ArduCopter/GCS_Copter.cpp b/ArduCopter/GCS_Copter.cpp index 7cd26f81e7..8df16bca2f 100644 --- a/ArduCopter/GCS_Copter.cpp +++ b/ArduCopter/GCS_Copter.cpp @@ -34,7 +34,7 @@ void GCS_Copter::update_vehicle_sensor_status_flags(void) MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION; - if (copter.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; } @@ -129,7 +129,7 @@ void GCS_Copter::update_vehicle_sensor_status_flags(void) AP_AHRS &ahrs = AP::ahrs(); const Compass &compass = AP::compass(); - if (copter.g.compass_enabled && compass.healthy() && ahrs.use_compass()) { + if (compass.enabled() && compass.healthy() && ahrs.use_compass()) { control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG; } if (gps.is_healthy()) { diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index c10a22c449..60bbd5398d 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -185,13 +185,6 @@ const AP_Param::Info Copter::var_info[] = { // @User: Advanced GSCALAR(gps_hdop_good, "GPS_HDOP_GOOD", GPS_HDOP_GOOD_DEFAULT), - // @Param: MAG_ENABLE - // @DisplayName: Compass enable/disable - // @Description: Setting this to Enabled(1) will enable the compass. Setting this to Disabled(0) will disable the compass - // @Values: 0:Disabled,1:Enabled - // @User: Standard - GSCALAR(compass_enabled, "MAG_ENABLE", MAGNETOMETER), - // @Param: SUPER_SIMPLE // @DisplayName: Super Simple Mode // @Description: Bitmask to enable Super Simple mode for some flight modes. Setting this to Disabled(0) will disable Super Simple Mode diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 8b937c2362..2eeac9acc2 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -227,7 +227,7 @@ public: k_param_curr_amp_per_volt, // deprecated - can be deleted k_param_input_voltage, // deprecated - can be deleted k_param_pack_capacity, // deprecated - can be deleted - k_param_compass_enabled, + k_param_compass_enabled_deprecated, k_param_compass, k_param_rangefinder_enabled_old, // deprecated k_param_frame_type, @@ -389,7 +389,6 @@ public: AP_Int8 failsafe_gcs; // ground station failsafe behavior AP_Int16 gps_hdop_good; // GPS Hdop value at or below this value represent a good position - AP_Int8 compass_enabled; AP_Int8 super_simple; AP_Int16 rtl_alt_final; AP_Int16 rtl_climb_min; // rtl minimum climb in cm diff --git a/ArduCopter/compassmot.cpp b/ArduCopter/compassmot.cpp index 3f2a5e2cfa..8e4850565d 100644 --- a/ArduCopter/compassmot.cpp +++ b/ArduCopter/compassmot.cpp @@ -41,7 +41,7 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan) GCS_MAVLINK_Copter &gcs_chan = gcs().chan(chan-MAVLINK_COMM_0); // check compass is enabled - if (!g.compass_enabled) { + if (!AP::compass().enabled()) { gcs_chan.send_text(MAV_SEVERITY_CRITICAL, "Compass disabled"); ap.compass_mot = false; return MAV_RESULT_TEMPORARILY_REJECTED; diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index 5b6d8efeba..9fc6d3a2ee 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -85,23 +85,6 @@ void Copter::rpm_update(void) #endif } -// initialise compass -void Copter::init_compass() -{ - if (!g.compass_enabled) { - return; - } - - compass.init(); - if (!compass.read()) { - // make sure we don't pass a broken compass to DCM - hal.console->printf("COMPASS INIT ERROR\n"); - AP::logger().Write_Error(LogErrorSubsystem::COMPASS, LogErrorCode::FAILED_TO_INITIALISE); - return; - } - ahrs.set_compass(&compass); -} - /* initialise compass's location used for declination */ diff --git a/ArduCopter/setup.cpp b/ArduCopter/setup.cpp index 6d5a546459..a3e10a8945 100644 --- a/ArduCopter/setup.cpp +++ b/ArduCopter/setup.cpp @@ -6,7 +6,7 @@ void Copter::report_compass() hal.console->printf("Compass\n"); print_divider(); - print_enabled(g.compass_enabled); + print_enabled(AP::compass().enabled()); // mag declination hal.console->printf("Mag Dec: %4.4f\n", diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index bed16433e8..980df42206 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -137,7 +137,7 @@ void Copter::init_ardupilot() gps.set_log_gps_bit(MASK_LOG_GPS); gps.init(serial_manager); - init_compass(); + AP::compass().init(); #if OPTFLOW == ENABLED // make optflow available to AHRS