mirror of https://github.com/ArduPilot/ardupilot
Compass: add DEV_ID param and configured method
These allow checking the offsets were created with the current compass device.
This commit is contained in:
parent
be9bfc5530
commit
b7f33d81ad
|
@ -115,6 +115,28 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
|
|||
AP_GROUPINFO("MOT3", 14, Compass, _motor_compensation[2], 0),
|
||||
#endif
|
||||
|
||||
#if COMPASS_MAX_INSTANCES > 1
|
||||
// @Param: DEV_ID
|
||||
// @DisplayName: Compass device id
|
||||
// @Description: Compass device id. Automatically detected, do not set manually
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("DEV_ID", 15, Compass, _dev_id[0], COMPASS_EXPECTED_DEV_ID),
|
||||
|
||||
// @Param: DEV_ID2
|
||||
// @DisplayName: Compass2 device id
|
||||
// @Description: Second compass's device id. Automatically detected, do not set manually
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("DEV_ID2", 16, Compass, _dev_id[1], COMPASS_EXPECTED_DEV_ID2),
|
||||
#endif
|
||||
|
||||
#if COMPASS_MAX_INSTANCES > 2
|
||||
// @Param: DEV_ID3
|
||||
// @DisplayName: Compass3 device id
|
||||
// @Description: Third compass's device id. Automatically detected, do not set manually
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("DEV_ID3", 17, Compass, _dev_id[2], COMPASS_EXPECTED_DEV_ID3),
|
||||
#endif
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
|
@ -147,7 +169,10 @@ void
|
|||
Compass::save_offsets()
|
||||
{
|
||||
for (uint8_t k=0; k<COMPASS_MAX_INSTANCES; k++) {
|
||||
_offset[k].save();
|
||||
_offset[k].save(); // save offsets
|
||||
#if COMPASS_MAX_INSTANCES > 1
|
||||
_dev_id[k].save(); // save device id corresponding to these offsets
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -229,4 +254,47 @@ Compass::calculate_heading(const Matrix3f &dcm_matrix) const
|
|||
return heading;
|
||||
}
|
||||
|
||||
/// Returns True if the compasses have been configured (i.e. offsets saved)
|
||||
///
|
||||
/// @returns True if compass has been configured
|
||||
///
|
||||
bool Compass::configured(uint8_t i)
|
||||
{
|
||||
// exit immediately if instance is beyond the number of compasses we have available
|
||||
if (i > get_count()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// exit immediately if all offsets are zero
|
||||
if (get_offsets(i).length() == 0.0f) {
|
||||
return false;
|
||||
}
|
||||
|
||||
#if COMPASS_MAX_INSTANCES > 1
|
||||
// backup detected dev_id
|
||||
int32_t dev_id_orig = _dev_id[i];
|
||||
|
||||
// load dev_id from eeprom
|
||||
_dev_id[i].load();
|
||||
|
||||
// if different then the device has not been configured
|
||||
if (_dev_id[i] != dev_id_orig) {
|
||||
// restore device id
|
||||
_dev_id[i] = dev_id_orig;
|
||||
// return failure
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
|
||||
// if we got here then it must be configured
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Compass::configured(void)
|
||||
{
|
||||
bool all_configured = true;
|
||||
for(uint8_t i=0; i<get_count(); i++) {
|
||||
all_configured = all_configured && configured(i);
|
||||
}
|
||||
return all_configured;
|
||||
}
|
||||
|
|
|
@ -52,6 +52,25 @@
|
|||
#define COMPASS_MAX_INSTANCES 1
|
||||
#endif
|
||||
|
||||
// default compass device ids for each board type to most common set-up to reduce eeprom usage
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
|
||||
# define COMPASS_EXPECTED_DEV_ID 0x011E0101 // external hmc5883
|
||||
# define COMPASS_EXPECTED_DEV_ID2 0x02020102 // internal ldm303d
|
||||
# define COMPASS_EXPECTED_DEV_ID3 0
|
||||
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
|
||||
# define COMPASS_EXPECTED_DEV_ID 0
|
||||
# define COMPASS_EXPECTED_DEV_ID2 0
|
||||
# define COMPASS_EXPECTED_DEV_ID3 0
|
||||
#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
||||
# define COMPASS_EXPECTED_DEV_ID 0
|
||||
# define COMPASS_EXPECTED_DEV_ID2 0
|
||||
# define COMPASS_EXPECTED_DEV_ID3 0
|
||||
#else
|
||||
# define COMPASS_EXPECTED_DEV_ID 0
|
||||
# define COMPASS_EXPECTED_DEV_ID2 0
|
||||
# define COMPASS_EXPECTED_DEV_ID3 0
|
||||
#endif
|
||||
|
||||
class Compass
|
||||
{
|
||||
public:
|
||||
|
@ -216,6 +235,13 @@ public:
|
|||
}
|
||||
}
|
||||
|
||||
/// Returns True if the compasses have been configured (i.e. offsets saved)
|
||||
///
|
||||
/// @returns True if compass has been configured
|
||||
///
|
||||
bool configured(uint8_t i);
|
||||
bool configured(void);
|
||||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
// settable parameters
|
||||
|
@ -235,6 +261,7 @@ protected:
|
|||
AP_Int8 _external; ///<compass is external
|
||||
#if COMPASS_MAX_INSTANCES > 1
|
||||
AP_Int8 _primary; ///primary instance
|
||||
AP_Int32 _dev_id[COMPASS_MAX_INSTANCES]; // device id detected at init. saved to eeprom when offsets are saved allowing ram & eeprom values to be compared as consistency check
|
||||
#endif
|
||||
|
||||
bool _null_init_done; ///< first-time-around flag used by offset nulling
|
||||
|
|
Loading…
Reference in New Issue