Skip to content

Commit a0efc3e

Browse files
acfloriaacfloria
authored
Merge pull request #26 from ethz-asl/feature/firmware_update
Feature/firmware update
2 parents 01307a4 + 9424d17 commit a0efc3e

File tree

8 files changed

+54
-30
lines changed

8 files changed

+54
-30
lines changed

‎ROMFS/px4fmu_common/init.d/rc.sensors‎

Lines changed: 17 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,23 @@ fi
2121
# Begin Optional drivers #
2222
###############################################################################
2323

24+
# ADIS16448 spi external IMU
25+
if param compare -s SENS_EN_ADIS164X 1
26+
then
27+
if param compare -s SENS_OR_ADIS164X 0
28+
then
29+
adis16448 -S start
30+
fi
31+
if param compare -s SENS_OR_ADIS164X 4
32+
then
33+
adis16448 -S start -R 4
34+
fi
35+
36+
# add a sleep to make sure the mag is the priority one
37+
usleep 1000000
38+
fi
39+
40+
2441
if param compare -s SENS_EN_BATT 1
2542
then
2643
batt_smbus start -X
@@ -108,19 +125,6 @@ then
108125
vl53l1x start -X
109126
fi
110127

111-
# ADIS16448 spi external IMU
112-
if param compare -s SENS_EN_ADIS164X 1
113-
then
114-
if param compare -s SENS_OR_ADIS164X 0
115-
then
116-
adis16448 -S start
117-
fi
118-
if param compare -s SENS_OR_ADIS164X 4
119-
then
120-
adis16448 -S start -R 4
121-
fi
122-
fi
123-
124128
# Hall effect sensors si7210
125129
# Potentially remove the -k option if possible and improve the startup if possible
126130
if param greater CAL_AV_AOA_ID -1
@@ -213,5 +217,3 @@ fi
213217
###############################################################################
214218
# End Optional drivers #
215219
###############################################################################
216-
217-
sensors start

‎ROMFS/px4fmu_common/init.d/rcS‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -332,6 +332,8 @@ else
332332
#
333333
# board sensors: rc.sensors
334334
#
335+
. ${R}etc/init.d/rc.sensors
336+
335337
set BOARD_RC_SENSORS ${R}etc/init.d/rc.board_sensors
336338
if [ -f $BOARD_RC_SENSORS ]
337339
then
@@ -340,7 +342,7 @@ else
340342
fi
341343
unset BOARD_RC_SENSORS
342344

343-
. ${R}etc/init.d/rc.sensors
345+
sensors start
344346

345347
if param compare -s BAT1_SOURCE 2
346348
then

‎boards/px4/fmu-v2/src/spi.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -159,8 +159,8 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
159159
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10})
160160
}),
161161
initSPIBusExternal(SPI::Bus::SPI4, {
162-
initSPIConfigExternal(SPI::CS{GPIO::PortC, GPIO::Pin14}),
163162
initSPIConfigExternal(SPI::CS{GPIO::PortE, GPIO::Pin4}),
163+
initSPIConfigExternal(SPI::CS{GPIO::PortC, GPIO::Pin14}),
164164
}),
165165
}),
166166

‎boards/px4/fmu-v3/src/spi.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -159,8 +159,8 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
159159
initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10})
160160
}),
161161
initSPIBusExternal(SPI::Bus::SPI4, {
162-
initSPIConfigExternal(SPI::CS{GPIO::PortC, GPIO::Pin14}),
163162
initSPIConfigExternal(SPI::CS{GPIO::PortE, GPIO::Pin4}),
163+
initSPIConfigExternal(SPI::CS{GPIO::PortC, GPIO::Pin14}),
164164
}),
165165
}),
166166

‎src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp‎

Lines changed: 29 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -105,6 +105,7 @@ ADIS16448::ADIS16448(I2CSPIBusOption bus_option, int bus, uint32_t device, enum
105105
_px4_gyro(get_device_id(), rotation),
106106
_px4_mag(get_device_id(), rotation)
107107
{
108+
_debug_enabled = true;
108109
}
109110

110111
ADIS16448::~ADIS16448()
@@ -160,12 +161,12 @@ int ADIS16448::probe()
160161
}
161162

162163
for (int attempt = 0; attempt < 3; attempt++) {
163-
const uint16_t PROD_ID = RegisterRead(Register::PROD_ID);
164+
const uint16_t PROD_ID = RegisterReadVerified(Register::PROD_ID);
164165

165166
if (PROD_ID == Product_identification) {
166-
const uint16_t SERIAL_NUM = RegisterRead(Register::SERIAL_NUM);
167-
const uint16_t LOT_ID1 = RegisterRead(Register::LOT_ID1);
168-
const uint16_t LOT_ID2 = RegisterRead(Register::LOT_ID2);
167+
const uint16_t SERIAL_NUM = RegisterReadVerified(Register::SERIAL_NUM);
168+
const uint16_t LOT_ID1 = RegisterReadVerified(Register::LOT_ID1);
169+
const uint16_t LOT_ID2 = RegisterReadVerified(Register::LOT_ID2);
169170

170171
PX4_INFO("Serial Number: 0x%02x, Lot ID1: 0x%02x ID2: 0x%02x", SERIAL_NUM, LOT_ID1, LOT_ID2);
171172

@@ -197,7 +198,7 @@ void ADIS16448::RunImpl()
197198
case STATE::WAIT_FOR_RESET:
198199

199200
if (_self_test_passed) {
200-
if ((RegisterRead(Register::PROD_ID) == Product_identification)) {
201+
if ((RegisterReadVerified(Register::PROD_ID) == Product_identification)) {
201202
// if reset succeeded then configure
202203
_state = STATE::CONFIGURE;
203204
ScheduleNow();
@@ -224,7 +225,7 @@ void ADIS16448::RunImpl()
224225
break;
225226

226227
case STATE::SELF_TEST_CHECK: {
227-
const uint16_t MSC_CTRL = RegisterRead(Register::MSC_CTRL);
228+
const uint16_t MSC_CTRL = RegisterReadVerified(Register::MSC_CTRL);
228229

229230
if (MSC_CTRL & MSC_CTRL_BIT::Internal_self_test) {
230231
// self test not finished, check again
@@ -244,7 +245,7 @@ void ADIS16448::RunImpl()
244245

245246
bool test_passed = true;
246247

247-
const uint16_t DIAG_STAT = RegisterRead(Register::DIAG_STAT);
248+
const uint16_t DIAG_STAT = RegisterReadVerified(Register::DIAG_STAT);
248249

249250
if (DIAG_STAT & DIAG_STAT_BIT::Self_test_diagnostic_error_flag) {
250251
PX4_ERR("self test failed");
@@ -477,7 +478,7 @@ void ADIS16448::RunImpl()
477478

478479
bool ADIS16448::Configure()
479480
{
480-
const uint16_t LOT_ID1 = RegisterRead(Register::LOT_ID1);
481+
const uint16_t LOT_ID1 = RegisterReadVerified(Register::LOT_ID1);
481482

482483
// Only enable CRC-16 for verified lots (HACK to support older ADIS16448AMLZ with no explicit detection)
483484
if (LOT_ID1 == 0x1824) {
@@ -603,7 +604,7 @@ bool ADIS16448::RegisterCheck(const register_config_t &reg_cfg)
603604
{
604605
bool success = true;
605606

606-
const uint16_t reg_value = RegisterRead(reg_cfg.reg);
607+
const uint16_t reg_value = RegisterReadVerified(reg_cfg.reg);
607608

608609
if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
609610
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
@@ -628,10 +629,27 @@ uint16_t ADIS16448::RegisterRead(Register reg)
628629
transferhword(cmd, nullptr, 1);
629630
px4_udelay(SPI_STALL_PERIOD);
630631
transferhword(nullptr, cmd, 1);
632+
px4_udelay(SPI_STALL_PERIOD);
631633

632634
return cmd[0];
633635
}
634636

637+
uint16_t ADIS16448::RegisterReadVerified(Register reg)
638+
{
639+
// 3 attempts
640+
for (int i = 0; i < 3; i++) {
641+
uint16_t read1 = RegisterRead(reg);
642+
uint16_t read2 = RegisterRead(reg);
643+
644+
if (read1 == read2) {
645+
return read1;
646+
}
647+
}
648+
649+
// failed
650+
return 0;
651+
}
652+
635653
void ADIS16448::RegisterWrite(Register reg, uint16_t value)
636654
{
637655
set_frequency(SPI_SPEED);
@@ -643,11 +661,12 @@ void ADIS16448::RegisterWrite(Register reg, uint16_t value)
643661
transferhword(cmd, nullptr, 1);
644662
px4_udelay(SPI_STALL_PERIOD);
645663
transferhword(cmd + 1, nullptr, 1);
664+
px4_udelay(SPI_STALL_PERIOD);
646665
}
647666

648667
void ADIS16448::RegisterSetAndClearBits(Register reg, uint16_t setbits, uint16_t clearbits)
649668
{
650-
const uint16_t orig_val = RegisterRead(reg);
669+
const uint16_t orig_val = RegisterReadVerified(reg);
651670

652671
uint16_t val = (orig_val & ~clearbits) | setbits;
653672

‎src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -94,6 +94,7 @@ class ADIS16448 : public device::SPI, public I2CSPIDriver<ADIS16448>
9494
bool RegisterCheck(const register_config_t &reg_cfg);
9595

9696
uint16_t RegisterRead(Register reg);
97+
uint16_t RegisterReadVerified(Register reg);
9798
void RegisterWrite(Register reg, uint16_t value);
9899
void RegisterSetAndClearBits(Register reg, uint16_t setbits, uint16_t clearbits);
99100

‎src/drivers/imu/analog_devices/adis16448/Analog_Devices_ADIS16448_registers.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -65,7 +65,7 @@ namespace Analog_Devices_ADIS16448
6565
static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000; // 2 MHz SPI serial interface
6666
static constexpr uint32_t SPI_SPEED_BURST = 1 * 1000 * 1000; // 1 MHz SPI serial interface for burst read
6767

68-
static constexpr uint32_t SPI_STALL_PERIOD = 10; // 9 us Stall period between data
68+
static constexpr uint32_t SPI_STALL_PERIOD = 11; // 9 us Stall period between data
6969

7070
static constexpr uint16_t DIR_WRITE = 0x80;
7171

‎src/drivers/imu/analog_devices/adis16448/CMakeLists.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -36,7 +36,7 @@ px4_add_module(
3636
MAIN adis16448
3737
COMPILE_FLAGS
3838
${MAX_CUSTOM_OPT_LEVEL}
39-
#-DDEBUG_BUILD
39+
-DDEBUG_BUILD
4040
SRCS
4141
ADIS16448.cpp
4242
ADIS16448.hpp

0 commit comments

Comments
 (0)