From 3b2efccad2db095bcd986471d4d781aeb8692d76 Mon Sep 17 00:00:00 2001 From: jfbblue0922 Date: Thu, 27 Jul 2023 15:03:31 +0900 Subject: [PATCH] update serial driver for JRE --- .../AP_HAL_ChibiOS/hwdef/JFB110/hwdef.dat | 24 +++++++++---------- .../AP_RangeFinder_JRE_Serial.cpp | 16 ++++++------- .../AP_RangeFinder_JRE_Serial.h | 3 ++- 3 files changed, 22 insertions(+), 21 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/hwdef/JFB110/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/JFB110/hwdef.dat index 880a7778298ff4..ef4b68bf8659b2 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/JFB110/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/JFB110/hwdef.dat @@ -124,7 +124,7 @@ define ANALOG_VCC_5V_PIN 5 define HAL_HAVE_BOARD_VOLTAGE 1 PB3 VBUS_RESERVED INPUT -# JFB110 has no SERVORAIL ADC +# JFB110 has SERVORAIL ADC # Set SENSOR_3.3V power signal insted. PC1 SCALED_V3V3 ADC1 SCALE(2) # ADC1_11 PA3 FMU_SERVORAIL_VCC ADC1 SCALE(2) # ADC1_15 @@ -161,11 +161,11 @@ define HAL_GPIO_SPEKTRUM_PWR 69 define HAL_SPEKTRUM_PWR_ENABLED 1 #Checked in Analogin.cpp -> MAV_POWER_STATUS -PG1 VDD_BRICK_nVALID INPUT PULLUP -PG2 VDD_BRICK2_nVALID INPUT PULLUP -PG3 VBUS_nVALID INPUT PULLUP -PE15 VDD_5V_PERIPH_nOC INPUT PULLUP -PF13 VDD_5V_HIPOWER_nOC INPUT PULLUP +PG1 VDD_BRICK_nVALID INPUT +PG2 VDD_BRICK2_nVALID INPUT +PG3 VBUS_nVALID INPUT +PE15 VDD_5V_PERIPH_nOC INPUT +PF13 VDD_5V_HIPOWER_nOC INPUT # ID pins PG0 HW_VER_REV_DRIVE OUTPUT LOW SPEED_VERYLOW @@ -204,10 +204,10 @@ PG15 IIM42652_2_CS CS SPEED_VERYLOW # SPI4_CS2 #PE2 SPI5_CS1 CS SPEED_VERYLOW # IMU Device Ready Signal Input -PF3 DRDY1_IIM42652_1 INPUT PULLUP -PF2 DRDY2_IIM42652_1 INPUT PULLUP -PA15 DRDY1_IIM42652_2 INPUT PULLUP -PA1 DRDY2_IIM42652_2 INPUT PULLUP +PF3 DRDY1_IIM42652_1 INPUT +PF2 DRDY2_IIM42652_1 INPUT +PA15 DRDY1_IIM42652_2 INPUT +PA1 DRDY2_IIM42652_2 INPUT PE7 SCHA63T_RESET OUTPUT LOW @@ -259,8 +259,8 @@ PD11 PWM_OE OUTPUT HIGH PD5 PWM_OE2 OUTPUT HIGH # GPIOs -PE11 FMU_CAP1 INPUT GPIO(66) PULLUP -PB11 FMU_CAP2 INPUT GPIO(67) PULLUP +PE11 FMU_CAP1 INPUT GPIO(66) +PB11 FMU_CAP2 INPUT GPIO(67) # CAN bus PD0 CAN1_RX CAN1 SPEED_VERYLOW diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp index 5e63f47fb12d6c..b1ab0a01ac710f 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.cpp @@ -31,20 +31,20 @@ /* CRC16CCITT */ static uint16_t CalcCRC16CCITT(uint8_t *cbuffer, uint16_t csize) { - uint16_t crc = CCITT_INITPARA; - for (int i = 0; i < csize; i++) { - crc ^= *cbuffer++; - for (int j = 0; j < 8; j++) { + uint16_t crc = CCITT_INITPARA; + for (int i = 0; i < csize; i++) { + crc ^= *cbuffer++; + for (int j = 0; j < 8; j++) { if ((crc & 0x0001) != 0) { crc >>= 1; crc ^= CCITT_POLYNOMIAL; } else { crc >>= 1; } - } - } - crc = crc ^ CCITT_OUTPARA; - return crc; + } + } + crc = crc ^ CCITT_OUTPARA; + return crc; } bool AP_RangeFinder_JRE_Serial::get_reading(float &reading_m) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h index 00f7676221e7a1..0249919543a447 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_JRE_Serial.h @@ -31,7 +31,8 @@ class AP_RangeFinder_JRE_Serial : public AP_RangeFinder_Backend_Serial return MAV_DISTANCE_SENSOR_RADAR; } - bool get_signal_quality_pct(uint8_t &quality_pct) const override { + bool get_signal_quality_pct(uint8_t &quality_pct) const override + { quality_pct = no_signal ? 0 : 100; return true; }