Handle invalid prelaunch commands gracefully instead of fail_fast

app/rev2/prelaunch.c

@@ -50,6 +50,30 @@ extern SENSOR_DATA sensor_data;
  Functions                                                               
 ------------------------------------------------------------------------------*/
 
+/*******************************************************************************
+*                                                                              *
+* PROCEDURE:                                                                   *
+* 		warn_invalid_command                                                   *
+*                                                                              *
+* DESCRIPTION:                                                                 *
+* 		Warns the user about an invalid command having been receieved by       *
+*       turning the LED red and beeping. Also flushes the USB buffer.          *
+*                                                                              *
+*******************************************************************************/
+void warn_invalid_command
+    (
+    )
+{
+    led_set_color( LED_RED );
+    BUZZ_STATUS buzz_result = buzzer_multi_beeps(300, 100, 2);
+
+    if (buzz_result != BUZZ_OK) {
+        //error_fail_fast()
+    }
+
+    usb_flush();
+
+} /* warn_invalid_command() */
 
 /*******************************************************************************
 *                                                                              *
@@ -133,15 +157,16 @@ if ( usb_detect() )
                 if ( command_status == USB_OK )
                     {
                     /* Execute sensor subcommand */
-                    sensor_cmd_execute( subcommand_code );
-                    }
-                else if ( command_status == USB_TIMEOUT )
+                    SENSOR_STATUS sensor_status = sensor_cmd_execute( subcommand_code );
+
+                    if (sensor_status == SENSOR_UNRECOGNIZED_OP) 
                     {
-                    break;
+                        warn_invalid_command();
+                    }
                     }
                 else
                     {
-                    error_fail_fast( ERROR_SENSOR_CMD_ERROR );
+                    warn_invalid_command();
                     }
                 break;
                 } /* SENSOR_OP */
@@ -155,7 +180,7 @@ if ( usb_detect() )
 
                 if ( usb_status != USB_OK )
                     {
-                    error_fail_fast( ERROR_SERVO_CMD_ERROR );
+                    warn_invalid_command();
                     }
 
                 if ( write_preset( flash_handle, flash_address) != FLASH_OK )
@@ -183,11 +208,16 @@ if ( usb_detect() )
                     /* Execute the subcommand */
                     *flash_status = flash_cmd_execute( subcommand_code,
                                                     flash_handle );
+
+                    if (*flash_status == FLASH_UNRECOGNIZED_OP) {
+                        warn_invalid_command();
                     }
+                }
                 else
                     {
                     /* Subcommand code not recieved */
-                    error_fail_fast( ERROR_FLASH_CMD_ERROR );
+                    warn_invalid_command();
+                    break;
                     }
 
                 /* Transmit status code to PC */
@@ -220,6 +250,11 @@ if ( usb_detect() )
                     IGN_STATUS ign_status;
                     /* Execute subcommand*/
                     ign_status = ign_cmd_execute( subcommand_code );
+
+                    /* Warn about invalid subcommand */
+                    if (ign_status == IGN_UNRECOGNIZED_CMD) {
+                        warn_invalid_command();
+                    }
 
                     /* Return response code to terminal */
                     usb_transmit( &ign_status, 
@@ -229,7 +264,7 @@ if ( usb_detect() )
                 else
                     {
                     /* Error: no subcommand recieved */
-                    error_fail_fast( ERROR_IGN_CMD_ERROR );
+                    warn_invalid_command();
                     }
 
                 break; 
@@ -251,11 +286,16 @@ if ( usb_detect() )
                     *flash_status = preset_cmd_execute( &subcommand_code,
                                                         flash_handle,
                                                         flash_address  );
+
+                    if (*flash_status == FLASH_UNRECOGNIZED_OP) {
+                        warn_invalid_command();
+                    }                           
+
                     }
                 else
                     {
                     /* Subcommand code not recieved */
-                    error_fail_fast( ERROR_FLASH_CMD_ERROR );
+                    warn_invalid_command();                    
                     }
 
                 /* Transmit status code to PC */
@@ -286,12 +326,18 @@ if ( usb_detect() )
                 if ( usb_status == USB_OK )
                     {
                     servo_status = servo_cmd_execute( subcommand_code );
+
+                    if ( servo_status != SERVO_OK )
+                        {
+                        led_set_color( LED_RED );
+                        HAL_Delay( 5000 );
+                        warn_invalid_command();
+                        }
                     }
-
-                if ( servo_status != SERVO_OK )
+                else 
                     {
-                    led_set_color( LED_RED );
-                    HAL_Delay( 5000 );
+                    /* No subcommand received */
+                    warn_invalid_command();
                     }
                 break;
                 }
@@ -339,7 +385,7 @@ if ( usb_detect() )
                 else if ( command_status != LORA_OK || subcommand_code != LORA_PRESET_DOWNLOAD )
                     {
                     /* unknown subcommand or usb fail */
-                    error_fail_fast( ERROR_LORA_CMD_ERROR );
+                    warn_invalid_command();
                     }
                 break;
                 }
@@ -349,7 +395,7 @@ if ( usb_detect() )
             default:
                 {
                 // TODO: Give warning ( via error_fail_safe() )
-                //error_fail_fast();
+                warn_invalid_command();
                 break;
                 }
 
@@ -497,8 +543,7 @@ switch (*subcommand_code)
     -------------------------------------------------------------*/
     default:
         {
-        error_fail_fast( ERROR_USB_UART_ERROR );
-        return FLASH_FAIL;
+        return FLASH_UNRECOGNIZED_OP;
         }
     }
 

test/app/rev2/prelaunch/mock/MOCK_flash.c

@@ -18,7 +18,17 @@ static inline uint32_t bytes_to_address(uint8_t address_bytes[3]);
 static FLASH_STATUS write_enable(void);
 static FLASH_STATUS write_disable(void);
 
-FLASH_STATUS flash_cmd_execute(uint8_t subcommand, HFLASH_BUFFER* pflash_handle) { return FLASH_OK; }
+extern int do_fail;
+
+FLASH_STATUS flash_cmd_execute(uint8_t subcommand, HFLASH_BUFFER* pflash_handle) 
+{ 
+    if ( do_fail == 1 ) {
+        return FLASH_UNRECOGNIZED_OP;
+    }
+    else {
+        return FLASH_OK;
+    }; 
+}
 FLASH_STATUS flash_init(HFLASH_BUFFER* pflash_handle);
 FLASH_STATUS flash_get_status(HFLASH_BUFFER* pflash_handle);
 FLASH_STATUS flash_set_status(uint8_t flash_status);
@@ -31,7 +41,6 @@ FLASH_STATUS flash_read(HFLASH_BUFFER* pflash_handle, uint32_t num_bytes);
 FLASH_STATUS flash_erase(HFLASH_BUFFER* pflash_handle);
 FLASH_STATUS flash_block_erase(FLASH_BLOCK flash_block_num, FLASH_BLOCK_SIZE size);
 
-extern int do_fail;
 
 /* From flash_appa */
 FLASH_STATUS write_preset

test/app/rev2/prelaunch/mock/buzzer.c

@@ -13,6 +13,7 @@
 #include "buzzer.h"
 
 extern int skip_loop;
+extern bool warn_invalid_command_buzzer_called;
 
 BUZZ_STATUS buzzer_beep
 (
@@ -31,6 +32,9 @@ BUZZ_STATUS buzzer_multi_beeps
 {
     if (skip_loop == 1) {;
     }
+	if (beep_duration == 300 && time_between_beeps == 100 && num_beeps == 2) {
+		warn_invalid_command_buzzer_called = true;
+	}
 }
 
 BUZZ_STATUS buzzer_num_beeps

test/app/rev2/prelaunch/mock/ignition.c

@@ -5,6 +5,7 @@
 extern int do_switch;
 extern int do_drogue;
 extern int do_main;
+extern int do_fail;
 
 bool ign_switch_armed(void) { 
     if (do_switch == 1) {
@@ -33,5 +34,12 @@ bool ign_main_cont(void) {
     IGN_SUBCOMMAND ign_subcommand
     )
 {
-return IGN_OK;
+    if (do_fail == 1)
+    {
+        return IGN_UNRECOGNIZED_CMD;
+    }
+    else
+    {
+        return IGN_OK;
+    }
 }

test/app/rev2/prelaunch/mock/led.c

@@ -8,6 +8,8 @@ extern jmp_buf env_buffer;
 
 int set_red_count = 0;
 extern bool ping_reached;
+extern bool warn_invalid_command_led_called;
+extern bool warn_invalid_command_buzzer_called;
 
 void led_set_color(LED_COLOR_CODES color) {
     // add a counter for times set red to count while loop in check_config_validity
@@ -25,14 +27,20 @@ void led_set_color(LED_COLOR_CODES color) {
             longjmp(env_buffer, jmp_val);
         }
     }
+
+    if (color == LED_RED) {
+        warn_invalid_command_led_called = true;
+    }
 }
 
 void buzzer_beep(int var0) {
 
 }
 
 void buzzer_multi_beeps(int var0, int var2, int var3) {
-
+    if (var0 == 300 && var2 == 100 && var3 == 2) {
+		warn_invalid_command_buzzer_called = true;
+	}
 }
 
 void ping () {

test/app/rev2/prelaunch/mock/sensor.c

@@ -1,3 +1,3 @@
 #include "sensor.h"
-
-SENSOR_STATUS sensor_cmd_execute(uint8_t subcommand) { return SENSOR_OK; }
+extern int do_fail;
+SENSOR_STATUS sensor_cmd_execute(uint8_t subcommand) { return do_fail == 1 ? SENSOR_UNRECOGNIZED_OP : SENSOR_OK; }

test/app/rev2/prelaunch/mock/servo.c

@@ -5,7 +5,7 @@
 #include "usb.h"
 
 SERVO_STATUS servo_cmd_execute(uint8_t subcommand) { 
-    if (subcommand == 0x00) {
+    if (subcommand == 0x00 || subcommand == 0x01) {
         return SERVO_OK;
     } else {
         return SERVO_FAIL;

test/app/rev2/prelaunch/mock/usb.c

@@ -53,6 +53,7 @@ extern int usb_receive_steps_count;
 extern USB_RECEIVE_STEP usb_receive_steps[10];
 extern int call_count;
 extern int do_fake_checksum;
+extern bool warn_invalid_command_usb_flush_called;
 
 /*------------------------------------------------------------------------------
  Procedures 
@@ -142,6 +143,14 @@ bool usb_detect
 	}
 }
 
+void usb_flush
+	(
+	void
+	) 
+{
+	warn_invalid_command_usb_flush_called = true;
+}
+
 
 /*******************************************************************************
 * END OF FILE                                                                  *