Implement vsense monitoring

firmware/SConstruct

@@ -87,7 +87,7 @@ Help('serial: Display serial output from the device.\n')
 env.Command('monitor', app, '../scripts/monitor.py ${SERIAL_PORT} ${SOURCE} -b ${BAUD_RATE}')
 Help('monitor: Display formatted output from the device.\n')
 
-env.Command("format", None, 'astyle --options=.astylerc --recursive src/*.c src/*.h --exclude=src/libopencm3')
+env.Command("format", None, 'astyle --options=.astylerc --recursive src/*.c src/*.h --exclude=src/libopencm3 --exclude=src/modules/third_party')
 Help('format: Format all source files.\n')
 
 compdb = env.CompilationDatabase()

firmware/src/app/application/application.c

@@ -148,13 +148,13 @@ void Application_Init(void)
     Logging_Init(SysTime_GetSystemTime);
 
     NVCom_Init();
+    ADC_Init();
     SystemMonitor_Init();
     CANInterface_Init();
     DeviceMonitoring_Init();
     Flash_Init();
     NVS_Init(Board_GetNVSAddress(), Board_GetNumberOfPagesInNVS());
     Config_Init();
-    ADC_Init();
     MotorController_Init();
     ADC_Start();
     SignalHandler_Init();
@@ -287,7 +287,8 @@ static void HandleModeSignal(struct signal_t *signal_p, uint8_t index)
 {
     Signal_Log(signal_p, module.logger);
 
-    if (SystemMonitor_GetState() != SYSTEM_MONITOR_EMERGENCY)
+    if ((SystemMonitor_GetState() != SYSTEM_MONITOR_EMERGENCY) &&
+            (SystemMonitor_GetState() != SYSTEM_MONITOR_FAIL))
     {
         const uint8_t mode = *(uint8_t *)signal_p->data_p;
         switch (mode)
@@ -328,6 +329,7 @@ static void HandleStateChanges(void)
                 DeviceMonitoring_Count(DEV_MON_METRIC_EMERGENCY_STOP, 1);
                 BrakeAllMotors();
                 break;
+            case SYSTEM_MONITOR_FAIL:
             case SYSTEM_MONITOR_INACTIVE:
                 BrakeAllMotors();
                 break;

firmware/src/app/application/test/test_application.c

@@ -121,13 +121,13 @@ static int Setup(void **state)
     expect_any(Serial_Init, baud_rate);
     expect_function_call(Logging_Init);
     expect_function_call(NVCom_Init);
+    expect_function_call(ADC_Init);
     expect_function_call(SystemMonitor_Init);
     expect_function_call(CANInterface_Init);
     will_return(Board_GetNVSAddress, 0x801F800);
     will_return(Board_GetNumberOfPagesInNVS, 2);
     expect_function_call(NVS_Init);
     expect_function_call(Config_Init);
-    expect_function_call(ADC_Init);
     expect_function_call(MotorController_Init);
     expect_function_call(SignalHandler_Init);
     will_return(Logging_GetLogger, dummy_logger);
@@ -180,13 +180,13 @@ static void test_Application_Init_NoMotors(void **state)
     expect_value(Serial_Init, baud_rate, BAUD_RATE);
     expect_function_call(Logging_Init);
     expect_function_call(NVCom_Init);
+    expect_function_call(ADC_Init);
     expect_function_call(SystemMonitor_Init);
     expect_function_call(CANInterface_Init);
     will_return(Board_GetNVSAddress, 0x801F800);
     will_return(Board_GetNumberOfPagesInNVS, 2);
     expect_function_call(NVS_Init);
     expect_function_call(Config_Init);
-    expect_function_call(ADC_Init);
     expect_function_call(MotorController_Init);
     expect_function_call(SignalHandler_Init);
     will_return(Logging_GetLogger, dummy_logger);
@@ -217,13 +217,13 @@ static void test_Application_Init(void **state)
     expect_value(Serial_Init, baud_rate, BAUD_RATE);
     expect_function_call(Logging_Init);
     expect_function_call(NVCom_Init);
+    expect_function_call(ADC_Init);
     expect_function_call(SystemMonitor_Init);
     expect_function_call(CANInterface_Init);
     will_return(Board_GetNVSAddress, 0x801F800);
     will_return(Board_GetNumberOfPagesInNVS, 2);
     expect_function_call(NVS_Init);
     expect_function_call(Config_Init);
-    expect_function_call(ADC_Init);
     expect_function_call(MotorController_Init);
     expect_function_call(SignalHandler_Init);
     will_return(Logging_GetLogger, dummy_logger);
@@ -309,6 +309,19 @@ static void test_Application_Run_StateChanges(void **state)
     will_return(SysTime_GetDifference, 0);
     Application_Run();
 
+    /* Fail */
+    expect_function_call(SignalHandler_Process);
+    expect_function_call(MotorController_Update);
+    expect_function_call(Console_Process);
+    expect_function_call(SystemMonitor_Update);
+    will_return(SystemMonitor_GetState, SYSTEM_MONITOR_FAIL);
+    for (size_t i = 0; i < number_of_motors; ++i)
+    {
+        expect_value(MotorController_Brake, index, i);
+    }
+    will_return(SysTime_GetDifference, 0);
+    Application_Run();
+
     /* Inactive */
     expect_function_call(SignalHandler_Process);
     expect_function_call(MotorController_Update);
@@ -404,7 +417,7 @@ static void test_Application_SignalHandlers(void **state)
     GetCallback(signal.id)(&signal);
 }
 
-static void test_Application_SignalHandlers_Emergency(void **state)
+static void test_Application_SignalHandlers_EmergencyState(void **state)
 {
     uint16_t data;
     struct signal_t signal;
@@ -422,6 +435,24 @@ static void test_Application_SignalHandlers_Emergency(void **state)
     GetCallback(signal.id)(&signal);
 }
 
+static void test_Application_SignalHandlers_FailState(void **state)
+{
+    uint16_t data;
+    struct signal_t signal;
+    signal.data_p = &data;
+
+    will_return_maybe(SystemMonitor_GetState, SYSTEM_MONITOR_FAIL);
+    will_return_maybe(Signal_IDToString, "MockSignal");
+
+    data = 1;
+    signal.id = SIGNAL_CONTROL_MODE1;
+    GetCallback(signal.id)(&signal);
+
+    data = 2;
+    signal.id = SIGNAL_CONTROL_MODE2;
+    GetCallback(signal.id)(&signal);
+}
+
 static void test_Application_ResetCheck(void **state)
 {
     const size_t number_of_motors = 1;
@@ -482,7 +513,8 @@ int main(int argc, char *argv[])
         cmocka_unit_test_setup(test_Application_Run, Setup),
         cmocka_unit_test_setup(test_Application_Run_StateChanges, Setup),
         cmocka_unit_test_setup(test_Application_SignalHandlers, Setup),
-        cmocka_unit_test_setup(test_Application_SignalHandlers_Emergency, Setup),
+        cmocka_unit_test_setup(test_Application_SignalHandlers_EmergencyState, Setup),
+        cmocka_unit_test_setup(test_Application_SignalHandlers_FailState, Setup),
         cmocka_unit_test_setup(test_Application_ResetCheck, Setup),
     };
 

firmware/src/modules/adc/adc.c

@@ -45,7 +45,7 @@ along with CANDrive firmware.  If not, see <http://www.gnu.org/licenses/>.
 #endif
 
 #define NUMBER_OF_READINGS_PER_SAMPLE 16
-#define MAX_NUMBER_OF_CHANNELS 2
+#define MAX_NUMBER_OF_CHANNELS 3
 
 //////////////////////////////////////////////////////////////////////////
 //TYPE DEFINITIONS

firmware/src/modules/adc/test/test_adc.c

@@ -81,7 +81,7 @@ static void ExpectDMASetup(void)
     expect_value(dma_set_memory_address, dma, dma);
     expect_value(dma_set_memory_address, channel, channel);
 
-    const uint16_t number_of_data = 32;
+    const uint16_t number_of_data = 48;
     expect_value(dma_set_number_of_data, dma, dma);
     expect_value(dma_set_number_of_data, channel, channel);
     expect_value(dma_set_number_of_data, number, number_of_data);
@@ -156,6 +156,7 @@ static void test_ADC_InitChannel_Invalid(void **state)
     adc_input_t adc_channel;
     ADC_InitChannel(&adc_channel, 0);
     ADC_InitChannel(&adc_channel, 1);
+    ADC_InitChannel(&adc_channel, 2);
     expect_assert_failure(ADC_InitChannel(&adc_channel, 3));
 }
 

firmware/src/modules/board/board.c

@@ -234,6 +234,15 @@ uintptr_t Board_GetBackupMemoryAddress(void)
     return (BACKUP_REGS_BASE + 0x04);
 }
 
+uint32_t Board_VSenseToVoltage(uint32_t value)
+{
+    /* Vsense voltage divider values in Ohm. */
+    const uint32_t r1 = 10000;
+    const uint32_t r2 = 1200;
+
+    return ((value * (r1 + r2)) + (r2 / 2)) / r2;
+}
+
 //////////////////////////////////////////////////////////////////////////
 //LOCAL FUNCTIONS
 //////////////////////////////////////////////////////////////////////////

firmware/src/modules/board/board.h

@@ -201,4 +201,13 @@ uint32_t Board_GetMaxCurrent(void);
  */
 uintptr_t Board_GetBackupMemoryAddress(void);
 
+/**
+ * Convert VSense reading to voltage.
+ *
+ * @param value VSense value.
+ *
+ * @return VSense voltage.
+ */
+uint32_t Board_VSenseToVoltage(uint32_t value);
+
 #endif

firmware/src/modules/board/test/mock/mock_board.c

@@ -129,6 +129,11 @@ __attribute__((weak)) uintptr_t Board_GetBackupMemoryAddress(void)
     return mock_type(uintptr_t);
 }
 
+__attribute__((weak)) uint32_t Board_VSenseToVoltage(uint32_t value)
+{
+    return mock_type(uint32_t);
+}
+
 //////////////////////////////////////////////////////////////////////////
 //LOCAL FUNCTIONS
 //////////////////////////////////////////////////////////////////////////

firmware/src/modules/board/test/test_board.c

@@ -120,6 +120,13 @@ static void test_Board_GetMaxCurrent(void **state)
     assert_int_equal(Board_GetMaxCurrent(), 5000);
 }
 
+static void test_Board_VSenseToVoltage(void **state)
+{
+    assert_int_equal(Board_VSenseToVoltage(0), 0);
+    assert_int_equal(Board_VSenseToVoltage(1650), 15400);
+    assert_int_equal(Board_VSenseToVoltage(3300), 30800);
+}
+
 //////////////////////////////////////////////////////////////////////////
 //FUNCTIONS
 //////////////////////////////////////////////////////////////////////////
@@ -137,6 +144,7 @@ int main(int argc, char *argv[])
         cmocka_unit_test(test_Board_GetNVSAddress),
         cmocka_unit_test(test_Board_GetNumberOfPagesInNVS),
         cmocka_unit_test(test_Board_GetMaxCurrent),
+        cmocka_unit_test(test_Board_VSenseToVoltage)
     };
 
     if (argc >= 2)

firmware/src/modules/device_monitoring/device_monitoring.h

@@ -40,16 +40,16 @@ along with SillyCat firmware.  If not, see <http://www.gnu.org/licenses/>.
 
 enum device_monitoring_reboot_reason
 {
-  DEV_MON_REBOOT_REAS_USER_RESET = 1,
-  DEV_MON_REBOOT_REAS_FW_UPDATE,
-  DEV_MON_REBOOT_REAS_SW_RESET,
+    DEV_MON_REBOOT_REAS_USER_RESET = 1,
+    DEV_MON_REBOOT_REAS_FW_UPDATE,
+    DEV_MON_REBOOT_REAS_SW_RESET,
 };
 
 enum device_monitoring_metric_id
 {
-  DEV_MON_METRIC_CAN_TX_ERROR = 1,
-  DEV_MON_METRIC_EMERGENCY_STOP,
-  DEV_MON_METRIC_MAIN_TASK_TIME,
+    DEV_MON_METRIC_CAN_TX_ERROR = 1,
+    DEV_MON_METRIC_EMERGENCY_STOP,
+    DEV_MON_METRIC_MAIN_TASK_TIME,
 };
 
 typedef void (*device_monitoring_timer_cb_t)(void);

firmware/src/modules/system_monitor/SConscript

@@ -30,7 +30,8 @@ env.Append(CPPPATH=[
     '#src/modules/logging',
     '#src/modules/systime',
     '#src/modules/system_monitor',
-    '#src/modules/nvcom'
+    '#src/modules/nvcom',
+    '#src/modules/filter'
 
 ])