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 @@
@@ -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 @@
@@ -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 @@
@@ -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 @@
@@ -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 @@
@@ -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 @@
@@ -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