BMI055::BMI055(const char *name, const char *devname, int bus, uint32_t device, enum spi_mode_e mode,
uint32_t frequency, enum Rotation rotation):
SPI(name, devname, bus, device, mode, frequency),
_whoami(0),
_call{},
_call_interval(0),
_dlpf_freq(0),
_sample_perf(perf_alloc(PC_ELAPSED, "bmi055_read")),
_bad_transfers(perf_alloc(PC_COUNT, "bmi055_bad_transfers")),
_bad_registers(perf_alloc(PC_COUNT, "bmi055_bad_registers")),
_good_transfers(perf_alloc(PC_COUNT, "bmi055_good_transfers")),
_reset_retries(perf_alloc(PC_COUNT, "bmi055_reset_retries")),
_duplicates(perf_alloc(PC_COUNT, "bmi055_duplicates")),
_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")),
_register_wait(0),
_reset_wait(0),
_rotation(rotation),
_checked_next(0)
{
}
MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const char *path_accel,
const char *path_gyro, const char *path_mag,
enum Rotation rotation) :
CDev("MPU9250", path_accel),
_interface(interface),
_gyro(new MPU9250_gyro(this, path_gyro)),
_mag(new MPU9250_mag(this, mag_interface, path_mag)),
_whoami(0),
#if defined(USE_I2C)
_work {},
_use_hrt(false),
#else
_use_hrt(true),
#endif
_call {},
_call_interval(0),
_accel_reports(nullptr),
_accel_scale{},
_accel_range_scale(0.0f),
_accel_range_m_s2(0.0f),
_accel_topic(nullptr),
_accel_orb_class_instance(-1),
_accel_class_instance(-1),
_gyro_reports(nullptr),
_gyro_scale{},
_gyro_range_scale(0.0f),
_gyro_range_rad_s(0.0f),
_dlpf_freq(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ),
_sample_rate(1000),
_accel_reads(perf_alloc(PC_COUNT, "mpu9250_acc_read")),
_gyro_reads(perf_alloc(PC_COUNT, "mpu9250_gyro_read")),
_sample_perf(perf_alloc(PC_ELAPSED, "mpu9250_read")),
_bad_transfers(perf_alloc(PC_COUNT, "mpu9250_bad_trans")),
_bad_registers(perf_alloc(PC_COUNT, "mpu9250_bad_reg")),
_good_transfers(perf_alloc(PC_COUNT, "mpu9250_good_trans")),
_reset_retries(perf_alloc(PC_COUNT, "mpu9250_reset")),
_duplicates(perf_alloc(PC_COUNT, "mpu9250_dupe")),
_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")),
_register_wait(0),
_reset_wait(0),
_accel_filter_x(MPU9250_ACCEL_DEFAULT_RATE, MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_y(MPU9250_ACCEL_DEFAULT_RATE, MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_z(MPU9250_ACCEL_DEFAULT_RATE, MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_x(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_y(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_z(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_accel_int(1000000 / MPU9250_ACCEL_MAX_OUTPUT_RATE),
_gyro_int(1000000 / MPU9250_GYRO_MAX_OUTPUT_RATE, true),
_rotation(rotation),
_checked_next(0),
_last_temperature(0),
_last_accel_data{},
_got_duplicate(false)
{
// disable debug() calls
_debug_enabled = false;
/* Set device parameters and make sure parameters of the bus device are adopted */
_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU9250;
_device_id.devid_s.bus = _interface->get_device_bus();;
_device_id.devid_s.address = _interface->get_device_address();;
/* Prime _gyro with parents devid. */
/* Set device parameters and make sure parameters of the bus device are adopted */
_gyro->_device_id.devid = _device_id.devid;
_gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_MPU9250;
_gyro->_device_id.devid_s.bus = _interface->get_device_bus();
_gyro->_device_id.devid_s.address = _interface->get_device_address();
/* Prime _mag with parents devid. */
_mag->_device_id.devid = _device_id.devid;
_mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_MPU9250;
/* For an independent mag, ensure that it is connected to the i2c bus */
// default accel scale factors
_accel_scale.x_offset = 0;
_accel_scale.x_scale = 1.0f;
_accel_scale.y_offset = 0;
_accel_scale.y_scale = 1.0f;
_accel_scale.z_offset = 0;
_accel_scale.z_scale = 1.0f;
// default gyro scale factors
_gyro_scale.x_offset = 0;
_gyro_scale.x_scale = 1.0f;
_gyro_scale.y_offset = 0;
_gyro_scale.y_scale = 1.0f;
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
memset(&_call, 0, sizeof(_call));
#if defined(USE_I2C)
memset(&_work, 0, sizeof(_work));
#endif
}
MulticopterAttitudeControl::MulticopterAttitudeControl() :
_task_should_exit(false),
_control_task(-1),
/* subscriptions */
_ctrl_state_sub(-1),
_v_att_sp_sub(-1),
_v_control_mode_sub(-1),
_params_sub(-1),
_manual_control_sp_sub(-1),
_armed_sub(-1),
_vehicle_status_sub(-1),
/* publications */
_v_rates_sp_pub(nullptr),
_actuators_0_pub(nullptr),
_controller_status_pub(nullptr),
_rates_sp_id(0),
_actuators_id(0),
_actuators_0_circuit_breaker_enabled(false),
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")),
_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")),
_ts_opt_recovery(nullptr)
{
memset(&_ctrl_state, 0, sizeof(_ctrl_state));
memset(&_v_att_sp, 0, sizeof(_v_att_sp));
memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
memset(&_v_control_mode, 0, sizeof(_v_control_mode));
memset(&_actuators, 0, sizeof(_actuators));
memset(&_armed, 0, sizeof(_armed));
memset(&_vehicle_status, 0, sizeof(_vehicle_status));
memset(&_motor_limits, 0, sizeof(_motor_limits));
memset(&_controller_status, 0, sizeof(_controller_status));
_vehicle_status.is_rotary_wing = true;
_params.att_p.zero();
_params.rate_p.zero();
_params.rate_i.zero();
_params.rate_d.zero();
_params.rate_ff.zero();
_params.yaw_ff = 0.0f;
_params.roll_rate_max = 0.0f;
_params.pitch_rate_max = 0.0f;
_params.yaw_rate_max = 0.0f;
_params.mc_rate_max.zero();
_params.acro_rate_max.zero();
_params.rattitude_thres = 1.0f;
_rates_prev.zero();
_rates_sp.zero();
_rates_sp_prev.zero();
_rates_int.zero();
_thrust_sp = 0.0f;
_att_control.zero();
_I.identity();
_params_handles.roll_p = param_find("MC_ROLL_P");
_params_handles.roll_rate_p = param_find("MC_ROLLRATE_P");
_params_handles.roll_rate_i = param_find("MC_ROLLRATE_I");
_params_handles.roll_rate_d = param_find("MC_ROLLRATE_D");
_params_handles.roll_rate_ff = param_find("MC_ROLLRATE_FF");
_params_handles.pitch_p = param_find("MC_PITCH_P");
_params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P");
_params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I");
_params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D");
_params_handles.pitch_rate_ff = param_find("MC_PITCHRATE_FF");
_params_handles.yaw_p = param_find("MC_YAW_P");
_params_handles.yaw_rate_p = param_find("MC_YAWRATE_P");
_params_handles.yaw_rate_i = param_find("MC_YAWRATE_I");
_params_handles.yaw_rate_d = param_find("MC_YAWRATE_D");
_params_handles.yaw_rate_ff = param_find("MC_YAWRATE_FF");
_params_handles.yaw_ff = param_find("MC_YAW_FF");
_params_handles.roll_rate_max = param_find("MC_ROLLRATE_MAX");
_params_handles.pitch_rate_max = param_find("MC_PITCHRATE_MAX");
_params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX");
_params_handles.acro_roll_max = param_find("MC_ACRO_R_MAX");
_params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX");
_params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX");
_params_handles.rattitude_thres = param_find("MC_RATT_TH");
_params_handles.vtol_type = param_find("VT_TYPE");
_params_handles.roll_tc = param_find("MC_ROLL_TC");
_params_handles.pitch_tc = param_find("MC_PITCH_TC");
/* fetch initial parameter values */
parameters_update();
if (_params.vtol_type == 0) {
// the vehicle is a tailsitter, use optimal recovery control strategy
_ts_opt_recovery = new TailsitterRecovery();
}
}
BMI160::BMI160(int bus, const char *path_accel, const char *path_gyro, uint32_t device, enum Rotation rotation) :
SPI("BMI160", path_accel, bus, device, SPIDEV_MODE3, BMI160_BUS_SPEED),
_gyro(new BMI160_gyro(this, path_gyro)),
_whoami(0),
_call{},
_call_interval(0),
_accel_reports(nullptr),
_accel_scale{},
_accel_range_scale(0.0f),
_accel_range_m_s2(0.0f),
_accel_topic(nullptr),
_accel_orb_class_instance(-1),
_accel_class_instance(-1),
_gyro_reports(nullptr),
_gyro_scale{},
_gyro_range_scale(0.0f),
_gyro_range_rad_s(0.0f),
_dlpf_freq(0),
_accel_sample_rate(BMI160_ACCEL_DEFAULT_RATE),
_gyro_sample_rate(BMI160_GYRO_DEFAULT_RATE),
_accel_reads(perf_alloc(PC_COUNT, "bmi160_accel_read")),
_gyro_reads(perf_alloc(PC_COUNT, "bmi160_gyro_read")),
_sample_perf(perf_alloc(PC_ELAPSED, "bmi160_read")),
_bad_transfers(perf_alloc(PC_COUNT, "bmi160_bad_transfers")),
_bad_registers(perf_alloc(PC_COUNT, "bmi160_bad_registers")),
_good_transfers(perf_alloc(PC_COUNT, "bmi160_good_transfers")),
_reset_retries(perf_alloc(PC_COUNT, "bmi160_reset_retries")),
_duplicates(perf_alloc(PC_COUNT, "bmi160_duplicates")),
_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")),
_register_wait(0),
_reset_wait(0),
_accel_filter_x(BMI160_ACCEL_DEFAULT_RATE, BMI160_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_y(BMI160_ACCEL_DEFAULT_RATE, BMI160_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_z(BMI160_ACCEL_DEFAULT_RATE, BMI160_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_x(BMI160_GYRO_DEFAULT_RATE, BMI160_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_y(BMI160_GYRO_DEFAULT_RATE, BMI160_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_z(BMI160_GYRO_DEFAULT_RATE, BMI160_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_accel_int(1000000 / BMI160_ACCEL_MAX_PUBLISH_RATE),
_gyro_int(1000000 / BMI160_GYRO_MAX_PUBLISH_RATE, true),
_rotation(rotation),
_checked_next(0),
_last_temperature(0),
_last_accel{},
_got_duplicate(false)
{
// disable debug() calls
_debug_enabled = false;
_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_BMI160;
/* Prime _gyro with parents devid. */
_gyro->_device_id.devid = _device_id.devid;
_gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_BMI160;
// default accel scale factors
_accel_scale.x_offset = 0;
_accel_scale.x_scale = 1.0f;
_accel_scale.y_offset = 0;
_accel_scale.y_scale = 1.0f;
_accel_scale.z_offset = 0;
_accel_scale.z_scale = 1.0f;
// default gyro scale factors
_gyro_scale.x_offset = 0;
_gyro_scale.x_scale = 1.0f;
_gyro_scale.y_offset = 0;
_gyro_scale.y_scale = 1.0f;
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
memset(&_call, 0, sizeof(_call));
}
MulticopterAttitudeControl::MulticopterAttitudeControl() :
_task_should_exit(false),
_control_task(-1),
/* subscriptions */
_ctrl_state_sub(-1),
_v_att_sp_sub(-1),
_v_control_mode_sub(-1),
_params_sub(-1),
_manual_control_sp_sub(-1),
_armed_sub(-1),
_vehicle_status_sub(-1),
_motor_limits_sub(-1),
_battery_status_sub(-1),
_sensor_correction_sub(-1),
/* gyro selection */
_gyro_count(1),
_selected_gyro(0),
/* publications */
_v_rates_sp_pub(nullptr),
_actuators_0_pub(nullptr),
_controller_status_pub(nullptr),
_rates_sp_id(0),
_actuators_id(0),
_actuators_0_circuit_breaker_enabled(false),
_ctrl_state{},
_v_att_sp{},
_v_rates_sp{},
_manual_control_sp{},
_v_control_mode{},
_actuators{},
_armed{},
_vehicle_status{},
_motor_limits{},
_controller_status{},
_battery_status{},
_sensor_gyro{},
_sensor_correction{},
_saturation_status{},
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")),
_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")),
_ts_opt_recovery(nullptr)
{
for (uint8_t i = 0; i < MAX_GYRO_COUNT; i++) {
_sensor_gyro_sub[i] = -1;
}
_vehicle_status.is_rotary_wing = true;
_params.att_p.zero();
_params.rate_p.zero();
_params.rate_i.zero();
_params.rate_int_lim.zero();
_params.rate_d.zero();
_params.rate_ff.zero();
_params.yaw_ff = 0.0f;
_params.roll_rate_max = 0.0f;
_params.pitch_rate_max = 0.0f;
_params.yaw_rate_max = 0.0f;
_params.mc_rate_max.zero();
_params.auto_rate_max.zero();
_params.acro_rate_max.zero();
_params.rattitude_thres = 1.0f;
_params.vtol_opt_recovery_enabled = false;
_params.vtol_wv_yaw_rate_scale = 1.0f;
_params.bat_scale_en = 0;
_params.board_rotation = 0;
_params.board_offset[0] = 0.0f;
_params.board_offset[1] = 0.0f;
_params.board_offset[2] = 0.0f;
_rates_prev.zero();
_rates_sp.zero();
_rates_sp_prev.zero();
_rates_int.zero();
_thrust_sp = 0.0f;
_att_control.zero();
_I.identity();
_board_rotation.identity();
_params_handles.roll_p = param_find("MC_ROLL_P");
_params_handles.roll_rate_p = param_find("MC_ROLLRATE_P");
_params_handles.roll_rate_i = param_find("MC_ROLLRATE_I");
_params_handles.roll_rate_integ_lim = param_find("MC_RR_INT_LIM");
_params_handles.roll_rate_d = param_find("MC_ROLLRATE_D");
_params_handles.roll_rate_ff = param_find("MC_ROLLRATE_FF");
_params_handles.pitch_p = param_find("MC_PITCH_P");
_params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P");
_params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I");
_params_handles.pitch_rate_integ_lim = param_find("MC_PR_INT_LIM");
_params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D");
_params_handles.pitch_rate_ff = param_find("MC_PITCHRATE_FF");
_params_handles.tpa_breakpoint_p = param_find("MC_TPA_BREAK_P");
_params_handles.tpa_breakpoint_i = param_find("MC_TPA_BREAK_I");
_params_handles.tpa_breakpoint_d = param_find("MC_TPA_BREAK_D");
_params_handles.tpa_rate_p = param_find("MC_TPA_RATE_P");
_params_handles.tpa_rate_i = param_find("MC_TPA_RATE_I");
_params_handles.tpa_rate_d = param_find("MC_TPA_RATE_D");
_params_handles.yaw_p = param_find("MC_YAW_P");
_params_handles.yaw_rate_p = param_find("MC_YAWRATE_P");
_params_handles.yaw_rate_i = param_find("MC_YAWRATE_I");
_params_handles.yaw_rate_integ_lim = param_find("MC_YR_INT_LIM");
_params_handles.yaw_rate_d = param_find("MC_YAWRATE_D");
_params_handles.yaw_rate_ff = param_find("MC_YAWRATE_FF");
_params_handles.yaw_ff = param_find("MC_YAW_FF");
_params_handles.roll_rate_max = param_find("MC_ROLLRATE_MAX");
_params_handles.pitch_rate_max = param_find("MC_PITCHRATE_MAX");
_params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX");
_params_handles.yaw_auto_max = param_find("MC_YAWRAUTO_MAX");
_params_handles.acro_roll_max = param_find("MC_ACRO_R_MAX");
_params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX");
_params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX");
_params_handles.rattitude_thres = param_find("MC_RATT_TH");
_params_handles.vtol_type = param_find("VT_TYPE");
_params_handles.roll_tc = param_find("MC_ROLL_TC");
_params_handles.pitch_tc = param_find("MC_PITCH_TC");
_params_handles.vtol_opt_recovery_enabled = param_find("VT_OPT_RECOV_EN");
_params_handles.vtol_wv_yaw_rate_scale = param_find("VT_WV_YAWR_SCL");
_params_handles.bat_scale_en = param_find("MC_BAT_SCALE_EN");
/* rotations */
_params_handles.board_rotation = param_find("SENS_BOARD_ROT");
/* rotation offsets */
_params_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF");
_params_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF");
_params_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF");
/* fetch initial parameter values */
parameters_update();
if (_params.vtol_type == 0 && _params.vtol_opt_recovery_enabled) {
// the vehicle is a tailsitter, use optimal recovery control strategy
_ts_opt_recovery = new TailsitterRecovery();
}
/* initialize _corrections to identity as we might not immediately get a topic update */
for (unsigned i = 0; i < 3; i++) {
_sensor_correction.gyro_scale[i] = 1.0f;
_sensor_correction.accel_scale[i] = 1.0f;
}
_sensor_correction.baro_scale = 1.0f;
}
