pid: add better tests

Author: bkleiner

test/test_native/test_main.c

@@ -14,6 +14,9 @@ extern void test_filter_cascade(void);
 extern void test_pid_proportional_control(void);
 extern void test_pid_integral_accumulation(void);
 extern void test_pid_derivative_calculation(void);
+extern void test_pid_dterm_setpoint_response(void);
+extern void test_pid_dterm_combined_response(void);
+extern void test_pid_dterm_stick_weighting(void);
 extern void test_pid_voltage_compensation(void);
 extern void test_pid_complete_loop(void);
 
@@ -104,6 +107,9 @@ int main(int argc, char **argv) {
   RUN_TEST(test_pid_proportional_control);
   RUN_TEST(test_pid_integral_accumulation);
   RUN_TEST(test_pid_derivative_calculation);
+  RUN_TEST(test_pid_dterm_setpoint_response);
+  RUN_TEST(test_pid_dterm_combined_response);
+  RUN_TEST(test_pid_dterm_stick_weighting);
   RUN_TEST(test_pid_voltage_compensation);
   RUN_TEST(test_pid_complete_loop);
 

test/test_native/test_pid.c

@@ -105,28 +105,128 @@ void test_pid_integral_accumulation(void) {
   TEST_ASSERT_FLOAT_WITHIN(0.001f, expected_integral, state.pidoutput.roll);
 }
 
-// Test derivative calculation
+// Test derivative calculation - gyro response
 void test_pid_derivative_calculation(void) {
   pid_setUp();
-  // Setup
+  // Setup - only D term enabled
   profile.pid.pid_rates[0].kp.roll = 0.0f;
   profile.pid.pid_rates[0].ki.roll = 0.0f;
   profile.pid.pid_rates[0].kd.roll = 1.0f;
 
-  // Initialize with no error
-  state.error.roll = 0.0f;
+  // Initialize stick profile (needed for D-term calculation)
+  profile.pid.stick_rates[0].accelerator.roll = 1.0f;
+  profile.pid.stick_rates[0].transition.roll = 1.0f;
+  
+  // Initialize state.timefactor (needed for D-term)
+  state.timefactor = 0.0032f / state.looptime; // Original timefactor calculation
+  
+  // Initialize with no gyro rate
+  state.gyro.roll = 0.0f;
+  state.setpoint.roll = 0.0f;
   pid_init();
   pid_calc();
 
-  // Introduce error step
-  state.error.roll = 1.0f;
+  // Introduce gyro rate change - this should produce negative D-term output
+  state.gyro.roll = 1.0f;
   pid_calc();
 
-  // The derivative should detect the change in error
-  // D = Kd * (error - previous_error) / dt
-  float expected_d = profile.pid.pid_rates[0].kd.roll * (1.0f - 0.0f) / state.looptime;
+  // QUICKSILVER D-term = setpoint_derivative - gyro_derivative
+  // With no setpoint change but gyro change, we expect negative output
+  TEST_ASSERT_TRUE(state.pidoutput.roll < -0.1f);
+}
+
+// Test D-term setpoint response
+void test_pid_dterm_setpoint_response(void) {
+  pid_setUp();
+  // Setup - only D term enabled
+  profile.pid.pid_rates[0].kp.roll = 0.0f;
+  profile.pid.pid_rates[0].ki.roll = 0.0f;
+  profile.pid.pid_rates[0].kd.roll = 1.0f;
+
+  // Initialize stick profile (need transition < 1 for setpoint to have effect)
+  profile.pid.stick_rates[0].accelerator.roll = 1.0f;
+  profile.pid.stick_rates[0].transition.roll = 0.0f; // No transition weighting
+  
+  // Initialize state.timefactor
+  state.timefactor = 0.0032f / state.looptime;
 
-  // Note: actual implementation might differ, this is a basic test
+  // Set some stick input so transition weight isn't 0
+  state.rx_filtered.roll = 0.0f; // No stick input
+  
+  // Initialize with no movement
+  state.gyro.roll = 0.0f;
+  state.setpoint.roll = 0.0f;
+  pid_init();
+  pid_calc();
+
+  // Introduce setpoint change - this should produce positive D-term output
+  state.setpoint.roll = 1.0f;
+  pid_calc();
+
+  // With setpoint change but no gyro change, we expect positive output
+  TEST_ASSERT_TRUE(state.pidoutput.roll > 0.1f);
+}
+
+// Test D-term combined response
+void test_pid_dterm_combined_response(void) {
+  pid_setUp();
+  // Setup - only D term enabled
+  profile.pid.pid_rates[0].kp.roll = 0.0f;
+  profile.pid.pid_rates[0].ki.roll = 0.0f;
+  profile.pid.pid_rates[0].kd.roll = 1.0f;
+
+  // Initialize stick profile
+  profile.pid.stick_rates[0].accelerator.roll = 1.0f;
+  profile.pid.stick_rates[0].transition.roll = 0.0f;
+  
+  // Initialize state.timefactor
+  state.timefactor = 0.0032f / state.looptime;
+  
+  // Initialize
+  state.gyro.roll = 0.0f;
+  state.setpoint.roll = 0.0f;
+  pid_init();
+  pid_calc();
+
+  // Introduce matching setpoint and gyro changes
+  // This should result in minimal D-term output (setpoint_deriv - gyro_deriv ≈ 0)
+  state.setpoint.roll = 1.0f;
+  state.gyro.roll = 1.0f;
+  pid_calc();
+
+  // The D-term should be close to zero but not exactly zero due to stick weighting
+  TEST_ASSERT_FLOAT_WITHIN(0.1f, 0.0f, state.pidoutput.roll);
+}
+
+// Test D-term stick transition weighting
+void test_pid_dterm_stick_weighting(void) {
+  pid_setUp();
+  // Setup - only D term enabled
+  profile.pid.pid_rates[0].kp.roll = 0.0f;
+  profile.pid.pid_rates[0].ki.roll = 0.0f;
+  profile.pid.pid_rates[0].kd.roll = 1.0f;
+
+  // Test different stick accelerator values
+  profile.pid.stick_rates[0].accelerator.roll = 2.0f; // Higher than 1
+  profile.pid.stick_rates[0].transition.roll = 0.0f; // Ensure setpoint has effect
+  
+  // Initialize state.timefactor
+  state.timefactor = 0.0032f / state.looptime;
+  
+  // Set some stick input
+  state.rx_filtered.roll = 0.5f;
+  
+  // Initialize
+  state.gyro.roll = 0.0f;
+  state.setpoint.roll = 0.0f;
+  pid_init();
+  pid_calc();
+
+  // Introduce setpoint change
+  state.setpoint.roll = 1.0f;
+  pid_calc();
+
+  // Should still have output (could be positive or negative depending on stick weighting)
   TEST_ASSERT_NOT_EQUAL(0.0f, state.pidoutput.roll);
 }