// calculate rotational and linear accelerations
void Frame::calculate_forces(const Aircraft &aircraft,
const Aircraft::sitl_input &input,
Vector3f &rot_accel,
Vector3f &body_accel)
{
// rotational acceleration, in rad/s/s, in body frame
float thrust = 0.0f; // newtons
for (uint8_t i=0; i<num_motors; i++) {
float motor_speed = constrain_float((input.servos[motor_offset+motors[i].servo]-1100)/900.0, 0, 1);
rot_accel.x += -radians(5000.0) * sinf(radians(motors[i].angle)) * motor_speed;
rot_accel.y += radians(5000.0) * cosf(radians(motors[i].angle)) * motor_speed;
rot_accel.z += motors[i].yaw_factor * motor_speed * radians(400.0);
thrust += motor_speed * thrust_scale; // newtons
}
body_accel = Vector3f(0, 0, -thrust / mass);
if (terminal_rotation_rate > 0) {
// rotational air resistance
const Vector3f &gyro = aircraft.get_gyro();
rot_accel.x -= gyro.x * radians(400.0) / terminal_rotation_rate;
rot_accel.y -= gyro.y * radians(400.0) / terminal_rotation_rate;
rot_accel.z -= gyro.z * radians(400.0) / terminal_rotation_rate;
}
if (terminal_velocity > 0) {
// air resistance
Vector3f air_resistance = -aircraft.get_velocity_ef() * (GRAVITY_MSS/terminal_velocity);
body_accel += aircraft.get_dcm().transposed() * air_resistance;
}
// add some noise
const float gyro_noise = radians(0.1);
const float accel_noise = 0.3;
const float noise_scale = thrust / (thrust_scale * num_motors);
rot_accel += Vector3f(aircraft.rand_normal(0, 1),
aircraft.rand_normal(0, 1),
aircraft.rand_normal(0, 1)) * gyro_noise * noise_scale;
body_accel += Vector3f(aircraft.rand_normal(0, 1),
aircraft.rand_normal(0, 1),
aircraft.rand_normal(0, 1)) * accel_noise * noise_scale;
}
// calculate rotational and linear accelerations
void Frame::calculate_forces(const Aircraft &aircraft,
const Aircraft::sitl_input &input,
Vector3f &rot_accel,
Vector3f &body_accel)
{
Vector3f thrust; // newtons
for (uint8_t i=0; i<num_motors; i++) {
Vector3f mraccel, mthrust;
motors[i].calculate_forces(input, thrust_scale, motor_offset, mraccel, mthrust);
rot_accel += mraccel;
thrust += mthrust;
}
body_accel = thrust/aircraft.gross_mass();
if (terminal_rotation_rate > 0) {
// rotational air resistance
const Vector3f &gyro = aircraft.get_gyro();
rot_accel.x -= gyro.x * radians(400.0) / terminal_rotation_rate;
rot_accel.y -= gyro.y * radians(400.0) / terminal_rotation_rate;
rot_accel.z -= gyro.z * radians(400.0) / terminal_rotation_rate;
}
if (terminal_velocity > 0) {
// air resistance
Vector3f air_resistance = -aircraft.get_velocity_air_ef() * (GRAVITY_MSS/terminal_velocity);
body_accel += aircraft.get_dcm().transposed() * air_resistance;
}
// add some noise
const float gyro_noise = radians(0.1);
const float accel_noise = 0.3;
const float noise_scale = thrust.length() / (thrust_scale * num_motors);
rot_accel += Vector3f(aircraft.rand_normal(0, 1),
aircraft.rand_normal(0, 1),
aircraft.rand_normal(0, 1)) * gyro_noise * noise_scale;
body_accel += Vector3f(aircraft.rand_normal(0, 1),
aircraft.rand_normal(0, 1),
aircraft.rand_normal(0, 1)) * accel_noise * noise_scale;
}