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