mirror of https://github.com/ArduPilot/ardupilot
Tracker: move enabled parameter into compass library
This commit is contained in:
parent
578438c178
commit
39db4eb871
|
@ -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()) {
|
||||
|
|
|
@ -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.
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -243,7 +243,6 @@ private:
|
|||
|
||||
// sensors.cpp
|
||||
void update_ahrs();
|
||||
void init_compass();
|
||||
void compass_save();
|
||||
void init_compass_location();
|
||||
void update_compass(void);
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
|
Loading…
Reference in New Issue