Пример #1
0
void AP_OpticalFlow_Onboard::update()
{
    AP_HAL::OpticalFlow::Data_Frame data_frame;
    // read at maximum 10Hz
    uint32_t now = AP_HAL::millis();
    if (now - _last_read_ms < 100) {
        return;
    }
    _last_read_ms = now;

    if (!hal.opticalflow->read(data_frame)) {
        return;
    }

    struct OpticalFlow::OpticalFlow_state state;
    state.surface_quality = data_frame.quality;
    if (data_frame.delta_time > 0) {
        const Vector2f flowScaler = _flowScaler();
        float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
        float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;

        // delta_time is in microseconds and flow is in milliradians
        // per second, so multiply by 1000
        state.flowRate.x = flowScaleFactorX * 1000.0f /
                           float(data_frame.delta_time) *
                           data_frame.pixel_flow_x_integral;

        state.flowRate.y = flowScaleFactorY * 1000.0f /
                           float(data_frame.delta_time) *
                           data_frame.pixel_flow_y_integral;

        // delta_time is in microseconds so multiply to get back to seconds
        state.bodyRate.x = 1000000.0f / float(data_frame.delta_time) *
                           data_frame.gyro_x_integral;

        state.bodyRate.y = 1000000.0f / float(data_frame.delta_time) *
                           data_frame.gyro_y_integral;

        _applyYaw(state.flowRate);
    } else {
        state.flowRate.zero();
        state.bodyRate.zero();
    }

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

#if OPTICALFLOW_ONBOARD_DEBUG
    hal.console->printf("FLOW_ONBOARD qual:%u FlowRateX:%4.2f Y:%4.2f"
                        "BodyRateX:%4.2f Y:%4.2f, delta_time = %u\n",
                        (unsigned)state.surface_quality,
                        (double)state.flowRate.x,
                        (double)state.flowRate.y,
                        (double)state.bodyRate.x,
                        (double)state.bodyRate.y,
                        data_frame.delta_time);
#endif
}
Пример #2
0
// update - read latest values from sensor and fill in x,y and totals.
void AP_OpticalFlow_Linux::update(void)
{
    optical_flow_s report;

    // return immediately if not initialised or more than half of last 40 reads have failed
    if (!initialised || num_errors >= 20) {
        return;
    }

    // throttle reads to no more than 10hz
    uint32_t now = hal.scheduler->millis();
    if (now - last_read_ms < 100) {
        return;
    }
    last_read_ms = now;

    // read the report from the sensor
    if (!read(&report)) {
        return;
    }

    // process
    struct OpticalFlow::OpticalFlow_state state;
    state.device_id = report.sensor_id;
    state.surface_quality = report.quality;
    if (report.integration_timespan > 0) {
        const Vector2f flowScaler = _flowScaler();
        float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
        float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
        float integralToRate = 1e6f / float(report.integration_timespan);
        state.flowRate.x = flowScaleFactorX * integralToRate * float(report.pixel_flow_x_integral); // rad/sec measured optically about the X sensor axis
        state.flowRate.y = flowScaleFactorY * integralToRate * float(report.pixel_flow_y_integral); // rad/sec measured optically about the Y sensor axis
        state.bodyRate.x = integralToRate * float(report.gyro_x_rate_integral); // rad/sec measured inertially about the X sensor axis
        state.bodyRate.y = integralToRate * float(report.gyro_y_rate_integral); // rad/sec measured inertially about the Y sensor axis
    } else {
        state.flowRate.zero();
        state.bodyRate.zero();
    }

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

#if PX4FLOW_DEBUG
    hal.console->printf_P(PSTR("PX4FLOW id:%u qual:%u FlowRateX:%4.2f Y:%4.2f BodyRateX:%4.2f y:%4.2f\n"),
                          (unsigned)state.device_id,
                          (unsigned)state.surface_quality,
                          (double)state.flowRate.x,
                          (double)state.flowRate.y,
                          (double)state.bodyRate.x,
                          (double)state.bodyRate.y);
#endif
}
// update - read latest values from sensor and fill in x,y and totals.
void AP_OpticalFlow_PX4::update(void)
{
    // return immediately if not initialised
    if (_fd == -1) {
        return;
    }

    float sum = 0;
    uint16_t count = 0;

    struct optical_flow_s report;
    while (::read(_fd, &report, sizeof(optical_flow_s)) == sizeof(optical_flow_s) && 
           report.timestamp != _last_timestamp) {
        struct OpticalFlow::OpticalFlow_state state;
        state.device_id = report.sensor_id;
        state.surface_quality = report.quality;
        if (report.integration_timespan > 0) {
            float yawAngleRad = _yawAngleRad();
            float cosYaw = cosf(yawAngleRad);
            float sinYaw = sinf(yawAngleRad);
            const Vector2f flowScaler = _flowScaler();
            float flowScaleFactorX = 1.0f + 0.001f * flowScaler.x;
            float flowScaleFactorY = 1.0f + 0.001f * flowScaler.y;
            float integralToRate = 1e6f / float(report.integration_timespan);
            // rotate sensor measurements from sensor to body frame through sensor yaw angle
            state.flowRate.x = flowScaleFactorX * integralToRate * (cosYaw * float(report.pixel_flow_x_integral) - sinYaw * float(report.pixel_flow_y_integral)); // rad/sec measured optically about the X body axis
            state.flowRate.y = flowScaleFactorY * integralToRate * (sinYaw * float(report.pixel_flow_x_integral) + cosYaw * float(report.pixel_flow_y_integral)); // rad/sec measured optically about the Y body axis
            state.bodyRate.x = integralToRate * (cosYaw * float(report.gyro_x_rate_integral) - sinYaw * float(report.gyro_y_rate_integral)); // rad/sec measured inertially about the X body axis
            state.bodyRate.y = integralToRate * (sinYaw * float(report.gyro_x_rate_integral) + cosYaw * float(report.gyro_y_rate_integral)); // rad/sec measured inertially about the Y body axis
        } else {
            state.flowRate.zero();
            state.bodyRate.zero();
        }
        sum += report.ground_distance_m;
        count++;
        _last_timestamp = report.timestamp;

        _update_frontend(state);
    }

    if (count != 0){
        float dist = sum / count * 100.0f;
        setDistance( dist );
    }
}