C++ perf_alloc函数代码示例

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;
}