implemented nvs in challenge_app to make robotMode non-volantile

ADIS_ESP32_Eclipse/main/challenge_app.c

@@ -16,14 +16,22 @@
 #include "wifi.h"
 #include "McuUtility.h"
 #include "challenge_com.h"
+#include "esp_system.h"
+#include "nvs_flash.h"
+#include "nvs.h"
 
 /* LOCAL Var */
 /* identifies robot Mode for the challenge: true = robot is stationary, false = robot is moveable */
-static bool robotModeStationary = false;
+static bool _robotModeStationary = false;
 
 static TaskHandle_t appTaskHandle;   /* task handle */
 #define TAG "CHALLENGE_APP" /* tag for logging with ESP_LOG */
 
+/* function declarations */
+static void Challenge_App_SetRobotMode(bool modeStationary, bool storeToNvs);
+static bool Challenge_App_StoreRobotModeToNVS(bool modeStationary);
+static bool Challenge_App_GetRobotModeFromNVS(void);
+
 
 static void appTask(void *pv){
 	/*Task argument not used
@@ -69,6 +77,18 @@ static void appTask(void *pv){
 
 BaseType_t res;
 void Challenge_App_Init(void){
+	// Initialize NVS
+	esp_err_t err = nvs_flash_init();
+	if (err == ESP_ERR_NVS_NO_FREE_PAGES || err == ESP_ERR_NVS_NEW_VERSION_FOUND) {
+		// NVS partition was truncated and needs to be erased
+		// Retry nvs_flash_init
+		ESP_ERROR_CHECK(nvs_flash_erase());
+		err = nvs_flash_init();
+	}
+	ESP_ERROR_CHECK( err );
+
+	Challenge_App_GetRobotModeFromNVS();
+
 	res = xTaskCreate(appTask, "ChallengeAppTask", 4096/sizeof(StackType_t), (void*)"ARG_0", tskIDLE_PRIORITY, &appTaskHandle);
 	if(res != pdPASS){
 		ESP_LOGE(TAG, "Creating appTask failed");
@@ -76,13 +96,102 @@ void Challenge_App_Init(void){
 }
 
 bool Challenge_App_GetRobotMode(void){
-	return robotModeStationary;
+	return _robotModeStationary;
+}
+
+static void Challenge_App_SetRobotMode(bool modeStationary, bool storeToNvs){
+	_robotModeStationary = modeStationary;
+	ESP_LOGI(TAG, "Changed robot mode to %s", _robotModeStationary?(unsigned char*)"stationary":(unsigned char*)"mobile");
+	if(storeToNvs){
+		Challenge_App_StoreRobotModeToNVS(modeStationary);
+	}
+}
+
+// returns true on success
+static bool Challenge_App_StoreRobotModeToNVS(bool modeStationary){
+	bool success = false;
+
+	// Open
+	ESP_LOGI(TAG, "Opening Non-Volatile Storage (NVS) handle... ");
+	nvs_handle_t my_handle;
+	esp_err_t err = nvs_open("storage", NVS_READWRITE, &my_handle);
+	if (err != ESP_OK) {
+		ESP_LOGE(TAG, "Error (%s) opening NVS handle!\n", esp_err_to_name(err));
+		return false;
 	}
 
-static void Challenge_App_SetRobotMode(bool modeStationary){
-	robotModeStationary = modeStationary;
-	ESP_LOGI(TAG, "Changed robot mode to %s", robotModeStationary?(unsigned char*)"stationary":(unsigned char*)"mobile");
+	// Write
+	uint8_t value = modeStationary==true?1:0;
+	ESP_LOGI(TAG, "Updating robotMode in NVS ... ");
+	err = nvs_set_u8(my_handle, "robotMode", value);
+	if(err != ESP_OK){
+		ESP_LOGE(TAG, "Failed to update robotMode in NVS. Error=(%s)", esp_err_to_name(err));
+		success = false;
+	}else{ // successfully set
+		// Commit written value.
+		// After setting any values, nvs_commit() must be called to ensure changes are written
+		// to flash storage. Implementations may write to storage at other times,
+		// but this is not guaranteed.
+		ESP_LOGI(TAG, "Committing updates in NVS ... ");
+		err = nvs_commit(my_handle);
+		if(err != ESP_OK){
+			ESP_LOGE(TAG, "Failed to commit to NVS. Error=(%s)", esp_err_to_name(err));
+			success = false;
+		}else{
+			ESP_LOGI(TAG, "Successfully committed changes to NVS.");
+			success = true;
 		}
+	}
+
+	// Close
+	nvs_close(my_handle);
+
+	return success;
+}
+
+// returns true on success
+static bool Challenge_App_GetRobotModeFromNVS(void){
+	bool success = false;
+	bool notInitialized = false;
+
+    // Open
+    ESP_LOGI(TAG, "Opening Non-Volatile Storage (NVS) handle... ");
+    nvs_handle_t my_handle;
+    esp_err_t err = nvs_open("storage", NVS_READWRITE, &my_handle);
+    if (err != ESP_OK) {
+    	ESP_LOGE(TAG, "Error (%s) opening NVS handle!\n", esp_err_to_name(err));
+    	return false;
+    }
+
+    // Read
+    ESP_LOGI(TAG, "Reading robot mode from NVS ... ");
+    uint8_t value = 0;
+	err = nvs_get_u8(my_handle, "robotMode", &value);
+	switch (err) {
+		case ESP_OK:
+			ESP_LOGI(TAG, "Reading robot mode done. Read robotMode = %i", value);
+			Challenge_App_SetRobotMode(value==1?true:false, false); // do not store to nvs (reading rn)
+			success = true;
+			break;
+		case ESP_ERR_NVS_NOT_FOUND:
+			ESP_LOGI(TAG, "Reading robot mode done. The value is not initialized yet!");
+			notInitialized = true;
+			break;
+		default :
+			ESP_LOGI(TAG, "Error reading robot mode. Error=(%s)", esp_err_to_name(err));
+			success = false;
+	}
+
+	// Close
+	nvs_close(my_handle);
+
+	if(notInitialized){
+		success = Challenge_App_StoreRobotModeToNVS(false);  // initialize to false
+	}
+
+	return success;
+}
+
 
 /*********/
 /* SHELL */
@@ -90,7 +199,7 @@ static void Challenge_App_SetRobotMode(bool modeStationary){
 #if PL_CONFIG_USE_SHELL
 static uint8_t PrintStatus(const McuShell_StdIOType *io) {
   McuShell_SendStatusStr((unsigned char*)"challenge", (unsigned char*)"ESP32 Challenge status\r\n", io->stdOut);
-  McuShell_SendStatusStr((unsigned char*)"  robotMode", robotModeStationary?(unsigned char*)"stationary\r\n":(unsigned char*)"mobile\r\n", io->stdOut);
+  McuShell_SendStatusStr((unsigned char*)"  robotMode", Challenge_App_GetRobotMode()?(unsigned char*)"stationary\r\n":(unsigned char*)"mobile\r\n", io->stdOut);
   return ERR_OK;
 }
 
@@ -110,11 +219,11 @@ uint8_t Challenge_ParseShellCommand(const unsigned char* cmd, bool *handled, con
   } else if (McuUtility_strcmp((char*)cmd, (char*)"challenge setMode stationary")==0 ||
 		     McuUtility_strcmp((char*)cmd, (char*)"challenge setMode s")==0) {
     *handled = TRUE;
-    Challenge_App_SetRobotMode(true); // set stationary
+    Challenge_App_SetRobotMode(true, true); // set stationary
   } else if (McuUtility_strcmp((char*)cmd, (char*)"challenge setMode mobile")==0  ||
 		     McuUtility_strcmp((char*)cmd, (char*)"challenge setMode m")==0 ) {
     *handled = TRUE;
-    Challenge_App_SetRobotMode(false);  // set mobile
+    Challenge_App_SetRobotMode(false, true);  // set mobile
   }
   return ERR_OK;
 }