SITL: allow for X8-m passthrough of motors inputs

bnsgeyer · Jun 7, 2024 · 0e095fe · 0e095fe
1 parent dbde230
commit 0e095fe
Showing 1 changed file with 21 additions and 12 deletions.
diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp
@@ -661,57 +661,66 @@ void Frame::calculate_forces_x8(const Aircraft &aircraft,
         }
         last_calc_us = now_us;
 
+    // determine pitch, roll, yaw, throttle from servo inputs
+    uint16_t servos_raw[16];
+    servos_raw[0] = (uint16_t)((input.servos[1] - input.servos[0]) * 0.5f + 1500.0f);
+    servos_raw[1] = (uint16_t)((input.servos[2] - input.servos[3]) * 0.5f + 1500.0f);
+    servos_raw[2] = (uint16_t)((input.servos[0] + input.servos[1] + input.servos[2] + input.servos[3]) * 0.25f);
+    servos_raw[3] = (uint16_t)((((input.servos[0] + input.servos[1]) * 0.5f - (input.servos[2] + input.servos[3]) * 0.5f) * 0.5) + 1500.0f);
+
 	if (_time_delay_rp == 0 || is_zero(dt)) {
 	    for (uint8_t i = 0; i < 2; i++) {
-		_servos_delayed[i] = input.servos[i];
+		_servos_delayed[i] = servos_raw[i];
             }
 	} else if (servos_stored_buffer == nullptr) {
             uint16_t buffer_size = constrain_int16(_time_delay_rp, 1, 100) * 0.001f / dt;
             servos_stored_buffer = new ObjectBuffer<servos_stored>(buffer_size);
             while (servos_stored_buffer->space() != 0) {
-                push_to_buffer(input.servos);
+                push_to_buffer(servos_raw);
             }
             for (uint8_t i = 0; i < 2; i++) {
-                _servos_delayed[i] = input.servos[i];
+                _servos_delayed[i] = servos_raw[i];
             }
 	} else {
             pull_from_buffer(_servos_delayed);
-            push_to_buffer(input.servos);
+            push_to_buffer(servos_raw);
 	}
 
 	if (_time_delay_col == 0 || is_zero(dt)) {
-	    _servo_delayed_col = input.servos[2];
+	    _servo_delayed_col = servos_raw[2];
 	} else if (servo_stored_col_buffer == nullptr) {
             uint16_t buffer_size = constrain_int16(_time_delay_col, 1, 100) * 0.001f / dt;
             servo_stored_col_buffer = new ObjectBuffer<servo_stored>(buffer_size);
             while (servo_stored_col_buffer->space() != 0) {
-                push_to_buffer_col(input.servos);
+                push_to_buffer_col(servos_raw);
             }
-            _servo_delayed_col = input.servos[2];
+            _servo_delayed_col = servos_raw[2];
 	} else {
             pull_from_buffer_col(_servo_delayed_col);
-            push_to_buffer_col(input.servos);
+            push_to_buffer_col(servos_raw);
 	}
 
 	if (_time_delay_ped == 0 || is_zero(dt)) {
-	    _servo_delayed_ped = input.servos[3];
+	    _servo_delayed_ped = servos_raw[3];
 	} else if (servo_stored_ped_buffer == nullptr) {
             uint16_t buffer_size = constrain_int16(_time_delay_ped, 1, 100) * 0.001f / dt;
             servo_stored_ped_buffer = new ObjectBuffer<servo_stored>(buffer_size);
             while (servo_stored_ped_buffer->space() != 0) {
-                push_to_buffer_ped(input.servos);
+                push_to_buffer_ped(servos_raw);
             }
-            _servo_delayed_ped = input.servos[3];
+            _servo_delayed_ped = servos_raw[3];
 	} else {
             pull_from_buffer_ped(_servo_delayed_ped);
-            push_to_buffer_ped(input.servos);
+            push_to_buffer_ped(servos_raw);
 	}
 
 	static float Dlonlag;
 	static float Dlatlag;
 	static float Dcollag;
 	static float Dpedlag;
 
+
+
 	float _roll_in = (_servos_delayed[0] / 500.0f) - 3.0f;  // roll
 	float _pitch_in = (_servos_delayed[1] / 500.0f) - 3.0f;  //pitch
 	float _throttle_in = (_servo_delayed_col / 1000.0f) - 1.0f;  // throttle