BMI055_accel::BMI055_accel(int bus, const char *path_accel, uint32_t device, enum Rotation rotation) : BMI055("BMI055_ACCEL", path_accel, bus, device, SPIDEV_MODE3, BMI055_BUS_SPEED, rotation), _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), _accel_sample_rate(BMI055_ACCEL_DEFAULT_RATE), _accel_reads(perf_alloc(PC_COUNT, "bmi055_accel_read")), _accel_filter_x(BMI055_ACCEL_DEFAULT_RATE, BMI055_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(BMI055_ACCEL_DEFAULT_RATE, BMI055_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(BMI055_ACCEL_DEFAULT_RATE, BMI055_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_int(1000000 / BMI055_ACCEL_MAX_PUBLISH_RATE), _last_temperature(0), _got_duplicate(false) { // disable debug() calls _debug_enabled = false; _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_BMI055; // 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; memset(&_call, 0, sizeof(_call)); }
L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) : SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 8000000), _call_interval(0), _num_reports(0), _next_report(0), _oldest_report(0), _reports(nullptr), _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _gyro_topic(-1), _current_rate(0), _current_range(0), _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")) { // enable debug() calls _debug_enabled = true; // default 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; }
BMI055_gyro::BMI055_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation) : BMI055("BMI055_GYRO", path_gyro, bus, device, SPIDEV_MODE3, BMI055_BUS_SPEED, rotation), _gyro_reports(nullptr), _gyro_scale{}, _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _gyro_topic(nullptr), _gyro_orb_class_instance(-1), _gyro_class_instance(-1), _gyro_sample_rate(BMI055_GYRO_DEFAULT_RATE), _gyro_reads(perf_alloc(PC_COUNT, "bmi055_gyro_read")), _gyro_filter_x(BMI055_GYRO_DEFAULT_RATE, BMI055_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_y(BMI055_GYRO_DEFAULT_RATE, BMI055_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_z(BMI055_GYRO_DEFAULT_RATE, BMI055_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_int(1000000 / BMI055_GYRO_MAX_PUBLISH_RATE, true), _last_temperature(0) { // disable debug() calls _debug_enabled = false; _device_id.devid_s.devtype = DRV_GYR_DEVTYPE_BMI055; // 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)); }
ADC::ADC(uint32_t channels) : CDev("adc", ADC0_DEVICE_PATH), _sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")), _channel_count(0), _samples(nullptr), _to_system_power(nullptr) { _debug_enabled = true; /* always enable the temperature sensor */ channels |= 1 << 16; /* allocate the sample array */ for (unsigned i = 0; i < 32; i++) { if (channels & (1 << i)) { _channel_count++; } } _samples = new adc_msg_s[_channel_count]; /* prefill the channel numbers in the sample array */ if (_samples != nullptr) { unsigned index = 0; for (unsigned i = 0; i < 32; i++) { if (channels & (1 << i)) { _samples[index].am_channel = i; _samples[index].am_data = 0; index++; } } } }
/* constructor */ DataFlash_MAVLink::DataFlash_MAVLink(mavlink_channel_t chan) : _chan(chan), _initialised(false), _total_blocks(NUM_BUFFER_BLOCKS), _block_max_size(BUFFER_BLOCK_SIZE), _latest_block_num(0), _cur_block_address(0), _latest_block_len(0) #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN ,_perf_errors(perf_alloc(PC_COUNT, "DF_errors")), _perf_overruns(perf_alloc(PC_COUNT, "DF_overruns")) #endif { memset(&mavlink, 0, sizeof(mavlink)); memset(_is_critical_block, 0, sizeof(_is_critical_block)); }
Navigator::Navigator() : SuperBlock(NULL, "NAV"), _task_should_exit(false), _navigator_task(-1), _mavlink_fd(-1), _global_pos_sub(-1), _gps_pos_sub(-1), _home_pos_sub(-1), _vstatus_sub(-1), _capabilities_sub(-1), _control_mode_sub(-1), _onboard_mission_sub(-1), _offboard_mission_sub(-1), _param_update_sub(-1), _pos_sp_triplet_pub(-1), _mission_result_pub(-1), _att_sp_pub(-1), _vstatus{}, _control_mode{}, _global_pos{}, _gps_pos{}, _sensor_combined{}, _home_pos{}, _mission_item{}, _nav_caps{}, _pos_sp_triplet{}, _mission_result{}, _att_sp{}, _mission_item_valid(false), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _geofence{}, _geofence_violation_warning_sent(false), _inside_fence(true), _navigation_mode(nullptr), _mission(this, "MIS"), _loiter(this, "LOI"), _rtl(this, "RTL"), _rcLoss(this, "RCL"), _dataLinkLoss(this, "DLL"), _engineFailure(this, "EF"), _gpsFailure(this, "GPSF"), _can_loiter_at_sp(false), _pos_sp_triplet_updated(false), _param_loiter_radius(this, "LOITER_RAD"), _param_acceptance_radius(this, "ACC_RAD"), _param_datalinkloss_obc(this, "DLL_OBC"), _param_rcloss_obc(this, "RCL_OBC") { /* Create a list of our possible navigation types */ _navigation_mode_array[0] = &_mission; _navigation_mode_array[1] = &_loiter; _navigation_mode_array[2] = &_rtl; _navigation_mode_array[3] = &_dataLinkLoss; _navigation_mode_array[4] = &_engineFailure; _navigation_mode_array[5] = &_gpsFailure; _navigation_mode_array[6] = &_rcLoss; updateParams(); }
Gimbal::Gimbal() : CDev("Gimbal", GIMBAL_DEVICE_PATH), _vehicle_command_sub(-1), _att_sub(-1), _attitude_compensation(true), _initialized(false), _actuator_controls_2_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "gimbal_read")), _comms_errors(perf_alloc(PC_COUNT, "gimbal_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "gimbal_buffer_overflows")) { // disable debug() calls _debug_enabled = false; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); }
LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000), _mag(new LSM303D_mag(this)), _call_accel_interval(0), _call_mag_interval(0), _accel_reports(nullptr), _mag_reports(nullptr), _accel_range_m_s2(0.0f), _accel_range_scale(0.0f), _accel_samplerate(0), _accel_onchip_filter_bandwith(0), _mag_range_ga(0.0f), _mag_range_scale(0.0f), _mag_samplerate(0), _accel_topic(-1), _mag_topic(-1), _accel_read(0), _mag_read(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), _reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")), _reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")), _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _reg1_expected(0), _reg7_expected(0) { // enable debug() calls _debug_enabled = true; // default scale factors _accel_scale.x_offset = 0.0f; _accel_scale.x_scale = 1.0f; _accel_scale.y_offset = 0.0f; _accel_scale.y_scale = 1.0f; _accel_scale.z_offset = 0.0f; _accel_scale.z_scale = 1.0f; _mag_scale.x_offset = 0.0f; _mag_scale.x_scale = 1.0f; _mag_scale.y_offset = 0.0f; _mag_scale.y_scale = 1.0f; _mag_scale.z_offset = 0.0f; _mag_scale.z_scale = 1.0f; }
PX4FLOW::PX4FLOW(int bus, int address) : I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000),//400khz _reports(nullptr), _sensor_ok(false), _measure_ticks(0), _collect_phase(false), _px4flow_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")), _comms_errors(perf_alloc(PC_COUNT, "px4flow_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")) { // enable debug() calls _debug_enabled = false; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); }
HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rotation) : CDev("HMC5883", path), _interface(interface), _work{}, _measure_ticks(0), _reports(nullptr), _scale{}, _range_scale(0), /* default range scale from counts to gauss */ _range_ga(1.9f), _collect_phase(false), _class_instance(-1), _orb_class_instance(-1), _mag_topic(nullptr), _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")), _comms_errors(perf_alloc(PC_COUNT, "hmc5883_com_err")), _range_errors(perf_alloc(PC_COUNT, "hmc5883_rng_err")), _conf_errors(perf_alloc(PC_COUNT, "hmc5883_conf_err")), _sensor_ok(false), _calibrated(false), _rotation(rotation), _last_report{0}, _range_bits(0), _conf_reg(0), _temperature_counter(0), _temperature_error_count(0) { // set the device type from the interface _device_id.devid_s.bus_type = _interface->get_device_bus_type(); _device_id.devid_s.bus = _interface->get_device_bus(); _device_id.devid_s.address = _interface->get_device_address(); _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; // enable debug() calls _debug_enabled = false; // default scaling _scale.x_offset = 0; _scale.x_scale = 1.0f; _scale.y_offset = 0; _scale.y_scale = 1.0f; _scale.z_offset = 0; _scale.z_scale = 1.0f; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); }
void interface_init(void) { debug("spi init"); pc_txns = perf_alloc(PC_ELAPSED, "txns"); pc_errors = perf_alloc(PC_COUNT, "errors"); pc_ore = perf_alloc(PC_COUNT, "overrun"); pc_fe = perf_alloc(PC_COUNT, "framing"); pc_ne = perf_alloc(PC_COUNT, "noise"); pc_idle = perf_alloc(PC_COUNT, "idle"); pc_badidle = perf_alloc(PC_COUNT, "badidle"); pc_regerr = perf_alloc(PC_COUNT, "regerr"); pc_crcerr = perf_alloc(PC_COUNT, "crcerr"); /* allocate DMA handles and initialise DMA */ rx_dma = stm32_dmachannel(DMACHAN_SPI3_RX); tx_dma = stm32_dmachannel(DMACHAN_SPI3_TX); /* enable the spi block clock and reset it */ modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_SPI3EN); modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_SPI3RST); modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_SPI3RST, 0); /* configure the spi GPIOs */ stm32_configgpio(RPI_SPI_NSS); stm32_configgpio(RPI_SPI_SCK); stm32_configgpio(RPI_SPI_MISO); stm32_configgpio(RPI_SPI_MOSI); /* reset and configure the SPI */ rCR1 = SPI_CR1_CPHA | SPI_CR1_CPOL; /* set for DMA operation */ rCR2 = SPI_CR2_RXDMAEN | SPI_CR2_TXDMAEN | SPI_CR2_ERRIE; /* enable event interrupts */ irq_attach(STM32_IRQ_SPI3, spi_interrupt); up_enable_irq(STM32_IRQ_SPI3); /* configure RX DMA and return to listening state */ dma_reset(); /* and enable the SPI port */ rCR1 |= SPI_CR1_SPE; #ifdef DEBUG spi_dump(); #endif }
IIS328DQ::IIS328DQ(int bus, const char* path, enum Rotation rotation) : CDev("iis328dq", path), _call(NULL), _call_interval(0), _reports(NULL), _accel_range_m_s2(0.0f), _accel_range_scale(0.0f), _accel_samplerate(0), _accel_onchip_filter_bandwidth(0), _accel_topic(NULL), _orb_class_instance(-1), _class_instance(-1), _read(0), _sample_perf(perf_alloc(PC_ELAPSED, "iis328dq_read")), _errors(perf_alloc(PC_COUNT, "iis328dq_errors")), _bad_registers(perf_alloc(PC_COUNT, "iis328dq_bad_registers")), _bad_values(perf_alloc(PC_COUNT, "iis328dq_bad_values")), _duplicates(perf_alloc(PC_COUNT, "iis328dq_duplicates")), _register_wait(0), _accel_filter_x(IIS328DQ_DEFAULT_RATE, IIS328DQ_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(IIS328DQ_DEFAULT_RATE, IIS328DQ_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(IIS328DQ_DEFAULT_RATE, IIS328DQ_DEFAULT_DRIVER_FILTER_FREQ), _rotation(rotation), _constant_accel_count(0), _checked_next(0) { _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_IIS328DQ; // default 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; iis328dq_spi.bus_id = bus; iis328dq_spi.cs_pin = GPIO_SPI_CS_ACCEL; iis328dq_spi.frequency = 11*1000*1000; //spi frequency spi_cs_init(&iis328dq_spi); spi_register_node(&iis328dq_spi); }
int logbuffer_init(struct logbuffer_s *lb, int size) { lb->size = size; lb->write_ptr = 0; lb->read_ptr = 0; lb->data = NULL; lb->perf_dropped = perf_alloc(PC_COUNT, "sd drop"); return PX4_OK; }
LM73::LM73(int bus) : I2C("LM73", LM73_DEVICE_PATH, bus, 0, 100000), _work{}, _measure_ticks(0), _reports(nullptr), _measurement_phase(false), temperature(-273.15), _ambient_temperature_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "LM73_read")), _comms_errors(perf_alloc(PC_COUNT, "LM73_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "LM73_buffer_overflows")) { // enable debug() calls _debug_enabled = true; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); }
PX4IO_serial::PX4IO_serial() : Device("PX4IO_serial"), _tx_dma(nullptr), _rx_dma(nullptr), _rx_dma_status(_dma_status_inactive), _pc_txns(perf_alloc(PC_ELAPSED, "io_txns ")), _pc_dmasetup(perf_alloc(PC_ELAPSED, "io_dmasetup ")), _pc_retries(perf_alloc(PC_COUNT, "io_retries ")), _pc_timeouts(perf_alloc(PC_COUNT, "io_timeouts ")), _pc_crcerrs(perf_alloc(PC_COUNT, "io_crcerrs ")), _pc_dmaerrs(perf_alloc(PC_COUNT, "io_dmaerrs ")), _pc_protoerrs(perf_alloc(PC_COUNT, "io_protoerrs")), _pc_uerrs(perf_alloc(PC_COUNT, "io_uarterrs ")), _pc_idle(perf_alloc(PC_COUNT, "io_idle ")), _pc_badidle(perf_alloc(PC_COUNT, "io_badidle ")) { g_interface = this; }
FXAS21002C::FXAS21002C(int bus, const char *path, uint32_t device, enum Rotation rotation) : SPI("FXAS21002C", path, bus, device, SPIDEV_MODE0, 2 * 1000 * 1000), _gyro_call{}, _call_interval(0), _reports(nullptr), _gyro_scale{}, _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _gyro_topic(nullptr), _orb_class_instance(-1), _class_instance(-1), _current_rate(800), _orientation(0), _last_temperature(0.0f), _read(0), _sample_perf(perf_alloc(PC_ELAPSED, "fxas21002c_acc_read")), _errors(perf_alloc(PC_COUNT, "fxas21002c_err")), _bad_registers(perf_alloc(PC_COUNT, "fxas21002c_bad_reg")), _duplicates(perf_alloc(PC_COUNT, "fxas21002c_acc_dupe")), _register_wait(0), _gyro_filter_x(FXAS21002C_DEFAULT_RATE, FXAS21002C_DEFAULT_FILTER_FREQ), _gyro_filter_y(FXAS21002C_DEFAULT_RATE, FXAS21002C_DEFAULT_FILTER_FREQ), _gyro_filter_z(FXAS21002C_DEFAULT_RATE, FXAS21002C_DEFAULT_FILTER_FREQ), _gyro_int(1000000 / FXAS21002C_MAX_OUTPUT_RATE, true), _rotation(rotation), _checked_values{}, _checked_next(0) { // enable debug() calls //_debug_enabled = false; _device_id.devid_s.devtype = DRV_GYR_DEVTYPE_FXAS2100C; // default scale factors _gyro_scale.x_offset = 0.0f; _gyro_scale.x_scale = 1.0f; _gyro_scale.y_offset = 0.0f; _gyro_scale.y_scale = 1.0f; _gyro_scale.z_offset = 0.0f; _gyro_scale.z_scale = 1.0f; }
int adc_init(void) { adc_perf = perf_alloc(PC_ELAPSED, "adc"); /* do calibration if supported */ #ifdef ADC_CR2_CAL rCR2 |= ADC_CR2_RSTCAL; up_udelay(1); if (rCR2 & ADC_CR2_RSTCAL) return -1; rCR2 |= ADC_CR2_CAL; up_udelay(100); if (rCR2 & ADC_CR2_CAL) return -1; #endif /* arbitrarily configure all channels for 55 cycle sample time */ rSMPR1 = 0b00000011011011011011011011011011; rSMPR2 = 0b00011011011011011011011011011011; /* XXX for F2/4, might want to select 12-bit mode? */ rCR1 = 0; /* enable the temperature sensor / Vrefint channel if supported*/ rCR2 = #ifdef ADC_CR2_TSVREFE /* enable the temperature sensor in CR2 */ ADC_CR2_TSVREFE | #endif 0; #ifdef ADC_CCR_TSVREFE /* enable temperature sensor in CCR */ rCCR = ADC_CCR_TSVREFE; #endif /* configure for a single-channel sequence */ rSQR1 = 0; rSQR2 = 0; rSQR3 = 0; /* will be updated with the channel each tick */ /* power-cycle the ADC and turn it on */ rCR2 &= ~ADC_CR2_ADON; up_udelay(10); rCR2 |= ADC_CR2_ADON; up_udelay(10); rCR2 |= ADC_CR2_ADON; up_udelay(10); return 0; }
/* constructor */ DataFlash_File::DataFlash_File(const char *log_directory) : _write_fd(-1), _read_fd(-1), _read_offset(0), _write_offset(0), _initialised(false), _log_directory(log_directory), _writebuf(NULL), _writebuf_size(16*1024), _writebuf_chunk(4096), _writebuf_head(0), _writebuf_tail(0), _last_write_time(0) #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 ,_perf_write(perf_alloc(PC_ELAPSED, "DF_write")), _perf_fsync(perf_alloc(PC_ELAPSED, "DF_fsync")), _perf_errors(perf_alloc(PC_COUNT, "DF_errors")) #endif {}
HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) : I2C("HMC5883", path, bus, HMC5883L_ADDRESS, 400000), _work{}, _measure_ticks(0), _reports(nullptr), _scale{}, _range_scale(0), /* default range scale from counts to gauss */ _range_ga(1.3f), _collect_phase(false), _class_instance(-1), _mag_topic(-1), _subsystem_pub(-1), _mag_orb_id(nullptr), _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")), _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")), _range_errors(perf_alloc(PC_COUNT, "hmc5883_range_errors")), _conf_errors(perf_alloc(PC_COUNT, "hmc5883_conf_errors")), _sensor_ok(false), _calibrated(false), _bus(bus), _rotation(rotation), _last_report{0}, _range_bits(0), _conf_reg(0) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; // enable debug() calls _debug_enabled = false; // default scaling _scale.x_offset = 0; _scale.x_scale = 1.0f; _scale.y_offset = 0; _scale.y_scale = 1.0f; _scale.z_offset = 0; _scale.z_scale = 1.0f; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); }
AirspeedSim::AirspeedSim(int bus, int address, unsigned conversion_interval, const char *path) : CDev("AIRSPEEDSIM", path), _reports(nullptr), _retries(0), _sensor_ok(false), _measure_ticks(0), _collect_phase(false), _diff_pres_offset(0.0f), _airspeed_pub(nullptr), _class_instance(-1), _conversion_interval(conversion_interval), _sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")), _comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors")) { // enable debug() calls _debug_enabled = false; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); }
Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) : I2C("Airspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000), _reports(nullptr), _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")), _max_differential_pressure_pa(0), _sensor_ok(false), _measure_ticks(0), _collect_phase(false), _diff_pres_offset(0.0f), _airspeed_pub(-1), _conversion_interval(conversion_interval), _sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")), _comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors")) { // enable debug() calls _debug_enabled = false; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); }
MB12XX::MB12XX(int bus, int address) : I2C("MB12xx", MB12XX_DEVICE_PATH, bus, address, 100000), _min_distance(MB12XX_MIN_DISTANCE), _max_distance(MB12XX_MAX_DISTANCE), _reports(nullptr), _sensor_ok(false), _measure_ticks(0), _collect_phase(false), _class_instance(-1), _range_finder_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")), _comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")) { // enable debug() calls _debug_enabled = false; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); }
InputPWM::InputPWM(uint8_t timer_index, const char* devName) : CDev("InputPWM", devName, input_pwm_timers[timer_index].irq_vector), _measure_ticks(0), _reports(nullptr), _input_rc_topic(-1), _temp_variable(2345), _sample_perf(perf_alloc(PC_ELAPSED, "input_pwm_read")), _buffer_overflows(perf_alloc(PC_COUNT, "input_pwm_buffer_overflows")), _timer_index(timer_index) { _debug_enabled = false; _pwmChannels[0].phase = pwmState::ARM; _pwmChannels[1].phase = pwmState::ARM; _pwmChannels[2].phase = pwmState::ARM; _pwmChannels[3].phase = pwmState::ARM; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); }
ETSAirspeed::ETSAirspeed(int bus, int address) : I2C("ETSAirspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000), _num_reports(0), _next_report(0), _oldest_report(0), _reports(nullptr), _sensor_ok(false), _measure_ticks(0), _collect_phase(false), _airspeed_pub(-1), _sample_perf(perf_alloc(PC_ELAPSED, "ets_airspeed_read")), _comms_errors(perf_alloc(PC_COUNT, "ets_airspeed_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "ets_airspeed_buffer_overflows")) { // enable debug() calls _debug_enabled = true; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); }
L3GD20::L3GD20(int bus, const char *path, spi_dev_e device, enum Rotation rotation) : SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */), _call{}, _call_interval(0), _reports(nullptr), _gyro_scale{}, _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _gyro_topic(nullptr), _orb_class_instance(-1), _class_instance(-1), _current_rate(0), _orientation(SENSOR_BOARD_ROTATION_DEFAULT), _read(0), _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), _errors(perf_alloc(PC_COUNT, "l3gd20_err")), _bad_registers(perf_alloc(PC_COUNT, "l3gd20_bad_reg")), _duplicates(perf_alloc(PC_COUNT, "l3gd20_dupe")), _register_wait(0), _gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_int(1000000 / L3GD20_MAX_OUTPUT_RATE, true), _is_l3g4200d(false), _rotation(rotation), _checked_next(0) { // enable debug() calls _debug_enabled = true; _device_id.devid_s.devtype = DRV_GYR_DEVTYPE_L3GD20; // default 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; }
LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) : SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 8000000), _mag(new LSM303D_mag(this)), _call_accel_interval(0), _call_mag_interval(0), _num_accel_reports(0), _next_accel_report(0), _oldest_accel_report(0), _accel_reports(nullptr), _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(-1), _num_mag_reports(0), _next_mag_report(0), _oldest_mag_report(0), _mag_reports(nullptr), _mag_range_scale(0.0f), _mag_range_ga(0.0f), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")) { // enable debug() calls _debug_enabled = true; _mag_range_scale = 12.0f/32767.0f; // default 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; _mag_scale.x_offset = 0; _mag_scale.x_scale = 1.0f; _mag_scale.y_offset = 0; _mag_scale.y_scale = 1.0f; _mag_scale.z_offset = 0; _mag_scale.z_scale = 1.0f; }
PMW3901::PMW3901(int bus, enum Rotation yaw_rotation) : SPI("PMW3901", PMW3901_DEVICE_PATH, bus, PMW3901_SPIDEV, SPIDEV_MODE0, PMW3901_SPI_BUS_SPEED), _reports(nullptr), _sensor_ok(false), _measure_ticks(0), _class_instance(-1), _orb_class_instance(-1), _optical_flow_pub(nullptr), _subsystem_pub(nullptr), _sample_perf(perf_alloc(PC_ELAPSED, "pmw3901_read")), _comms_errors(perf_alloc(PC_COUNT, "pmw3901_com_err")), _previous_collect_timestamp(0), _yaw_rotation(yaw_rotation) { // enable debug() calls _debug_enabled = false; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); }
PX4FLOW::PX4FLOW(int bus, int address, enum Rotation rotation) : I2C("PX4FLOW", PX4FLOW0_DEVICE_PATH, bus, address, PX4FLOW_I2C_MAX_BUS_SPEED), /* 100-400 KHz */ _reports(nullptr), _sensor_ok(false), _measure_ticks(0), _collect_phase(false), _class_instance(-1), _orb_class_instance(-1), _px4flow_topic(nullptr), _distance_sensor_topic(nullptr), _sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")), _comms_errors(perf_alloc(PC_COUNT, "px4flow_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")), _sensor_rotation(rotation) { // disable debug() calls _debug_enabled = false; // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); }
SF1XX::SF1XX(int bus, int address) : I2C("SF1XX", SF1XX_DEVICE_PATH, bus, address, 400000), _min_distance(-1.0f), _max_distance(-1.0f), _conversion_interval(-1), _reports(nullptr), _sensor_ok(false), _measure_ticks(0), _class_instance(-1), _orb_class_instance(-1), _distance_sensor_topic(nullptr), _sample_perf(perf_alloc(PC_ELAPSED, "sf1xx_read")), _comms_errors(perf_alloc(PC_COUNT, "sf1xx_com_err")) { /* enable debug() calls */ _debug_enabled = false; /* work_cancel in the dtor will explode if we don't do this... */ memset(&_work, 0, sizeof(_work)); }
GroundRoverAttitudeControl::GroundRoverAttitudeControl() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "gnda_dt")), _nonfinite_input_perf(perf_alloc(PC_COUNT, "gnda_nani")), _nonfinite_output_perf(perf_alloc(PC_COUNT, "gnda_nano")) { _parameter_handles.w_p = param_find("GND_WR_P"); _parameter_handles.w_i = param_find("GND_WR_I"); _parameter_handles.w_d = param_find("GND_WR_D"); _parameter_handles.w_imax = param_find("GND_WR_IMAX"); _parameter_handles.trim_yaw = param_find("TRIM_YAW"); _parameter_handles.man_yaw_scale = param_find("GND_MAN_Y_SC"); _parameter_handles.bat_scale_en = param_find("GND_BAT_SCALE_EN"); /* fetch initial parameter values */ parameters_update(); }