/// 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
}
// 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);
}
}
}
//余弦矩阵更新姿态
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);
}
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;
}
// 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;
}
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);
}
/*
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);
}
// 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;
}
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);
}
/// 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;
}
}
}
/*
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);
}
