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