Ardupilot — AP_OpticalFlow コードレビュー

void Copter::setup()
{
    // Load the default values of variables listed in var_info[]s
    AP_Param::setup_sketch_defaults();

    // setup storage layout for copter
    StorageManager::set_layout_copter();

    init_ardupilot();

    // initialise the main loop scheduler
    scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks), MASK_LOG_PM);
}

2 システム.cpp

2.1 void Copter::init_ardupilot()

init_ardupilot()関数は、無線による再起動に必要なすべてを処理します。航空機が実際に地上にあるかどうかは後で判断され、この場合は地上スタートが処理されます。

void Copter::init_ardupilot()
{
    ...

    // init the optical flow sensor
    init_optflow();

    ...
}

3 センサー.cpp

3.1 void Copter::init_optflow()

オプティカルフローセンサーを初期化します。

// initialise optical flow sensor
void Copter::init_optflow()
{
#if OPTFLOW == ENABLED
    // initialise optical flow sensor
    optflow.init(MASK_LOG_OPTFLOW);
#endif      // OPTFLOW == ENABLED
}

3.2 オブジェクトのオプトフローの説明

Copter.hファイルでは、 OpticalFlowクラスを使用してoptflowオブジェクトを定義します。

    // Optical flow sensor
#if OPTFLOW == ENABLED
    OpticalFlow optflow;
#endif

4 オプティカルフロー.cpp

4.1 void OpticalFlow::init(uint32_t log_bit)

したがって、 init()メンバー関数にジャンプすると、 OpticalFlowクラスのinit()関数にジャンプします。

パラメータFLOW_TYPEに従って、検出用に異なるオプティカルフローセンサーを選択します。

この例では、例としてPixart(2)を使用します。

void OpticalFlow::init(uint32_t log_bit)
{
     _log_bit = log_bit;

    // return immediately if not enabled or backend already created
    if ((_type == (int8_t)OpticalFlowType::NONE) || (backend != nullptr)) {
        return;
    }

    switch ((OpticalFlowType)_type.get()) {
    case OpticalFlowType::NONE:
        break;
    case OpticalFlowType::PX4FLOW:
        backend = AP_OpticalFlow_PX4Flow::detect(*this);
        break;
    case OpticalFlowType::PIXART:
        backend = AP_OpticalFlow_Pixart::detect("pixartflow", *this);
        if (backend == nullptr) {
            backend = AP_OpticalFlow_Pixart::detect("pixartPC15", *this);
        }
        break;
    case OpticalFlowType::BEBOP:
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
        backend = new AP_OpticalFlow_Onboard(*this);
#endif
        break;
    case OpticalFlowType::CXOF:
        backend = AP_OpticalFlow_CXOF::detect(*this);
        break;
    case OpticalFlowType::MAVLINK:
        backend = AP_OpticalFlow_MAV::detect(*this);
        break;
    case OpticalFlowType::UAVCAN:
#if HAL_WITH_UAVCAN
        backend = new AP_OpticalFlow_HereFlow(*this);
#endif
        break;
    case OpticalFlowType::SITL:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
        backend = new AP_OpticalFlow_SITL(*this);
#endif
        break;
    }

    if (backend != nullptr) {
        backend->init();
    }
}

5 AP_OpticalFlow_Pixart.cpp

5.1 AP_OpticalFlow_Pixart *AP_OpticalFlow_Pixart::detect(...)

まず、に渡されたパラメータに基づいて、新しいAP_OpticalFlow_Pixartクラスオブジェクトが作成されます。

次に、オプティカルフローセンサーのプロダクト IDを検出し、センサーを設定します( sensor->setup_sensor() )。

// detect the device
AP_OpticalFlow_Pixart *AP_OpticalFlow_Pixart::detect(const char *devname, OpticalFlow &_frontend)
{
    AP_OpticalFlow_Pixart *sensor = new AP_OpticalFlow_Pixart(devname, _frontend);
    if (!sensor) {
        return nullptr;
    }
    if (!sensor->setup_sensor()) {
        delete sensor;
        return nullptr;
    }
    return sensor;
}

5.2 bool AP_OpticalFlow_Pixart::setup_sensor(void)

まずオプティカルフローセンサーのプロダクトIDを読み取り、オプティカルフローセンサーの種類を特定します。次に、さまざまなタイプに応じて、さまざまなパラメータを書き込み、オプティカルフローセンサーを設定します。

最後にオプティカルフローセンサーの定期実行関数timer()を登録します。

// setup the device
bool AP_OpticalFlow_Pixart::setup_sensor(void)
{
    ...
    // check product ID
    uint8_t id1 = reg_read(PIXART_REG_PRODUCT_ID);
    uint8_t id2;
    if (id1 == 0x3f) {
        id2 = reg_read(PIXART_REG_INV_PROD_ID);
    } else {
        id2 = reg_read(PIXART_REG_INV_PROD_ID2);
    }
    debug("id1=0x%02x id2=0x%02x ~id1=0x%02x\n", id1, id2, uint8_t(~id1));
    if (id1 == 0x3F && id2 == uint8_t(~id1)) {
        model = PIXART_3900;
    } else if (id1 == 0x49 && id2 == uint8_t(~id1)) {
        model = PIXART_3901;
    } else {
        debug("Not a recognised device\n");
        return false;
    }

    if (model == PIXART_3900) {
        srom_download();

        id = reg_read(PIXART_REG_SROM_ID);
        if (id != srom_id) {
            debug("Pixart: bad SROM ID: 0x%02x\n", id);
            return false;
        }

        reg_write(PIXART_REG_SROM_EN, 0x15);
        hal.scheduler->delay(10);

        crc = reg_read16u(PIXART_REG_DOUT_L);
        if (crc != 0xBEEF) {
            debug("Pixart: bad SROM CRC: 0x%04x\n", crc);
            return false;
        }
    }

    if (model == PIXART_3900) {
        load_configuration(init_data_3900, ARRAY_SIZE(init_data_3900));
    } else {
        load_configuration(init_data_3901_1, ARRAY_SIZE(init_data_3901_1));
        hal.scheduler->delay(100);
        load_configuration(init_data_3901_2, ARRAY_SIZE(init_data_3901_2));
    }

    hal.scheduler->delay(50);

    debug("Pixart %s ready\n", model==PIXART_3900?"3900":"3901");

    integral.last_frame_us = AP_HAL::micros();

    _dev->register_periodic_callback(2000, FUNCTOR_BIND_MEMBER(&AP_OpticalFlow_Pixart::timer, void));
    return true;
}

5.3 void AP_OpticalFlow_Pixart::timer(void)

timer()関数は2msごとに呼び出され、オプティカルフローセンサーの測定値を読み取ります。

#if 0 #endifをブロックして、デバッグ情報をオンにすることができます。

void AP_OpticalFlow_Pixart::timer(void)
{
    if (AP_HAL::micros() - last_burst_us < 500) {
        return;
    }
    motion_burst();
    last_burst_us = AP_HAL::micros();

    uint32_t dt_us = last_burst_us - integral.last_frame_us;
    float dt = dt_us * 1.0e-6;
    const Vector3f &gyro = AP::ahrs_navekf().get_gyro();

    {
        WITH_SEMAPHORE(_sem);

        integral.sum.x += burst.delta_x;
        integral.sum.y += burst.delta_y;
        integral.sum_us += dt_us;
        integral.last_frame_us = last_burst_us;
        integral.gyro += Vector2f(gyro.x, gyro.y) * dt;
    }

#if 0
    static uint32_t last_print_ms;
    static int fd = -1;
    if (fd == -1) {
        fd = open("/dev/ttyACM0", O_WRONLY);
    }
    // used for debugging
    static int32_t sum_x;
    static int32_t sum_y;
    sum_x += burst.delta_x;
    sum_y += burst.delta_y;

    uint32_t now = AP_HAL::millis();
    if (now - last_print_ms >= 100 && (sum_x != 0 || sum_y != 0)) {
        last_print_ms = now;
        dprintf(fd, "Motion: %d %d obs:0x%02x squal:%u rds:%u maxr:%u minr:%u sup:%u slow:%u\n",
               (int)sum_x, (int)sum_y, (unsigned)burst.squal, (unsigned)burst.rawdata_sum, (unsigned)burst.max_raw,
               (unsigned)burst.max_raw, (unsigned)burst.min_raw, (unsigned)burst.shutter_upper, (unsigned)burst.shutter_lower);
        sum_x = sum_y = 0;
    }
#endif
}

5.4 void AP_OpticalFlow_Pixart::motion_burst(void)

SPIを介してオプティカルフローセンサーの移動値を読み取ります。

void AP_OpticalFlow_Pixart::motion_burst(void)
{
    uint8_t *b = (uint8_t *)&burst;

    burst.delta_x = 0;
    burst.delta_y = 0;

    _dev->set_chip_select(true);
    uint8_t reg = model==PIXART_3900?PIXART_REG_MOT_BURST:PIXART_REG_MOT_BURST2;

    _dev->transfer(&reg, 1, nullptr, 0);
    hal.scheduler->delay_microseconds(150);

    for (uint8_t i=0; i<sizeof(burst); i++) {
        _dev->transfer(nullptr, 0, &b[i], 1);
        if (i == 0 && (burst.motion & 0x80) == 0) {
            // no motion, save some bus bandwidth
            _dev->set_chip_select(false);
            return;
        }
    }
    _dev->set_chip_select(false);
}

5.5 SCHED_TASK_CLASS(OpticalFlow、&copter.optflow、更新、200、160)、

Copter.cppファイルの定期タスクリストには、呼び出し頻度200Hzのオプティカルフローセンサーupdate()関数が登録されています。

const AP_Scheduler::Task Copter::scheduler_tasks[] = {
...

#if OPTFLOW == ENABLED
    SCHED_TASK_CLASS(OpticalFlow,          &copter.optflow,             update,         200, 160),
#endif
...
}

5.6 void AP_OpticalFlow_Pixart::update(void)

5.1で返された AP_OpticalFlow_Pixartオブジェクトに基づいて、AP_OpticalFlow_Pixartクラスの update()関数を呼び出します。

更新 - センサーから最新の値を読み取り、x、y、および合計を入力します。

// update - read latest values from sensor and fill in x,y and totals.
void AP_OpticalFlow_Pixart::update(void)
{
    uint32_t now = AP_HAL::millis();
    if (now - last_update_ms < 100) {
        return;
    }
    last_update_ms = now;

    struct OpticalFlow::OpticalFlow_state state;
    state.surface_quality = burst.squal;

    if (integral.sum_us > 0) {
        WITH_SEMAPHORE(_sem);

        const Vector2f flowScaler = _flowScaler();
        float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
        float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
        float dt = integral.sum_us * 1.0e-6;

        state.flowRate = Vector2f(integral.sum.x * flowScaleFactorX,
                                  integral.sum.y * flowScaleFactorY);
        state.flowRate *= flow_pixel_scaling / dt;

        // we only apply yaw to flowRate as body rate comes from AHRS
        _applyYaw(state.flowRate);

        state.bodyRate = integral.gyro / dt;

        integral.sum.zero();
        integral.sum_us = 0;
        integral.gyro.zero();
    } else {
        state.flowRate.zero();
        state.bodyRate.zero();
    }

    // copy results to front end
    _update_frontend(state);
}