AC_AttitudeControl: add SITL panic to remind us to implent the quarternion

libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@@ -212,6 +212,11 @@ void AC_AttitudeControl::reset_rate_controller_I_terms_smoothly()
 // Command a Quaternion attitude with feedforward and smoothing
 void AC_AttitudeControl::input_quaternion(Quaternion attitude_desired_quat)
 {
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    // this function is not currently used, this is a reminder that we need to add a 6DoF implementation
+    AP_HAL::panic("input_quaternion not implemented AC_AttitudeControl_Multi_6DoF");
+#endif
+
     // calculate the attitude target euler angles
     _attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
 

libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp

@@ -124,6 +124,20 @@ void AC_AttitudeControl_Multi_6DoF::input_angle_step_bf_roll_pitch_yaw(float rol
     AC_AttitudeControl_Multi::input_angle_step_bf_roll_pitch_yaw(roll_angle_step_bf_cd, pitch_angle_step_bf_cd, yaw_angle_step_bf_cd);
 }
 
+// Command a Quaternion attitude with feedforward and smoothing
+// not used anywhere in current code, panic in SITL so this implementaiton is not overlooked
+void AC_AttitudeControl_Multi_6DoF::input_quaternion(Quaternion attitude_desired_quat) {
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    AP_HAL::panic("input_quaternion not implemented AC_AttitudeControl_Multi_6DoF");
+#endif
+
+    _motors.set_lateral(0.0f);
+    _motors.set_forward(0.0f);
+
+    AC_AttitudeControl_Multi::input_quaternion(attitude_desired_quat);
+}
+
+
 AC_AttitudeControl_Multi_6DoF *AC_AttitudeControl_Multi_6DoF::_singleton = nullptr;
 
 #endif // ENABLE_SCRIPTING

libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h

@@ -20,10 +20,7 @@ public:
 
     // Command a Quaternion attitude with feedforward and smoothing
     // not used anywhere in current code, panic so this implementaiton is not overlooked
-    void input_quaternion(Quaternion attitude_desired_quat) override {
-        AP_HAL::panic("input_quaternion not implemented AC_AttitudeControl_Multi_6DoF");
-    }
-
+    void input_quaternion(Quaternion attitude_desired_quat) override;
     /*
         override input functions to attitude controller and convert desired angles into thrust angles and substitute for osset angles
     */