diff --git a/libraries/AP_HAL_ChibiOS/hwdef/HerePro/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/HerePro/defaults.parm deleted file mode 100644 index 16b4cd021a..0000000000 --- a/libraries/AP_HAL_ChibiOS/hwdef/HerePro/defaults.parm +++ /dev/null @@ -1,3 +0,0 @@ -NTF_LED_OVERRIDE 1 -SCR_ENABLE 1 -GPS_INJECT_TO 0 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/HerePro/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/HerePro/hwdef-bl.dat deleted file mode 100644 index 5f8cffae71..0000000000 --- a/libraries/AP_HAL_ChibiOS/hwdef/HerePro/hwdef-bl.dat +++ /dev/null @@ -1,147 +0,0 @@ -# hw definition file for processing by chibios_hwdef.py -# for H743 bootloader - -# MCU class and specific type -MCU STM32H7xx STM32H743xx - -FULL_FEATURED_BOOTLOADER 1 - -# USB setup -USB_VENDOR 0x2DAE # ONLY FOR USE BY HEX! NOBODY ELSE -USB_PRODUCT 0x5011 -USB_STRING_MANUFACTURER "CubePilot" - -# setup build for a peripheral firmware -env AP_PERIPH 1 - -# crystal frequency -OSCILLATOR_HZ 24000000 - -# board ID for firmware load -APJ_BOARD_ID 1037 - -FLASH_SIZE_KB 2048 - -# bootloader is installed at zero offset -FLASH_RESERVE_START_KB 0 - -# the location where the bootloader will put the firmware -# the H743 has 128k sectors -FLASH_BOOTLOADER_LOAD_KB 256 - -PB15 LED_2 OUTPUT LOW -PB14 LED OUTPUT LOW - -# board voltage -STM32_VDD 330U - -# order of UARTs (and USB) -SERIAL_ORDER OTG1 UART7 - -define HAL_NO_MONITOR_THREAD - -# USART3 F9 -PD9 USART3_RX USART3 -PD8 USART3_TX USART3 - -# UART7 F9 -PE7 UART7_RX UART7 -PE8 UART7_TX UART7 NODMA - - -PA11 OTG_FS_DM OTG1 -PA12 OTG_FS_DP OTG1 - -PA13 JTMS-SWDIO SWD -PA14 JTCK-SWCLK SWD - - -define USB_USE_WAIT TRUE -define HAL_USE_USB_MSD 1 - -define HAL_DEVICE_THREAD_STACK 2048 - -#define AP_PARAM_MAX_EMBEDDED_PARAM 0 - -define HAL_BARO_ALLOW_INIT_NO_BARO - -define HAL_COMPASS_MAX_SENSORS 1 - -# GPS+MAG -define HAL_PERIPH_ENABLE_GPS -define HAL_PERIPH_ENABLE_MAG - -# Add MAVLink -env FORCE_MAVLINK_INCLUDE 1 - -# enable CAN support -PB8 CAN1_RX CAN1 -PB9 CAN1_TX CAN1 -PB5 CAN2_RX CAN2 -PB6 CAN2_TX CAN2 -#PB5 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW - -PF8 blank CS -SPIDEV profiled SPI5 DEVID1 blank MODE3 1*MHZ 1*MHZ - -PE4 MPU_EXT_CS CS -PE2 SPI4_SCK SPI4 -PE5 SPI4_MISO SPI4 -PE6 SPI4_MOSI SPI4 - - -PG15 spi6cs CS -PG13 SPI6_SCK SPI6 -PG12 SPI6_MISO SPI6 -PG14 SPI6_MOSI SPI6 - -PF7 SPI5_SCK SPI5 -PF9 SPI5_MOSI SPI5 - -define BOARD_SER1_RTSCTS_DEFAULT 0 - -#gps f9 -PD10 F91TXR INPUT -PD11 F92RST INPUT -PD13 F93SB INPUT -PD14 F94EXTINT INPUT -PD15 F95RSV1 INPUT -PG2 F97 INPUT GPIO(100) -PG3 F96RSV2 INPUT -PD4 RTKSTAT INPUT -PD5 GEOFENCESTAT INPUT - -#others -PG10 can2int INPUT -PG11 can2sleep OUTPUT PUSHPULL SPEED_LOW LOW -PF1 GNDDET2 INPUT -PF2 GNDDET1 INPUT -PC6 EXTERN_GPIO1 OUTPUT GPIO(1) -PC7 EXTERN_GPIO2 OUTPUT GPIO(2) -#sensor enable -PC0 SENSEN OUTPUT HIGH -PB1 IMUINT INPUT -PB3 QCAN2TERM OUTPUT LOW GPIO(4) -PB4 QCAN2INT0 INPUT -PC13 QCAN1SLEEP OUTPUT PUSHPULL SPEED_LOW LOW -PE0 QCAN1INT INPUT -PE1 QCAN1INT0 INPUT -PE3 QCAN1INT1 INPUT - -PD6 QCAN1TERM OUTPUT LOW GPIO(3) -PD7 QCAN2INT1 INPUT -PA8 QCAN12OSC1 INPUT - -# This input pin is used to detect that power is valid on USB. -PA9 VBUS INPUT -PH13 VDD_33V_SENS INPUT - -# This defines some input pins, currently unused. -PB2 BOOT1 INPUT - - - -# passthrough CAN1 -define HAL_DEFAULT_CPORT 1 - -FULL_CHIBIOS_BOOTLOADER 1 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/HerePro/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/HerePro/hwdef.dat deleted file mode 100644 index 0c8c2b46a8..0000000000 --- a/libraries/AP_HAL_ChibiOS/hwdef/HerePro/hwdef.dat +++ /dev/null @@ -1,221 +0,0 @@ -# hw definition file for processing by chibios_hwdef.py -# for H743 bootloader - -# MCU class and specific type -MCU STM32H7xx STM32H743xx - -# USB setup -USB_VENDOR 0x2DAE # ONLY FOR USE BY HEX! NOBODY ELSE -USB_PRODUCT 0x5001 -USB_STRING_MANUFACTURER "CubePilot" - -# setup build for a peripheral firmware -env AP_PERIPH 1 -env AP_PERIPH_HEAVY 1 - -# crystal frequency -OSCILLATOR_HZ 24000000 - -# board ID for firmware load -APJ_BOARD_ID 1037 - -FLASH_SIZE_KB 2048 - -# the location where the bootloader will put the firmware -# the H743 has 128k sectors, we leave 2 sectors for BL -FLASH_BOOTLOADER_LOAD_KB 256 - -PB15 LED_SWITCH_2 OUTPUT HIGH -PB14 LED_SWITCH_1 OUTPUT HIGH - -# board voltage -STM32_VDD 330U - -# order of UARTs (and USB) -SERIAL_ORDER OTG1 OTG2 EMPTY USART3 UART7 - -define DEFAULT_SERIAL0_PROTOCOL SerialProtocol_None - -# USART3 F9 -PD9 USART3_RX USART3 -PD8 USART3_TX USART3 - -# UART7 F9 -PE7 UART7_RX UART7 -PE8 UART7_TX UART7 NODMA - - -PA11 OTG_FS_DM OTG1 -PA12 OTG_FS_DP OTG1 - -PA13 JTMS-SWDIO SWD -PA14 JTCK-SWCLK SWD - -########################### -define AP_NOTIFY_PROFILED_SPI_ENABLED 1 -define DEFAULT_NTF_LED_TYPES Notify_LED_ProfiLED_SPI -define NOTIFY_LED_LEN_DEFAULT 16 -define CONFIGURE_PPS_PIN TRUE -define HAL_PERIPH_ENABLE_NOTIFY -define HAL_PERIPH_NOTIFY_WITHOUT_RCOUT - -FLASH_RESERVE_START_KB 256 - -define GPS_UBLOX_MOVING_BASELINE TRUE -define HAL_GCS_ENABLED 1 -define HAL_LOGGING_ENABLED TRUE -define HAL_PERIPH_ENABLE_RTC 1 -define HAL_NO_MONITOR_THREAD - - -define HAL_DISABLE_LOOP_DELAY -define USB_USE_WAIT TRUE -define HAL_USE_USB_MSD 1 - - -define HAL_DEVICE_THREAD_STACK 2048 - -#define AP_PARAM_MAX_EMBEDDED_PARAM 0 - -define HAL_BARO_ALLOW_INIT_NO_BARO - -define HAL_STORAGE_SIZE 16384 - -# disable dual GPS and GPS blending to save flash space -define GPS_MAX_RECEIVERS 1 -define GPS_MAX_INSTANCES 1 -define HAL_COMPASS_MAX_SENSORS 1 - -# GPS+MAG -define HAL_PERIPH_ENABLE_AHRS -define HAL_PERIPH_ENABLE_GPS -define HAL_PERIPH_ENABLE_MAG -define AP_INERTIALSENSOR_ENABLED 1 - - -# enable CAN support -PB8 CAN1_RX CAN1 -PB9 CAN1_TX CAN1 -PB5 CAN2_RX CAN2 -PB6 CAN2_TX CAN2 - -PA4 MPU_CS CS -PA5 SPI1_SCK SPI1 -PA6 SPI1_MISO SPI1 -PA7 SPI1_MOSI SPI1 -SPIDEV icm20948 SPI1 DEVID4 MPU_CS MODE3 4*MHZ 8*MHZ - -PF8 blank CS -SPIDEV profiled SPI5 DEVID1 blank MODE3 1*MHZ 1*MHZ - -IMU Invensensev2 SPI:icm20948 ROTATION_YAW_270 - -define HAL_DEFAULT_INS_FAST_SAMPLE 5 - -# compass as part of ICM20948 on newer cubes -COMPASS AK09916:probe_ICM20948 0 ROTATION_ROLL_180_YAW_90 - -# one baro -#BARO MS56XX I2C:0:0x77 - -PE4 CSPI4_CS CS -PE2 SPI4_SCK SPI4 -PE5 SPI4_MISO SPI4 -PE6 SPI4_MOSI SPI4 - - -PG15 CSPI6_CS CS -PG13 SPI6_SCK SPI6 -PG12 SPI6_MISO SPI6 -PG14 SPI6_MOSI SPI6 - -PF7 SPI5_SCK SPI5 -PF9 SPI5_MOSI SPI5 - -# Now setup the pins for the microSD card, if available. -PC8 SDMMC1_D0 SDMMC1 -PC9 SDMMC1_D1 SDMMC1 -PC10 SDMMC1_D2 SDMMC1 -PC11 SDMMC1_D3 SDMMC1 -PC12 SDMMC1_CK SDMMC1 -PD2 SDMMC1_CMD SDMMC1 - -define BOARD_SER1_RTSCTS_DEFAULT 0 - -define HAL_OS_FATFS_IO 1 -define HAL_WITH_DRONECAN 1 -env HAL_WITH_DRONECAN 1 -define HAL_PICCOLO_CAN_ENABLE 0 -define AP_CAN_SLCAN_ENABLED 1 - -PA1 ANALOG_CAN_VOLTAGE1 ADC1 SCALE(21.404) -PA2 ANALOG_CAN_VOLTAGE2 ADC1 SCALE(21.404) - -#gps f9 -PD10 F91TXR INPUT -PD11 F92RST INPUT -PD13 F93SB INPUT -PD14 F94EXTINT INPUT -PD15 F95RSV1 INPUT -PG2 F97 INPUT GPIO(100) -PG3 F96RSV2 INPUT -PD4 RTKSTAT INPUT -PD5 GEOFENCESTAT INPUT - -#others -PG10 can2int INPUT -PG11 can2sleep OUTPUT PUSHPULL SPEED_LOW LOW -PF1 GNDDET2 INPUT -PF2 GNDDET1 INPUT -PC6 EXTERN_GPIO1 OUTPUT GPIO(1) -PC7 EXTERN_GPIO2 OUTPUT GPIO(2) -PD6 TERMCAN1 OUTPUT LOW GPIO(3) -PB3 TERMCAN2 OUTPUT LOW GPIO(4) - -#sensor enable -PC0 SENSEN OUTPUT HIGH -PB1 IMUINT INPUT -PB4 QCAN2INT0 INPUT -PC13 QCAN1SLEEP OUTPUT PUSHPULL SPEED_LOW LOW -PE0 QCAN1INT INPUT -PE1 QCAN1INT0 INPUT -PE3 QCAN1INT1 INPUT - -PD7 QCAN2INT1 INPUT -PA8 QCAN12OSC1 INPUT - -# This defines more ADC inputs. -PC3 ANALOG_VCC_1_8V ADC1 SCALE(2) -PC4 ANALOG_VCC_5V_PIN ADC1 SCALE(2.044) - -# This input pin is used to detect that power is valid on USB. -PA9 VBUS INPUT -PH13 VDD_33V_SENS INPUT - -# This defines some input pins, currently unused. -PB2 BOOT1 INPUT - - - -# disabled compass cal as it doesn't work for now -# define COMPASS_CAL_ENABLED 1 - -define HAL_ENABLE_SLCAN 1 -# passthrough CAN1 -define HAL_DEFAULT_CPORT 1 - -define CONFIGURE_PPS_PIN TRUE -define AP_PERIPH_HAVE_LED TRUE - -define AP_SCRIPTING_ENABLED 1 -define SCRIPTING_HEAP_SIZE (64*1024) - -define GPS_MOVING_BASELINE 1 - -define AP_RC_CHANNEL_ENABLED 1 - -define AP_RELAY_ENABLED 1 -define AP_SERVORELAYEVENTS_ENABLED 1 - -# bootloader embedding / bootloader flashing not available -define AP_BOOTLOADER_FLASHING_ENABLED 0 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/HerePro/scripts/hereproled.lua b/libraries/AP_HAL_ChibiOS/hwdef/HerePro/scripts/hereproled.lua deleted file mode 100644 index 106d27e150..0000000000 --- a/libraries/AP_HAL_ChibiOS/hwdef/HerePro/scripts/hereproled.lua +++ /dev/null @@ -1,315 +0,0 @@ --- This script is a test of led override --- luacheck: only 0 - -local count = 0 -local num_leds = 16 -local total_time = 1 -local animation_end = 0 -local current_anim = 0 - --- constrain a value between limits -function constrain(v, vmin, vmax) - if v < vmin then - v = vmin - end - if v > vmax then - v = vmax - - end - return v -end - ---[[ -Table of neon colors on a rainbow, red first ---]] --- local rainbow = { --- { 252, 23, 0 }, --- { 253, 72, 37 }, --- { 250, 237, 39 }, --- { 51, 255, 20 }, --- { 27, 3, 163 } --- } - -local rainbow = { - { 252, 23, 0 }, - { 253, 100, 0 }, - { 250, 237, 0 }, - { 51, 255, 20 }, - { 27, 3, 163 } -} - -local led_map = { - 14, - 15, - 2, - 3, - 4, - 5, - 6, - 7, - 8, - 9, - 1, - 0, - 10, - 11, - 12, - 13} - -function table.contains(table, element) - for _, value in pairs(table) do - if value == element then - return true - end - end - return false -end - ---[[ -Function to set a LED to a color on a classic rainbow spectrum, with v=0 giving red ---]] -function get_Rainbow(v, num_rows) - local row = math.floor(constrain(v * (num_rows-1)+1, 1, num_rows-1)) - local v0 = (row-1) / (num_rows-1) - local v1 = row / (num_rows-1) - local p = (v - v0) / (v1 - v0) - r = math.max(math.floor(rainbow[row][1] + p * (rainbow[row+1][1] - rainbow[row][1])),0) - g = math.max(math.floor(rainbow[row][2] + p * (rainbow[row+1][2] - rainbow[row][2])),0) - b = math.max(math.floor(rainbow[row][3] + p * (rainbow[row+1][3] - rainbow[row][3])),0) - return r,g,b -end - -function do_initialisation() - local pos = math.rad(total_time/2) - local v =0.5 + 0.5 * math.sin(pos) - local invert = 1 - local r, g, b = get_Rainbow(v, 5) - local led_trail_length = 3 - local softness = 10 - local bfact = 1 - local updated_leds = {} - if math.cos(pos) > 0 then - invert = 1 - else - invert = 0 - end - - pos = math.floor(6 + (v*8))%16 - if pos == 6 then - animation_end = 1 - else - animation_end = 0 - end - - for trail = 0, led_trail_length-1 do - if invert == 1 then - bfact = softness^(2*(led_trail_length - 1 - trail)/(led_trail_length-1)) - else - bfact = softness^(2*trail/(led_trail_length-1)) - end - local led_id = 1 + ((pos + trail) % num_leds) - notify:handle_rgb_id(math.floor(r/bfact), math.floor(g/bfact), math.floor(b/bfact), led_map[led_id]) - table.insert(updated_leds, led_id) - end - - pos = math.floor(6 - (v*8))%16 - for trail = 0, led_trail_length-1 do - if invert == 0 then - bfact = softness^(2*(led_trail_length - 1 - trail)/(led_trail_length-1)) - else - bfact = softness^(2*trail/(led_trail_length-1)) - end - local led_id = 1 + ((pos + trail) % num_leds) - notify:handle_rgb_id(math.floor(r/bfact), math.floor(g/bfact), math.floor(b/bfact), led_map[led_id]) - table.insert(updated_leds, led_id) - end - - for led = 1, num_leds do - if not table.contains(updated_leds, led) then - notify:handle_rgb_id(0, 0, 0, led_map[led]) - end - end -end - -function do_point_north(r, g, b, led_trail_length, softness) - local north_bias = 4 - local yaw = 8 - ((16 * periph:get_yaw_earth()/(2*math.pi))) - north_bias - local ofs = yaw - math.floor(yaw+0.5) - yaw = math.floor(yaw+0.5) % 16 - local updated_leds = {} - for trail = 0, led_trail_length-1 do - local bfact = softness^(2*(math.abs((led_trail_length-1)/2 - trail + ofs))/(led_trail_length-1)) - local led_id = 1 + ((yaw + trail) % 16) - notify:handle_rgb_id(math.floor(r/bfact), math.floor(g/bfact), math.floor(b/bfact), led_map[led_id]) - table.insert(updated_leds, led_id) - end - - for led = 1, num_leds do - if not table.contains(updated_leds, led) then - notify:handle_rgb_id(0, 0, 0, led_map[led]) - end - end - return yaw -end - -function finish_initialisation(r, g, b, led_trail_length, softness, speed_factor, next_call) - local north_bias = 4 - local yaw = 8 - ((16 * periph:get_yaw_earth()/(2*math.pi))) - north_bias - yaw = math.floor(yaw+0.5) % 16 - local led_id = math.floor((count/speed_factor) + north_bias) % 16 - if yaw == led_id then - if total_time > 3000 then - animation_end = 1 - end - return - end - count = count + next_call - local updated_leds = {} - for trail = 0, led_trail_length-1 do - local bfact = softness^(2*(math.abs((led_trail_length-1)/2 - trail))/(led_trail_length-1)) - local this_led_id = 1 + ((led_id + trail) % 16) - notify:handle_rgb_id(math.floor(r/bfact), math.floor(g/bfact), math.floor(b/bfact), led_map[this_led_id]) - table.insert(updated_leds, this_led_id) - end - - for led = 1, num_leds do - if not table.contains(updated_leds, led) then - notify:handle_rgb_id(0, 0, 0, led_map[led]) - end - end -end - -function do_arm_spin(r, g, b, softness, speed_factor, arming) - -- reset led states - local updated_leds = {} - - -- calculate next call based on time - local next_call = 1 - if arming then - next_call = speed_factor - math.floor(((total_time)/speed_factor) + 0.5) - if next_call < 1 then - return next_call - end - else - next_call = 1 + math.floor(((total_time)/speed_factor) + 0.5) - if next_call > speed_factor then - return next_call - end - end - -- set trail length bassed on spin progress - local led_trail_length = 3 + (13 * (1 - (next_call/75))) - - - -- create a trail - for trail = 0, led_trail_length-1 do - local bfact = softness^(led_trail_length - trail) - local led_id = 1 + (count + trail) % 16 - notify:handle_rgb_id(math.floor(r/bfact), math.floor(g/bfact), math.floor(b/bfact), led_map[led_id]) - table.insert(updated_leds, led_id) - end - - for led = 1, num_leds do - if not table.contains(updated_leds, led) then - notify:handle_rgb_id(0, 0, 0, led_map[led]) - end - end - return next_call -end - -function set_all(r, g, b) - for led = 1, num_leds do - notify:handle_rgb_id(r, g, b, led_map[led]) - end -end - -function animation_state_machine(vehicle_state) - -- change state only when last loop finished - if animation_end == 0 then - return - end - - if ((vehicle_state & 1) == uint32_t(1)) then - -- do_initialisation - if current_anim ~= 0 then - current_anim = 0 - total_time = 0 - animation_end = 0 - end - elseif current_anim == 0 then - -- do finish by move to north - count = 0 - total_time = 0 - current_anim = 1 - animation_end = 0 - elseif current_anim == 1 then - -- do always point north - current_anim = 2 - elseif current_anim ~= 3 and ((vehicle_state & (1 << 1)) ~= uint32_t(0)) then --VEHICLE_STATE_ARMED - total_time = 0 - current_anim = 3 - animation_end = 0 - elseif current_anim == 3 and ((vehicle_state & (1 << 1)) == uint32_t(0)) then - total_time = 0 - current_anim = 4 - animation_end = 0 - elseif current_anim == 4 then - current_anim = 1 - end -end - -function update() -- this is the loop which periodically runs - local next_call = 20 - local vehicle_state = periph:get_vehicle_state() - animation_state_machine(vehicle_state) - - -- Initialisation - if current_anim == 0 then - do_initialisation() - total_time = total_time + next_call - elseif current_anim == 1 then - finish_initialisation(rainbow[1][1], rainbow[1][2], rainbow[1][3], 5, 20, 50, next_call) - total_time = total_time + next_call - elseif current_anim == 2 then - local v - if (vehicle_state & (1<<3)) ~= uint32_t(0) or --VEHICLE_STATE_PREARM - (vehicle_state & (1<<4)) ~= uint32_t(0) or --VEHICLE_STATE_PREARM_GPS - (vehicle_state & (1<<7)) ~= uint32_t(0) or --VEHICLE_STATE_FAILSAFE_RADIO - (vehicle_state & (1<<8)) ~= uint32_t(0) or --VEHICLE_STATE_FAILSAFE_BATT - (vehicle_state & (1<<11)) ~= uint32_t(0) then --VEHICLE_STATE_EKF_BAD - v = 0.0 - end - if (vehicle_state & (1<<17)) == uint32_t(0) then--VEHICLE_STATE_POS_ABS_AVAIL - v = 0.5 + (0.5 * math.min(gps:num_sats(0)/20, 1.0)) - else - v = 1.0 - end - local r, g, b = get_Rainbow(v, 4) - count = do_point_north(r, g, b, 5, 20) - -- --[[ ARM Display - elseif current_anim == 3 then - if animation_end == 0 then - next_call = do_arm_spin(rainbow[4][1], rainbow[4][2], rainbow[4][3], 2, 75, true) - count = count + 1 - end - if next_call < 1 then - set_all(rainbow[4][1], rainbow[4][2], rainbow[4][3]) - animation_end = 1 - else - total_time = total_time + next_call - end - elseif current_anim == 4 then - if animation_end == 0 then - next_call = do_arm_spin(rainbow[4][1], rainbow[4][2], rainbow[4][3], 2, 75, false) - count = count - 1 - end - if next_call > 75 then - animation_end = 1 - else - total_time = total_time + next_call - end - end - -- gcs:send_text(0, string.format("NCALL: %s", tostring(next_call))) - return update, next_call -- reschedules the loop in next_call milliseconds -end - -return update() -- run immediately before starting to reschedule \ No newline at end of file