Skip to content

Commit

Permalink
working
Browse files Browse the repository at this point in the history
  • Loading branch information
muramura committed Apr 10, 2024
1 parent f06381e commit 961b47d
Show file tree
Hide file tree
Showing 27 changed files with 80 additions and 111 deletions.
2 changes: 1 addition & 1 deletion libraries/AP_Arming/AP_Arming.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1629,7 +1629,7 @@ bool AP_Arming::pre_arm_checks(bool report)


& opendroneid_checks(report)
#endif

#if AP_ARMING_CRASHDUMP_ACK_ENABLED
& crashdump_checks(report)
#endif
Expand Down
8 changes: 4 additions & 4 deletions libraries/AP_Baro/AP_Baro.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -613,12 +613,12 @@ void AP_Baro::init(void)
}
#endif

#if AP_BARO_EXTERNALAHRS_ENABLED

const int8_t serial_port = AP::externalAHRS().get_port(AP_ExternalAHRS::AvailableSensor::BARO);
if (serial_port >= 0) {
ADD_BACKEND(new AP_Baro_ExternalAHRS(*this, serial_port));
}
#endif


// macro for use by HAL_INS_PROBE_LIST
#define GET_I2C_DEVICE(bus, address) _have_i2c_driver(bus, address)?nullptr:hal.i2c_mgr->get_device(bus, address)
Expand Down Expand Up @@ -1100,7 +1100,7 @@ void AP_Baro::handle_msp(const MSP::msp_baro_data_message_t &pkt)
}
#endif

#if AP_BARO_EXTERNALAHRS_ENABLED

/*
handle ExternalAHRS barometer data
*/
Expand All @@ -1110,7 +1110,7 @@ void AP_Baro::handle_external(const AP_ExternalAHRS::baro_data_message_t &pkt)
drivers[i]->handle_external(pkt);
}
}
#endif // AP_BARO_EXTERNALAHRS_ENABLED


// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
bool AP_Baro::arming_checks(size_t buflen, char *buffer) const
Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_Baro/AP_Baro.h
Original file line number Diff line number Diff line change
Expand Up @@ -195,9 +195,9 @@ class AP_Baro
#if AP_BARO_MSP_ENABLED
void handle_msp(const MSP::msp_baro_data_message_t &pkt);
#endif
#if AP_BARO_EXTERNALAHRS_ENABLED

void handle_external(const AP_ExternalAHRS::baro_data_message_t &pkt);
#endif


enum Options : uint16_t {
TreatMS5611AsMS5607 = (1U << 0U),
Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_Baro/AP_Baro_Backend.h
Original file line number Diff line number Diff line change
Expand Up @@ -28,9 +28,9 @@ class AP_Baro_Backend
virtual void handle_msp(const MSP::msp_baro_data_message_t &pkt) {}
#endif

#if AP_BARO_EXTERNALAHRS_ENABLED

virtual void handle_external(const AP_ExternalAHRS::baro_data_message_t &pkt) {}
#endif


/*
device driver IDs. These are used to fill in the devtype field
Expand Down
4 changes: 1 addition & 3 deletions libraries/AP_Baro/AP_Baro_ExternalAHRS.cpp
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
#include "AP_Baro_ExternalAHRS.h"

#if AP_BARO_EXTERNALAHRS_ENABLED


AP_Baro_ExternalAHRS::AP_Baro_ExternalAHRS(AP_Baro &baro, uint8_t port) :
AP_Baro_Backend(baro)
Expand Down Expand Up @@ -31,5 +31,3 @@ void AP_Baro_ExternalAHRS::handle_external(const AP_ExternalAHRS::baro_data_mess
sum_temp += pkt.temperature;
count++;
}

#endif // AP_BARO_EXTERNALAHRS_ENABLED
4 changes: 1 addition & 3 deletions libraries/AP_Baro/AP_Baro_ExternalAHRS.h
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@

#include "AP_Baro_Backend.h"

#if AP_BARO_EXTERNALAHRS_ENABLED


class AP_Baro_ExternalAHRS : public AP_Baro_Backend
{
Expand All @@ -20,5 +20,3 @@ class AP_Baro_ExternalAHRS : public AP_Baro_Backend
float sum_temp;
uint16_t count;
};

#endif // AP_BARO_EXTERNALAHRS_ENABLED
28 changes: 14 additions & 14 deletions libraries/AP_Compass/AP_Compass.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,9 +39,9 @@
#if AP_COMPASS_MSP_ENABLED
#include "AP_Compass_MSP.h"
#endif
#if AP_COMPASS_EXTERNALAHRS_ENABLED

#include "AP_Compass_ExternalAHRS.h"
#endif

#include "AP_Compass.h"
#include "Compass_learn.h"
#include <stdio.h>
Expand Down Expand Up @@ -1117,7 +1117,7 @@ void Compass::_probe_external_i2c_compasses(void)
#endif // !defined(HAL_SKIP_AUTO_INTERNAL_I2C_PROBE)


#if AP_COMPASS_QMC5883L_ENABLED

//external i2c bus
FOREACH_I2C_EXTERNAL(i) {
ADD_BACKEND(DRIVER_QMC5883L, AP_Compass_QMC5883L::probe(GET_I2C_DEVICE(i, HAL_COMPASS_QMC5883L_I2C_ADDR),
Expand All @@ -1135,9 +1135,9 @@ void Compass::_probe_external_i2c_compasses(void)
}
}
#endif
#endif // AP_COMPASS_QMC5883L_ENABLED

#if AP_COMPASS_QMC5883P_ENABLED


//external i2c bus
FOREACH_I2C_EXTERNAL(i) {
ADD_BACKEND(DRIVER_QMC5883P, AP_Compass_QMC5883P::probe(GET_I2C_DEVICE(i, HAL_COMPASS_QMC5883P_I2C_ADDR),
Expand All @@ -1155,7 +1155,7 @@ void Compass::_probe_external_i2c_compasses(void)
}
}
#endif
#endif // AP_COMPASS_QMC5883P_ENABLED


#ifndef HAL_BUILD_AP_PERIPH
// AK09916 on ICM20948
Expand Down Expand Up @@ -1250,7 +1250,7 @@ void Compass::_probe_external_i2c_compasses(void)
}


#if AP_COMPASS_IST8308_ENABLED

// external i2c bus
FOREACH_I2C_EXTERNAL(i) {
ADD_BACKEND(DRIVER_IST8308, AP_Compass_IST8308::probe(GET_I2C_DEVICE(i, HAL_COMPASS_IST8308_I2C_ADDR),
Expand All @@ -1262,7 +1262,7 @@ void Compass::_probe_external_i2c_compasses(void)
all_external, ROTATION_NONE));
}
#endif
#endif // AP_COMPASS_IST8308_ENABLED



// external i2c bus
Expand All @@ -1278,7 +1278,7 @@ void Compass::_probe_external_i2c_compasses(void)
#endif


#if AP_COMPASS_RM3100_ENABLED

#ifdef HAL_COMPASS_RM3100_I2C_ADDR
const uint8_t rm3100_addresses[] = { HAL_COMPASS_RM3100_I2C_ADDR };
#else
Expand All @@ -1302,7 +1302,7 @@ void Compass::_probe_external_i2c_compasses(void)
}
}
#endif
#endif // AP_COMPASS_RM3100_ENABLED


#if AP_COMPASS_BMM150_DETECT_BACKENDS_ENABLED
// BMM150 on I2C
Expand All @@ -1320,12 +1320,12 @@ void Compass::_probe_external_i2c_compasses(void)
*/
void Compass::_detect_backends(void)
{
#if AP_COMPASS_EXTERNALAHRS_ENABLED

const int8_t serial_port = AP::externalAHRS().get_port(AP_ExternalAHRS::AvailableSensor::COMPASS);
if (serial_port >= 0) {
ADD_BACKEND(DRIVER_EXTERNALAHRS, new AP_Compass_ExternalAHRS(serial_port));
}
#endif


#if AP_FEATURE_BOARD_DETECT
if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK2) {
Expand Down Expand Up @@ -2206,7 +2206,7 @@ void Compass::handle_msp(const MSP::msp_compass_data_message_t &pkt)
}
#endif // AP_COMPASS_MSP_ENABLED

#if AP_COMPASS_EXTERNALAHRS_ENABLED

void Compass::handle_external(const AP_ExternalAHRS::mag_data_message_t &pkt)
{
if (!_driver_enabled(DRIVER_EXTERNALAHRS)) {
Expand All @@ -2216,7 +2216,7 @@ void Compass::handle_external(const AP_ExternalAHRS::mag_data_message_t &pkt)
_backends[i]->handle_external(pkt);
}
}
#endif // AP_COMPASS_EXTERNALAHRS_ENABLED


// force save of current calibration as valid
void Compass::force_save_calibration(void)
Expand Down
32 changes: 16 additions & 16 deletions libraries/AP_Compass/AP_Compass.h
Original file line number Diff line number Diff line change
Expand Up @@ -355,9 +355,9 @@ friend class AP_Compass_Backend;
void handle_msp(const MSP::msp_compass_data_message_t &pkt);
#endif

#if AP_COMPASS_EXTERNALAHRS_ENABLED

void handle_external(const AP_ExternalAHRS::mag_data_message_t &pkt);
#endif


// force save of current calibration as valid
void force_save_calibration(void);
Expand Down Expand Up @@ -461,33 +461,33 @@ friend class AP_Compass_Backend;
#if AP_COMPASS_DRONECAN_ENABLED
DRIVER_UAVCAN =11,
#endif
#if AP_COMPASS_QMC5883L_ENABLED

DRIVER_QMC5883L =12,
#endif

#if AP_COMPASS_SITL_ENABLED
DRIVER_SITL =13,
#endif
#if AP_COMPASS_MAG3110_ENABLED

DRIVER_MAG3110 =14,
#endif
#if AP_COMPASS_IST8308_ENABLED


DRIVER_IST8308 =15,
#endif
#if AP_COMPASS_RM3100_ENABLED


DRIVER_RM3100 =16,
#endif

#if AP_COMPASS_MSP_ENABLED
DRIVER_MSP =17,
#endif
#if AP_COMPASS_EXTERNALAHRS_ENABLED

DRIVER_EXTERNALAHRS =18,
#endif
#if AP_COMPASS_MMC5XX3_ENABLED


DRIVER_MMC5XX3 =19,
#endif
#if AP_COMPASS_QMC5883P_ENABLED


DRIVER_QMC5883P =20,
#endif

};

bool _driver_enabled(enum DriverType driver_type);
Expand Down
8 changes: 4 additions & 4 deletions libraries/AP_Compass/AP_Compass_Backend.h
Original file line number Diff line number Diff line change
Expand Up @@ -21,9 +21,9 @@

#include "AP_Compass_config.h"

#if AP_COMPASS_EXTERNALAHRS_ENABLED

#include <AP_ExternalAHRS/AP_ExternalAHRS.h>
#endif


#if AP_COMPASS_MSP_ENABLED
#include <AP_MSP/msp.h>
Expand Down Expand Up @@ -80,9 +80,9 @@ class AP_Compass_Backend
virtual void handle_msp(const MSP::msp_compass_data_message_t &pkt) {}
#endif

#if AP_COMPASS_EXTERNALAHRS_ENABLED

virtual void handle_external(const AP_ExternalAHRS::mag_data_message_t &pkt) {}
#endif


protected:

Expand Down
4 changes: 1 addition & 3 deletions libraries/AP_Compass/AP_Compass_ExternalAHRS.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@
#include <AP_HAL/AP_HAL.h>
#include "AP_Compass_ExternalAHRS.h"

#if AP_COMPASS_EXTERNALAHRS_ENABLED


AP_Compass_ExternalAHRS::AP_Compass_ExternalAHRS(uint8_t port)
{
Expand All @@ -37,5 +37,3 @@ void AP_Compass_ExternalAHRS::read(void)
{
drain_accumulated_samples(instance);
}

#endif // AP_COMPASS_EXTERNALAHRS_ENABLED
4 changes: 1 addition & 3 deletions libraries/AP_Compass/AP_Compass_ExternalAHRS.h
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

#include "AP_Compass_config.h"

#if AP_COMPASS_EXTERNALAHRS_ENABLED


#include "AP_Compass.h"
#include "AP_Compass_Backend.h"
Expand All @@ -19,5 +19,3 @@ class AP_Compass_ExternalAHRS : public AP_Compass_Backend
void handle_external(const AP_ExternalAHRS::mag_data_message_t &pkt) override;
uint8_t instance;
};

#endif // AP_COMPASS_EXTERNALAHRS_ENABLED
4 changes: 1 addition & 3 deletions libraries/AP_Compass/AP_Compass_IST8308.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@
*/
#include "AP_Compass_IST8308.h"

#if AP_COMPASS_IST8308_ENABLED


#include <stdio.h>
#include <utility>
Expand Down Expand Up @@ -226,5 +226,3 @@ void AP_Compass_IST8308::read()
{
drain_accumulated_samples(_instance);
}

#endif // AP_COMPASS_IST8308_ENABLED
4 changes: 1 addition & 3 deletions libraries/AP_Compass/AP_Compass_IST8308.h
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@

#include "AP_Compass_config.h"

#if AP_COMPASS_IST8308_ENABLED


#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
Expand Down Expand Up @@ -55,5 +55,3 @@ class AP_Compass_IST8308 : public AP_Compass_Backend
uint8_t _instance;
bool _force_external;
};

#endif // AP_COMPASS_IST8308_ENABLED
4 changes: 1 addition & 3 deletions libraries/AP_Compass/AP_Compass_MAG3110.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@
*/
#include "AP_Compass_MAG3110.h"

#if AP_COMPASS_MAG3110_ENABLED


#include <utility>

Expand Down Expand Up @@ -221,5 +221,3 @@ void AP_Compass_MAG3110::read()

drain_accumulated_samples(_compass_instance);
}

#endif // AP_COMPASS_MAG3110_ENABLED
4 changes: 1 addition & 3 deletions libraries/AP_Compass/AP_Compass_MAG3110.h
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

#include "AP_Compass_config.h"

#if AP_COMPASS_MAG3110_ENABLED


#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
Expand Down Expand Up @@ -48,5 +48,3 @@ class AP_Compass_MAG3110 : public AP_Compass_Backend
uint8_t _compass_instance;
bool _initialised;
};

#endif // AP_COMPASS_MAG3110_ENABLED
5 changes: 1 addition & 4 deletions libraries/AP_Compass/AP_Compass_MMC5xx3.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@

#include "AP_Compass_MMC5xx3.h"

#if AP_COMPASS_MMC5XX3_ENABLED


#include <AP_HAL/AP_HAL.h>
#include <stdio.h>
Expand Down Expand Up @@ -308,6 +308,3 @@ void AP_Compass_MMC5XX3::read()
{
drain_accumulated_samples(compass_instance);
}

#endif // AP_COMPASS_MMC5XX3_ENABLED

Loading

0 comments on commit 961b47d

Please sign in to comment.