void PlanetViewController::move(vec3d &oldp, vec3d &p)
{
oldp = oldp.normalize();
p = p.normalize();
double oldlat = safe_asin(oldp.z);
double oldlon = atan2(oldp.y, oldp.x);
double lat = safe_asin(p.z);
double lon = atan2(p.y, p.x);
x0 -= lon - oldlon;
y0 -= lat - oldlat;
y0 = max(-M_PI / 2.0, min(M_PI / 2.0, y0));
}
// set_linear_servo_out - sets swashplate servo output to be linear
float AP_MotorsHeli_Swash::get_linear_servo_output(float input) const
{
input = constrain_float(input, -1.0f, 1.0f);
//servo output is calculated by normalizing input to 50 deg arm rotation as full input for a linear throw
return safe_asin(0.766044f * input) * 1.145916;
}
// calculate euler angles from a rotation matrix
// this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
void Matrix3f_to_euler(Matrix3f *matrix3,float *roll, float *pitch, float *yaw){
if (pitch != NULL) {
*pitch = -safe_asin(matrix3->c.x);
}
if (roll != NULL) {
*roll = atan2f(matrix3->c.y, matrix3->c.z);
}
if (yaw != NULL) {
*yaw = atan2f(matrix3->b.x, matrix3->a.x);
}
}
void Matrix3<T>::to_euler(float *roll, float *pitch, float *yaw) const
{
if (pitch != NULL) {
*pitch = -safe_asin(c.x);
}
if (roll != NULL) {
*roll = atan2f(c.y, c.z);
}
if (yaw != NULL) {
*yaw = atan2f(b.x, a.x);
}
}
//static void get_of_angles(int16_t roll_in, int16_t pitch_in, int16_t &roll_out, int16_t &pitch_out)
void AC_OpticalFlowPX4::get_of_angles(float vx_in,float vy_in, int16_t &pitch_out, int16_t &roll_out, float G_Dt)
{
Vector3f vel_of_error,angles_target;
vx_in = constrain_float(vx_in, -200, 200);//max velocity
vy_in = constrain_float(vy_in, -200, 200);
//filtering of
vel_of_x=vel_of_x*_alpha_of+((1-_alpha_of)*(flow_comp_m_x*100));//en cm
vel_of_y=vel_of_y*_alpha_of+((1-_alpha_of)*(flow_comp_m_y*100));//en cm
pos_of_x+=vel_of_x*G_Dt;
pos_of_y+=vel_of_y*G_Dt;
vel_of_error.x = (vx_in - vel_of_x);
vel_of_error.y =(vy_in - vel_of_y);
angles_target.x = _pid_optflow.get_pid(vel_of_error.x, G_Dt);
angles_target.y = _pid_optflow.get_pid(vel_of_error.y, G_Dt);
target_roll = constrain_float(safe_asin(angles_target.y)*(18000/M_PI), -1500, 1500);
target_pitch = constrain_float(safe_asin(-angles_target.x)*(18000/M_PI),-1500, 1500);
//targetsAng_XY.x=roll_target;
//targetsAng_XY.y=pitch_target;
tmr_of_rst=hal.scheduler->millis();
if(tmr_of_rst>=tmr_of_rst_n+40000){
pos_of_x=0;
pos_of_y=0;
tmr_of_rst_n=hal.scheduler->millis();
}
roll_out = (int16_t)target_roll;
pitch_out = (int16_t)target_pitch;
}
void PlanetViewController::moveForward(double distance)
{
double co = cos(x0); // x0 = longitude
double so = sin(x0);
double ca = cos(y0); // y0 = latitude
double sa = sin(y0);
vec3d po = vec3d(co*ca, so*ca, sa) * R;
vec3d px = vec3d(-so, co, 0.0);
vec3d py = vec3d(-co*sa, -so*sa, ca);
vec3d pd = (po - px * sin(phi) * distance + py * cos(phi) * distance).normalize();
x0 = atan2(pd.y, pd.x);
y0 = safe_asin(pd.z);
}
// create eulers from a quaternion
void Quaternion::to_vector312(float &roll, float &pitch, float &yaw) const
{
Matrix3f m;
rotation_matrix(m);
float T21 = m.a.y;
float T22 = m.b.y;
float T23 = m.c.y;
float T13 = m.c.x;
float T33 = m.c.z;
yaw = atan2f(-T21, T22);
roll = safe_asin(T23);
pitch = atan2f(-T13, T33);
}
// create eulers from a quaternion
void Quaternion_D::to_euler(float *roll, float *pitch, float *yaw)
{
if (roll) {
*roll = (atan2f(2.0f*(q4*q1 + q2*q3),
1 - 2.0f*(q1*q1 + q2*q2)));
}
if (pitch) {
// we let safe_asin() handle the singularities near 90/-90 in pitch
*pitch = safe_asin(2.0f*(q4*q2 - q3*q1));
}
if (yaw) {
*yaw = atan2f(2.0f*(q4*q3 + q1*q2),
1 - 2.0f*(q2*q2 + q3*q3));
}
}
void PlanetViewController::turn(double angle)
{
double co = cos(x0); // x0 = longitude
double so = sin(x0);
double ca = cos(y0); // y0 = latitude
double sa = sin(y0);
double l = d * sin(theta);
vec3d po = vec3d(co*ca, so*ca, sa) * R;
vec3d px = vec3d(-so, co, 0.0);
vec3d py = vec3d(-co*sa, -so*sa, ca);
vec3d f = -px * sin(phi) + py * cos(phi);
vec3d r = px * cos(phi) + py * sin(phi);
vec3d pd = (po + f * (cos(angle) - 1.0) * l - r * sin(angle) * l).normalize();
x0 = atan2(pd.y, pd.x);
y0 = safe_asin(pd.z);
phi += angle;
}
/*
* Update AOA and SSA estimation based on airspeed, velocity vector and wind vector
*
* Based on:
* "On estimation of wind velocity, angle-of-attack and sideslip angle of small UAVs using standard sensors" by
* Tor A. Johansen, Andrea Cristofaro, Kim Sorensen, Jakob M. Hansen, Thor I. Fossen
*
* "Multi-Stage Fusion Algorithm for Estimation of Aerodynamic Angles in Mini Aerial Vehicle" by
* C.Ramprasadh and Hemendra Arya
*
* "ANGLE OF ATTACK AND SIDESLIP ESTIMATION USING AN INERTIAL REFERENCE PLATFORM" by
* JOSEPH E. ZEIS, JR., CAPTAIN, USAF
*/
void AP_AHRS::update_AOA_SSA(void)
{
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
const uint32_t now = AP_HAL::millis();
if (now - _last_AOA_update_ms < 50) {
// don't update at more than 20Hz
return;
}
_last_AOA_update_ms = now;
Vector3f aoa_velocity, aoa_wind;
// get velocity and wind
if (get_velocity_NED(aoa_velocity) == false) {
return;
}
aoa_wind = wind_estimate();
// Rotate vectors to the body frame and calculate velocity and wind
const Matrix3f &rot = get_rotation_body_to_ned();
aoa_velocity = rot.mul_transpose(aoa_velocity);
aoa_wind = rot.mul_transpose(aoa_wind);
// calculate relative velocity in body coordinates
aoa_velocity = aoa_velocity - aoa_wind;
const float vel_len = aoa_velocity.length();
// do not calculate if speed is too low
if (vel_len < 2.0) {
_AOA = 0;
_SSA = 0;
return;
}
// Calculate AOA and SSA
if (aoa_velocity.x > 0) {
_AOA = degrees(atanf(aoa_velocity.z / aoa_velocity.x));
} else {
_AOA = 0;
}
_SSA = degrees(safe_asin(aoa_velocity.y / vel_len));
#endif
}
// output_armed - sends commands to the motors
// includes new scaling stability patch
void AP_MotorsTri::output_armed_stabilizing()
{
float roll_thrust; // roll thrust input value, +/- 1.0
float pitch_thrust; // pitch thrust input value, +/- 1.0
float yaw_thrust; // yaw thrust input value, +/- 1.0
float throttle_thrust; // throttle thrust input value, 0.0 - 1.0
float throttle_thrust_best_rpy; // throttle providing maximum roll, pitch and yaw range without climbing
float rpy_scale = 1.0f; // this is used to scale the roll, pitch and yaw to fit within the motor limits
float rpy_low = 0.0f; // lowest motor value
float rpy_high = 0.0f; // highest motor value
float thr_adj; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy
// sanity check YAW_SV_ANGLE parameter value to avoid divide by zero
_yaw_servo_angle_max_deg = constrain_float(_yaw_servo_angle_max_deg, AP_MOTORS_TRI_SERVO_RANGE_DEG_MIN, AP_MOTORS_TRI_SERVO_RANGE_DEG_MAX);
// apply voltage and air pressure compensation
roll_thrust = _roll_in * get_compensation_gain();
pitch_thrust = _pitch_in * get_compensation_gain();
yaw_thrust = _yaw_in * get_compensation_gain()*sinf(radians(_yaw_servo_angle_max_deg)); // we scale this so a thrust request of 1.0f will ask for full servo deflection at full rear throttle
throttle_thrust = get_throttle() * get_compensation_gain();
// calculate angle of yaw pivot
_pivot_angle = safe_asin(yaw_thrust);
if (fabsf(_pivot_angle) > radians(_yaw_servo_angle_max_deg)) {
limit.yaw = true;
_pivot_angle = constrain_float(_pivot_angle, -radians(_yaw_servo_angle_max_deg), radians(_yaw_servo_angle_max_deg));
}
float pivot_thrust_max = cosf(_pivot_angle);
float thrust_max = 1.0f;
// sanity check throttle is above zero and below current limited throttle
if (throttle_thrust <= 0.0f) {
throttle_thrust = 0.0f;
limit.throttle_lower = true;
}
if (throttle_thrust >= _throttle_thrust_max) {
throttle_thrust = _throttle_thrust_max;
limit.throttle_upper = true;
}
_throttle_avg_max = constrain_float(_throttle_avg_max, throttle_thrust, _throttle_thrust_max);
// The following mix may be offer less coupling between axis but needs testing
//_thrust_right = roll_thrust * -0.5f + pitch_thrust * 1.0f;
//_thrust_left = roll_thrust * 0.5f + pitch_thrust * 1.0f;
//_thrust_rear = 0;
_thrust_right = roll_thrust * -0.5f + pitch_thrust * 0.5f;
_thrust_left = roll_thrust * 0.5f + pitch_thrust * 0.5f;
_thrust_rear = pitch_thrust * -0.5f;
// calculate roll and pitch for each motor
// set rpy_low and rpy_high to the lowest and highest values of the motors
// record lowest roll pitch command
rpy_low = MIN(_thrust_right,_thrust_left);
rpy_high = MAX(_thrust_right,_thrust_left);
if (rpy_low > _thrust_rear){
rpy_low = _thrust_rear;
}
// check to see if the rear motor will reach maximum thrust before the front two motors
if ((1.0f - rpy_high) > (pivot_thrust_max - _thrust_rear)){
thrust_max = pivot_thrust_max;
rpy_high = _thrust_rear;
}
// calculate throttle that gives most possible room for yaw (range 1000 ~ 2000) which is the lower of:
// 1. 0.5f - (rpy_low+rpy_high)/2.0 - this would give the maximum possible room margin above the highest motor and below the lowest
// 2. the higher of:
// a) the pilot's throttle input
// b) the point _throttle_rpy_mix between the pilot's input throttle and hover-throttle
// Situation #2 ensure we never increase the throttle above hover throttle unless the pilot has commanded this.
// Situation #2b allows us to raise the throttle above what the pilot commanded but not so far that it would actually cause the copter to rise.
// We will choose #1 (the best throttle for yaw control) if that means reducing throttle to the motors (i.e. we favor reducing throttle *because* it provides better yaw control)
// We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low. We favor reducing throttle instead of better yaw control because the pilot has commanded it
// check everything fits
throttle_thrust_best_rpy = MIN(0.5f*thrust_max - (rpy_low+rpy_high)/2.0, _throttle_avg_max);
if(is_zero(rpy_low)){
rpy_scale = 1.0f;
} else {
rpy_scale = constrain_float(-throttle_thrust_best_rpy/rpy_low, 0.0f, 1.0f);
}
// calculate how close the motors can come to the desired throttle
thr_adj = throttle_thrust - throttle_thrust_best_rpy;
if(rpy_scale < 1.0f){
// Full range is being used by roll, pitch, and yaw.
limit.roll_pitch = true;
if (thr_adj > 0.0f){
limit.throttle_upper = true;
}
thr_adj = 0.0f;
}else{
if(thr_adj < -(throttle_thrust_best_rpy+rpy_low)){
// Throttle can't be reduced to desired value
thr_adj = -(throttle_thrust_best_rpy+rpy_low);
}else if(thr_adj > thrust_max - (throttle_thrust_best_rpy+rpy_high)){
// Throttle can't be increased to desired value
thr_adj = thrust_max - (throttle_thrust_best_rpy+rpy_high);
limit.throttle_upper = true;
}
}
// add scaled roll, pitch, constrained yaw and throttle for each motor
_thrust_right = throttle_thrust_best_rpy+thr_adj + rpy_scale*_thrust_right;
_thrust_left = throttle_thrust_best_rpy+thr_adj + rpy_scale*_thrust_left;
_thrust_rear = throttle_thrust_best_rpy+thr_adj + rpy_scale*_thrust_rear;
// scale pivot thrust to account for pivot angle
// we should not need to check for divide by zero as _pivot_angle is constrained to the 5deg ~ 80 deg range
_thrust_rear = _thrust_rear/cosf(_pivot_angle);
// constrain all outputs to 0.0f to 1.0f
// test code should be run with these lines commented out as they should not do anything
_thrust_right = constrain_float(_thrust_right, 0.0f, 1.0f);
_thrust_left = constrain_float(_thrust_left, 0.0f, 1.0f);
_thrust_rear = constrain_float(_thrust_rear, 0.0f, 1.0f);
}
// get euler pitch angle
float Quaternion::get_euler_pitch() const
{
return safe_asin(2.0f*(q1*q3 - q4*q2));
}
// create eulers from a quaternion
void Quaternion::to_euler(float &roll, float &pitch, float &yaw) const
{
roll = (atan2f(2.0f*(q1*q2 + q3*q4), 1 - 2.0f*(q2*q2 + q3*q3)));
pitch = safe_asin(2.0f*(q1*q3 - q4*q2));
yaw = atan2f(2.0f*(q1*q4 + q2*q3), 1 - 2.0f*(q3*q3 + q4*q4));
}
