C++ Matrix3f::mul_transpose方法代码示例

// Update raw magnetometer values from HIL data
//
void Compass::setHIL(uint8_t instance, float roll, float pitch, float yaw)
{
    Matrix3f R;

    // create a rotation matrix for the given attitude
    R.from_euler(roll, pitch, yaw);

    if (!is_equal(_hil.last_declination,get_declination())) {
        _setup_earth_field();
        _hil.last_declination = get_declination();
    }

    // convert the earth frame magnetic vector to body frame, and
    // apply the offsets
    _hil.field[instance] = R.mul_transpose(_hil.Bearth);

    // apply default board orientation for this compass type. This is
    // a noop on most boards
    _hil.field[instance].rotate(MAG_BOARD_ORIENTATION);

    // add user selectable orientation
    _hil.field[instance].rotate((enum Rotation)_state[0].orientation.get());

    if (!_state[0].external) {
        // and add in AHRS_ORIENTATION setting if not an external compass
        _hil.field[instance].rotate(_board_orientation);
    }
    _hil.healthy[instance] = true;
}

// Update raw magnetometer values from HIL data
//
void AP_Compass_HIL::setHIL(float roll, float pitch, float yaw)
{
    Matrix3f R;

    // create a rotation matrix for the given attitude
    R.from_euler(roll, pitch, yaw);

    if (_last_declination != _declination.get()) {
        _setup_earth_field();
        _last_declination = _declination.get();
    }

    // convert the earth frame magnetic vector to body frame, and
    // apply the offsets
    _hil_mag = R.mul_transpose(_Bearth);
    _hil_mag -= Vector3f(MAG_OFS_X, MAG_OFS_Y, MAG_OFS_Z);

    // apply default board orientation for this compass type. This is
    // a noop on most boards
    _hil_mag.rotate(MAG_BOARD_ORIENTATION);

    // add user selectable orientation
    _hil_mag.rotate((enum Rotation)_orientation.get());

    // and add in AHRS_ORIENTATION setting
    _hil_mag.rotate(_board_orientation);

    healthy = true;
}

/*
  update the optical flow with new data
 */
void SITL_State::_update_flow(void)
{
    Vector3f gyro;
    static uint32_t last_flow_ms;

    if (!_optical_flow ||
            !_sitl->flow_enable) {
        return;
    }

    // update at the requested rate
    uint32_t now = hal.scheduler->millis();
    if (now - last_flow_ms < 1000*(1.0f/_sitl->flow_rate)) {
        return;
    }
    last_flow_ms = now;

    gyro(radians(_sitl->state.rollRate), 
         radians(_sitl->state.pitchRate), 
         radians(_sitl->state.yawRate));

    OpticalFlow::OpticalFlow_state state;

    // NED velocity vector in m/s
    Vector3f velocity(_sitl->state.speedN,
                      _sitl->state.speedE,
                      _sitl->state.speedD);

    // a rotation matrix following DCM conventions
    Matrix3f rotmat;
    rotmat.from_euler(radians(_sitl->state.rollDeg),
                      radians(_sitl->state.pitchDeg),
                      radians(_sitl->state.yawDeg));


    state.device_id = 1;
    state.surface_quality = 255;

    // estimate range to centre of image
    float range;
    if (rotmat.c.z > 0.05f && height_agl() > 0) {
        range = height_agl() / rotmat.c.z;
    } else {
        range = 1e38f;
    }

    // Calculate relative velocity in sensor frame assuming no misalignment between sensor and vehicle body axes
    Vector3f relVelSensor = rotmat.mul_transpose(velocity);

    // Divide velocity by range and add body rates to get predicted sensed angular
    // optical rates relative to X and Y sensor axes assuming no misalignment or scale
    // factor error. Note - these are instantaneous values. The sensor sums these values across the interval from the last
    // poll to provide a delta angle across the interface
    state.flowRate.x =  -relVelSensor.y/range + gyro.x;
    state.flowRate.y =   relVelSensor.x/range + gyro.y;

    // The flow sensors body rates are assumed to be the same as the vehicle body rates (ie no misalignment)
    // Note - these are instantaneous values. The sensor sums these values across the interval from the last
    // poll to provide a delta angle across the interface.
    state.bodyRate = Vector2f(gyro.x, gyro.y);

    optflow_data[next_optflow_index++] = state;
    if (next_optflow_index >= optflow_delay+1) {
        next_optflow_index = 0;
    }

    state = optflow_data[next_optflow_index];

    if (_sitl->flow_delay != optflow_delay) {
        // cope with updates to the delay control
        if (_sitl->flow_delay > MAX_OPTFLOW_DELAY) {
            _sitl->flow_delay = MAX_OPTFLOW_DELAY;
        }
        optflow_delay = _sitl->flow_delay;
        for (uint8_t i=0; i<optflow_delay; i++) {
            optflow_data[i] = state;
        }
    }

    _optical_flow->setHIL(state);
}