/* calculate sin and cos of roll/pitch/yaw from a body_to_ned rotation matrix */ void AP_AHRS::calc_trig(const Matrix3f &rot, float &cr, float &cp, float &cy, float &sr, float &sp, float &sy) const { Vector2f yaw_vector(rot.a.x, rot.b.x); if (fabsf(yaw_vector.x) > 0 || fabsf(yaw_vector.y) > 0) { yaw_vector.normalize(); } sy = constrain_float(yaw_vector.y, -1.0f, 1.0f); cy = constrain_float(yaw_vector.x, -1.0f, 1.0f); // sanity checks if (yaw_vector.is_inf() || yaw_vector.is_nan()) { sy = 0.0f; cy = 1.0f; } const float cx2 = rot.c.x * rot.c.x; if (cx2 >= 1.0f) { cp = 0; cr = 1.0f; } else { cp = safe_sqrt(1 - cx2); cr = rot.c.z / cp; } cp = constrain_float(cp, 0.0f, 1.0f); cr = constrain_float(cr, -1.0f, 1.0f); // this relies on constrain_float() of infinity doing the right thing sp = -rot.c.x; if (!is_zero(cp)) { sr = rot.c.y / cp; } if (is_zero(cp) || isinf(cr) || isnan(cr) || isinf(sr) || isnan(sr)) { float r, p, y; rot.to_euler(&r, &p, &y); cr = cosf(r); sr = sinf(r); } }
void FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) { if (_global_pos_valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) { /* rotate ground speed vector with current attitude */ math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); yaw_vector.normalize(); float ground_speed_body = yaw_vector * ground_speed; /* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/ float distance = 0.0f; float delta_altitude = 0.0f; if (pos_sp_triplet.previous.valid) { distance = get_distance_to_next_waypoint(pos_sp_triplet.previous.lat, pos_sp_triplet.previous.lon, pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); delta_altitude = pos_sp_triplet.current.alt - pos_sp_triplet.previous.alt; } else { distance = get_distance_to_next_waypoint(current_position(0), current_position(1), pos_sp_triplet.current.lat, pos_sp_triplet.current.lon); delta_altitude = pos_sp_triplet.current.alt - _global_pos.alt; } float ground_speed_desired = _parameters.airspeed_min * cosf(atan2f(delta_altitude, distance)); /* * Ground speed undershoot is the amount of ground velocity not reached * by the plane. Consequently it is zero if airspeed is >= min ground speed * and positive if airspeed < min ground speed. * * This error value ensures that a plane (as long as its throttle capability is * not exceeded) travels towards a waypoint (and is not pushed more and more away * by wind). Not countering this would lead to a fly-away. */ _groundspeed_undershoot = math::max(ground_speed_desired - ground_speed_body, 0.0f); } else { _groundspeed_undershoot = 0; } }
void Beacon::loadBeacon(int x, int y, float roll, float pitch, int size) { const Vector3<float> FLOOR_POINT(0, 0, 0); const Vector3<float> POINT_PLANE(0, 0, -1); const Vector3<float> UP_VECTOR(0, 0, 1); Vector3<float> roll_vector(0, 1, 0); // Y Vector3<float> pitch_vector(1, 0, 0); // X Vector3<float> yaw_vector(0, 0, 1); // Z this->size = size; // Backup image point this->imagePoint.x = x; this->imagePoint.y = y; // Convert point from image RS to air RS img2air(x, y); // PITCH yaw_vector = yaw_vector.rotate(pitch_vector, -pitch); roll_vector = roll_vector.rotate(pitch_vector, -pitch); // ROLL pitch_vector = pitch_vector.rotate(roll_vector, -roll); yaw_vector = yaw_vector.rotate(roll_vector, -roll); // Vector pointing to the beacon Vector3<float> line_vector = yaw_vector * FOCAL_LENGTH + roll_vector * y * D_PER_PIXEL + pitch_vector * x * D_PER_PIXEL; // Project point to an horizontal plane float d = UP_VECTOR.dot(line_vector); if (d < 0.000001) // Parallel this->x = this->y = 0; else { this->x = line_vector.x / d; this->y = line_vector.y / d; } this->z = 0; this->used(false); }