[Spinal][AttitudeControl] add nan value checker for gimbal angles

aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp

@@ -962,13 +962,10 @@ void AttitudeController::pwmConversion()
             f_i.x = target_thrust_[i*2];
             f_i.z = target_thrust_[i*2+1];
             target_thrust_[i] = ap::pythagorous2(f_i.x,f_i.z);
-
-            if(integrate_flag_){
-              target_gimbal_angles_[i] = atan2f(-f_i.x, f_i.z);
-            }
-            else
+            float gimbal_candidate = atan2f(-f_i.x, f_i.z);
+            if(std::isfinite(gimbal_candidate))
               {
-                target_gimbal_angles_[i] = 0.0;
+                target_gimbal_angles_[i] =(target_gimbal_angles_[i]+ gimbal_candidate)/2;
               }
           }
 
@@ -1003,6 +1000,8 @@ void AttitudeController::pwmConversion()
         }
       else
         {
+          // gimbal_map[i+1] = 0;
+          // int target_angle = (int)(0);          
           gimbal_map[i+1] = 100.0;
         }
     }