Browse Source
- include icm20948 on most boards by default - create more test variants for default boards near flash limit (cuav_nora_test, cuav_x7pro_test, holybro_durandal-v1_test)release/1.12
Daniel Agar
4 years ago
committed by
GitHub
57 changed files with 1076 additions and 98 deletions
@ -0,0 +1,139 @@ |
|||||||
|
|
||||||
|
px4_add_board( |
||||||
|
PLATFORM nuttx |
||||||
|
VENDOR cuav |
||||||
|
MODEL nora |
||||||
|
LABEL test |
||||||
|
TOOLCHAIN arm-none-eabi |
||||||
|
ARCHITECTURE cortex-m7 |
||||||
|
ROMFSROOT px4fmu_common |
||||||
|
BUILD_BOOTLOADER |
||||||
|
TESTING |
||||||
|
UAVCAN_INTERFACES 2 |
||||||
|
UAVCAN_TIMER_OVERRIDE 2 |
||||||
|
SERIAL_PORTS |
||||||
|
GPS1:/dev/ttyS0 |
||||||
|
TEL1:/dev/ttyS1 |
||||||
|
GPS2:/dev/ttyS2 |
||||||
|
TEL2:/dev/ttyS3 |
||||||
|
# CONSOLE: /dev/ttyS4 |
||||||
|
# RC: /dev/ttyS5 |
||||||
|
DRIVERS |
||||||
|
adc/ads1115 |
||||||
|
adc/board_adc |
||||||
|
barometer # all available barometer drivers |
||||||
|
batt_smbus |
||||||
|
camera_capture |
||||||
|
camera_trigger |
||||||
|
differential_pressure # all available differential pressure drivers |
||||||
|
distance_sensor # all available distance sensor drivers |
||||||
|
dshot |
||||||
|
gps |
||||||
|
heater |
||||||
|
#imu # all available imu drivers |
||||||
|
imu/analog_devices/adis16448 |
||||||
|
imu/bosch/bmi088 |
||||||
|
imu/invensense/icm20649 |
||||||
|
imu/invensense/icm20689 |
||||||
|
imu/invensense/icm20948 # required for ak09916 mag |
||||||
|
#irlock |
||||||
|
lights # all available light drivers |
||||||
|
lights/rgbled_pwm |
||||||
|
magnetometer # all available magnetometer drivers |
||||||
|
optical_flow # all available optical flow drivers |
||||||
|
osd |
||||||
|
pca9685 |
||||||
|
pca9685_pwm_out |
||||||
|
power_monitor/ina226 |
||||||
|
#protocol_splitter |
||||||
|
pwm_out_sim |
||||||
|
pwm_out |
||||||
|
rc_input |
||||||
|
roboclaw |
||||||
|
rpm |
||||||
|
safety_button |
||||||
|
telemetry # all available telemetry drivers |
||||||
|
test_ppm |
||||||
|
tone_alarm |
||||||
|
uavcan |
||||||
|
MODULES |
||||||
|
airspeed_selector |
||||||
|
attitude_estimator_q |
||||||
|
battery_status |
||||||
|
camera_feedback |
||||||
|
commander |
||||||
|
dataman |
||||||
|
ekf2 |
||||||
|
esc_battery |
||||||
|
events |
||||||
|
flight_mode_manager |
||||||
|
fw_att_control |
||||||
|
fw_pos_control_l1 |
||||||
|
gyro_calibration |
||||||
|
gyro_fft |
||||||
|
land_detector |
||||||
|
landing_target_estimator |
||||||
|
load_mon |
||||||
|
local_position_estimator |
||||||
|
logger |
||||||
|
mavlink |
||||||
|
mc_att_control |
||||||
|
mc_hover_thrust_estimator |
||||||
|
mc_pos_control |
||||||
|
mc_rate_control |
||||||
|
#micrortps_bridge |
||||||
|
navigator |
||||||
|
rc_update |
||||||
|
rover_pos_control |
||||||
|
sensors |
||||||
|
sih |
||||||
|
temperature_compensation |
||||||
|
#uuv_att_control |
||||||
|
#uuv_pos_control |
||||||
|
vmount |
||||||
|
vtol_att_control |
||||||
|
SYSTEMCMDS |
||||||
|
bl_update |
||||||
|
dmesg |
||||||
|
dumpfile |
||||||
|
esc_calib |
||||||
|
gpio |
||||||
|
hardfault_log |
||||||
|
i2cdetect |
||||||
|
led_control |
||||||
|
mft |
||||||
|
mixer |
||||||
|
motor_ramp |
||||||
|
motor_test |
||||||
|
mtd |
||||||
|
nshterm |
||||||
|
param |
||||||
|
perf |
||||||
|
pwm |
||||||
|
reboot |
||||||
|
reflect |
||||||
|
sd_bench |
||||||
|
serial_test |
||||||
|
system_time |
||||||
|
tests # tests and test runner |
||||||
|
top |
||||||
|
topic_listener |
||||||
|
tune_control |
||||||
|
uorb |
||||||
|
usb_connected |
||||||
|
ver |
||||||
|
work_queue |
||||||
|
EXAMPLES |
||||||
|
fake_gps |
||||||
|
#fake_gyro |
||||||
|
#fake_magnetometer |
||||||
|
#fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control |
||||||
|
#hello |
||||||
|
#hwtest # Hardware test |
||||||
|
#matlab_csv_serial |
||||||
|
#px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html |
||||||
|
#px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html |
||||||
|
#rover_steering_control # Rover example app |
||||||
|
#uuv_example_app |
||||||
|
#work_item |
||||||
|
) |
||||||
@ -0,0 +1,139 @@ |
|||||||
|
|
||||||
|
px4_add_board( |
||||||
|
PLATFORM nuttx |
||||||
|
VENDOR cuav |
||||||
|
MODEL x7pro |
||||||
|
LABEL test |
||||||
|
TOOLCHAIN arm-none-eabi |
||||||
|
ARCHITECTURE cortex-m7 |
||||||
|
ROMFSROOT px4fmu_common |
||||||
|
BUILD_BOOTLOADER |
||||||
|
TESTING |
||||||
|
UAVCAN_INTERFACES 2 |
||||||
|
UAVCAN_TIMER_OVERRIDE 2 |
||||||
|
SERIAL_PORTS |
||||||
|
GPS1:/dev/ttyS0 |
||||||
|
TEL1:/dev/ttyS1 |
||||||
|
GPS2:/dev/ttyS2 |
||||||
|
TEL2:/dev/ttyS3 |
||||||
|
# CONSOLE: /dev/ttyS4 |
||||||
|
# RC: /dev/ttyS5 |
||||||
|
DRIVERS |
||||||
|
adc/ads1115 |
||||||
|
adc/board_adc |
||||||
|
barometer # all available barometer drivers |
||||||
|
batt_smbus |
||||||
|
camera_capture |
||||||
|
camera_trigger |
||||||
|
differential_pressure # all available differential pressure drivers |
||||||
|
distance_sensor # all available distance sensor drivers |
||||||
|
dshot |
||||||
|
gps |
||||||
|
heater |
||||||
|
#imu # all available imu drivers |
||||||
|
imu/analog_devices/adis16470 |
||||||
|
imu/bosch/bmi088 |
||||||
|
imu/invensense/icm20649 |
||||||
|
imu/invensense/icm20689 |
||||||
|
imu/invensense/icm20948 # required for ak09916 mag |
||||||
|
#irlock |
||||||
|
lights # all available light drivers |
||||||
|
lights/rgbled_pwm |
||||||
|
magnetometer # all available magnetometer drivers |
||||||
|
optical_flow # all available optical flow drivers |
||||||
|
osd |
||||||
|
pca9685 |
||||||
|
pca9685_pwm_out |
||||||
|
power_monitor/ina226 |
||||||
|
#protocol_splitter |
||||||
|
pwm_out_sim |
||||||
|
pwm_out |
||||||
|
rc_input |
||||||
|
roboclaw |
||||||
|
rpm |
||||||
|
safety_button |
||||||
|
telemetry # all available telemetry drivers |
||||||
|
test_ppm |
||||||
|
tone_alarm |
||||||
|
uavcan |
||||||
|
MODULES |
||||||
|
airspeed_selector |
||||||
|
attitude_estimator_q |
||||||
|
battery_status |
||||||
|
camera_feedback |
||||||
|
commander |
||||||
|
dataman |
||||||
|
ekf2 |
||||||
|
esc_battery |
||||||
|
events |
||||||
|
flight_mode_manager |
||||||
|
fw_att_control |
||||||
|
fw_pos_control_l1 |
||||||
|
gyro_calibration |
||||||
|
gyro_fft |
||||||
|
land_detector |
||||||
|
landing_target_estimator |
||||||
|
load_mon |
||||||
|
local_position_estimator |
||||||
|
logger |
||||||
|
mavlink |
||||||
|
mc_att_control |
||||||
|
mc_hover_thrust_estimator |
||||||
|
mc_pos_control |
||||||
|
mc_rate_control |
||||||
|
#micrortps_bridge |
||||||
|
navigator |
||||||
|
rc_update |
||||||
|
rover_pos_control |
||||||
|
sensors |
||||||
|
sih |
||||||
|
temperature_compensation |
||||||
|
#uuv_att_control |
||||||
|
#uuv_pos_control |
||||||
|
vmount |
||||||
|
vtol_att_control |
||||||
|
SYSTEMCMDS |
||||||
|
bl_update |
||||||
|
dmesg |
||||||
|
dumpfile |
||||||
|
esc_calib |
||||||
|
gpio |
||||||
|
hardfault_log |
||||||
|
i2cdetect |
||||||
|
led_control |
||||||
|
mft |
||||||
|
mixer |
||||||
|
motor_ramp |
||||||
|
motor_test |
||||||
|
mtd |
||||||
|
nshterm |
||||||
|
param |
||||||
|
perf |
||||||
|
pwm |
||||||
|
reboot |
||||||
|
reflect |
||||||
|
sd_bench |
||||||
|
serial_test |
||||||
|
system_time |
||||||
|
tests # tests and test runner |
||||||
|
top |
||||||
|
topic_listener |
||||||
|
tune_control |
||||||
|
uorb |
||||||
|
usb_connected |
||||||
|
ver |
||||||
|
work_queue |
||||||
|
EXAMPLES |
||||||
|
fake_gps |
||||||
|
#fake_gyro |
||||||
|
#fake_magnetometer |
||||||
|
#fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control |
||||||
|
#hello |
||||||
|
#hwtest # Hardware test |
||||||
|
#matlab_csv_serial |
||||||
|
#px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html |
||||||
|
#px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html |
||||||
|
#rover_steering_control # Rover example app |
||||||
|
#uuv_example_app |
||||||
|
#work_item |
||||||
|
) |
||||||
@ -0,0 +1,135 @@ |
|||||||
|
|
||||||
|
px4_add_board( |
||||||
|
PLATFORM nuttx |
||||||
|
VENDOR holybro |
||||||
|
MODEL durandal-v1 |
||||||
|
LABEL test |
||||||
|
TOOLCHAIN arm-none-eabi |
||||||
|
ARCHITECTURE cortex-m7 |
||||||
|
ROMFSROOT px4fmu_common |
||||||
|
BUILD_BOOTLOADER |
||||||
|
IO px4_io-v2_default |
||||||
|
TESTING |
||||||
|
UAVCAN_INTERFACES 2 |
||||||
|
SERIAL_PORTS |
||||||
|
GPS1:/dev/ttyS0 |
||||||
|
TEL1:/dev/ttyS1 |
||||||
|
TEL2:/dev/ttyS2 |
||||||
|
TEL3:/dev/ttyS4 |
||||||
|
TEL4:/dev/ttyS3 |
||||||
|
DRIVERS |
||||||
|
#adc/ads1115 |
||||||
|
adc/board_adc |
||||||
|
barometer # all available barometer drivers |
||||||
|
batt_smbus |
||||||
|
camera_capture |
||||||
|
camera_trigger |
||||||
|
differential_pressure # all available differential pressure drivers |
||||||
|
distance_sensor # all available distance sensor drivers |
||||||
|
dshot |
||||||
|
gps |
||||||
|
heater |
||||||
|
#imu # all available imu drivers |
||||||
|
#imu/analog_devices/adis16448 |
||||||
|
imu/bosch/bmi088 |
||||||
|
imu/invensense/icm20689 |
||||||
|
imu/invensense/icm20948 # required for ak09916 mag |
||||||
|
#irlock |
||||||
|
lights # all available light drivers |
||||||
|
magnetometer # all available magnetometer drivers |
||||||
|
optical_flow # all available optical flow drivers |
||||||
|
#osd |
||||||
|
#pca9685 |
||||||
|
#pca9685_pwm_out |
||||||
|
power_monitor/ina226 |
||||||
|
#protocol_splitter |
||||||
|
pwm_out_sim |
||||||
|
pwm_out |
||||||
|
px4io |
||||||
|
#roboclaw |
||||||
|
#rpm |
||||||
|
telemetry # all available telemetry drivers |
||||||
|
test_ppm |
||||||
|
tone_alarm |
||||||
|
uavcan |
||||||
|
MODULES |
||||||
|
airspeed_selector |
||||||
|
#attitude_estimator_q |
||||||
|
battery_status |
||||||
|
camera_feedback |
||||||
|
commander |
||||||
|
dataman |
||||||
|
ekf2 |
||||||
|
#esc_battery |
||||||
|
events |
||||||
|
flight_mode_manager |
||||||
|
fw_att_control |
||||||
|
fw_pos_control_l1 |
||||||
|
gyro_calibration |
||||||
|
gyro_fft |
||||||
|
land_detector |
||||||
|
landing_target_estimator |
||||||
|
load_mon |
||||||
|
#local_position_estimator |
||||||
|
logger |
||||||
|
mavlink |
||||||
|
mc_att_control |
||||||
|
mc_hover_thrust_estimator |
||||||
|
mc_pos_control |
||||||
|
mc_rate_control |
||||||
|
#micrortps_bridge |
||||||
|
navigator |
||||||
|
rc_update |
||||||
|
#rover_pos_control |
||||||
|
sensors |
||||||
|
#sih |
||||||
|
#temperature_compensation |
||||||
|
#uuv_att_control |
||||||
|
#uuv_pos_control |
||||||
|
vmount |
||||||
|
vtol_att_control |
||||||
|
SYSTEMCMDS |
||||||
|
bl_update |
||||||
|
dmesg |
||||||
|
dumpfile |
||||||
|
esc_calib |
||||||
|
gpio |
||||||
|
hardfault_log |
||||||
|
i2cdetect |
||||||
|
led_control |
||||||
|
mft |
||||||
|
mixer |
||||||
|
motor_ramp |
||||||
|
motor_test |
||||||
|
mtd |
||||||
|
nshterm |
||||||
|
param |
||||||
|
perf |
||||||
|
pwm |
||||||
|
reboot |
||||||
|
#reflect |
||||||
|
sd_bench |
||||||
|
serial_test |
||||||
|
system_time |
||||||
|
tests # tests and test runner |
||||||
|
top |
||||||
|
topic_listener |
||||||
|
tune_control |
||||||
|
uorb |
||||||
|
usb_connected |
||||||
|
ver |
||||||
|
work_queue |
||||||
|
EXAMPLES |
||||||
|
fake_gps |
||||||
|
#fake_gyro |
||||||
|
#fake_magnetometer |
||||||
|
#fixedwing_control # Tutorial code from https://px4.io/dev/example_fixedwing_control |
||||||
|
#hello |
||||||
|
#hwtest # Hardware test |
||||||
|
#matlab_csv_serial |
||||||
|
#px4_mavlink_debug # Tutorial code from http://dev.px4.io/en/debug/debug_values.html |
||||||
|
#px4_simple_app # Tutorial code from http://dev.px4.io/en/apps/hello_sky.html |
||||||
|
#rover_steering_control # Rover example app |
||||||
|
#uuv_example_app |
||||||
|
#work_item |
||||||
|
) |
||||||
@ -0,0 +1,289 @@ |
|||||||
|
/****************************************************************************
|
||||||
|
* |
||||||
|
* Copyright (c) 2021 PX4 Development Team. All rights reserved. |
||||||
|
* |
||||||
|
* Redistribution and use in source and binary forms, with or without |
||||||
|
* modification, are permitted provided that the following conditions |
||||||
|
* are met: |
||||||
|
* |
||||||
|
* 1. Redistributions of source code must retain the above copyright |
||||||
|
* notice, this list of conditions and the following disclaimer. |
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright |
||||||
|
* notice, this list of conditions and the following disclaimer in |
||||||
|
* the documentation and/or other materials provided with the |
||||||
|
* distribution. |
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be |
||||||
|
* used to endorse or promote products derived from this software |
||||||
|
* without specific prior written permission. |
||||||
|
* |
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||||
|
* POSSIBILITY OF SUCH DAMAGE. |
||||||
|
* |
||||||
|
****************************************************************************/ |
||||||
|
|
||||||
|
#include "ICM20948_I2C_Passthrough.hpp" |
||||||
|
|
||||||
|
using namespace time_literals; |
||||||
|
|
||||||
|
static constexpr int16_t combine(uint8_t msb, uint8_t lsb) |
||||||
|
{ |
||||||
|
return (msb << 8u) | lsb; |
||||||
|
} |
||||||
|
|
||||||
|
ICM20948_I2C_Passthrough::ICM20948_I2C_Passthrough(I2CSPIBusOption bus_option, int bus, int bus_frequency) : |
||||||
|
I2C(DRV_IMU_DEVTYPE_ICM20948, MODULE_NAME, bus, I2C_ADDRESS_DEFAULT, bus_frequency), |
||||||
|
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus) |
||||||
|
{ |
||||||
|
} |
||||||
|
|
||||||
|
ICM20948_I2C_Passthrough::~ICM20948_I2C_Passthrough() |
||||||
|
{ |
||||||
|
perf_free(_bad_register_perf); |
||||||
|
perf_free(_bad_transfer_perf); |
||||||
|
} |
||||||
|
|
||||||
|
int ICM20948_I2C_Passthrough::init() |
||||||
|
{ |
||||||
|
int ret = I2C::init(); |
||||||
|
|
||||||
|
if (ret != PX4_OK) { |
||||||
|
DEVICE_DEBUG("I2C::init failed (%i)", ret); |
||||||
|
return ret; |
||||||
|
} |
||||||
|
|
||||||
|
return Reset() ? 0 : -1; |
||||||
|
} |
||||||
|
|
||||||
|
bool ICM20948_I2C_Passthrough::Reset() |
||||||
|
{ |
||||||
|
_state = STATE::RESET; |
||||||
|
ScheduleClear(); |
||||||
|
ScheduleNow(); |
||||||
|
return true; |
||||||
|
} |
||||||
|
|
||||||
|
void ICM20948_I2C_Passthrough::print_status() |
||||||
|
{ |
||||||
|
I2CSPIDriverBase::print_status(); |
||||||
|
|
||||||
|
PX4_INFO("temperature: %.1f degC", (double)_temperature); |
||||||
|
|
||||||
|
perf_print_counter(_bad_register_perf); |
||||||
|
perf_print_counter(_bad_transfer_perf); |
||||||
|
} |
||||||
|
|
||||||
|
int ICM20948_I2C_Passthrough::probe() |
||||||
|
{ |
||||||
|
const uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); |
||||||
|
|
||||||
|
if (whoami != WHOAMI) { |
||||||
|
DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); |
||||||
|
return PX4_ERROR; |
||||||
|
} |
||||||
|
|
||||||
|
return PX4_OK; |
||||||
|
} |
||||||
|
|
||||||
|
void ICM20948_I2C_Passthrough::RunImpl() |
||||||
|
{ |
||||||
|
const hrt_abstime now = hrt_absolute_time(); |
||||||
|
|
||||||
|
switch (_state) { |
||||||
|
case STATE::RESET: |
||||||
|
// PWR_MGMT_1: Device Reset
|
||||||
|
RegisterWrite(Register::BANK_0::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET); |
||||||
|
_reset_timestamp = now; |
||||||
|
_failure_count = 0; |
||||||
|
_state = STATE::WAIT_FOR_RESET; |
||||||
|
ScheduleDelayed(1_ms); |
||||||
|
break; |
||||||
|
|
||||||
|
case STATE::WAIT_FOR_RESET: |
||||||
|
|
||||||
|
// The reset value is 0x00 for all registers other than the registers below
|
||||||
|
if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI) && (RegisterRead(Register::BANK_0::PWR_MGMT_1) == 0x41)) { |
||||||
|
// Wakeup and reset
|
||||||
|
RegisterWrite(Register::BANK_0::USER_CTRL, USER_CTRL_BIT::SRAM_RST | USER_CTRL_BIT::I2C_MST_RST); |
||||||
|
|
||||||
|
// if reset succeeded then configure
|
||||||
|
_state = STATE::CONFIGURE; |
||||||
|
ScheduleDelayed(1_ms); |
||||||
|
|
||||||
|
} else { |
||||||
|
// RESET not complete
|
||||||
|
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { |
||||||
|
PX4_DEBUG("Reset failed, retrying"); |
||||||
|
_state = STATE::RESET; |
||||||
|
ScheduleDelayed(100_ms); |
||||||
|
|
||||||
|
} else { |
||||||
|
PX4_DEBUG("Reset not complete, check again in 10 ms"); |
||||||
|
ScheduleDelayed(10_ms); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
break; |
||||||
|
|
||||||
|
case STATE::CONFIGURE: |
||||||
|
if (Configure()) { |
||||||
|
_state = STATE::READ; |
||||||
|
ScheduleOnInterval(500_ms); |
||||||
|
|
||||||
|
} else { |
||||||
|
// CONFIGURE not complete
|
||||||
|
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { |
||||||
|
PX4_DEBUG("Configure failed, resetting"); |
||||||
|
_state = STATE::RESET; |
||||||
|
|
||||||
|
} else { |
||||||
|
PX4_DEBUG("Configure failed, retrying"); |
||||||
|
} |
||||||
|
|
||||||
|
ScheduleDelayed(10_ms); |
||||||
|
} |
||||||
|
|
||||||
|
break; |
||||||
|
|
||||||
|
case STATE::READ: { |
||||||
|
if (hrt_elapsed_time(&_last_config_check_timestamp) > 1000_ms) { |
||||||
|
// check configuration registers periodically or immediately following any failure
|
||||||
|
if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])) { |
||||||
|
_last_config_check_timestamp = now; |
||||||
|
_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg; |
||||||
|
|
||||||
|
} else { |
||||||
|
// register check failed, force reset
|
||||||
|
perf_count(_bad_register_perf); |
||||||
|
Reset(); |
||||||
|
} |
||||||
|
|
||||||
|
} else { |
||||||
|
// periodically update temperature (~1 Hz)
|
||||||
|
if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) { |
||||||
|
UpdateTemperature(); |
||||||
|
_temperature_update_timestamp = now; |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
break; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
void ICM20948_I2C_Passthrough::SelectRegisterBank(enum REG_BANK_SEL_BIT bank) |
||||||
|
{ |
||||||
|
if (bank != _last_register_bank) { |
||||||
|
// select BANK_0
|
||||||
|
uint8_t cmd_bank_sel[2]; |
||||||
|
cmd_bank_sel[0] = static_cast<uint8_t>(Register::BANK_0::REG_BANK_SEL); |
||||||
|
cmd_bank_sel[1] = bank; |
||||||
|
|
||||||
|
transfer(cmd_bank_sel, 2, nullptr, 0); |
||||||
|
|
||||||
|
_last_register_bank = bank; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
bool ICM20948_I2C_Passthrough::Configure() |
||||||
|
{ |
||||||
|
// first set and clear all configured register bits
|
||||||
|
for (const auto ®_cfg : _register_bank0_cfg) { |
||||||
|
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); |
||||||
|
} |
||||||
|
|
||||||
|
// now check that all are configured
|
||||||
|
bool success = true; |
||||||
|
|
||||||
|
for (const auto ®_cfg : _register_bank0_cfg) { |
||||||
|
if (!RegisterCheck(reg_cfg)) { |
||||||
|
success = false; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
return success; |
||||||
|
} |
||||||
|
|
||||||
|
template <typename T> |
||||||
|
bool ICM20948_I2C_Passthrough::RegisterCheck(const T ®_cfg) |
||||||
|
{ |
||||||
|
bool success = true; |
||||||
|
|
||||||
|
const uint8_t reg_value = RegisterRead(reg_cfg.reg); |
||||||
|
|
||||||
|
if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) { |
||||||
|
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits); |
||||||
|
success = false; |
||||||
|
} |
||||||
|
|
||||||
|
if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) { |
||||||
|
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits); |
||||||
|
success = false; |
||||||
|
} |
||||||
|
|
||||||
|
return success; |
||||||
|
} |
||||||
|
|
||||||
|
template <typename T> |
||||||
|
uint8_t ICM20948_I2C_Passthrough::RegisterRead(T reg) |
||||||
|
{ |
||||||
|
SelectRegisterBank(reg); |
||||||
|
|
||||||
|
uint8_t cmd = static_cast<uint8_t>(reg); |
||||||
|
uint8_t value = 0; |
||||||
|
transfer(&cmd, 1, &value, 1); |
||||||
|
return value; |
||||||
|
} |
||||||
|
|
||||||
|
template <typename T> |
||||||
|
void ICM20948_I2C_Passthrough::RegisterWrite(T reg, uint8_t value) |
||||||
|
{ |
||||||
|
SelectRegisterBank(reg); |
||||||
|
|
||||||
|
uint8_t cmd[2]; |
||||||
|
cmd[0] = static_cast<uint8_t>(reg); |
||||||
|
cmd[1] = value; |
||||||
|
transfer(cmd, sizeof(cmd), nullptr, 0); |
||||||
|
} |
||||||
|
|
||||||
|
template <typename T> |
||||||
|
void ICM20948_I2C_Passthrough::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits) |
||||||
|
{ |
||||||
|
const uint8_t orig_val = RegisterRead(reg); |
||||||
|
|
||||||
|
uint8_t val = (orig_val & ~clearbits) | setbits; |
||||||
|
|
||||||
|
if (orig_val != val) { |
||||||
|
RegisterWrite(reg, val); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
void ICM20948_I2C_Passthrough::UpdateTemperature() |
||||||
|
{ |
||||||
|
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); |
||||||
|
|
||||||
|
// read current temperature
|
||||||
|
uint8_t cmd = static_cast<uint8_t>(Register::BANK_0::TEMP_OUT_H); |
||||||
|
uint8_t temperature_buf[2] {}; |
||||||
|
|
||||||
|
if (transfer(&cmd, 1, temperature_buf, 2) != PX4_OK) { |
||||||
|
perf_count(_bad_transfer_perf); |
||||||
|
return; |
||||||
|
} |
||||||
|
|
||||||
|
const int16_t TEMP_OUT = combine(temperature_buf[0], temperature_buf[1]); |
||||||
|
const float TEMP_degC = (TEMP_OUT / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET; |
||||||
|
|
||||||
|
if (PX4_ISFINITE(TEMP_degC)) { |
||||||
|
_temperature = TEMP_degC; |
||||||
|
} |
||||||
|
} |
||||||
@ -0,0 +1,119 @@ |
|||||||
|
/****************************************************************************
|
||||||
|
* |
||||||
|
* Copyright (c) 2021 PX4 Development Team. All rights reserved. |
||||||
|
* |
||||||
|
* Redistribution and use in source and binary forms, with or without |
||||||
|
* modification, are permitted provided that the following conditions |
||||||
|
* are met: |
||||||
|
* |
||||||
|
* 1. Redistributions of source code must retain the above copyright |
||||||
|
* notice, this list of conditions and the following disclaimer. |
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright |
||||||
|
* notice, this list of conditions and the following disclaimer in |
||||||
|
* the documentation and/or other materials provided with the |
||||||
|
* distribution. |
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be |
||||||
|
* used to endorse or promote products derived from this software |
||||||
|
* without specific prior written permission. |
||||||
|
* |
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||||
|
* POSSIBILITY OF SUCH DAMAGE. |
||||||
|
* |
||||||
|
****************************************************************************/ |
||||||
|
|
||||||
|
/**
|
||||||
|
* @file ICM20948_I2C_Passthrough.hpp |
||||||
|
* |
||||||
|
* Driver for the Invensense ICM20948 connected via SPI. |
||||||
|
* |
||||||
|
*/ |
||||||
|
|
||||||
|
#pragma once |
||||||
|
|
||||||
|
#include "InvenSense_ICM20948_registers.hpp" |
||||||
|
|
||||||
|
#include <drivers/drv_hrt.h> |
||||||
|
#include <lib/drivers/device/i2c.h> |
||||||
|
#include <lib/perf/perf_counter.h> |
||||||
|
#include <px4_platform_common/atomic.h> |
||||||
|
#include <px4_platform_common/i2c_spi_buses.h> |
||||||
|
|
||||||
|
using namespace InvenSense_ICM20948; |
||||||
|
|
||||||
|
class ICM20948_I2C_Passthrough : public device::I2C, public I2CSPIDriver<ICM20948_I2C_Passthrough> |
||||||
|
{ |
||||||
|
public: |
||||||
|
ICM20948_I2C_Passthrough(I2CSPIBusOption bus_option, int bus, int bus_frequency); |
||||||
|
~ICM20948_I2C_Passthrough() override; |
||||||
|
|
||||||
|
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, |
||||||
|
int runtime_instance); |
||||||
|
static void print_usage(); |
||||||
|
|
||||||
|
void RunImpl(); |
||||||
|
|
||||||
|
int init() override; |
||||||
|
void print_status() override; |
||||||
|
|
||||||
|
private: |
||||||
|
struct register_bank0_config_t { |
||||||
|
Register::BANK_0 reg; |
||||||
|
uint8_t set_bits{0}; |
||||||
|
uint8_t clear_bits{0}; |
||||||
|
}; |
||||||
|
|
||||||
|
int probe() override; |
||||||
|
|
||||||
|
bool Reset(); |
||||||
|
|
||||||
|
bool Configure(); |
||||||
|
|
||||||
|
void SelectRegisterBank(enum REG_BANK_SEL_BIT bank); |
||||||
|
void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); } |
||||||
|
|
||||||
|
template <typename T> bool RegisterCheck(const T ®_cfg); |
||||||
|
|
||||||
|
template <typename T> uint8_t RegisterRead(T reg); |
||||||
|
template <typename T> void RegisterWrite(T reg, uint8_t value); |
||||||
|
template <typename T> void RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits); |
||||||
|
|
||||||
|
void UpdateTemperature(); |
||||||
|
|
||||||
|
perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; |
||||||
|
perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; |
||||||
|
|
||||||
|
hrt_abstime _reset_timestamp{0}; |
||||||
|
hrt_abstime _last_config_check_timestamp{0}; |
||||||
|
hrt_abstime _temperature_update_timestamp{0}; |
||||||
|
int _failure_count{0}; |
||||||
|
|
||||||
|
float _temperature{NAN}; |
||||||
|
|
||||||
|
enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0}; |
||||||
|
|
||||||
|
enum class STATE : uint8_t { |
||||||
|
RESET, |
||||||
|
WAIT_FOR_RESET, |
||||||
|
CONFIGURE, |
||||||
|
READ, |
||||||
|
} _state{STATE::RESET}; |
||||||
|
|
||||||
|
uint8_t _checked_register_bank0{0}; |
||||||
|
static constexpr uint8_t size_register_bank0_cfg{3}; |
||||||
|
register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] { |
||||||
|
// Register | Set bits, Clear bits
|
||||||
|
{ Register::BANK_0::USER_CTRL, 0, USER_CTRL_BIT::DMP_EN | USER_CTRL_BIT::I2C_MST_EN | USER_CTRL_BIT::I2C_IF_DIS }, |
||||||
|
{ Register::BANK_0::PWR_MGMT_1, 0, PWR_MGMT_1_BIT::DEVICE_RESET | PWR_MGMT_1_BIT::SLEEP }, |
||||||
|
{ Register::BANK_0::INT_PIN_CFG, INT_PIN_CFG_BIT::BYPASS_EN, 0 }, |
||||||
|
}; |
||||||
|
}; |
||||||
@ -0,0 +1,98 @@ |
|||||||
|
/****************************************************************************
|
||||||
|
* |
||||||
|
* Copyright (c) 2021 PX4 Development Team. All rights reserved. |
||||||
|
* |
||||||
|
* Redistribution and use in source and binary forms, with or without |
||||||
|
* modification, are permitted provided that the following conditions |
||||||
|
* are met: |
||||||
|
* |
||||||
|
* 1. Redistributions of source code must retain the above copyright |
||||||
|
* notice, this list of conditions and the following disclaimer. |
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright |
||||||
|
* notice, this list of conditions and the following disclaimer in |
||||||
|
* the documentation and/or other materials provided with the |
||||||
|
* distribution. |
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be |
||||||
|
* used to endorse or promote products derived from this software |
||||||
|
* without specific prior written permission. |
||||||
|
* |
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||||
|
* POSSIBILITY OF SUCH DAMAGE. |
||||||
|
* |
||||||
|
****************************************************************************/ |
||||||
|
|
||||||
|
#include "ICM20948_I2C_Passthrough.hpp" |
||||||
|
|
||||||
|
#include <px4_platform_common/getopt.h> |
||||||
|
#include <px4_platform_common/module.h> |
||||||
|
|
||||||
|
void ICM20948_I2C_Passthrough::print_usage() |
||||||
|
{ |
||||||
|
PRINT_MODULE_USAGE_NAME("icm20948_i2c_passthrough", "driver"); |
||||||
|
PRINT_MODULE_USAGE_SUBCATEGORY("imu"); |
||||||
|
PRINT_MODULE_USAGE_COMMAND("start"); |
||||||
|
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); |
||||||
|
PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x39); |
||||||
|
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); |
||||||
|
PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); |
||||||
|
} |
||||||
|
|
||||||
|
I2CSPIDriverBase *ICM20948_I2C_Passthrough::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, |
||||||
|
int runtime_instance) |
||||||
|
{ |
||||||
|
ICM20948_I2C_Passthrough *instance = new ICM20948_I2C_Passthrough(iterator.configuredBusOption(), iterator.bus(), |
||||||
|
cli.bus_frequency); |
||||||
|
|
||||||
|
if (!instance) { |
||||||
|
PX4_ERR("alloc failed"); |
||||||
|
return nullptr; |
||||||
|
} |
||||||
|
|
||||||
|
if (OK != instance->init()) { |
||||||
|
delete instance; |
||||||
|
return nullptr; |
||||||
|
} |
||||||
|
|
||||||
|
return instance; |
||||||
|
} |
||||||
|
|
||||||
|
extern "C" int icm20948_i2c_passthrough_main(int argc, char *argv[]) |
||||||
|
{ |
||||||
|
using ThisDriver = ICM20948_I2C_Passthrough; |
||||||
|
BusCLIArguments cli{true, false}; |
||||||
|
cli.default_i2c_frequency = I2C_SPEED; |
||||||
|
|
||||||
|
const char *verb = cli.parseDefaultArguments(argc, argv); |
||||||
|
|
||||||
|
if (!verb) { |
||||||
|
ThisDriver::print_usage(); |
||||||
|
return -1; |
||||||
|
} |
||||||
|
|
||||||
|
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_ICM20948); |
||||||
|
|
||||||
|
if (!strcmp(verb, "start")) { |
||||||
|
return ThisDriver::module_start(cli, iterator); |
||||||
|
} |
||||||
|
|
||||||
|
if (!strcmp(verb, "stop")) { |
||||||
|
return ThisDriver::module_stop(iterator); |
||||||
|
} |
||||||
|
|
||||||
|
if (!strcmp(verb, "status")) { |
||||||
|
return ThisDriver::module_status(iterator); |
||||||
|
} |
||||||
|
|
||||||
|
ThisDriver::print_usage(); |
||||||
|
return -1; |
||||||
|
} |
||||||
Loading…
Reference in new issue