diff --git a/CommunicationSystemsSubmodule b/CommunicationSystemsSubmodule index 9fb7b8a..b0d5390 160000 --- a/CommunicationSystemsSubmodule +++ b/CommunicationSystemsSubmodule @@ -1 +1 @@ -Subproject commit 9fb7b8a5c4e1b40baff4d47704290b106aa0e2cc +Subproject commit b0d5390dad2a7308d8bbb15d89e60d7d7c833766 diff --git a/Components/Communication/CANMessageHandler.cpp b/Components/Communication/CANMessageHandler.cpp index 8fa8801..aeed25d 100644 --- a/Components/Communication/CANMessageHandler.cpp +++ b/Components/Communication/CANMessageHandler.cpp @@ -89,5 +89,35 @@ bool CANTask::HandleCANCommands() { } } + { + RPB_CAMERA_SIMULATE_BUTTON_COMMAND cmd; + if(dau.ReadMessageByLogIndex(_RPB_CAMERA_SIMULATE_BUTTON_COMMAND_LOGINDEX, (uint8_t*)&cmd,sizeof(cmd))) { + SOAR_PRINT("got sim button cmd %d btn %d\n",cmd.cam,cmd.button); + Command cm = {TASK_SPECIFIC_COMMAND,CAMERA_COMMAND_SIM_BUTTON}; + cm.CopyDataToCommand((uint8_t*)&cmd, sizeof(cmd)); + CameraTask::Inst().SendCommandReference(cm); + } + } + + { + RPB_CAM_TX_CONTROL_COMMAND cmd; + if(dau.ReadMessageByLogIndex(_RPB_CAM_TX_CONTROL_COMMAND_LOGINDEX, (uint8_t*)&cmd,sizeof(cmd))) { + SOAR_PRINT("got tx cmd %d\n",cmd.fieldToSet); + if(cmd.fieldToSet == cmd.ENABLED) { + Command cm = {TASK_SPECIFIC_COMMAND,cmd.enabled ? CAMERA_COMMAND_VIDEO_ENABLE : CAMERA_COMMAND_VIDEO_DISABLE}; + CameraTask::Inst().SendCommandReference(cm); + } else if(cmd.fieldToSet == cmd.POWER) { + Command cm = {TASK_SPECIFIC_COMMAND,CAMERA_COMMAND_SET_TX_POWER}; + cm.CopyDataToCommand((uint8_t*)&cmd.power, sizeof(cmd.power)); + CameraTask::Inst().SendCommandReference(cm); + } else if(cmd.fieldToSet == cmd.FREQUENCY) { + Command cm = {TASK_SPECIFIC_COMMAND,CAMERA_COMMAND_SET_TX_FREQ}; + cm.CopyDataToCommand((uint8_t*)&cmd.freq, sizeof(cmd.freq)); + CameraTask::Inst().SendCommandReference(cm); + } + + } + } + return foundone; }; diff --git a/Components/PeriphTasks/CameraTask.cpp b/Components/PeriphTasks/CameraTask.cpp index c014eaa..fc538ab 100644 --- a/Components/PeriphTasks/CameraTask.cpp +++ b/Components/PeriphTasks/CameraTask.cpp @@ -15,6 +15,7 @@ #include "callsign.h" #include "logo.h" #include "RPBLogs.hpp" +#include "IRCTramp.hpp" /** * @brief Constructor, sets up task @@ -145,12 +146,15 @@ void CameraTask::HandleCommand(Command &cm) switch (cam) { case 0: muxDriver.Select(Camera::CAMERA1); + cm.Reset(); return; case 1: muxDriver.Select(Camera::CAMERA2); + cm.Reset(); return; case 2: muxDriver.Select(Camera::CAMERA3); + cm.Reset(); return; } SOAR_PRINT("Invalid camera %d should be 0-2\n",cam); @@ -168,6 +172,29 @@ void CameraTask::HandleCommand(Command &cm) HAL_GPIO_WritePin(VideoTX_Enable_GPIO_Port,VideoTX_Enable_Pin,GPIO_PIN_SET); break; + case CAMERA_COMMAND_SIM_BUTTON: { + RPB_CAMERA_SIMULATE_BUTTON_COMMAND cmd = *(RPB_CAMERA_SIMULATE_BUTTON_COMMAND*)cm.GetDataPointer(); + USART_TypeDef* uart; + switch (cmd.cam) { + case 0: + uart = USART1; + break; + case 1: + uart = USART2; + break; + case 2: + uart = USART3; + break; + default: + SOAR_PRINT("invalid cam %d\n",cmd.cam); + cm.Reset(); + return; + } + RunCamCommand(uart, cmd.button); + break; + } + + case CAMERA_COMMAND_START_RECORDING: { uint8_t cam = *cm.GetDataPointer(); USART_TypeDef* uart; @@ -183,6 +210,7 @@ void CameraTask::HandleCommand(Command &cm) break; default: SOAR_PRINT("invalid cam %d\n",cam); + cm.Reset(); return; } RunCamCommand(uart, 0x03); @@ -203,6 +231,7 @@ void CameraTask::HandleCommand(Command &cm) break; default: SOAR_PRINT("invalid cam %d\n",cam); + cm.Reset(); return; } RunCamCommand(uart, 0x04); @@ -223,6 +252,7 @@ void CameraTask::HandleCommand(Command &cm) break; default: SOAR_PRINT("invalid cam %d\n",cam); + cm.Reset(); return; } break; @@ -241,10 +271,65 @@ void CameraTask::HandleCommand(Command &cm) break; default: SOAR_PRINT("invalid cam %d\n",cam); + cm.Reset(); return; } break; } + case CAMERA_COMMAND_SET_TX_POWER: { + uint16_t power = *cm.GetDataPointer(); + SOAR_PRINT("set tx power %d\n",power); + + IRCTramp::POWER pow; + bool invalid = false; + switch(power) { + case 25: + pow = IRCTramp::POWER_25MW; + break; + case 200: + pow = IRCTramp::POWER_200MW; + break; + case 1000: + pow = IRCTramp::POWER_1W; + break; + case 4000: + pow = IRCTramp::POWER_4W; + break; + default: + SOAR_PRINT("invalid power\n"); + invalid = true; + break; + } + if(!invalid){ + IRCTramp::SetVTXPower(pow, UART5); + } + break; + + } + case CAMERA_COMMAND_SET_TX_FREQ: { + uint16_t freq = *cm.GetDataPointer(); + SOAR_PRINT("set tx freq %d\n",freq); + + IRCTramp::FREQUENCY frequency; + bool invalid = false; + switch(freq) { + case 1258: + frequency = IRCTramp::FREQ_1258MHZ; + break; + case 1280: + frequency = IRCTramp::FREQ_1280MHZ; + break; + default: + SOAR_PRINT("invalid freq\n"); + invalid = true; + break; + } + if(!invalid){ + IRCTramp::SetVTXFrequency(frequency,UART5); + } + break; + + } default: SOAR_PRINT("CameraTask - Received Unsupported Task Command {%d}\n", cm.GetTaskCommand()); break; diff --git a/Components/PeriphTasks/Inc/CameraTask.hpp b/Components/PeriphTasks/Inc/CameraTask.hpp index 5f7ffc3..df1c89e 100644 --- a/Components/PeriphTasks/Inc/CameraTask.hpp +++ b/Components/PeriphTasks/Inc/CameraTask.hpp @@ -27,7 +27,10 @@ enum CAMERA_TASK_COMMANDS CAMERA_COMMAND_START_RECORDING, CAMERA_COMMAND_STOP_RECORDING, CAMERA_COMMAND_POWER_ON, - CAMERA_COMMAND_POWER_OFF + CAMERA_COMMAND_POWER_OFF, + CAMERA_COMMAND_SIM_BUTTON, + CAMERA_COMMAND_SET_TX_POWER, + CAMERA_COMMAND_SET_TX_FREQ }; /* Macros ------------------------------------------------------------------*/ diff --git a/SoarDrivers b/SoarDrivers index f3b1e9d..f8ed3ba 160000 --- a/SoarDrivers +++ b/SoarDrivers @@ -1 +1 @@ -Subproject commit f3b1e9de3797ff512853d1f1efc6139afaf4dad1 +Subproject commit f8ed3ba7c6b90ecd2a3c24916e28a4542f567056