Merge pull request #90 from ISSUIUC/camera-b2b-interface-i2c

Add B2B communication interface for camera board

MIDAS/src/b2b_interface.cpp

@@ -0,0 +1,100 @@
+#include "b2b_interface.h"
+
+
+ErrorCode B2BInterface::init() {
+    // No special init
+    return ErrorCode::NoError;
+}
+
+
+
+void CameraB2B::transmit_command(CameraCommand command) {
+    #ifdef B2B_I2C
+
+    Wire.beginTransmission(0x69); // 0x69 --> Camera board i2c address
+    Wire.write((uint8_t) command);
+    if (Wire.endTransmission()) {
+        Serial.println("Camera B2B i2c write error");
+    }
+
+    #endif
+
+    #ifdef B2B_CAN
+        // todo :D
+    #endif
+}
+
+void CameraB2B::vtx_on() {
+    transmit_command(CameraCommand::VTX_ON);
+    vtx_state_ = true;
+}
+
+void CameraB2B::vtx_off() {
+    transmit_command(CameraCommand::VTX_OFF);
+    vtx_state_ = false;
+}
+
+void CameraB2B::vtx_toggle() {
+    if(vtx_state_) {
+        vtx_off();
+    } else {
+        vtx_on();
+    }
+}
+
+void CameraB2B::camera_on(int cam_index) {
+    switch (cam_index) {
+        case 0:
+            transmit_command(CameraCommand::CAMERA0_ON);
+            cam_state_[0] = true;
+            break;
+        case 1:
+            transmit_command(CameraCommand::CAMERA1_ON);
+            cam_state_[1] = true;
+            break;
+        default:
+            Serial.print("B2B camera on -- invalid index ");
+            Serial.println(cam_index);  
+            break;
+    }
+}
+
+void CameraB2B::camera_off(int cam_index) {
+    switch (cam_index) {
+        case 0:
+            transmit_command(CameraCommand::CAMERA0_OFF);
+            cam_state_[0] = false;
+            break;
+        case 1:
+            transmit_command(CameraCommand::CAMERA1_OFF);
+            cam_state_[1] = false;
+            break;
+        default:
+            Serial.print("B2B camera on -- invalid index ");
+            Serial.println(cam_index);  
+            break;
+    }
+}
+
+void CameraB2B::camera_toggle(int cam_index) {
+    switch (cam_index) {
+        case 0:
+            if (cam_state_[0]) {
+                camera_off(0);
+            } else {
+                camera_on(0);
+            }
+            break;
+        case 1:
+            if (cam_state_[1]) {
+                camera_off(1);
+            } else {
+                camera_on(1);
+            }
+            break;
+        default:
+            Serial.print("B2B camera on -- invalid index ");
+            Serial.println(cam_index);  
+            break;
+    }
+}

MIDAS/src/b2b_interface.h

@@ -0,0 +1,52 @@
+#include <stdint.h>
+#include <stddef.h>
+#include <Wire.h>
+#include "errors.h"
+#include "hal.h"
+
+// Which b2b communication we should use
+#define B2B_I2C
+// #define B2B_CAN
+
+#if defined(B2B_I2C) && defined(B2B_CAN)
+#error "B2B can only use one option of B2B_I2C, or B2B_CAN"
+#elif !defined(B2B_I2C) && !defined(B2B_CAN)
+#error "At least one B2B_I2C or B2B_CAN must be defined"
+#endif
+
+
+enum class CameraCommand {
+    CAMERA0_OFF = 0,
+    CAMERA0_ON = 1,
+    CAMERA1_OFF = 2,
+    CAMERA1_ON = 3,
+    VTX_OFF = 4,
+    VTX_ON = 5,
+    MUX_0 = 6,
+    MUX_1 = 7
+};
+
+struct CameraB2B {
+
+    void camera_on(int cam_index);
+    void camera_off(int cam_index);
+    void camera_toggle(int cam_index);
+
+    void vtx_on();
+    void vtx_off();
+    void vtx_toggle();
+
+    private:
+    void transmit_command(CameraCommand command);
+    bool cam_state_[2] = { false, false }; // false: off, true: on
+    bool vtx_state_ = false;
+};
+
+/**
+ * @struct Interface for B2B communication protocol
+ */
+struct B2BInterface {
+    ErrorCode init();
+
+    CameraB2B camera;
+};

MIDAS/src/systems.cpp

@@ -253,6 +253,10 @@ void handle_tlm_command(TelemetryCommand& command, RocketSystems* arg, FSMState
                 arg->rocket_data.command_flags.should_fire_pyro_d = true;
             }
             break;
+        case CommandType::TOGGLE_CAM:
+            arg->b2b.camera.camera_toggle(0); // For stargazer, we will just toggle cam here.
+            arg->b2b.camera.vtx_toggle();
+            break;
         default:
             break; // how
     }
@@ -305,6 +309,7 @@ ErrorCode init_systems(RocketSystems& systems) {
     INIT_SYSTEM(systems.sensors.pyro);
     INIT_SYSTEM(systems.led);
     INIT_SYSTEM(systems.buzzer);
+    INIT_SYSTEM(systems.b2b);
     #ifdef ENABLE_TELEM
         INIT_SYSTEM(systems.tlm);
     #endif

MIDAS/src/systems.h

@@ -10,6 +10,7 @@
 #include "led.h"
 #include "telemetry.h"
 #include "finite-state-machines/fsm.h"
+#include "b2b_interface.h"
 
 #if defined(SILSIM)
 #include "silsim/emulated_sensors.h"
@@ -50,6 +51,7 @@ struct RocketSystems {
     BuzzerController buzzer;
     LEDController led;
     Telemetry tlm;
+    B2BInterface b2b;
 };
 
 [[noreturn]] void begin_systems(RocketSystems* config);

MIDAS/src/telemetry_packet.h

@@ -29,7 +29,7 @@ struct TelemetryPacket {
 
 
 // Commands transmitted from ground station to rocket
-enum class CommandType: uint8_t { RESET_KF, SWITCH_TO_SAFE, SWITCH_TO_PYRO_TEST, SWITCH_TO_IDLE, FIRE_PYRO_A, FIRE_PYRO_B, FIRE_PYRO_C, FIRE_PYRO_D };
+enum class CommandType: uint8_t { RESET_KF, SWITCH_TO_SAFE, SWITCH_TO_PYRO_TEST, SWITCH_TO_IDLE, FIRE_PYRO_A, FIRE_PYRO_B, FIRE_PYRO_C, FIRE_PYRO_D, TOGGLE_CAM };
 
 /**
  * @struct TelemetryCommand

ground/src/feather/main.cpp

@@ -140,7 +140,7 @@ struct FullTelemetryData {
 
 
 
-enum class CommandType: uint8_t { RESET_KF, SWITCH_TO_SAFE, SWITCH_TO_PYRO_TEST, SWITCH_TO_IDLE, FIRE_PYRO_A, FIRE_PYRO_B, FIRE_PYRO_C, FIRE_PYRO_D, EMPTY };
+enum class CommandType: uint8_t { RESET_KF, SWITCH_TO_SAFE, SWITCH_TO_PYRO_TEST, SWITCH_TO_IDLE, FIRE_PYRO_A, FIRE_PYRO_B, FIRE_PYRO_C, FIRE_PYRO_D, CAM_TOGGLE, EMPTY };
 // Commands transmitted from ground station to rocket
 struct TelemetryCommand {
     CommandType command;
@@ -324,43 +324,8 @@ void set_freq_local_bug_fix(float freq) {
     Serial.println("}");
 }
 
-void SerialInput(const char* key, const char* value) {
-    if (!cmd_queue.empty()) {
-        Serial.println(json_buffer_full_error);
-        return;
-    }
-
-    TelemetryCommand command{};
-
-    if (strcmp(key, "RESET_KF") == 0) {
-        command.command = CommandType::RESET_KF;
-    } else if (strcmp(key, "SAFE") == 0) {
-        command.command = CommandType::SWITCH_TO_SAFE;
-    } else if (strcmp(key, "IDLE") == 0) {
-        command.command = CommandType::SWITCH_TO_IDLE;
-    } else if (strcmp(key, "PT") == 0) {
-        command.command = CommandType::SWITCH_TO_PYRO_TEST;
-    } else if (strcmp(key, "PA") == 0) {
-        command.command = CommandType::FIRE_PYRO_A;
-    } else if (strcmp(key, "PB") == 0) {
-        command.command = CommandType::FIRE_PYRO_B;
-    } else if (strcmp(key, "PC") == 0) {
-        command.command = CommandType::FIRE_PYRO_C;
-    } else if (strcmp(key, "PD") == 0) {
-        command.command = CommandType::FIRE_PYRO_D;
-    } else {
-        Serial.println(json_command_bad);
-        return;
-    }
-
-
-    Serial.println(json_command_success);
-    // Send the command until acknowledge or 5 attempts
-    cmd_queue.push({command, 5});
-}
-
 
-void Stest(const String key) {
+void HandleSerial(const String key) {
     if (!cmd_queue.empty()) {
         Serial.println(json_buffer_full_error);
         return;
@@ -384,6 +349,8 @@ void Stest(const String key) {
         command.command = CommandType::FIRE_PYRO_C;
     } else if (key == "PD") {
         command.command = CommandType::FIRE_PYRO_D;
+    } else if (key == "CAMT") {
+        command.command = CommandType::CAM_TOGGLE;
     } else {
         Serial.println(json_command_bad);
         return;
@@ -406,7 +373,6 @@ void process_command_queue() {
     rf95.waitPacketSent();
 }
 
-SerialParser serial_parser(SerialInput, SerialError);
 
 void setup() {
     while (!Serial)
@@ -539,7 +505,7 @@ void loop() {
             cur_input += input;
             cur_input.replace("\r", ""); // Remove carriage returns
 
-            Stest(input.substring(0, input.length() - 2));
+            HandleSerial(input.substring(0, input.length() - 2));
 
             cur_input = "";
         } else {