better tuning for simple_flight

AirLib/include/controllers/simple_flight/firmware/Params.hpp

@@ -24,12 +24,12 @@ struct Params {
     float min_armed_output = 0.2f;
 
     static constexpr float pi = 3.14159265359f; //180-degrees
-    float max_pitch_rate = 1.5f, max_roll_rate = 1.5f, max_yaw_rate = 1.5f; //in radians/sec
+    float max_pitch_rate = 2.0f, max_roll_rate = 2.0f, max_yaw_rate = 2.0f; //in radians/sec
     float max_pitch_angle = pi / 6, max_roll_angle = pi / 6, max_yaw_angle = pi; // in radians
 
     //stabilizer params
     float p_pitch_rate = 0.5f, p_roll_rate = 0.5f, p_yaw_rate = 0.5f;
-    float p_pitch_angle = 0.5f, p_roll_angle = 0.5f, p_yaw_angle = 0.5f;
+    float p_pitch_angle = 1.5f, p_roll_angle = 1.5f, p_yaw_angle = 1.5f;
 
     ControlMode default_control_mode = ControlMode::Angle;
 };

AirLib/include/physics/FastPhysicsEngine.hpp

@@ -186,6 +186,9 @@ class FastPhysicsEngine : public PhysicsEngineBase {
             VectorMath::toEulerianAngle(next.pose.orientation, pitch, roll, yaw);
             pitch = roll = 0;
             next.pose.orientation = VectorMath::toQuaternion(pitch, roll, yaw);
+
+            //there is a lot of random angular velocity when vehicle is on the ground
+            next.twist.angular = Vector3r::Zero();
         }
         //else keep the orientation
         next.pose.position = collison_info.position + (collison_info.normal * collison_info.penetration_depth) + next.twist.linear * (dt_real * kCollisionResponseCycles);