Ejemplo n.º 1
0
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));
}
Ejemplo n.º 2
0
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;
}
Ejemplo n.º 3
0
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));
}
Ejemplo n.º 4
0
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));
}
Ejemplo n.º 6
0
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();
}
Ejemplo n.º 7
0
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));
}
Ejemplo n.º 8
0
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;
}
Ejemplo n.º 9
0
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));
}
Ejemplo n.º 10
0
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));
}
Ejemplo n.º 11
0
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
}
Ejemplo n.º 12
0
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);

}
Ejemplo n.º 13
0
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;
}
Ejemplo n.º 14
0
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));
}
Ejemplo n.º 15
0
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;
}
Ejemplo n.º 16
0
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;

}
Ejemplo n.º 17
0
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;
}
Ejemplo n.º 18
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
{}
Ejemplo n.º 19
0
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));
}
Ejemplo n.º 20
0
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));
}
Ejemplo n.º 21
0
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));
}
Ejemplo n.º 22
0
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));
}
Ejemplo n.º 23
0
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));
}
Ejemplo n.º 24
0
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));
}
Ejemplo n.º 25
0
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;
}
Ejemplo n.º 27
0
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));
}
Ejemplo n.º 28
0
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));
}
Ejemplo n.º 29
0
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();
}