C++ AP_ADC_ADS7844类代码示例

static void show_data()
{
    float result[6];
    uint32_t deltat = 0;
    uint16_t ch3;
    float min[6], max[6];
    uint8_t i;

    for (i=0; i<6; i++) {
        /* clearly out of bounds values: */
        min[i] = 99999999.0f;
        max[i] = -88888888.0f;
    } 


    do {
        ch3 = adc.Ch(3);
        deltat += adc.Ch6(channel_map, result);
        for (i=0; i<6; i++) {
            if (result[i] < min[i]) min[i] = result[i];
            if (result[i] > max[i]) max[i] = result[i];
            if (fabsf(result[i]) > 0x8000) {
                hal.console->printf("result[%u]=%f\n", (unsigned)i, result[i]);
            }
        }
    } while ((AP_HAL::millis() - timer) < 200);

    timer = AP_HAL::millis();
    hal.console->printf("g=(%f,%f,%f) a=(%f,%f,%f) +/-(%.0f,%.0f,%.0f,%.0f,%.0f,%.0f) gt=%u dt=%u\n",
                  result[0], result[1], result[2],
                  result[3], result[4], result[5],
                  (max[0]-min[0]), (max[1]-min[1]), (max[2]-min[2]),
                  (max[3]-min[3]), (max[4]-min[4]), (max[5]-min[5]),
                  ch3, (unsigned)(deltat/1000));
}

bool AP_InertialSensor_Oilpan::_init_sensor(void)
{
    apm1_adc.Init();

    switch (_imu.get_sample_rate()) {
    case AP_InertialSensor::RATE_50HZ:
        _sample_threshold = 20;
        break;
    case AP_InertialSensor::RATE_100HZ:
        _sample_threshold = 10;
        break;
    case AP_InertialSensor::RATE_200HZ:
        _sample_threshold = 5;
        break;
    default:
        // can't do this speed
        return false;
    }

    _gyro_instance = _imu.register_gyro();
    _accel_instance = _imu.register_accel();

    _product_id = AP_PRODUCT_ID_APM1_2560;

    return true;
}

void setup()
{
    hal.console->println("ArduPilot Mega ADC library test");
    hal.scheduler->delay(1000);

    adc.Init();       // APM ADC initialization

    hal.scheduler->delay(1000);
    timer = AP_HAL::millis();
}

static void show_timing()
{
    uint32_t mint = (uint32_t)-1, maxt = 0, totalt=0;
    uint32_t start_time = AP_HAL::millis();
    float result[6];
    uint32_t count = 0;

    hal.console->println("Starting timing test");

    adc.Ch6(channel_map, result);

    do {
        uint32_t deltat = adc.Ch6(channel_map, result);
        if (deltat > maxt) maxt = deltat;
        if (deltat < mint) mint = deltat;
        totalt += deltat;
        count++;
    } while ((AP_HAL::millis() - start_time) < 5000);

    hal.console->printf("timing: mint=%lu maxt=%lu avg=%lu\n", mint, maxt, totalt/count);
}

/*
  copy data from ADC to frontend
 */
bool AP_InertialSensor_Oilpan::update()
{
    float adc_values[6];

    apm1_adc.Ch6(_sensors, adc_values);

    // copy gyros to frontend
    Vector3f v;
    v(_sensor_signs[0] * ( adc_values[0] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_x,
      _sensor_signs[1] * ( adc_values[1] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_y,
      _sensor_signs[2] * ( adc_values[2] - OILPAN_RAW_GYRO_OFFSET ) * _gyro_gain_z);
    _rotate_and_correct_gyro(_gyro_instance, v);
    _publish_gyro(_gyro_instance, v);

    // copy accels to frontend
    v(_sensor_signs[3] * (adc_values[3] - OILPAN_RAW_ACCEL_OFFSET),
      _sensor_signs[4] * (adc_values[4] - OILPAN_RAW_ACCEL_OFFSET),
      _sensor_signs[5] * (adc_values[5] - OILPAN_RAW_ACCEL_OFFSET));
    v *= OILPAN_ACCEL_SCALE_1G;
    _rotate_and_correct_accel(_accel_instance, v);
    _publish_accel(_accel_instance, v);

    return true;
}

void Plane::init_ardupilot()
{
    // initialise serial port
    serial_manager.init_console();

    cliSerial->printf_P(PSTR("\n\nInit " FIRMWARE_STRING
                         "\n\nFree RAM: %u\n"),
                        hal.util->available_memory());


    //
    // Check the EEPROM format version before loading any parameters from EEPROM
    //
    load_parameters();

    if (g.hil_mode == 1) {
        // set sensors to HIL mode
        ins.set_hil_mode();
        compass.set_hil_mode();
        barometer.set_hil_mode();
    }

#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
    // this must be before BoardConfig.init() so if
    // BRD_SAFETYENABLE==0 then we don't have safety off yet
    for (uint8_t tries=0; tries<10; tries++) {
        if (setup_failsafe_mixing()) {
            break;
        }
        hal.scheduler->delay(10);
    }
#endif

    BoardConfig.init();

    // initialise serial ports
    serial_manager.init();

    // allow servo set on all channels except first 4
    ServoRelayEvents.set_channel_mask(0xFFF0);

    set_control_channels();

    // keep a record of how many resets have happened. This can be
    // used to detect in-flight resets
    g.num_resets.set_and_save(g.num_resets+1);

    // init baro before we start the GCS, so that the CLI baro test works
    barometer.init();

    // initialise rangefinder
    init_rangefinder();

    // initialise battery monitoring
    battery.init();

    // init the GCS
    gcs[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_Console, 0);

    // we start by assuming USB connected, as we initialed the serial
    // port with SERIAL0_BAUD. check_usb_mux() fixes this if need be.    
    usb_connected = true;
    check_usb_mux();

    // setup serial port for telem1
    gcs[1].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0);

#if MAVLINK_COMM_NUM_BUFFERS > 2
    // setup serial port for telem2
    gcs[2].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 1);
#endif

#if MAVLINK_COMM_NUM_BUFFERS > 3
    // setup serial port for fourth telemetry port (not used by default)
    gcs[3].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 2);
#endif

    // setup frsky
#if FRSKY_TELEM_ENABLED == ENABLED
    frsky_telemetry.init(serial_manager);
#endif

    mavlink_system.sysid = g.sysid_this_mav;

#if LOGGING_ENABLED == ENABLED
    log_init();
#endif

#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
    apm1_adc.Init();      // APM ADC library initialization
#endif

    // initialise airspeed sensor
    airspeed.init();

    if (g.compass_enabled==true) {
        if (!compass.init() || !compass.read()) {
            cliSerial->println_P(PSTR("Compass initialisation failed!"));
            g.compass_enabled = false;
        } else {
//.........这里部分代码省略.........

// return true if a new sample is available
bool AP_InertialSensor_Oilpan::_sample_available() const
{
    return apm1_adc.num_samples_available(_sensors) >= _sample_threshold;
}

void Rover::init_ardupilot()
{
    // initialise console serial port
    serial_manager.init_console();

	cliSerial->printf_P(PSTR("\n\nInit " FIRMWARE_STRING
						 "\n\nFree RAM: %u\n"),
                        hal.util->available_memory());
                    
	//
	// Check the EEPROM format version before loading any parameters from EEPROM.
	//
	
    load_parameters();

    BoardConfig.init();

    // initialise serial ports
    serial_manager.init();

    ServoRelayEvents.set_channel_mask(0xFFF0);

    set_control_channels();

    battery.init();

    // keep a record of how many resets have happened. This can be
    // used to detect in-flight resets
    g.num_resets.set_and_save(g.num_resets+1);

    // init baro before we start the GCS, so that the CLI baro test works
    barometer.init();

	// init the GCS
    gcs[0].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_Console, 0);

    // we start by assuming USB connected, as we initialed the serial
    // port with SERIAL0_BAUD. check_usb_mux() fixes this if need be.    
    usb_connected = true;
    check_usb_mux();

    // setup serial port for telem1
    gcs[1].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 0);

#if MAVLINK_COMM_NUM_BUFFERS > 2
    // setup serial port for telem2
    gcs[2].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 1);
#endif

#if MAVLINK_COMM_NUM_BUFFERS > 3
    // setup serial port for fourth telemetry port (not used by default)
    gcs[3].setup_uart(serial_manager, AP_SerialManager::SerialProtocol_MAVLink, 2);
#endif

    // setup frsky telemetry
#if FRSKY_TELEM_ENABLED == ENABLED
    frsky_telemetry.init(serial_manager);
#endif

	mavlink_system.sysid = g.sysid_this_mav;

#if LOGGING_ENABLED == ENABLED
    log_init();
#endif

    // Register mavlink_delay_cb, which will run anytime you have
    // more than 5ms remaining in your call to hal.scheduler->delay
    hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5);

#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
    apm1_adc.Init();      // APM ADC library initialization
#endif

	if (g.compass_enabled==true) {
		if (!compass.init()|| !compass.read()) {
            cliSerial->println_P(PSTR("Compass initialisation failed!"));
            g.compass_enabled = false;
        } else {
            ahrs.set_compass(&compass);
            //compass.get_offsets();						// load offsets to account for airframe magnetic interference
        }
	}

	// initialise sonar
    init_sonar();

    // and baro for EKF
    init_barometer();

	// Do GPS init
    gps.init(&DataFlash, serial_manager);

    rc_override_active = hal.rcin->set_overrides(rc_override, 8);

	init_rc_in();		// sets up rc channels from radio
	init_rc_out();		// sets up the timer libs

    relay.init();

#if MOUNT == ENABLED
//.........这里部分代码省略.........