mirror of https://github.com/ArduPilot/ardupilot
AP_Compass: regularise compass defines
start to make compass backends look like backends from our other libraries
This commit is contained in:
parent
e804b71a79
commit
3adbaddab0
|
@ -10,6 +10,8 @@
|
|||
#include <AP_CustomRotations/AP_CustomRotations.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
|
||||
#include "AP_Compass_config.h"
|
||||
|
||||
#include "AP_Compass_SITL.h"
|
||||
#include "AP_Compass_AK8963.h"
|
||||
#include "AP_Compass_Backend.h"
|
||||
|
@ -22,17 +24,17 @@
|
|||
#include "AP_Compass_LIS3MDL.h"
|
||||
#include "AP_Compass_AK09916.h"
|
||||
#include "AP_Compass_QMC5883L.h"
|
||||
#if HAL_ENABLE_LIBUAVCAN_DRIVERS
|
||||
#if AP_COMPASS_UAVCAN_ENABLED
|
||||
#include "AP_Compass_UAVCAN.h"
|
||||
#endif
|
||||
#include "AP_Compass_MMC3416.h"
|
||||
#include "AP_Compass_MMC5xx3.h"
|
||||
#include "AP_Compass_MAG3110.h"
|
||||
#include "AP_Compass_RM3100.h"
|
||||
#if HAL_MSP_COMPASS_ENABLED
|
||||
#if AP_COMPASS_MSP_ENABLED
|
||||
#include "AP_Compass_MSP.h"
|
||||
#endif
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
#if AP_COMPASS_EXTERNALAHRS_ENABLED
|
||||
#include "AP_Compass_ExternalAHRS.h"
|
||||
#endif
|
||||
#include "AP_Compass.h"
|
||||
|
@ -1265,7 +1267,7 @@ void Compass::_probe_external_i2c_compasses(void)
|
|||
*/
|
||||
void Compass::_detect_backends(void)
|
||||
{
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
#if AP_COMPASS_EXTERNALAHRS_ENABLED
|
||||
const int8_t serial_port = AP::externalAHRS().get_port();
|
||||
if (serial_port >= 0) {
|
||||
ADD_BACKEND(DRIVER_SERIAL, new AP_Compass_ExternalAHRS(serial_port));
|
||||
|
@ -1279,7 +1281,7 @@ void Compass::_detect_backends(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
#if AP_SIM_COMPASS_ENABLED
|
||||
#if AP_COMPASS_SITL_ENABLED
|
||||
ADD_BACKEND(DRIVER_SITL, new AP_Compass_SITL());
|
||||
#endif
|
||||
|
||||
|
@ -1289,7 +1291,7 @@ void Compass::_detect_backends(void)
|
|||
CHECK_UNREG_LIMIT_RETURN;
|
||||
#endif
|
||||
|
||||
#if HAL_MSP_COMPASS_ENABLED
|
||||
#if AP_COMPASS_MSP_ENABLED
|
||||
for (uint8_t i=0; i<8; i++) {
|
||||
if (msp_instance_mask & (1U<<i)) {
|
||||
ADD_BACKEND(DRIVER_MSP, new AP_Compass_MSP(i));
|
||||
|
@ -1408,7 +1410,7 @@ void Compass::_detect_backends(void)
|
|||
#endif
|
||||
|
||||
|
||||
#if HAL_ENABLE_LIBUAVCAN_DRIVERS
|
||||
#if AP_COMPASS_UAVCAN_ENABLED
|
||||
if (_driver_enabled(DRIVER_UAVCAN)) {
|
||||
for (uint8_t i=0; i<COMPASS_MAX_BACKEND; i++) {
|
||||
AP_Compass_Backend* _uavcan_backend = AP_Compass_UAVCAN::probe(i);
|
||||
|
@ -1476,9 +1478,9 @@ void Compass::_detect_backends(void)
|
|||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
#endif // #if COMPASS_MAX_UNREG_DEV > 0
|
||||
}
|
||||
#endif
|
||||
#endif // AP_COMPASS_UAVCAN_ENABLED
|
||||
|
||||
if (_backend_count == 0 ||
|
||||
_compass_count == 0) {
|
||||
|
@ -1567,7 +1569,7 @@ void Compass::_reset_compass_id()
|
|||
void
|
||||
Compass::_detect_runtime(void)
|
||||
{
|
||||
#if HAL_ENABLE_LIBUAVCAN_DRIVERS
|
||||
#if AP_COMPASS_UAVCAN_ENABLED
|
||||
if (!available()) {
|
||||
return;
|
||||
}
|
||||
|
@ -1591,7 +1593,7 @@ Compass::_detect_runtime(void)
|
|||
CHECK_UNREG_LIMIT_RETURN;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
#endif // AP_COMPASS_UAVCAN_ENABLED
|
||||
}
|
||||
|
||||
bool
|
||||
|
@ -2054,7 +2056,7 @@ bool Compass::have_scale_factor(uint8_t i) const
|
|||
return true;
|
||||
}
|
||||
|
||||
#if HAL_MSP_COMPASS_ENABLED
|
||||
#if AP_COMPASS_MSP_ENABLED
|
||||
void Compass::handle_msp(const MSP::msp_compass_data_message_t &pkt)
|
||||
{
|
||||
if (!_driver_enabled(DRIVER_MSP)) {
|
||||
|
@ -2070,9 +2072,9 @@ void Compass::handle_msp(const MSP::msp_compass_data_message_t &pkt)
|
|||
}
|
||||
}
|
||||
}
|
||||
#endif // HAL_MSP_COMPASS_ENABLED
|
||||
#endif // AP_COMPASS_MSP_ENABLED
|
||||
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
#if AP_COMPASS_EXTERNALAHRS_ENABLED
|
||||
void Compass::handle_external(const AP_ExternalAHRS::mag_data_message_t &pkt)
|
||||
{
|
||||
if (!_driver_enabled(DRIVER_SERIAL)) {
|
||||
|
@ -2082,7 +2084,7 @@ void Compass::handle_external(const AP_ExternalAHRS::mag_data_message_t &pkt)
|
|||
_backends[i]->handle_external(pkt);
|
||||
}
|
||||
}
|
||||
#endif // HAL_EXTERNAL_AHRS_ENABLED
|
||||
#endif // AP_COMPASS_EXTERNALAHRS_ENABLED
|
||||
|
||||
// force save of current calibration as valid
|
||||
void Compass::force_save_calibration(void)
|
||||
|
|
|
@ -72,10 +72,6 @@
|
|||
|
||||
#define MAX_CONNECTED_MAGS (COMPASS_MAX_UNREG_DEV+COMPASS_MAX_INSTANCES)
|
||||
|
||||
#ifndef AP_SIM_COMPASS_ENABLED
|
||||
#define AP_SIM_COMPASS_ENABLED AP_SIM_ENABLED
|
||||
#endif
|
||||
|
||||
#include "CompassCalibrator.h"
|
||||
|
||||
class CompassLearn;
|
||||
|
@ -347,11 +343,11 @@ public:
|
|||
float lat_deg, float lon_deg,
|
||||
bool force_use=false);
|
||||
|
||||
#if HAL_MSP_COMPASS_ENABLED
|
||||
#if AP_COMPASS_MSP_ENABLED
|
||||
void handle_msp(const MSP::msp_compass_data_message_t &pkt);
|
||||
#endif
|
||||
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
#if AP_COMPASS_EXTERNALAHRS_ENABLED
|
||||
void handle_external(const AP_ExternalAHRS::mag_data_message_t &pkt);
|
||||
#endif
|
||||
|
||||
|
@ -602,7 +598,7 @@ private:
|
|||
|
||||
bool _cal_thread_started;
|
||||
|
||||
#if HAL_MSP_COMPASS_ENABLED
|
||||
#if AP_COMPASS_MSP_ENABLED
|
||||
uint8_t msp_instance_mask;
|
||||
#endif
|
||||
bool init_done;
|
||||
|
|
|
@ -19,12 +19,15 @@
|
|||
*/
|
||||
#pragma once
|
||||
|
||||
#include <AP_MSP/msp.h>
|
||||
#ifndef HAL_MSP_COMPASS_ENABLED
|
||||
#define HAL_MSP_COMPASS_ENABLED HAL_MSP_SENSORS_ENABLED
|
||||
#include "AP_Compass_config.h"
|
||||
|
||||
#if AP_COMPASS_EXTERNALAHRS_ENABLED
|
||||
#include <AP_ExternalAHRS/AP_ExternalAHRS.h>
|
||||
#endif
|
||||
|
||||
#include "AP_Compass.h"
|
||||
#if AP_COMPASS_MSP_ENABLED
|
||||
#include <AP_MSP/msp.h>
|
||||
#endif
|
||||
|
||||
class Compass; // forward declaration
|
||||
class AP_Compass_Backend
|
||||
|
@ -70,11 +73,11 @@ public:
|
|||
DEVTYPE_AK09915 = 0x15,
|
||||
};
|
||||
|
||||
#if HAL_MSP_COMPASS_ENABLED
|
||||
#if AP_COMPASS_MSP_ENABLED
|
||||
virtual void handle_msp(const MSP::msp_compass_data_message_t &pkt) {}
|
||||
#endif
|
||||
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
#if AP_COMPASS_EXTERNALAHRS_ENABLED
|
||||
virtual void handle_external(const AP_ExternalAHRS::mag_data_message_t &pkt) {}
|
||||
#endif
|
||||
|
||||
|
|
|
@ -16,7 +16,7 @@
|
|||
#include <AP_HAL/AP_HAL.h>
|
||||
#include "AP_Compass_ExternalAHRS.h"
|
||||
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
#if AP_COMPASS_EXTERNALAHRS_ENABLED
|
||||
|
||||
AP_Compass_ExternalAHRS::AP_Compass_ExternalAHRS(uint8_t port)
|
||||
{
|
||||
|
@ -38,6 +38,4 @@ void AP_Compass_ExternalAHRS::read(void)
|
|||
drain_accumulated_samples(instance);
|
||||
}
|
||||
|
||||
#endif // HAL_EXTERNAL_AHRS_ENABLED
|
||||
|
||||
|
||||
#endif // AP_COMPASS_EXTERNALAHRS_ENABLED
|
||||
|
|
|
@ -1,11 +1,13 @@
|
|||
#pragma once
|
||||
|
||||
#include "AP_Compass_config.h"
|
||||
|
||||
#if AP_COMPASS_EXTERNALAHRS_ENABLED
|
||||
|
||||
#include "AP_Compass.h"
|
||||
#include "AP_Compass_Backend.h"
|
||||
#include <AP_ExternalAHRS/AP_ExternalAHRS.h>
|
||||
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
|
||||
class AP_Compass_ExternalAHRS : public AP_Compass_Backend
|
||||
{
|
||||
public:
|
||||
|
@ -18,5 +20,4 @@ private:
|
|||
uint8_t instance;
|
||||
};
|
||||
|
||||
#endif // HAL_EXTERNAL_AHRS_ENABLED
|
||||
|
||||
#endif // AP_COMPASS_EXTERNALAHRS_ENABLED
|
||||
|
|
|
@ -13,10 +13,11 @@
|
|||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include "AP_Compass_MSP.h"
|
||||
|
||||
#if HAL_MSP_COMPASS_ENABLED
|
||||
#if AP_COMPASS_MSP_ENABLED
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
AP_Compass_MSP::AP_Compass_MSP(uint8_t _msp_instance)
|
||||
{
|
||||
|
@ -43,5 +44,5 @@ void AP_Compass_MSP::read(void)
|
|||
drain_accumulated_samples(instance);
|
||||
}
|
||||
|
||||
#endif // HAL_MSP_COMPASS_ENABLED
|
||||
#endif // AP_COMPASS_MSP_ENABLED
|
||||
|
||||
|
|
|
@ -1,11 +1,13 @@
|
|||
#pragma once
|
||||
|
||||
#include "AP_Compass_config.h"
|
||||
|
||||
#if AP_COMPASS_MSP_ENABLED
|
||||
|
||||
#include "AP_Compass.h"
|
||||
#include "AP_Compass_Backend.h"
|
||||
#include <AP_MSP/msp.h>
|
||||
|
||||
#if HAL_MSP_COMPASS_ENABLED
|
||||
|
||||
class AP_Compass_MSP : public AP_Compass_Backend
|
||||
{
|
||||
public:
|
||||
|
@ -19,4 +21,4 @@ private:
|
|||
uint8_t instance;
|
||||
};
|
||||
|
||||
#endif // HAL_MSP_COMPASS_ENABLED
|
||||
#endif // AP_COMPASS_MSP_ENABLED
|
||||
|
|
|
@ -1,6 +1,6 @@
|
|||
#include "AP_Compass_SITL.h"
|
||||
|
||||
#if AP_SIM_COMPASS_ENABLED
|
||||
#if AP_COMPASS_SITL_ENABLED
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
|
@ -154,4 +154,4 @@ void AP_Compass_SITL::read()
|
|||
drain_accumulated_samples(_compass_instance[i], nullptr);
|
||||
}
|
||||
}
|
||||
#endif // AP_SIM_COMPASS_ENABLED
|
||||
#endif // AP_COMPASS_SITL_ENABLED
|
||||
|
|
|
@ -2,7 +2,7 @@
|
|||
|
||||
#include "AP_Compass.h"
|
||||
|
||||
#if AP_SIM_COMPASS_ENABLED
|
||||
#if AP_COMPASS_SITL_ENABLED
|
||||
|
||||
#include "AP_Compass_Backend.h"
|
||||
|
||||
|
@ -44,4 +44,4 @@ private:
|
|||
Vector3f _last_odi;
|
||||
Vector3f _last_data[MAX_SITL_COMPASSES];
|
||||
};
|
||||
#endif // AP_SIM_COMPASS_ENABLED
|
||||
#endif // AP_COMPASS_SITL_ENABLED
|
||||
|
|
|
@ -13,12 +13,12 @@
|
|||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#if HAL_ENABLE_LIBUAVCAN_DRIVERS
|
||||
|
||||
#include "AP_Compass_UAVCAN.h"
|
||||
|
||||
#if AP_COMPASS_UAVCAN_ENABLED
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#include <AP_CANManager/AP_CANManager.h>
|
||||
#include <AP_UAVCAN/AP_UAVCAN.h>
|
||||
|
||||
|
@ -204,4 +204,4 @@ void AP_Compass_UAVCAN::read(void)
|
|||
drain_accumulated_samples(_instance);
|
||||
}
|
||||
|
||||
#endif
|
||||
#endif // AP_COMPASS_UAVCAN_ENABLED
|
||||
|
|
|
@ -1,6 +1,9 @@
|
|||
#pragma once
|
||||
|
||||
#include "AP_Compass.h"
|
||||
|
||||
#if AP_COMPASS_UAVCAN_ENABLED
|
||||
|
||||
#include "AP_Compass_Backend.h"
|
||||
|
||||
#include <AP_UAVCAN/AP_UAVCAN.h>
|
||||
|
@ -46,3 +49,5 @@ private:
|
|||
|
||||
static HAL_Semaphore _sem_registry;
|
||||
};
|
||||
|
||||
#endif // AP_COMPASS_UAVCAN_ENABLED
|
||||
|
|
|
@ -1,7 +1,26 @@
|
|||
#pragma once
|
||||
|
||||
#include <AP_HAL/AP_HAL_Boards.h>
|
||||
#include <AP_ExternalAHRS/AP_ExternalAHRS_config.h>
|
||||
#include <AP_MSP/msp.h>
|
||||
|
||||
#ifndef AP_COMPASS_DIAGONALS_ENABLED
|
||||
#define AP_COMPASS_DIAGONALS_ENABLED 1
|
||||
#endif
|
||||
|
||||
// Backend support
|
||||
#ifndef AP_COMPASS_EXTERNALAHRS_ENABLED
|
||||
#define AP_COMPASS_EXTERNALAHRS_ENABLED HAL_EXTERNAL_AHRS_ENABLED
|
||||
#endif
|
||||
|
||||
#ifndef AP_COMPASS_MSP_ENABLED
|
||||
#define AP_COMPASS_MSP_ENABLED HAL_MSP_SENSORS_ENABLED
|
||||
#endif
|
||||
|
||||
#ifndef AP_COMPASS_SITL_ENABLED
|
||||
#define AP_COMPASS_SITL_ENABLED AP_SIM_ENABLED
|
||||
#endif
|
||||
|
||||
#ifndef AP_COMPASS_UAVCAN_ENABLED
|
||||
#define AP_COMPASS_UAVCAN_ENABLED HAL_ENABLE_LIBUAVCAN_DRIVERS
|
||||
#endif
|
||||
|
|
|
@ -34,9 +34,9 @@ class DummyVehicle {
|
|||
public:
|
||||
AP_AHRS ahrs; // Need since https://github.com/ArduPilot/ardupilot/pull/10890
|
||||
AP_Baro baro; // Compass tries to set magnetic model based on location.
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
#if AP_COMPASS_EXTERNALAHRS_ENABLED
|
||||
AP_ExternalAHRS eAHRS;
|
||||
#endif // HAL_EXTERNAL_AHRS_ENABLED
|
||||
#endif // AP_COMPASS_EXTERNALAHRS_ENABLED
|
||||
};
|
||||
|
||||
static DummyVehicle vehicle;
|
||||
|
|
Loading…
Reference in New Issue