Camera flip with pair button (#51)

* CamFlip, settings

* Altitude sensor reading fix

* CamFlip timeout, added voice line

main/main.cpp

@@ -32,6 +32,7 @@
 #include "JigHWTest/JigHWTest.h"
 #include "Services/InactivityService.h"
 #include "Util/HWVersion.h"
+#include "Settings.h"
 
 [[noreturn]] void shutdown(){
 	ESP_ERROR_CHECK(esp_sleep_pd_config(ESP_PD_DOMAIN_RTC_PERIPH, ESP_PD_OPTION_AUTO));
@@ -57,6 +58,10 @@ void init(){
 		}
 	}
 
+
+	auto settings = new Settings();
+	Services.set(Service::Settings, settings);
+
 	auto adc1 = new ADC(ADC_UNIT_1);
 
 	auto i2c = new I2C(I2C_NUM_0, (gpio_num_t) I2C_SDA, (gpio_num_t) I2C_SCL);

main/src/Devices/Camera.cpp

@@ -15,7 +15,7 @@ Camera::~Camera(){
 	deinit();
 }
 
-esp_err_t Camera::init(){
+esp_err_t Camera::init(bool horizontalFlip){
 	if(resWait == res && formatWait == format && inited) return ESP_OK;
 
 	if(inited){
@@ -77,8 +77,8 @@ esp_err_t Camera::init(){
 		return ESP_ERR_CAMERA_NOT_DETECTED;
 	}
 
-	sensor->set_hmirror(sensor, 0);
-	sensor->set_vflip(sensor, 0);
+	sensor->set_hmirror(sensor, horizontalFlip);
+	sensor->set_vflip(sensor, horizontalFlip);
 
 	// sensor->set_saturation(sensor, 2);
 	// sensor->set_awb_gain(sensor, 1);

main/src/Devices/Camera.h

@@ -18,7 +18,7 @@ class Camera {
 	pixformat_t getFormat() const;
 	void setFormat(pixformat_t format);
 
-	esp_err_t init();
+	esp_err_t init(bool horizontalFlip = false);
 	void deinit();
 	bool isInited();
 

main/src/Modules/AltPressModule.cpp

@@ -18,25 +18,32 @@ AltPressModule::~AltPressModule(){
 	stop();
 }
 
+void AltPressModule::sendReadSignal() const{
+	i2c.write(Addr, 0b01010100);
+	delayMillis(100);
+}
+
 int AltPressModule::readSensor(AltPressModule::Sensor sensor){
-	uint8_t read[3];
+	uint8_t read[3] = {0};
 
 	i2c.readReg(Addr, sensor, read, 3);
 
-	const int data = (read[0] << 16) | (read[1] << 8) | read[2];
+	int data = ((read[0]) << 16) | (read[1] << 8) | read[2];
+
+	if(sensor == ALTITUDE && (data & 0x80000)){
+		data |= 0xFFF00000;
+	}
 
 	return data / 100;
 }
 
 void AltPressModule::sleepyLoop(){
-	i2c.write(Addr, 0x40);
-	delayMillis(100);
-
-	const auto alt = (int16_t) readSensor(ALTITUDE);
-	const uint16_t press = readSensor(PRESSURE);
+	sendReadSignal();
 
 	const ModuleData data = {
-			ModuleType::AltPress, bus, { .altPress = { alt, press } }
+			ModuleType::AltPress, bus, { .altPress = {
+					(int16_t) readSensor(AltPressModule::ALTITUDE),
+					(uint16_t) readSensor(AltPressModule::PRESSURE) } }
 	};
 
 	comm.sendModuleData(data);

main/src/Modules/AltPressModule.h

@@ -21,6 +21,8 @@ class AltPressModule : private SleepyThreaded {
 	};
 	int readSensor(Sensor sensor);
 
+	void sendReadSignal() const;
+
 	static constexpr uint8_t Addr = 0x76;
 };
 

main/src/Services/Feed.cpp

@@ -5,6 +5,7 @@
 #include "Services/LEDService.h"
 #include "Util/Services.h"
 #include "Audio.h"
+#include "Settings.h"
 
 const char* tag = "Feed";
 
@@ -52,6 +53,13 @@ void Feed::disableScanning(){
 	communicationQueue.post(data, portMAX_DELAY);
 }
 
+void Feed::flipCam(bool flip){
+	EventData data;
+	data.type = EventData::CamFlip;
+	data.flip = flip;
+	communicationQueue.post(data, portMAX_DELAY);
+}
+
 void Feed::sleepyLoop(){
 	::Event event{};
 	if(queue.get(event, portMAX_DELAY)){
@@ -112,6 +120,9 @@ void IRAM_ATTR Feed::sendFrame(){
 					led->blink(LED::Camera, 0);
 				}
 			}
+		}else if(data.type == EventData::CamFlip){
+			camera->deinit();
+			camera->init(data.flip);
 		}
 	}
 
@@ -151,7 +162,13 @@ void IRAM_ATTR Feed::sendFrame(){
 
 		const bool wasCamOff = !camera->isInited();
 
-		const esp_err_t err = camera->init();
+		bool flip = false;
+		if(Settings* settings = (Settings*) Services.get(Service::Settings)){
+			flip = settings->get().cameraHorizontalFlip;
+		}
+
+
+		const esp_err_t err = camera->init(flip);
 		if(err != ESP_OK){
 			if(Comm* comm = (Comm*) Services.get(Service::Comm)){
 				comm->sendNoFeed(true);

main/src/Services/Feed.h

@@ -29,6 +29,8 @@ class Feed : private SleepyThreaded {
 
 	void disableScanning();
 
+	void flipCam(bool flip);
+
 protected:
 	virtual void sleepyLoop() override;
 
@@ -47,13 +49,15 @@ class Feed : private SleepyThreaded {
 		enum Type {
 			None,
 			ScanningEnableChange,
-			FeedQualityChange
+			FeedQualityChange,
+			CamFlip
 		};
 
 		Type type = None;
 		union {
 			bool isScanningEnabled = false;
 			uint8_t feedQuality;
+			bool flip;
 		};
 	};
 	Queue<EventData> communicationQueue;

main/src/Settings.cpp

@@ -0,0 +1,48 @@
+#include "Settings.h"
+#include <esp_log.h>
+#include <nvs_flash.h>
+
+static const char* TAG = "Settings";
+
+Settings::Settings(){
+	nvs_flash_init();
+
+	auto err = nvs_open(NVSNamespace, NVS_READWRITE, &handle);
+	ESP_ERROR_CHECK(err);
+	load();
+}
+
+SettingsStruct Settings::get(){
+	return settingsStruct;
+}
+
+void Settings::set(SettingsStruct& settings){
+	settingsStruct = settings;
+}
+
+void Settings::store(){
+	esp_err_t err = nvs_set_blob(handle, BlobName, &settingsStruct, sizeof(SettingsStruct));
+
+	if(err != ESP_OK){
+		ESP_LOGW(TAG, "NVS settings store error: %d", err);
+		return;
+	}
+
+	err = nvs_commit(handle);
+	if(err != ESP_OK){
+		ESP_LOGW(TAG, "NVS settings commit error: %d", err);
+	}
+}
+
+void Settings::load(){
+	size_t out_size = sizeof(SettingsStruct);
+	auto err = nvs_get_blob(handle, BlobName, &settingsStruct, &out_size);
+	if(err != ESP_OK){
+		ESP_LOGI(TAG, "Settings not found, writing defaults");
+		store();
+		err = nvs_get_blob(handle, BlobName, &settingsStruct, &out_size);
+		if(err != ESP_OK){
+			ESP_LOGE(TAG, "Couldn't access NVS settings: %d", err);
+		}
+	}
+}

main/src/Settings.h

@@ -0,0 +1,30 @@
+#ifndef PERSE_ROVER_SETTINGS_H
+#define PERSE_ROVER_SETTINGS_H
+
+#include <nvs.h>
+
+struct SettingsStruct {
+	bool cameraHorizontalFlip = false;
+};
+
+class Settings {
+public:
+	Settings();
+
+	SettingsStruct get();
+	void set(SettingsStruct& settings);
+	void store();
+
+private:
+	nvs_handle_t handle{};
+	SettingsStruct settingsStruct;
+
+	static constexpr const char* NVSNamespace = "Rover";
+	static constexpr const char* BlobName = "Settings";
+
+	void load();
+};
+
+
+
+#endif //PERSE_ROVER_SETTINGS_H

main/src/States/DriveState/DriveState.cpp

@@ -22,6 +22,9 @@
 #include "Services/Audio.h"
 #include "States/PairState.h"
 #include "Devices/Battery.h"
+#include "Devices/Input.h"
+#include "Util/stdafx.h"
+#include "Settings.h"
 
 const std::map<MarkerAction, std::function<std::unique_ptr<Action>(void)>> DriveState::actionMappings = {
 		{ MarkerAction::None,                 []() -> std::unique_ptr<Action>{ return std::make_unique<Action>(); }},
@@ -64,11 +67,17 @@ DriveState::DriveState() : State(), queue(10), activeAction(nullptr), audio(*(Au
 	Events::listen(Facility::TCP, &queue);
 	Events::listen(Facility::Feed, &queue);
 	Events::listen(Facility::Comm, &queue);
+	Events::listen(Facility::Input, &queue);
 
 	if (LEDService* led = (LEDService*)Services.get(Service::LED)) {
 		led->breathe(LED::StatusGreen);
 		led->breathe(LED::Rear);
 	}
+
+	lastSetMillis = millis();
+	if(Settings* settings = (Settings*)Services.get(Service::Settings)){
+		camFlip = settings->get().cameraHorizontalFlip;
+	}
 }
 
 DriveState::~DriveState() {
@@ -134,6 +143,27 @@ void DriveState::loop(){
 					}
 				}
 			}
+		}else if (event.facility == Facility::Input) {
+			const Input::Data* data = (Input::Data*)event.data;
+			if(data != nullptr && data->action == Input::Data::Press && millis() - lastSetMillis >= CamFlipPause){
+				camFlip = !camFlip;
+				lastSetMillis = millis();
+
+				if(audio.getCurrentPlayingFile() != "/spiffs/General/CamFlip.aac"){
+					audio.play("/spiffs/General/CamFlip.aac");
+				}
+
+				if(auto feed = (Feed*)Services.get(Service::Feed)){
+					feed->flipCam(camFlip);
+				}
+
+				if(Settings* settings = (Settings*)Services.get(Service::Settings)){
+					auto setts = settings->get();
+					setts.cameraHorizontalFlip = camFlip;
+					settings->set(setts);
+					settings->store();
+				}
+			}
 		}
 
 		free(event.data);

main/src/States/DriveState/DriveState.h

@@ -29,6 +29,9 @@ class DriveState : public State {
 	static const std::unordered_set<CommType> IdleResetComms;
 
 	Audio& audio;
+	static constexpr uint32_t CamFlipPause = 1000; //[ms] - pause from start of DriveState, to prevent camera flip from accidental button presses
+	uint32_t lastSetMillis = 0;
+	bool camFlip = false;
 };
 
 #endif //PERSE_ROVER_DRIVESTATE_H

main/src/Util/Services.h

@@ -18,7 +18,8 @@ enum class Service {
 	ArmController,
 	CameraController,
 	MotorDriveController,
-	LowBattery
+	LowBattery,
+	Settings
 };
 
 class ServiceLocator {