Exemple #1
0
/// Stabilizes mount relative to the Earth's frame
/// Inputs:
///    _roll_control_angle   desired roll       angle in radians,
///    _tilt_control_angle   desired tilt/pitch angle in radians,
///    _pan_control_angle    desired pan/yaw    angle in radians
/// Outputs:
///    _roll_angle           stabilized roll       angle in degrees,
///    _tilt_angle           stabilized tilt/pitch angle in degrees,
///    _pan_angle            stabilized pan/yaw    angle in degrees
void
AP_Mount::stabilize()
{
#if MNT_STABILIZE_OPTION == ENABLED
    // only do the full 3D frame transform if we are doing pan control
    if (_stab_pan) {
        Matrix3f m;                         ///< holds 3 x 3 matrix, var is used as temp in calcs
        Matrix3f cam;                       ///< Rotation matrix earth to camera. Desired camera from input.
        Matrix3f gimbal_target;             ///< Rotation matrix from plane to camera. Then Euler angles to the servos.
        m = _ahrs.get_dcm_matrix();
        m.transpose();
        cam.from_euler(_roll_control_angle, _tilt_control_angle, _pan_control_angle);
        gimbal_target = m * cam;
        gimbal_target.to_euler(&_roll_angle, &_tilt_angle, &_pan_angle);
        _roll_angle  = _stab_roll ? degrees(_roll_angle) : degrees(_roll_control_angle);
        _tilt_angle  = _stab_tilt ? degrees(_tilt_angle) : degrees(_tilt_control_angle);
        _pan_angle   = degrees(_pan_angle);
    } else {
        // otherwise base mount roll and tilt on the ahrs
        // roll/tilt attitude, plus any requested angle
        _roll_angle  = degrees(_roll_control_angle);
        _tilt_angle  = degrees(_tilt_control_angle);
        _pan_angle   = degrees(_pan_control_angle);
        if (_stab_roll) {
            _roll_angle -= degrees(_ahrs.roll);
        }
        if (_stab_tilt) {
            _tilt_angle -= degrees(_ahrs.pitch);
        }
    }
#endif
}
Exemple #2
0
// 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;
}
void AP_AHRS_NavEKF::update_SITL(void)
{
    if (_sitl == nullptr) {
        _sitl = (SITL::SITL *)AP_Param::find_object("SIM_");
        if (_sitl == nullptr) {
            return;
        }
    }

    const struct SITL::sitl_fdm &fdm = _sitl->state;

    if (active_EKF_type() == EKF_TYPE_SITL) {
        roll  = radians(fdm.rollDeg);
        pitch = radians(fdm.pitchDeg);
        yaw   = radians(fdm.yawDeg);

        fdm.quaternion.rotation_matrix(_dcm_matrix);

        update_cd_values();
        update_trig();

        _gyro_drift.zero();

        _gyro_estimate = Vector3f(radians(fdm.rollRate),
                                  radians(fdm.pitchRate),
                                  radians(fdm.yawRate));

        for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
            const Vector3f accel(fdm.xAccel,
                           fdm.yAccel,
                           fdm.zAccel);
            _accel_ef_ekf[i] = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel;
        }
        _accel_ef_ekf_blended = _accel_ef_ekf[0];

    }

    if (_sitl->odom_enable) {
        // use SITL states to write body frame odometry data at 20Hz
        uint32_t timeStamp_ms = AP_HAL::millis();
        if (timeStamp_ms - _last_body_odm_update_ms > 50) {
            const float quality = 100.0f;
            const Vector3f posOffset(0.0f, 0.0f, 0.0f);
            const float delTime = 0.001f * (timeStamp_ms - _last_body_odm_update_ms);
            _last_body_odm_update_ms = timeStamp_ms;
            timeStamp_ms -= (timeStamp_ms - _last_body_odm_update_ms)/2; // correct for first order hold average delay
            Vector3f delAng(radians(fdm.rollRate),
                                       radians(fdm.pitchRate),
                                       radians(fdm.yawRate));
            delAng *= delTime;
            // rotate earth velocity into body frame and calculate delta position
            Matrix3f Tbn;
            Tbn.from_euler(radians(fdm.rollDeg),radians(fdm.pitchDeg),radians(fdm.yawDeg));
            const Vector3f earth_vel(fdm.speedN,fdm.speedE,fdm.speedD);
            const Vector3f delPos = Tbn.transposed() * (earth_vel * delTime);
            // write to EKF
            EKF3.writeBodyFrameOdom(quality, delPos, delAng, delTime, timeStamp_ms, posOffset);
        }
    }
}
Exemple #4
0
//余弦矩阵更新姿态
static void DCM_CF(Vector3f gyro,Vector3f acc, float deltaT)
{
	static Vector3f deltaGyroAngle, LastGyro;
	static Vector3f Vector_G(0, 0, ACC_1G), Vector_M(1000, 0, 0);
	Matrix3f dcm;
	
	//计算陀螺仪角度变化,二阶龙格库塔积分	
	deltaGyroAngle = (gyro + LastGyro) * 0.5 * deltaT;
	LastGyro = gyro;
	
	//计算表示单次旋转的余弦矩阵
	dcm.from_euler(deltaGyroAngle);
	
	//利用余弦矩阵更新重力向量在机体坐标系的投影
	Vector_G = dcm * Vector_G;
	
	//利用余弦矩阵更新地磁向量在机体坐标系的投影
	Vector_M = dcm * Vector_M;
	
	//互补滤波,使用加速度测量值矫正角速度积分漂移
	Vector_G = ComplementaryFilter_1st(Vector_G, acc, config.factor.gyro_cf);

	//计算飞行器的ROLL和PITCH
	Vector_G.get_rollpitch(imu.angle);	
	
	//计算飞行器的YAW
	Vector_M.get_yaw(imu.angle);
}
Exemple #5
0
void test_frame_transforms(void)
{
    Vector3f v, v2;
    Quaternion q;
    Matrix3f m;

    hal.console->println("frame transform tests\n");

    q.from_euler(ToRad(45), ToRad(45), ToRad(45));
    q.normalize();
    m.from_euler(ToRad(45), ToRad(45), ToRad(45));

    v2 = v = Vector3f(0, 0, 1);
    q.earth_to_body(v2);
    hal.console->printf("%f %f %f\n", v2.x, v2.y, v2.z);
    v2 = m * v;
    hal.console->printf("%f %f %f\n\n", v2.x, v2.y, v2.z);

    v2 = v = Vector3f(0, 1, 0);
    q.earth_to_body(v2);
    hal.console->printf("%f %f %f\n", v2.x, v2.y, v2.z);
    v2 = m * v;
    hal.console->printf("%f %f %f\n\n", v2.x, v2.y, v2.z);

    v2 = v = Vector3f(1, 0, 0);
    q.earth_to_body(v2);
    hal.console->printf("%f %f %f\n", v2.x, v2.y, v2.z);
    v2 = m * v;
    hal.console->printf("%f %f %f\n", v2.x, v2.y, v2.z);
}
/*
   update body magnetic field from position and rotation
*/
void Aircraft::update_mag_field_bf()
{
    // get the magnetic field intensity and orientation
    float intensity;
    float declination;
    float inclination;
    get_mag_field_ef(location.lat*1e-7f,location.lng*1e-7f,intensity,declination,inclination);

    // create a field vector and rotate to the required orientation
    Vector3f mag_ef(1e3f * intensity, 0, 0);
    Matrix3f R;
    R.from_euler(0, -ToRad(inclination), ToRad(declination));
    mag_ef = R * mag_ef;

    // calculate frame height above ground
    float frame_height_agl = fmaxf((-position.z) + home.alt*0.01f - ground_level, 0.0f);

    // calculate scaling factor that varies from 1 at ground level to 1/8 at sitl->mag_anomaly_hgt
    // Assume magnetic anomaly strength scales with 1/R**3
    float anomaly_scaler = (sitl->mag_anomaly_hgt / (frame_height_agl + sitl->mag_anomaly_hgt));
    anomaly_scaler = anomaly_scaler * anomaly_scaler * anomaly_scaler;

    // add scaled anomaly to earth field
    mag_ef += sitl->mag_anomaly_ned.get() * anomaly_scaler;

    // Rotate into body frame
    mag_bf = dcm.transposed() * mag_ef;

    // add motor interference
    mag_bf += sitl->mag_mot.get() * battery_current;
}
Exemple #7
0
// 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;
}
Exemple #8
0
static void test_euler(float roll, float pitch, float yaw)
{
    Matrix3f m;
    float roll2, pitch2, yaw2;

    m.from_euler(roll, pitch, yaw);
    m.to_euler(&roll2, &pitch2, &yaw2);
    check_result("test_euler", roll, pitch, yaw, roll2, pitch2, yaw2);
}
Exemple #9
0
/*
  given a magnetic heading, and roll, pitch, yaw values,
  calculate consistent magnetometer components

  All angles are in radians
 */
static Vector3f heading_to_mag(float roll, float pitch, float yaw)
{
	Vector3f Bearth, m;
	Matrix3f R;

	// Bearth is the magnetic field in Canberra. We need to adjust
	// it for inclination and declination
	Bearth(MAG_FIELD_STRENGTH, 0, 0);
	R.from_euler(0, -ToRad(MAG_INCLINATION), ToRad(MAG_DECLINATION));
	Bearth = R * Bearth;

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

	// convert the earth frame magnetic vector to body frame, and
	// apply the offsets
	m = R.transposed() * Bearth - Vector3f(MAG_OFS_X, MAG_OFS_Y, MAG_OFS_Z);
	return m + (rand_vec3f() * sitl.mag_noise);
}
Exemple #10
0
// setup _Bearth
void Compass::_setup_earth_field(void)
{
    // assume a earth field strength of 400
    _hil.Bearth(400, 0, 0);

    // rotate _Bearth for inclination and declination. -66 degrees
    // is the inclination in Canberra, Australia
    Matrix3f R;
    R.from_euler(0, ToRad(66), get_declination());
    _hil.Bearth = R * _hil.Bearth;
}
Exemple #11
0
static void test_quaternion(float roll, float pitch, float yaw)
{
    Quaternion q;
    Matrix3f m;
    float roll2, pitch2, yaw2;

    q.from_euler(roll, pitch, yaw);
    q.to_euler(roll2, pitch2, yaw2);
    check_result("test_quaternion1", roll, pitch, yaw, roll2, pitch2, yaw2);

    m.from_euler(roll, pitch, yaw);
    m.to_euler(&roll2, &pitch2, &yaw2);
    check_result("test_quaternion2", roll, pitch, yaw, roll2, pitch2, yaw2);

    m.from_euler(roll, pitch, yaw);
    q.from_rotation_matrix(m);
    q.to_euler(roll2, pitch2, yaw2);
    check_result("test_quaternion3", roll, pitch, yaw, roll2, pitch2, yaw2);

    q.rotation_matrix(m);
    m.to_euler(&roll2, &pitch2, &yaw2);
    check_result("test_quaternion4", roll, pitch, yaw, roll2, pitch2, yaw2);
}
Exemple #12
0
/// Stabilizes mount relative to the Earth's frame
/// Inputs:
///    _roll_control_angle   desired roll       angle in radians,
///    _tilt_control_angle   desired tilt/pitch angle in radians,
///    _pan_control_angle    desired pan/yaw    angle in radians
/// Outputs:
///    _roll_angle           stabilized roll       angle in degrees,
///    _tilt_angle           stabilized tilt/pitch angle in degrees,
///    _pan_angle            stabilized pan/yaw    angle in degrees
void
AP_Mount::stabilize()
{
#if MNT_STABILIZE_OPTION == ENABLED
    // only do the full 3D frame transform if we are doing pan control
    if (_stab_pan) {
        Matrix3f m;                         ///< holds 3 x 3 matrix, var is used as temp in calcs
        Matrix3f cam;                       ///< Rotation matrix earth to camera. Desired camera from input.
        Matrix3f gimbal_target;             ///< Rotation matrix from plane to camera. Then Euler angles to the servos.
        m = _ahrs.get_dcm_matrix();
        m.transpose();
        cam.from_euler(_roll_control_angle, _tilt_control_angle, _pan_control_angle);
        gimbal_target = m * cam;
        gimbal_target.to_euler(&_roll_angle, &_tilt_angle, &_pan_angle);
        _roll_angle  = _stab_roll ? degrees(_roll_angle) : degrees(_roll_control_angle);
        _tilt_angle  = _stab_tilt ? degrees(_tilt_angle) : degrees(_tilt_control_angle);
        _pan_angle   = degrees(_pan_angle);
    } else {
        // otherwise base mount roll and tilt on the ahrs
        // roll/tilt attitude, plus any requested angle
        _roll_angle  = degrees(_roll_control_angle);
        _tilt_angle  = degrees(_tilt_control_angle);
        _pan_angle   = degrees(_pan_control_angle);
        if (_stab_roll) {
            _roll_angle -= degrees(_ahrs.roll);
        }
        if (_stab_tilt) {
            _tilt_angle -= degrees(_ahrs.pitch);
        }

        // Add lead filter.
        const Vector3f &gyro = _ahrs.get_gyro();

        if (_stab_roll && _roll_stb_lead != 0.0f && fabsf(_ahrs.pitch) < M_PI/3.0f) {
            // Compute rate of change of euler roll angle
            float roll_rate = gyro.x + (_ahrs.sin_pitch() / _ahrs.cos_pitch()) * (gyro.y * _ahrs.sin_roll() + gyro.z * _ahrs.cos_roll());
            _roll_angle -= degrees(roll_rate) * _roll_stb_lead;
        }

        if (_stab_tilt && _pitch_stb_lead != 0.0f) {
            // Compute rate of change of euler pitch angle
            float pitch_rate = _ahrs.cos_pitch() * gyro.y - _ahrs.sin_roll() * gyro.z;
            _tilt_angle -= degrees(pitch_rate) * _pitch_stb_lead;
        }
    }
#endif
}
// stabilize - stabilizes the mount relative to the Earth's frame
//  input: _angle_ef_target_rad (earth frame targets in radians)
//  output: _angle_bf_output_deg (body frame angles in degrees)
void AP_Mount_Servo::stabilize()
{
    AP_AHRS &ahrs = AP::ahrs();
    // only do the full 3D frame transform if we are doing pan control
    if (_state._stab_pan) {
        Matrix3f m;                         ///< holds 3 x 3 matrix, var is used as temp in calcs
        Matrix3f cam;                       ///< Rotation matrix earth to camera. Desired camera from input.
        Matrix3f gimbal_target;             ///< Rotation matrix from plane to camera. Then Euler angles to the servos.
        m = ahrs.get_rotation_body_to_ned();
        m.transpose();
        cam.from_euler(_angle_ef_target_rad.x, _angle_ef_target_rad.y, _angle_ef_target_rad.z);
        gimbal_target = m * cam;
        gimbal_target.to_euler(&_angle_bf_output_deg.x, &_angle_bf_output_deg.y, &_angle_bf_output_deg.z);
        _angle_bf_output_deg.x  = _state._stab_roll ? degrees(_angle_bf_output_deg.x) : degrees(_angle_ef_target_rad.x);
        _angle_bf_output_deg.y  = _state._stab_tilt ? degrees(_angle_bf_output_deg.y) : degrees(_angle_ef_target_rad.y);
        _angle_bf_output_deg.z = degrees(_angle_bf_output_deg.z);
    } else {
        // otherwise base mount roll and tilt on the ahrs
        // roll/tilt attitude, plus any requested angle
        _angle_bf_output_deg.x = degrees(_angle_ef_target_rad.x);
        _angle_bf_output_deg.y = degrees(_angle_ef_target_rad.y);
        _angle_bf_output_deg.z = degrees(_angle_ef_target_rad.z);
        if (_state._stab_roll) {
            _angle_bf_output_deg.x -= degrees(ahrs.roll);
        }
        if (_state._stab_tilt) {
            _angle_bf_output_deg.y -= degrees(ahrs.pitch);
        }

        // lead filter
        const Vector3f &gyro = ahrs.get_gyro();

        if (_state._stab_roll && !is_zero(_state._roll_stb_lead) && fabsf(ahrs.pitch) < M_PI/3.0f) {
            // Compute rate of change of euler roll angle
            float roll_rate = gyro.x + (ahrs.sin_pitch() / ahrs.cos_pitch()) * (gyro.y * ahrs.sin_roll() + gyro.z * ahrs.cos_roll());
            _angle_bf_output_deg.x -= degrees(roll_rate) * _state._roll_stb_lead;
        }

        if (_state._stab_tilt && !is_zero(_state._pitch_stb_lead)) {
            // Compute rate of change of euler pitch angle
            float pitch_rate = ahrs.cos_pitch() * gyro.y - ahrs.sin_roll() * gyro.z;
            _angle_bf_output_deg.y -= degrees(pitch_rate) * _state._pitch_stb_lead;
        }
    }
}
Exemple #14
0
/*
  emulate an analog rangefinder
 */
uint16_t SITL_State::_ground_sonar(void)
{
    float altitude = height_agl();

    // sensor position offset in body frame
    Vector3f relPosSensorBF = _sitl->rngfnd_pos_offset;

    // adjust altitude for position of the sensor on the vehicle if position offset is non-zero
    if (!relPosSensorBF.is_zero()) {
        // get a rotation matrix following DCM conventions (body to earth)
        Matrix3f rotmat;
        rotmat.from_euler(radians(_sitl->state.rollDeg),
                          radians(_sitl->state.pitchDeg),
                          radians(_sitl->state.yawDeg));
        // rotate the offset into earth frame
        Vector3f relPosSensorEF = rotmat * relPosSensorBF;
        // correct the altitude at the sensor
        altitude -= relPosSensorEF.z;
    }

    float voltage = 5.0f;
    if (fabsf(_sitl->state.rollDeg) < 90 &&
            fabsf(_sitl->state.pitchDeg) < 90) {
        // adjust for apparent altitude with roll
        altitude /= cosf(radians(_sitl->state.rollDeg)) * cosf(radians(_sitl->state.pitchDeg));

        altitude += _sitl->sonar_noise * _rand_float();

        // Altitude in in m, scaler in meters/volt
        voltage = altitude / _sitl->sonar_scale;
        voltage = constrain_float(voltage, 0, 5.0f);

        if (_sitl->sonar_glitch >= (_rand_float() + 1.0f)/2.0f) {
            voltage = 5.0f;
        }
    }

    return 1023*(voltage / 5.0f);
}
void AC_AttitudeControl::get_rotation_reference_to_ned(Matrix3f& m)
{
    m.from_euler(_att_target_euler_rad.x,_att_target_euler_rad.y,_att_target_euler_rad.z);
}
/*
  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);
}