Add way to force Josh into state

telemetry/Kconfig

@@ -314,4 +314,36 @@ config INSPACE_TELEMETRY_ACCEL_AVG_FILT_SIZE
 		The number of samples the acceleration average filter will use at a
 		time.
 
+comment "Flight State Debug Options"
+
+config INSPACE_TELEMETRY_DEBUG_FLIGHT_STATES
+	bool "Force Josh into a Flight State"
+	default n
+	---help---
+		Forces the system into a specific flight state for testing purposes.
+
+if INSPACE_TELEMETRY_DEBUG_FLIGHT_STATES
+
+config INSPACE_TELEMETRY_DEBUG_FLIGHT_STATE
+	int "Flight State"
+	default 0
+	range 0 2
+	---help---
+		The flight state to force the system into.
+		0: Idle
+		1: Airborne
+		2: Unknown
+
+config INSPACE_TELEMETRY_DEBUG_FLIGHT_SUB_STATE
+	int "Flight Sub-State"
+	default 0
+	range 0 2
+	---help---
+		The flight sub-state to force the system into.
+		0: Unknown
+		1: Ascent
+		2: Descent
+endif # INSPACE_DEBUG_FLIGHT_STATES
+
+
 endif # INSPACE_TELEMETRY

telemetry/src/telemetry_main.c

@@ -59,9 +59,17 @@ int main(int argc, char **argv) {
             inerr("Could not set flight substate, continuing anyways: %d\n", err);
         }
     } else {
+#ifndef CONFIG_INSPACE_TELEMETRY_DEBUG_FLIGHT_STATES
         enum flight_state_e flight_state;
         state_get_flightstate(&state, &flight_state);
         ininfo("Loaded state: %d from EEPROM\n", flight_state);
+#else
+        enum flight_state_e flight_state = CONFIG_INSPACE_TELEMETRY_DEBUG_FLIGHT_STATE;
+        enum flight_substate_e flight_substate = CONFIG_INSPACE_TELEMETRY_DEBUG_FLIGHT_SUB_STATE;
+        state_set_flightstate(&state, flight_state);
+        state_set_flightsubstate(&state, flight_substate);
+        ininfo("Overrided loaded state to: %d, %d\n", flight_state, flight_substate);
+#endif
     }
 
     /* Grab configuration parameters from EEPROM */
@@ -118,12 +126,14 @@ int main(int argc, char **argv) {
         goto exit_error;
     }
 
+#ifndef CONFIG_INSPACE_TELEMETRY_DEBUG_FLIGHT_STATES
     struct fusion_args fusion_thread_args = {.state = &state};
     err = pthread_create(&fusion_thread, NULL, fusion_main, &fusion_thread_args);
     if (err) {
         inerr("Problem starting fusion thread: %d\n", err);
         goto exit_error;
     }
+#endif
 
 #ifdef CONFIG_INSPACE_TELEMETRY_USBSH
     struct shell_args shell_args;