C++ ToRad函数代码示例

// convert a set of roll rates from earth frame to body frame
void SITL::convert_body_frame(double rollDeg, double pitchDeg,
                              double rollRate, double pitchRate, double yawRate,
                              double *p, double *q, double *r)
{
	double phi, theta, phiDot, thetaDot, psiDot;

	phi = ToRad(rollDeg);
	theta = ToRad(pitchDeg);
	phiDot = ToRad(rollRate);
	thetaDot = ToRad(pitchRate);
	psiDot = ToRad(yawRate);

	*p = phiDot - psiDot*sin(theta);
	*q = cos(phi)*thetaDot + sin(phi)*psiDot*cos(theta);
	*r = cos(phi)*psiDot*cos(theta) - sin(phi)*thetaDot;    
}

// add_trim - adjust the roll and pitch trim up to a total of 10 degrees
void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians, bool save_to_eeprom)
{
    Vector3f trim = _trim.get();

    // add new trim
    trim.x = constrain_float(trim.x + roll_in_radians, ToRad(-AP_AHRS_TRIM_LIMIT), ToRad(AP_AHRS_TRIM_LIMIT));
    trim.y = constrain_float(trim.y + pitch_in_radians, ToRad(-AP_AHRS_TRIM_LIMIT), ToRad(AP_AHRS_TRIM_LIMIT));

    // set new trim values
    _trim.set(trim);

    // save to eeprom
    if( save_to_eeprom ) {
        _trim.save();
    }
}

static void calc_nav_roll()
{
#define NAV_ROLL_BY_RATE 0
#if NAV_ROLL_BY_RATE
    // Scale from centidegrees (PID input) to radians per second. A P gain of 1.0 should result in a
    // desired rate of 1 degree per second per degree of error - if you're 15 degrees off, you'll try
    // to turn at 15 degrees per second.
    float turn_rate = ToRad(g.pidNavRoll.get_pid(bearing_error_cd) * .01);

    // Use airspeed_cruise as an analogue for airspeed if we don't have airspeed.
    float speed;
    if (!ahrs.airspeed_estimate(&speed)) {
        speed = g.airspeed_cruise_cm*0.01;

        // Floor the speed so that the user can't enter a bad value
        if(speed < 6) {
            speed = 6;
        }
    }

    // Bank angle = V*R/g, where V is airspeed, R is turn rate, and g is gravity.
    nav_roll = ToDeg(atan(speed*turn_rate/GRAVITY_MSS)*100);

#else
    // this is the old nav_roll calculation. We will use this for 2.50
    // then remove for a future release
    float nav_gain_scaler = 0.01 * g_gps->ground_speed / g.scaling_speed;
    nav_gain_scaler = constrain(nav_gain_scaler, 0.2, 1.4);
    nav_roll_cd = g.pidNavRoll.get_pid(bearing_error_cd, nav_gain_scaler); //returns desired bank angle in degrees*100
#endif

    nav_roll_cd = constrain_int32(nav_roll_cd, -g.roll_limit_cd.get(), g.roll_limit_cd.get());
}

void DCM_driftCorrection(float* accelVector, float scaledAccelMag, float magneticHeading)
{
    //Compensation the Roll, Pitch and Yaw drift.
    float magneticHeading_X;
    float magneticHeading_Y;
    static float Scaled_Omega_P[3];
    static float Scaled_Omega_I[3];
    float Accel_weight;
    float Integrator_magnitude;
    float tempfloat;


    //*****Roll and Pitch***************
    // Dynamic weighting of accelerometer info (reliability filter)
    // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
    Accel_weight = constrain(1 - 2*abs(1 - scaledAccelMag),0,1);


    Vector_Cross_Product(&errorRollPitch[0],&accelVector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
    Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);

    Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
    Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);


    //*****YAW***************
    //Calculate Heading_X and Heading_Y
    magneticHeading_X = cos(magneticHeading);
    magneticHeading_Y = sin(magneticHeading);

    // We make the gyro YAW drift correction based on compass magnetic heading
    errorCourse=(DCM_Matrix[0][0]*magneticHeading_Y) - (DCM_Matrix[1][0]*magneticHeading_X);  //Calculating YAW error
    Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.

    Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
    Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding  Proportional.

    Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
    Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I


    //  Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
    Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I));
    if (Integrator_magnitude > ToRad(300)) {
    Vector_Scale(Omega_I,Omega_I,0.5f*ToRad(300)/Integrator_magnitude);
    }
}

// update mount position - should be called periodically
void AP_Mount_MAVLink::update()
{
    // exit immediately if not initialised
    if (!_initialised) {
        return;
    }

    // update based on mount mode
    switch(get_mode()) {
        // move mount to a "retracted" position.  we do not implement a separate servo based retract mechanism
        case MAV_MOUNT_MODE_RETRACT:
            break;

        // move mount to a neutral position, typically pointing forward
        case MAV_MOUNT_MODE_NEUTRAL:
            {
            const Vector3f &target = _state._neutral_angles.get();
            _angle_ef_target_rad.x = ToRad(target.x);
            _angle_ef_target_rad.y = ToRad(target.y);
            _angle_ef_target_rad.z = ToRad(target.z);
            }
            break;

        // point to the angles given by a mavlink message
        case MAV_MOUNT_MODE_MAVLINK_TARGETING:
            // do nothing because earth-frame angle targets (i.e. _angle_ef_target_rad) should have already been set by a MOUNT_CONTROL message from GCS
            break;

        // RC radio manual angle control, but with stabilization from the AHRS
        case MAV_MOUNT_MODE_RC_TARGETING:
            // update targets using pilot's rc inputs
            update_targets_from_rc();
            break;

        // point mount to a GPS point given by the mission planner
        case MAV_MOUNT_MODE_GPS_POINT:
            if(_frontend._ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
                calc_angle_to_location(_state._roi_target, _angle_ef_target_rad, true, true);
            }
            break;

        default:
            // we do not know this mode so do nothing
            break;
    }
}

void
GPS::update(void)
{
    bool result;
    uint32_t tnow;

    // call the GPS driver to process incoming data
    result = read();

    tnow = hal.scheduler->millis();

    // if we did not get a message, and the idle timer has expired, re-init
    if (!result) {
        if ((tnow - _idleTimer) > idleTimeout) {
            Debug("gps read timeout %lu %lu", (unsigned long)tnow, (unsigned long)_idleTimer);
            _status = NO_GPS;

            init(_port, _nav_setting);
            // reset the idle timer
            _idleTimer = tnow;
        }
    } else {
        // we got a message, update our status correspondingly
        _status = fix ? GPS_OK : NO_FIX;

        valid_read = true;
        new_data = true;

        // reset the idle timer
        _idleTimer = tnow;

        if (_status == GPS_OK) {
            last_fix_time = _idleTimer;
            _last_ground_speed_cm = ground_speed;

            if (_have_raw_velocity) {
                // the GPS is able to give us velocity numbers directly
                _velocity_north = _vel_north * 0.01;
                _velocity_east  = _vel_east * 0.01;
                _velocity_down  = _vel_down * 0.01;
            } else {
                float gps_heading = ToRad(ground_course * 0.01);
                float gps_speed   = ground_speed * 0.01;
                float sin_heading, cos_heading;

                cos_heading = cos(gps_heading);
                sin_heading = sin(gps_heading);

                _velocity_north = gps_speed * cos_heading;
                _velocity_east  = gps_speed * sin_heading;

				// no good way to get descent rate
				_velocity_down  = 0;
            }
        }
    }
}

/*
  fill in 3D velocity for a GPS that doesn't give vertical velocity numbers
 */
void AP_GPS_Backend::fill_3d_velocity(void)
{
    float gps_heading = ToRad(state.ground_course_cd * 0.01f);

    state.velocity.x = state.ground_speed * cosf(gps_heading);
    state.velocity.y = state.ground_speed * sinf(gps_heading);
    state.velocity.z = 0;
    state.have_vertical_velocity = false;
}

	/**************************************************************************
	* \brief Filter DCM Matrix Multiply
	*	The predict function. Updates 2 variables:
	*	our model-state x and the 2x2 matrix P /n
	*
	* x = [ angle, bias ]'
	*
	*   = F x + B u
	*
	*   = [ 1 -dt, 0 1 ] [ angle, bias ] + [ dt, 0 ] [ dotAngle 0 ]
	*
	*   => angle = angle + dt (dotAngle - bias)
	*      bias  = bias
	*
	*
	* P = F P transpose(F) + Q
	*
	*   = [ 1 -dt, 0 1 ] * P * [ 1 0, -dt 1 ] + Q
	*
	*  P(0,0) = P(0,0) - dt * ( P(1,0) + P(0,1) ) + dt² * P(1,1) + Q(0,0)
	*  P(0,1) = P(0,1) - dt * P(1,1) + Q(0,1)
	*  P(1,0) = P(1,0) - dt * P(1,1) + Q(1,0)
	*  P(1,1) = P(1,1) + Q(1,1)
	*
	* \param ---
	*
	* \return  ---
	***************************************************************************/
	static void filter_kalman_ars_predict(void)
	{
		x_angle += dt * (ToRad(filterdata->xGyr) - x_bias);

		P_00 +=  - dt * (P_10 + P_01) + filterdata->Q_angle * dt;
		P_01 +=  - dt * P_11;
		P_10 +=  - dt * P_11;
		P_11 +=  + filterdata->Q_gyro * dt;
	}

/// accel_to_lean_angles - horizontal desired acceleration to lean angles
///    converts desired accelerations provided in lat/lon frame to roll/pitch angles
void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler, bool use_althold_lean_angle)
{
    float accel_total;                          // total acceleration in cm/s/s
    float accel_right, accel_forward;
    float lean_angle_max = _attitude_control.lean_angle_max();
    float accel_max = POSCONTROL_ACCEL_XY_MAX;

    // limit acceleration if necessary
    if (use_althold_lean_angle) {
        accel_max = MIN(accel_max, GRAVITY_MSS * 100.0f * tanf(ToRad(constrain_float(_attitude_control.get_althold_lean_angle_max(),1000,8000)/100.0f)));
    }

    // scale desired acceleration if it's beyond acceptable limit
    accel_total = norm(_accel_target.x, _accel_target.y);
    if (accel_total > accel_max && accel_total > 0.0f) {
        _accel_target.x = accel_max * _accel_target.x/accel_total;
        _accel_target.y = accel_max * _accel_target.y/accel_total;
        _limit.accel_xy = true;     // unused
    } else {
        // reset accel limit flag
        _limit.accel_xy = false;
    }

    // reset accel to current desired acceleration
    if (_flags.reset_accel_to_lean_xy) {
        _accel_target_jerk_limited.x = _accel_target.x;
        _accel_target_jerk_limited.y = _accel_target.y;
        _accel_target_filter.reset(Vector2f(_accel_target.x, _accel_target.y));
        _flags.reset_accel_to_lean_xy = false;
    }

    // apply jerk limit of 17 m/s^3 - equates to a worst case of about 100 deg/sec/sec
    float max_delta_accel = dt * _jerk_cmsss;

    Vector2f accel_in(_accel_target.x, _accel_target.y);
    Vector2f accel_change = accel_in-_accel_target_jerk_limited;
    float accel_change_length = accel_change.length();

    if(accel_change_length > max_delta_accel) {
        accel_change *= max_delta_accel/accel_change_length;
    }
    _accel_target_jerk_limited += accel_change;

    // lowpass filter on NE accel
    _accel_target_filter.set_cutoff_frequency(MIN(_accel_xy_filt_hz, 5.0f*ekfNavVelGainScaler));
    Vector2f accel_target_filtered = _accel_target_filter.apply(_accel_target_jerk_limited, dt);

    // rotate accelerations into body forward-right frame
    accel_forward = accel_target_filtered.x*_ahrs.cos_yaw() + accel_target_filtered.y*_ahrs.sin_yaw();
    accel_right = -accel_target_filtered.x*_ahrs.sin_yaw() + accel_target_filtered.y*_ahrs.cos_yaw();

    // update angle targets that will be passed to stabilize controller
    _pitch_target = constrain_float(atanf(-accel_forward/(GRAVITY_MSS * 100))*(18000/M_PI),-lean_angle_max, lean_angle_max);
    float cos_pitch_target = cosf(_pitch_target*M_PI/18000);
    _roll_target = constrain_float(atanf(accel_right*cos_pitch_target/(GRAVITY_MSS * 100))*(18000/M_PI), -lean_angle_max, lean_angle_max);
}

/*
  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;
}

void
AP_Mount::calc_GPS_target_angle(struct Location *target)
{
    float GPS_vector_x = (target->lng-_current_loc->lng)*cos(ToRad((_current_loc->lat+target->lat)*.00000005))*.01113195;
    float GPS_vector_y = (target->lat-_current_loc->lat)*.01113195;
    float GPS_vector_z = (target->alt-_current_loc->alt);                 // baro altitude(IN CM) should be adjusted to known home elevation before take off (Set altimeter).
    float target_distance = 100.0*sqrt(GPS_vector_x*GPS_vector_x + GPS_vector_y*GPS_vector_y);      // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters.
    _roll_control_angle  = 0;
    _tilt_control_angle  = atan2(GPS_vector_z, target_distance);
    _pan_control_angle   = atan2(GPS_vector_x, GPS_vector_y);
}

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);
}

void test_quaternion_eulers(void)
{
    uint8_t i, j, k;
    uint8_t N = ARRAY_SIZE(angles);

    hal.console->println("quaternion unit tests\n");

    test_quaternion(M_PI/4, 0, 0);
    test_quaternion(0, M_PI/4, 0);
    test_quaternion(0, 0, M_PI/4);
    test_quaternion(-M_PI/4, 0, 0);
    test_quaternion(0, -M_PI/4, 0);
    test_quaternion(0, 0, -M_PI/4);
    test_quaternion(-M_PI/4, 1, 1);
    test_quaternion(1, -M_PI/4, 1);
    test_quaternion(1, 1, -M_PI/4);

    test_quaternion(ToRad(89), 0, 0.1f);
    test_quaternion(0, ToRad(89), 0.1f);
    test_quaternion(0.1f, 0, ToRad(89));

    test_quaternion(ToRad(91), 0, 0.1f);
    test_quaternion(0, ToRad(91), 0.1f);
    test_quaternion(0.1f, 0, ToRad(91));

    for (i=0; i<N; i++)
        for (j=0; j<N; j++)
            for (k=0; k<N; k++)
                test_quaternion(angles[i], angles[j], angles[k]);

    hal.console->println("tests done\n");
}

// calc_velocities - calculate angular velocity max and acceleration based on radius and rate
//      this should be called whenever the radius or rate are changed
//      initialises the yaw and current position around the circle
void AC_Circle::calc_velocities(bool init_velocity)
{
    // if we are doing a panorama set the circle_angle to the current heading
    if (_radius <= 0) {
        _angular_vel_max = ToRad(_rate);
        _angular_accel = max(fabsf(_angular_vel_max),ToRad(AC_CIRCLE_ANGULAR_ACCEL_MIN));  // reach maximum yaw velocity in 1 second
    }else{
        // calculate max velocity based on waypoint speed ensuring we do not use more than half our max acceleration for accelerating towards the center of the circle
        float velocity_max = min(_pos_control.get_speed_xy(), safe_sqrt(0.5f*_pos_control.get_accel_xy()*_radius));

        // angular_velocity in radians per second
        _angular_vel_max = velocity_max/_radius;
        _angular_vel_max = constrain_float(ToRad(_rate),-_angular_vel_max,_angular_vel_max);

        // angular_velocity in radians per second
        _angular_accel = max(_pos_control.get_accel_xy()/_radius, ToRad(AC_CIRCLE_ANGULAR_ACCEL_MIN));
    }

    // initialise angular velocity
    if (init_velocity) {
        _angular_vel = 0;
    }
}