mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-23 16:23:56 -04:00
AP_Baro: moved MS5611 reset before prom read
thanks to Jacob and Lucas for pointing this out
This commit is contained in:
parent
439cc3e4c5
commit
8a1275356d
@ -91,6 +91,9 @@ bool AP_Baro_MS56XX::_init()
|
|||||||
uint16_t prom[8];
|
uint16_t prom[8];
|
||||||
bool prom_read_ok = false;
|
bool prom_read_ok = false;
|
||||||
|
|
||||||
|
_dev->transfer(&CMD_MS56XX_RESET, 1, nullptr, 0);
|
||||||
|
hal.scheduler->delay(4);
|
||||||
|
|
||||||
const char *name = "MS5611";
|
const char *name = "MS5611";
|
||||||
switch (_ms56xx_type) {
|
switch (_ms56xx_type) {
|
||||||
case BARO_MS5607:
|
case BARO_MS5607:
|
||||||
@ -111,9 +114,6 @@ bool AP_Baro_MS56XX::_init()
|
|||||||
|
|
||||||
printf("%s found on bus %u address 0x%02x\n", name, _dev->bus_num(), _dev->get_bus_address());
|
printf("%s found on bus %u address 0x%02x\n", name, _dev->bus_num(), _dev->get_bus_address());
|
||||||
|
|
||||||
_dev->transfer(&CMD_MS56XX_RESET, 1, nullptr, 0);
|
|
||||||
hal.scheduler->delay(4);
|
|
||||||
|
|
||||||
// Save factory calibration coefficients
|
// Save factory calibration coefficients
|
||||||
_cal_reg.c1 = prom[1];
|
_cal_reg.c1 = prom[1];
|
||||||
_cal_reg.c2 = prom[2];
|
_cal_reg.c2 = prom[2];
|
||||||
|
Loading…
Reference in New Issue
Block a user