Feat/rowzaw/ptz cam

common_libraries/comms/comms.c

@@ -92,6 +92,8 @@ static size_t expected_payload_len(uint16_t type)
         case MSG_TYPE__SET_LED_PWM:                         return sizeof(uint8_t) + sizeof(float); // 5
         case MSG_TYPE__HARDWARE_ERROR:                      return 2u;
         case MSG_TYPE__LED_PANEL_FEEDBACK:                  return sizeof(uint8_t) + sizeof(float);
+        case MSG_TYPE__PTZ_VEL_CMD:                         return 8u;
+        case MSG_TYPE__PTZ_FEEDBACK:                        return 8u;
         default:                                            return SIZE_MAX; 
     }
 }
@@ -136,6 +138,14 @@ static size_t pack_payload(const msg_t* msg, uint8_t* out, size_t cap)
         PACK(out, pos, msg->payload.hardware_error.error_type);
         PACK(out, pos, msg->payload.hardware_error.info);
         break;
+    case MSG_TYPE__PTZ_VEL_CMD:
+        PACK(out, pos, msg->payload.ptz_vel_cmd.vel_tilt);
+        PACK(out, pos, msg->payload.ptz_vel_cmd.vel_pan);
+        break;
+    case MSG_TYPE__PTZ_FEEDBACK:
+        PACK(out, pos, msg->payload.ptz_feedback.curr_vel_tilt);
+        PACK(out, pos, msg->payload.ptz_feedback.curr_vel_pan);
+        break;
     default:
         return SIZE_MAX; // This is an error
     }
@@ -165,6 +175,14 @@ static bool unpack_payload(msg_t* msg, const uint8_t* in, size_t len)
         UNPACK(in, pos, msg->payload.hardware_error.error_type);
         UNPACK(in, pos, msg->payload.hardware_error.info);
         break;
+    case MSG_TYPE__PTZ_VEL_CMD:
+        UNPACK(in, pos, msg->payload.ptz_vel_cmd.vel_tilt);
+        UNPACK(in, pos, msg->payload.ptz_vel_cmd.vel_pan);
+        break;
+    case MSG_TYPE__PTZ_FEEDBACK:
+        UNPACK(in, pos, msg->payload.ptz_feedback.curr_vel_tilt);
+        UNPACK(in, pos, msg->payload.ptz_feedback.curr_vel_pan);
+        break;
     default:
         return false;
     }

common_libraries/comms/comms.h

@@ -72,13 +72,26 @@ typedef struct {
     uint8_t info;
 } HardwareError_t;
 
+typedef struct {
+    float vel_tilt;
+    float vel_pan;
+    // Thats all we got rn, open loop control
+} PTZVelCmd_t;
+
+typedef struct {
+    float curr_vel_tilt;
+    float curr_vel_pan;
+    // Thats all we got rn, open loop control
+} PTZFeedback_t;
 
 typedef union {
     Ping_t          ping;
     HeartBeat_t     heartbeat;
     LedPanelCMD_t   led_panel_cmd;
     LedPanelFeedback_t led_panel_feedback;
     HardwareError_t     hardware_error;
+    PTZVelCmd_t         ptz_vel_cmd;
+    PTZFeedback_t       ptz_feedback;
 } Payload_t;
 
 // C friendly comms helper functions
@@ -112,6 +125,8 @@ typedef enum {
     MSG_TYPE__SET_LED_PWM,
     MSG_TYPE__HARDWARE_ERROR, // Not really used rn
     MSG_TYPE__LED_PANEL_FEEDBACK,
+    MSG_TYPE__PTZ_VEL_CMD,
+    MSG_TYPE__PTZ_FEEDBACK,
     NUM_MSG_TYPES
 } msg_type_t;
 

ptz_stm32_firmware/CMakeLists.txt

@@ -0,0 +1,123 @@
+cmake_minimum_required(VERSION 3.22)
+project(ptz_stm32_firmware C CXX ASM)
+
+set(CMAKE_C_STANDARD 11)
+#use modern CPP std 20
+set(CMAKE_CXX_STANDARD 20)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)  # disables -std=gnu++20, forces -std=c++20
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+if(NOT CMAKE_BUILD_TYPE)
+    set(CMAKE_BUILD_TYPE Debug CACHE STRING "" FORCE)
+endif()
+
+set(COMMON_LIBS_PATH ../common_libraries)
+
+set(MCU_FLAGS "-mcpu=cortex-m4 -mthumb -mfpu=fpv4-sp-d16 -mfloat-abi=hard")
+set(LINKER_SCRIPT ${CMAKE_SOURCE_DIR}/STM32G474RETx_FLASH.ld)
+# set(LINKER_SCRIPT ${CMAKE_SOURCE_DIR}/STM32G431CBUx_FLASH.ld)
+
+set(CMAKE_C_FLAGS   "${MCU_FLAGS} -Wall -fdata-sections -ffunction-sections")
+set(CMAKE_CXX_FLAGS "${MCU_FLAGS} -Wall -fdata-sections -ffunction-sections -fno-exceptions -fno-rtti")
+set(CMAKE_ASM_FLAGS "${MCU_FLAGS} -Wall -fdata-sections -ffunction-sections")
+set(CMAKE_EXE_LINKER_FLAGS
+    "${MCU_FLAGS} -specs=nano.specs -T${LINKER_SCRIPT} -Wl,-Map=output.map,--cref -Wl,--gc-sections -lc -lm -lnosys"
+)
+
+#TODO glob sources / platform specific
+add_executable(${PROJECT_NAME}
+    Core/Src/main.cpp
+    Core/Src/app/app.cpp
+    Core/Inc/usr_leds/usr_led.cpp
+    Core/Inc/clock/stopwatch.cpp
+    Core/Inc/clock/timer.cpp
+
+    # Platform Specific
+    Core/Inc/clock/clock_stm32g4xx.cpp
+    Core/Inc/primitive_io/primitive_io_stm32g4xx.cpp
+    Core/Inc/servo/servo_stm32g4xx.cpp
+    Core/Inc/can_comms/can_stm32g4xx.cpp
+
+    # Common Lib
+    ${COMMON_LIBS_PATH}/comms/comms.c
+
+
+    # Core/Src/app_freertos.c
+    Core/Src/stm32g4xx_it.c
+    Core/Src/stm32g4xx_hal_msp.cpp
+    Core/Src/stm32g4xx_hal_timebase_tim.c
+    Core/Src/system_stm32g4xx.c
+
+    Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_fdcan.c
+    Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal.c
+    Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_rcc.c
+    Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_rcc_ex.c
+    Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_flash.c
+    Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_flash_ex.c
+    Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_flash_ramfunc.c
+    Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_gpio.c
+    Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_exti.c
+    Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_dma.c
+    Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_dma_ex.c
+    Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_pwr.c
+    Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_pwr_ex.c
+    Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_cortex.c
+    Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_tim.c
+    Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_tim_ex.c
+    Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_adc.c
+    Drivers/STM32G4xx_HAL_Driver/Src/stm32g4xx_hal_adc_ex.c
+
+    #! Not using freertos currently
+    # Middlewares/Third_Party/FreeRTOS/Source/croutine.c
+    # Middlewares/Third_Party/FreeRTOS/Source/event_groups.c
+    # Middlewares/Third_Party/FreeRTOS/Source/list.c
+    # Middlewares/Third_Party/FreeRTOS/Source/queue.c
+    # Middlewares/Third_Party/FreeRTOS/Source/stream_buffer.c
+    # Middlewares/Third_Party/FreeRTOS/Source/tasks.c
+    # Middlewares/Third_Party/FreeRTOS/Source/timers.c
+    # Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c
+    # Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang/heap_4.c
+    # Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/port.c
+
+    startup_stm32g474xx.s
+    # startup_stm32g431xx.s
+)
+
+target_compile_definitions(${PROJECT_NAME} PRIVATE USE_HAL_DRIVER STM32G474xx)
+# target_compile_definitions(${PROJECT_NAME} PRIVATE USE_HAL_DRIVER STM32G431xx)
+
+#TODO seperate platform includes
+target_include_directories(${PROJECT_NAME} PRIVATE
+    Core/Inc
+    Core/Inc/app
+    Core/Inc/pinout_defs
+    Core/Inc/primitive_io 
+    Core/Inc/usr_leds
+    Core/Inc/clock
+    Core/Inc/servo
+    Core/Inc/can_comms
+
+    #common
+    ${COMMON_LIBS_PATH}/comms
+
+    Drivers/STM32G4xx_HAL_Driver/Inc
+    Drivers/STM32G4xx_HAL_Driver/Inc/Legacy
+    # No RTOS yet
+    # Middlewares/Third_Party/FreeRTOS/Source/include
+    # Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2
+    # Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F
+    Drivers/CMSIS/Device/ST/STM32G4xx/Include
+    Drivers/CMSIS/Include
+)
+#-gdwarf-2
+target_compile_options(${PROJECT_NAME} PRIVATE
+    $<$<CONFIG:Debug>:-g  -ggdb3 -Og>
+    $<$<CONFIG:Release>:-g -ggdb3 -Og>
+)
+
+add_custom_command(TARGET ${PROJECT_NAME} POST_BUILD
+    COMMAND ${CMAKE_SIZE} $<TARGET_FILE:${PROJECT_NAME}>
+    COMMAND ${CMAKE_OBJCOPY} -O ihex   $<TARGET_FILE:${PROJECT_NAME}> ${PROJECT_NAME}.hex
+    COMMAND ${CMAKE_OBJCOPY} -O binary $<TARGET_FILE:${PROJECT_NAME}> ${PROJECT_NAME}.bin
+)

ptz_stm32_firmware/Core/Inc/app/app.hpp

@@ -0,0 +1,90 @@
+// Application
+#pragma once
+#include <usr_led.hpp>
+#include <servo_interface.hpp>
+#include <app_substates.hpp>
+#include <can_interface.h>
+#include <comms.h>
+
+//TODO organize
+#define this_device_id 0x00AC //Hex 0x1FFF is the max (13 bit)
+static_assert((this_device_id) <= 0x1FFFu, "this_device_id exceeds 13-bit range (max 0x1FFF, min 0x10)");
+static_assert((this_device_id) > 0x000F, "this_device_id is invalid. 0x0000 to 0x000F are reserved");
+
+
+#define HEARTBEAT_PERIOD_MS 1000u
+#define SENSOR_UPDATE_PERIOD_MS 1000u
+static constexpr uint32_t PTZ_CMD_TIMEOUT_MS = 250;  // ~5 missed cmds @ 20Hz
+
+enum class AppState 
+{
+    INIT,
+    FANCY_STARTUP,
+    IDLE,
+
+
+
+    // Other random states.
+    TEST_PANELS
+};
+
+enum class AppStatus
+{
+    OK,
+    NOT_OK,
+    CAN_INIT_ERROR,
+    CAN_SEND_ERROR
+};
+
+//TODO LedStackController relies on hardware... improper interface
+class App {
+public:    
+    App(ClockInterface& clock, ServoInterface& servo_tilt, ServoInterface& servo_pan, CanInterface& can)
+    : substate_timer(Timer(clock, false)), heartbeat_timer(Timer(clock, true)), sensor_read_and_send_timer(Timer(clock)), ptz_cmd_timer(Timer(clock)),  clock(clock), servo_tilt(servo_tilt), servo_pan(servo_pan), can(can) {};
+
+    void main(void);
+private:
+    Timer substate_timer;
+    Timer heartbeat_timer;
+    Timer sensor_read_and_send_timer;
+    Timer ptz_cmd_timer;
+    ClockInterface& clock;
+    ServoInterface& servo_tilt;
+    ServoInterface& servo_pan;
+    CanInterface& can;
+
+    AppState current_state;
+    uint32_t current_substate;
+
+    bool go_to_state(AppState next_state);
+    void handle_app_error(AppStatus error_or_status, msg_t error_msg = {});
+    void handle_ptz_timeout(void);
+
+    //Communication helpers
+    void send_msg(const msg_t &msg);
+    void send_ping();
+    void handle_heartbeat(); // Will send heartbeat at correct intervals
+    void poll_can();
+    // void handle_panel_cmd(LedPanelCMD_t& cmd);
+
+    void handle_sending_light_readings();
+
+    void run_state_machine(void);
+    // State based functions. run_<state_name>
+    // Not all states have a run function if they are simple enough
+    void run_INIT(void);
+    void run_FANCY_STARTUP(void);
+    void run_TEST_PANELS(void);
+        uint32_t test_panel_index = 0;
+        float    test_duty        = 0.0f;
+        bool     test_ramp_rising = true;
+
+    // Once a state is complete, it runs this function.
+    // This is what will switch current state
+    void complete_state(AppStatus state_end_status);
+
+
+
+
+
+};

ptz_stm32_firmware/Core/Inc/app/app_substates.hpp

@@ -0,0 +1,29 @@
+#pragma once
+
+#define NULL_STATE 9999
+
+
+enum class FancyStartupSubstate
+{
+    START_FIRST_FLASH,
+    RUN_FIRST_FLASH,
+    SECOND_FLASH,
+    COMPLETE
+};
+#define FIRST_FLASH_TIME_MS 500
+
+
+enum class TestPanelsSubstate : uint32_t
+{
+    START_SEQUENTIAL_ON,
+    RUN_SEQUENTIAL_ON,
+    ALL_OFF,
+    RUN_RAMP,
+    COMPLETE
+};
+
+// Tunables for the panel test
+static constexpr uint32_t TEST_STEP_ON_MS   = 300u;   // gap between each panel lighting up
+static constexpr float    TEST_ON_DUTY_PCT  = 20.0f;
+static constexpr uint32_t TEST_RAMP_STEP_MS = 20u;    // time per duty step in the ramp
+static constexpr float    TEST_RAMP_STEP    = 5.0f;   // % per step → 0..100 in 20 steps