begining of can logger

libraries/AP_CANManager/AP_CANManager.cpp

@@ -651,6 +651,79 @@ void AP_CANManager::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &fram
 }
 #endif // HAL_GCS_ENABLED
 
+
+#if AP_FILESYSTEM_FILE_WRITING_ENABLED
+
+uint16_t crcAdd(uint16_t crc_val, const uint8_t* bytes, size_t len)
+{
+    while (len--)
+    {
+        crc_val = crcAddByte(crc_val, *bytes++);
+    }
+    return crc_val;
+}
+
+uint16_t crcAddByte(uint16_t crc_val, uint8_t byte)
+{
+    crc_val ^= (uint16_t) ((uint16_t) (byte) << 8U);
+    for (uint8_t j = 0; j < 8; j++)
+    {
+        if (crc_val & 0x8000U)
+        {
+            crc_val = (uint16_t) ((uint16_t) (crc_val << 1U) ^ 0x1021U);
+        }
+        else
+        {
+            crc_val = (uint16_t) (crc_val << 1U);
+        }
+    }
+    return crc_val;
+}
+
+#define CANLOGFILE_HEADER_SIZE 16
+void AP_CANManager::log_frame(const CANFrame& frame)
+{
+    if (logging_buffer_.space() > (CANLOGFILE_HEADER_SIZE+frame.dlcToDataLength(frame.dlc))) {
+        return;
+    }
+    uint16_t magic = 0x2934;
+    logging_buffer_.write((uint8_t*)&magic, sizeof(magic));
+    uint64_t timestamp_us = AP_HAL::native_micros64();
+    logging_buffer_.write((uint8_t*)&timestamp_us, sizeof(timestamp_us));
+    uint16_t crc = crcAdd(0xFFFF, frame.data, frame.dlcToDataLength(frame.dlc));
+    logging_buffer_.write((uint8_t*)&crc, sizeof(crc));
+    uint16_t length = frame.dlcToDataLength(frame.dlc);
+    logging_buffer_.write((uint8_t*)&length, sizeof(length));
+    uint16_t flags = frame.canfd ? 1 : 0;
+    logging_buffer_.write((uint8_t*)&flags, sizeof(flags));
+    uint32_t id = frame.id & (frame.isExtended() ? AP_HAL::CANFrame::FlagEFF : 0);
+    logging_buffer_.write((uint8_t*)&id, sizeof(id));
+    logging_buffer_.write(frame.data, length);
+}
+
+
+void AP_CANManager::can_log_update()
+{
+    if (enable_logging_) {
+        return;
+    }
+    int i = 0;
+    if (log_fd == -1) {
+        struct stat st;
+        char filename[sizeof(HAL_BOARD_LOG_DIRECTORY) + 32];
+        while (i++) {
+            snprintf(filename, sizeof(filename), HAL_BOARD_LOG_DIRECTORY "CAN%u_%u.clog", get_iface_num(), i);
+            if (AP::FS().stat(filename, &st) != 0) {
+                break;
+            }
+        }
+        if (AP::FS().open(filename, O_CREAT | O_WRONLY | O_TRUNC) < 0) {
+            return;
+        }
+    }
+}
+#endif // AP_FILESYSTEM_FILE_WRITING_ENABLED
+
 AP_CANManager& AP::can()
 {
     return *AP_CANManager::get_singleton();

libraries/AP_HAL/CANIface.h

@@ -20,6 +20,8 @@
 #include <stdint.h>
 #include "AP_HAL_Namespace.h"
 #include "AP_HAL_Boards.h"
+#include <AP_HAL/utility/RingBuffer.h>
+#include <AP_Filesystem/AP_Filesystem.h>
 
 class ExpandingString;
 

libraries/AP_HAL_ChibiOS/hwdef/CubeOrangePlus/hwdef-bl.dat

@@ -1,20 +1,60 @@
 # hw definition file for processing by chibios_hwdef.py
+# for F765 bootloader
 
 # MCU class and specific type
-MCU STM32H7xx STM32H757xx
-define CORE_CM7
-define SMPS_PWR
+MCU STM32F7xx STM32F767xx
 
-include ../CubeOrange/hwdef-bl.inc
+# crystal frequency
+OSCILLATOR_HZ 16000000
 
-undef USB_STRING_PRODUCT
-undef USB_STRING_MANUFACTURER
-undef APJ_BOARD_ID
-undef USB_PRODUCT
+# board ID for firmware load
+APJ_BOARD_ID 50
 
-# USB setup
-USB_PRODUCT 0x1058
-USB_STRING_MANUFACTURER "CubePilot"
-USB_STRING_PRODUCT "CubeOrange+-BL"
+FLASH_SIZE_KB 2048
 
-APJ_BOARD_ID 1063
+# bootloader is installed at zero offset
+FLASH_RESERVE_START_KB 0
+
+# the location where the bootloader will put the firmware
+FLASH_BOOTLOADER_LOAD_KB 32
+
+PC6 LED_BOOTLOADER OUTPUT HIGH
+PC7 LED_ACTIVITY OUTPUT HIGH
+define HAL_LED_ON 0
+
+
+# order of UARTs (and USB)
+SERIAL_ORDER OTG1 USART2 UART7
+
+define BOOTLOADER_DEBUG SD7
+
+# USART2 is telem1
+PD6 USART2_RX USART2
+PD5 USART2_TX USART2
+PD3 USART2_CTS USART2
+PD4 USART2_RTS USART2
+
+PF6 UART7_RX UART7 NODMA
+PE8 UART7_TX UART7 NODMA
+
+PA11 OTG_FS_DM OTG1
+PA12 OTG_FS_DP OTG1
+
+PA13 JTMS-SWDIO SWD
+PA14 JTCK-SWCLK SWD
+
+# Add CS pins to ensure they are high in bootloader
+PF10 MS5611_CS CS
+PF2  ICM20689_CS CS SPEED_VERYLOW
+PF3  ICM20602_CS CS SPEED_VERYLOW
+PF4  BMI055_G_CS CS
+PG10 BMI055_A_CS CS
+PF5  FRAM_CS CS SPEED_VERYLOW
+PF11 SPARE_CS CS
+PH5  AUXMEM_CS CS
+PI4  EXTERNAL1_CS1 CS
+PI10 EXTERNAL1_CS2 CS
+PI11 EXTERNAL1_CS3 CS
+PI6  EXTERNAL2_CS1 CS
+PI7  EXTERNAL2_CS2 CS
+PI8  EXTERNAL2_CS3 CS

libraries/AP_HAL_ChibiOS/hwdef/Pixhawk4/hwdef-bl.dat

@@ -1,4 +1,60 @@
 # hw definition file for processing by chibios_hwdef.py
-# for Pixhawk4 bootloader
+# for F765 bootloader
 
-include ../fmuv5/hwdef-bl.dat
+# MCU class and specific type
+MCU STM32F7xx STM32F767xx
+
+# crystal frequency
+OSCILLATOR_HZ 16000000
+
+# board ID for firmware load
+APJ_BOARD_ID 50
+
+FLASH_SIZE_KB 2048
+
+# bootloader is installed at zero offset
+FLASH_RESERVE_START_KB 0
+
+# the location where the bootloader will put the firmware
+FLASH_BOOTLOADER_LOAD_KB 32
+
+PC6 LED_BOOTLOADER OUTPUT HIGH
+PC7 LED_ACTIVITY OUTPUT HIGH
+define HAL_LED_ON 0
+
+
+# order of UARTs (and USB)
+SERIAL_ORDER OTG1 USART2 UART7
+
+define BOOTLOADER_DEBUG SD7
+
+# USART2 is telem1
+PD6 USART2_RX USART2
+PD5 USART2_TX USART2
+PD3 USART2_CTS USART2
+PD4 USART2_RTS USART2
+
+PF6 UART7_RX UART7 NODMA
+PE8 UART7_TX UART7 NODMA
+
+PA11 OTG_FS_DM OTG1
+PA12 OTG_FS_DP OTG1
+
+PA13 JTMS-SWDIO SWD
+PA14 JTCK-SWCLK SWD
+
+# Add CS pins to ensure they are high in bootloader
+PF10 MS5611_CS CS
+PF2  ICM20689_CS CS SPEED_VERYLOW
+PF3  ICM20602_CS CS SPEED_VERYLOW
+PF4  BMI055_G_CS CS
+PG10 BMI055_A_CS CS
+PF5  FRAM_CS CS SPEED_VERYLOW
+PF11 SPARE_CS CS
+PH5  AUXMEM_CS CS
+PI4  EXTERNAL1_CS1 CS
+PI10 EXTERNAL1_CS2 CS
+PI11 EXTERNAL1_CS3 CS
+PI6  EXTERNAL2_CS1 CS
+PI7  EXTERNAL2_CS2 CS
+PI8  EXTERNAL2_CS3 CS