增加NMEA雷达

APMrover2/Log.cpp

@@ -49,6 +49,15 @@ void Rover::Log_Write_Depth()
     if (!ahrs.get_position(loc)) {
         return;
     }
+    float deepthR = (float)(rangefinder.distance_cm_orient(ROTATION_PITCH_270) * 0.01f);
+    static float last_deep;
+    static uint32_t last_1s;
+    uint32_t tnow = AP_HAL::millis();
+    if (tnow - last_1s > 1000 || fabs(last_deep - deepthR) > 0.01) {
+        last_1s = tnow;
+        last_deep = deepthR;
+        logger.Write_Underwater_Sonar(loc);
+    }
 
     // check if new sensor reading has arrived
     uint32_t reading_ms = rangefinder.last_reading_ms(ROTATION_PITCH_270);
@@ -62,7 +71,7 @@ void Rover::Log_Write_Depth()
                         AP_HAL::micros64(),
                         loc.lat,
                         loc.lng,
-                        (double)(rangefinder.distance_cm_orient(ROTATION_PITCH_270) * 0.01f));
+                        deepthR);
 }
 
 // guided mode logging

APMrover2/Rover.cpp

@@ -261,9 +261,16 @@ void Rover::update_logging2(void)
         logger.Write_Vibration();
     }
     
-    underwater_sonar.update(serial_manager,current_loc);
+    // underwater_sonar.update(serial_manager,current_loc);
+    // uint16_t rc8_in = RC_Channels::rc_channel(CH_8)->get_radio_in();
+    // uint16_t rc9_in = RC_Channels::rc_channel(CH_9)->get_radio_in();
+    // AP_Relay *_apm_relay = AP::relay();
+    // rc8_in > 1500?_apm_relay->on(1):_apm_relay->off(1);
+    // rc9_in > 1500?_apm_relay->on(2):_apm_relay->off(2);
+
 }
 
+#define DUAL_ANT_HEADING  1
 
 /*
   once a second events
@@ -294,12 +301,85 @@ void Rover::one_second_loop(void)
 
     // need to set "likely flying" when armed to allow for compass
     // learning to run
-    ahrs.set_likely_flying(hal.util->get_soft_armed());
+    ahrs.set_likely_flying(hal.util->get_soft_armed()); 
 
     // send latest param values to wp_nav
     g2.wp_nav.set_turn_params(g.turn_max_g, g2.turn_radius, g2.motors.have_skid_steering());
 
     // gcs().send_text(MAV_SEVERITY_CRITICAL, "test");
+    // rover.g2.proximity.re_init();
+	static uint32_t prx_init_time = AP_HAL::millis();
+    static bool init_prx = false;
+    if (init_prx == false ) {
+        uint32_t tnow = AP_HAL::millis();
+        if(tnow - prx_init_time > 30000){
+            init_prx = true;
+            init_proximity();
+            gcs().send_text(MAV_SEVERITY_INFO, "--- init prx:%dms -> %dms ---",prx_init_time,tnow);
+        }
+    }
+
+#if DUAL_ANT_HEADING
+    // static uint8_t prt_cnt;
+    // prt_cnt++;
+    // if(prt_cnt > 5 && gps.gps_heading_enable()){
+    //     prt_cnt = 0;
+    //     float heading = gps.gps_heading()*0.01f;
+    //     float gps_heading_rad = ToRad(heading);
+    //     gcs().send_text(MAV_SEVERITY_INFO, "[GNSS]:heading:%3.2f , heading_rad:%3.2f  ", heading, gps_heading_rad);
+    // }
+#endif
+
+#if DUAL_ANT_HEADING
+
+    uint16_t rc14_in = RC_Channels::rc_channel(CH_14)->get_radio_in();
+    uint8_t ch_flag ;
+    if(rc14_in < 1400){
+        ch_flag = 0;
+    }else if(rc14_in < 1700){
+        ch_flag = 1;
+    }else
+    {
+        ch_flag = 2;
+    }
+    
+    static uint8_t last_type = 3;
+    switch (ch_flag) {
+        case 2: {
+            if(last_type != 2){
+                gps.set_gps_heading_enable(1);
+                compass.set_use_for_yaw(0,0);
+                type_yaw = 2;
+                compass.set_alway_healthy(true);
+                gcs().send_text(MAV_SEVERITY_INFO, "[INFO]:定向模式：抗磁(%d)",type_yaw);
+                last_type = type_yaw;
+            }
+            break;
+        }
+
+        case 1: {
+            if(last_type != 1){
+                gps.set_gps_heading_enable(0);
+                compass.set_use_for_yaw(0,1);
+                type_yaw = 1;
+                compass.set_alway_healthy(false);
+                gcs().send_text(MAV_SEVERITY_INFO, "[INFO]:定向模式：普通(%d)",type_yaw);
+                last_type = type_yaw;
+            }
+            break;
+        }
+
+        case 0: {
+            if(last_type != 0){
+            // gcs().send_text(MAV_SEVERITY_INFO, "[INFO]:Yaw Switch auto");
+                type_yaw = 0;
+                last_type = type_yaw;
+            }
+            break;
+        }
+    }
+
+#endif
 
 }
 

APMrover2/Rover.h

@@ -441,7 +441,7 @@ private:
     static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1,
                   "_failsafe_priorities is missing the sentinel");
 
-    Underwater_Sonar underwater_sonar;
+    // Underwater_Sonar underwater_sonar;
 public:
     void mavlink_delay_cb();
     void failsafe_check();
@@ -457,6 +457,8 @@ public:
 
     // Simple mode
     float simple_sin_yaw;
+    
+    uint8_t type_yaw;
 };
 
 extern Rover rover;

APMrover2/system.cpp

@@ -99,7 +99,7 @@ void Rover::init_ardupilot()
     rangefinder.init(ROTATION_NONE);
 
     // init proximity sensor
-    init_proximity();
+    // init_proximity();
 
     // init beacons used for non-gps position estimation
     init_beacon();
@@ -163,7 +163,7 @@ void Rover::init_ardupilot()
     // flag that initialisation has completed
     initialised = true;
 
-    underwater_sonar.init(serial_manager);
+    // underwater_sonar.init(serial_manager);
 #if AP_PARAM_KEY_DUMP
     AP_Param::show_all(hal.console, true);
 #endif

APMrover2/version.cpp

@@ -29,7 +29,7 @@ const AP_FWVersion AP_FWVersion::fwver{
     .fw_hash_str = "",
 #else
     // .fw_string = THISFIRMWARE " (" GIT_VERSION ")",
-    .fw_string = "Version: v4.0.2 ,Board ID:BD202103001",
+    .fw_string = "Version: v4.0.3 ,Board ID:BD202103001",
     .fw_hash_str = GIT_VERSION,
 #endif
     .middleware_name = nullptr,

APMrover2/version.h

@@ -6,12 +6,21 @@
 
 #include "ap_version.h"
 
-#define THISFIRMWARE "ZR_Boat V4.0.2"
+#define THISFIRMWARE "ZR_Boat V4.0.3 RC3"
 
 // the following line is parsed by the autotest scripts
-#define FIRMWARE_VERSION 4,0,1,FIRMWARE_VERSION_TYPE_DEV
+#define FIRMWARE_VERSION 4,0,3,FIRMWARE_VERSION_TYPE_DEV
 
 #define FW_MAJOR 4
 #define FW_MINOR 0
-#define FW_PATCH 0
+#define FW_PATCH 3
 #define FW_TYPE FIRMWARE_VERSION_TYPE_DEV
+
+/**
+ * @brief 更新说明
+ * 
+ * 4.0.3  去除声呐串口读取，使用uavcan，增加8、9通道控制relay 1、2
+ * 4.0.3 rc2 uavcan避障调整
+ *       rc3 增加NMEA声呐
+ * 
+ */

airbotf4.sh

@@ -0,0 +1,3 @@
+./waf configure --board airbotf4
+./waf rover
+cp build/airbotf4/bin/ardurover.apj /mnt/f/_data/other/airbotf407.px4

antracker.sh

@@ -0,0 +1,3 @@
+./waf configure --board Pixracer
+./waf antennatracker
+cp build/Pixracer/bin/antennatracker.apj /mnt/f/_01-work/100=data/固件/other/pixracer_ant.px4

bpx4.sh

@@ -0,0 +1,3 @@
+./waf configure --board pixhawk4
+./waf rover
+cp build/Pixhawk4/bin/ardurover.apj /mnt/f/_01-work/100=data/固件/zr-v4/wsl/px4rover2.px4

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -514,7 +514,47 @@ AP_AHRS_DCM::drift_correction_yaw(void)
             // don't suddenly change yaw with a reset
             _gps_last_update = _gps.last_fix_time_ms();
         }
-    } else if (_flags.fly_forward && have_gps()) {
+    } 
+    else if (_gps.gps_heading_enable()  && have_gps()) {
+        /*
+          we are using GPS for yaw
+         */
+        if (_gps.last_fix_time_ms() != _gps_last_update ) {
+            yaw_deltat = (_gps.last_fix_time_ms() - _gps_last_update) * 1.0e-3f;
+            _gps_last_update = _gps.last_fix_time_ms();
+            new_value = true;
+            const float gps_course_rad = ToRad(_gps.gps_heading() * 0.01f);
+            const float yaw_error_rad = wrap_PI(gps_course_rad - yaw);
+            yaw_error = sinf(yaw_error_rad);
+
+            /* reset yaw to match GPS heading under any of the
+               following 3 conditions:
+
+               1) if we have reached GPS_SPEED_MIN and have never had
+               yaw information before
+
+               2) if the last time we got yaw information from the GPS
+               is more than 20 seconds ago, which means we may have
+               suffered from considerable gyro drift
+
+               3) if we are over 3*GPS_SPEED_MIN (which means 9m/s)
+               and our yaw error is over 60 degrees, which means very
+               poor yaw. This can happen on bungee launch when the
+               operator pulls back the plane rapidly enough then on
+               release the GPS heading changes very rapidly
+            */
+            if (!_flags.have_initial_yaw ||
+                    yaw_deltat > 20 ||
+                    (_gps.ground_speed() >= 3*GPS_SPEED_MIN && fabsf(yaw_error_rad) >= 1.047f)) {
+                // reset DCM matrix based on current yaw
+                _dcm_matrix.from_euler(roll, pitch, gps_course_rad);
+                _omega_yaw_P.zero();
+                _flags.have_initial_yaw = true;
+                yaw_error = 0;
+            }
+        }
+    }
+    else if (_flags.fly_forward && have_gps()) {
         /*
           we are using GPS for yaw
          */

libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@@ -177,7 +177,10 @@ void AP_AHRS_NavEKF::update_EKF2(void)
             EKF2.getEulerAngles(-1,eulers);
             roll  = eulers.x;
             pitch = eulers.y;
-            yaw   = eulers.z;
+            // yaw   = eulers.z;  // @Brown
+            if (AP_AHRS_DCM::use_compass()){
+                yaw = eulers.z;   
+            }
 
             update_cd_values();
             update_trig();

libraries/AP_Compass/AP_Compass.cpp

@@ -1480,7 +1480,15 @@ Compass::read(void)
     }
     uint32_t time = AP_HAL::millis();
     for (StateIndex i(0); i < COMPASS_MAX_INSTANCES; i++) {
-        _state[i].healthy = (time - _state[i].last_update_ms < 500);
+        // _state[i].healthy = (time - _state[i].last_update_ms < 500);
+        
+        if(!_alway_healthy){
+            _state[i].healthy = (time - _state[i].last_update_ms < 500);
+        }
+        else
+        {
+            _state[i].healthy = true;
+        }
     }
 #if COMPASS_LEARN_ENABLED
     if (_learn == LEARN_INFLIGHT && !learn_allocated) {
@@ -1494,8 +1502,15 @@ Compass::read(void)
     if (ret && _log_bit != (uint32_t)-1 && AP::logger().should_log(_log_bit) && !AP::ahrs().have_ekf_logging()) {
         AP::logger().Write_Compass();
     }
+    
+    if(_alway_healthy){
+        ret = true;
+    }
     return ret;
 #else
+    if(_alway_healthy){
+        return true;
+    }
     return healthy();
 #endif
 }

libraries/AP_Compass/AP_Compass.h

@@ -337,6 +337,7 @@ public:
     }
 
     uint8_t get_filter_range() const { return uint8_t(_filter_range.get()); }
+    void set_alway_healthy(bool flag) { _alway_healthy = flag;}
 
     /*
       fast compass calibration given vehicle position and yaw
@@ -587,6 +588,9 @@ private:
     ///
     void try_set_initial_location();
     bool _initial_location_set;
+
+    bool _alway_healthy;
+
 };
 
 namespace AP {

libraries/AP_GPS/AP_GPS.cpp

@@ -288,6 +288,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
     // @Range: 5.0 30.0
     // @User: Advanced
     AP_GROUPINFO("BLEND_TC", 21, AP_GPS, _blend_tc, 10.0f),
+
 #endif
 
 #if GPS_UBLOX_MOVING_BASELINE
@@ -299,6 +300,8 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
     AP_GROUPINFO("DRV_OPTIONS", 22, AP_GPS, _driver_options, 0),
 #endif
 
+    AP_GROUPINFO("HEAD_OFFSET", 23, AP_GPS, _heading_offset, 0.0f),
+    AP_GROUPINFO("HEAD_ENABLE", 24, AP_GPS, _heading_enable, 0),
     AP_GROUPEND
 };
 

libraries/AP_GPS/AP_GPS.h

@@ -185,6 +185,10 @@ public:
         int32_t  rtk_baseline_z_mm;        ///< Current baseline in ECEF z or NED down component in mm
         uint32_t rtk_accuracy;             ///< Current estimate of 3D baseline accuracy (receiver dependent, typical 0 to 9999)
         int32_t  rtk_iar_num_hypotheses;   ///< Current number of integer ambiguity hypotheses
+
+        float heading; //RTKGPS heading
+        float heading_offset; //RTKGPS heading_offset
+        uint32_t last_gps_fixed_time_ms;
     };
 
     /// Startup initialisation.
@@ -476,6 +480,35 @@ public:
         return instance>=GPS_MAX_RECEIVERS? GPS_Type::GPS_TYPE_NONE : GPS_Type(_type[instance].get());
     }
 
+
+// @Brown
+    // GPS heading in degrees
+    float gps_heading(uint8_t instance) const {
+        // return (state[instance].heading >  _heading_offset )?(state[instance].heading - _heading_offset):(state[instance].heading + 360.0 - _heading_offset  );
+        return state[instance].heading;
+    }
+
+    int32_t gps_heading() const {
+        return gps_heading(primary_instance)*100;
+    }
+    float get_heading_offset() const{
+        return _heading_offset;
+    }
+    //get gps fixed time
+    uint32_t gps_last_fixed_time(uint8_t instance) const {
+        return state[instance].last_gps_fixed_time_ms;
+    }
+
+    uint32_t gps_last_fixed_time() const {
+        return gps_last_fixed_time(primary_instance);
+    }
+    uint8_t gps_heading_enable() const {
+        return _heading_enable;
+    }
+    void set_gps_heading_enable(int8_t enable) {
+        _heading_enable = enable;
+    }
+
 protected:
 
     // configuration parameters
@@ -500,7 +533,9 @@ protected:
     AP_Int16 _driver_options;
 
     uint32_t _log_gps_bit = -1;
-
+    
+    AP_Float _heading_offset;
+    AP_Int8  _heading_enable;
 private:
     static AP_GPS *_singleton;
     HAL_Semaphore rsem;

libraries/AP_GPS/AP_GPS_NMEA.cpp

@@ -296,6 +296,16 @@ bool AP_GPS_NMEA::_term_complete()
                     _last_HDT_ms = now;
                     state.gps_yaw = wrap_360(_new_gps_yaw*0.01f);
                     state.have_gps_yaw = true;
+                    // if (state.gps_yaw >state.heading_offset )
+                    // {
+                    //     state.heading  = (float) state.gps_yaw - state.heading_offset ;
+                    // }else{
+                    //     state.heading  = (float) state.gps_yaw +360.0 - state.heading_offset ;
+                    // }
+                    state.heading  =  (state.heading >  gps.get_heading_offset() )?(state.heading - gps.get_heading_offset()):(state.heading + 360.0 - gps.get_heading_offset()  );
+                    state.last_gps_fixed_time_ms = AP_HAL::millis();
+                    state.gps_yaw = state.heading;
+                    // state.have_gps_yaw = true;
                     // remember that we are setup to provide yaw. With
                     // a NMEA GPS we can only tell if the GPS is
                     // configured to provide yaw when it first sends a

libraries/AP_HAL_ChibiOS/hwdef/zr-v4/README.md

@@ -0,0 +1,393 @@
+# Pixhawk4 Flight Controller
+
+The Pixhawk4 flight controller is sold by [Holybro](http://www.holybro.com/product/pixhawk-4)
+
+## Features
+
+ - STM32F765 microcontroller
+ - Two IMUs: ICM20689 and BMI055
+ - MS5611 SPI barometer
+ - builtin I2C IST8310 magnetometer
+ - microSD card slot
+ - 6 UARTs plus USB
+ - 14 PWM outputs
+ - Four I2C and two CAN ports
+ - External Buzzer
+ - external safety Switch
+ - voltage monitoring for servo rail and Vcc
+ - two dedicated power input ports for external power bricks
+
+## Pinout
+
+![Pixhawk4 Board](pixhawk4-pinout.jpg "Pixhawk4")
+
+## UART Mapping
+
+ - SERIAL0 -> USB
+ - SERIAL1 -> UART2 (Telem1)
+ - SERIAL2 -> UART3 (Telem2)
+ - SERIAL3 -> UART1 (GPS)
+ - SERIAL4 -> UART4 (GPS2)
+ - SERIAL5 -> UART6 (spare)
+ - SERIAL6 -> UART7 (spare, debug)
+
+The Telem1 and Telem2 ports have RTS/CTS pins, the other UARTs do not
+have RTS/CTS.
+
+The UART7 connector is labelled debug, but is available as a general
+purpose UART with ArduPilot.
+
+### TELEM1, TELEM2 ports
+
+   <table border="1" class="docutils">
+   <tbody>
+   <tr>
+   <th>Pin </th>
+   <th>Signal </th>
+   <th>Volt </th>
+   </tr>
+   <tr>
+   <td>1 (red)</td>
+   <td>VCC</td>
+   <td>+5V</td>
+   </tr>
+   <tr>
+   <td>2 (blk)</td>
+   <td>TX (OUT)</td>
+   <td>+3.3V</td>
+   </tr>
+   <tr>
+   <td>3 (blk)</td>
+   <td>RX (IN)</td>
+   <td>+3.3V</td>
+   </tr>
+   <tr>
+   <td>4 (blk)</td>
+   <td>CTS</td>
+   <td>+3.3V</td>
+   </tr>
+   <tr>
+   <td>5 (blk)</td>
+   <td>RTS</td>
+   <td>+3.3V</td>
+   </tr>
+   <tr>
+   <td>6 (blk)</td>
+   <td>GND</td>
+   <td>GND</td>
+   </tr>
+   </tbody>
+   </table>
+
+
+### GPS port
+
+   <table border="1" class="docutils">
+   <tbody>
+   <tr>
+   <th>Pin </th>
+   <th>Signal </th>
+   <th>Volt </th>
+   </tr>
+   <tr>
+   <td>1 (red)</td>
+   <td>VCC</td>
+   <td>+5V</td>
+   </tr>
+   <tr>
+   <td>2 (blk)</td>
+   <td>SERIAL3 TX (OUT)</td>
+   <td>+3.3V</td>
+   </tr>
+   <tr>
+   <td>3 (blk)</td>
+   <td>SERIAL3 RX (IN)</td>
+   <td>+3.3V</td>
+   </tr>
+   <tr>
+   <td>4 (blk)</td>
+   <td>SCL</td>
+   <td>+3.3 (pullups)</td>
+   </tr>
+   <tr>
+   <td>5 (blk)</td>
+   <td>SDA</td>
+   <td>+3.3 (pullups)</td>
+   </tr>
+   <tr>
+   <td>6 (blk)</td>
+   <td>SafetyButton</td>
+   <td>+3.3V</td>
+   </tr>
+   <tr>
+   <td>7 (blk)</td>
+   <td>SafetyLED</td>
+   <td>+3.3V</td>
+   </tr>
+   <tr>
+   <td>8 (blk)</td>
+   <td>VDD 3.3 (OUT)</td>
+   <td>+3.3V</td>
+   </tr>
+   <tr>
+   <td>9 (blk)</td>
+   <td>Buzzer</td>
+   <td>+3.3V</td>
+   </tr>
+   <tr>
+   <td>10 (blk)</td>
+   <td>GND</td>
+   <td>GND</td>
+   </tr>
+   </tbody>
+   </table>
+
+## RC Input
+ 
+RC input is configured on the port marked DSM/SBUS RC. This connector
+supports all RC protocols. Two cables are available for this port. To
+use software binding of Spektrum satellite receivers you need to use
+the Spektrum satellite cable.
+
+## PWM Output
+
+The Pixhawk4 supports up to 16 PWM outputs. First first 8 outputs (labelled
+"MAIN") are controlled by a dedicated STM32F100 IO controller. These 8
+outputs support all PWM output formats, but not DShot.
+
+The remaining 8 outputs (labelled AUX1 to AUX8) are the "auxillary"
+outputs. These are directly attached to the STM32F765 and support all
+PWM protocols. The first 6 of the auxillary PWM outputs support DShot.
+
+The 8 main PWM outputs are in 3 groups:
+
+ - PWM 1 and 2 in group1
+ - PWM 3 and 4 in group2
+ - PWM 5, 6, 7 and 8 in group3
+
+The 8 auxillary PWM outputs are in 2 groups:
+
+ - PWM 1, 2, 3 and 4 in group1
+ - PWM 5 and 6 in group2
+ - PWM 7 and 8 in group3
+
+Channels within the same group need to use the same output rate. If
+any channel in a group uses DShot then all channels in the group need
+to use DShot.
+
+## Battery Monitoring
+
+The board has two dedicated power monitor ports on 6 pin
+connectors. The correct battery setting parameters are dependent on
+the type of power brick which is connected.
+
+## Compass
+
+The Pixhawk4 has a builtin IST8310 compass. Due to potential
+interference the board is usually used with an external I2C compass as
+part of a GPS/Compass combination.
+
+## GPIOs
+
+The 6 PWM ports can be used as GPIOs (relays, buttons, RPM etc). To
+use them you need to limit the number of these pins that is used for
+PWM by setting the BRD_PWM_COUNT to a number less than 6. For example
+if you set BRD_PWM_COUNT to 4 then PWM5 and PWM6 will be available for
+use as GPIOs.
+
+The numbering of the GPIOs for PIN variables in ArduPilot is:
+
+ - AUX1 50
+ - AUX2 51
+ - AUX3 52
+ - AUX4 53
+ - AUX5 54
+ - AUX6 55
+
+## Analog inputs
+
+The Pixhawk4 has 7 analog inputs
+
+ - ADC Pin0 -> Battery Voltage
+ - ADC Pin1 -> Battery Current Sensor
+ - ADC Pin2 -> Battery Voltage 2
+ - ADC Pin3 -> Battery Current Sensor 2
+ - ADC Pin4 -> ADC port pin 2
+ - ADC Pin14 -> ADC port pin 3
+ - ADC Pin10 -> ADC 5V Sense
+ - ADC Pin11 -> ADC 3.3V Sense
+ - ADC Pin103 -> RSSI voltage monitoring
+
+## I2C Buses
+
+ - the internal I2C port is bus 0 in ArduPilot (I2C3 in hardware)
+ - the port labelled I2CA is bus 3 in ArduPilot (I2C4 in hardware)
+ - the port labelled I2CB is bus 2 in ArduPilot (I2c2 in hardware)
+ - the port labelled GPS is bus 1 in ArduPilot (I2c1 in hardware)
+
+### Pinout for I2CA
+
+   <table border="1" class="docutils">
+   <tbody>
+   <tr>
+   <th>Pin</th>
+   <th>Signal</th>
+   <th>Volt</th>
+   </tr>
+   <tr>
+   <td>1 (red)</td>
+   <td>VCC</td>
+   <td>+5V</td>
+   </tr>
+   <tr>
+   <td>2 (blk)</td>
+   <td>SCL</td>
+   <td>+3.3 (pullups)</td>
+   </tr>
+   <tr>
+   <td>3 (blk)</td>
+   <td>SDA</td>
+   <td>+3.3 (pullups)</td>
+   </tr>
+   <tr>
+   <td>4 (blk)</td>
+   <td>GND</td>
+   <td>GND</td>
+   </tr>
+   </tbody>
+   </table>
+
+### Pinout for I2CB+UART
+
+   <table border="1" class="docutils">
+   <tbody>
+   <tr>
+   <th>Pin</th>
+   <th>Signal</th>
+   <th>Volt</th>
+   </tr>
+   <tr>
+   <td>1 (red)</td>
+   <td>VCC</td>
+   <td>+5V</td>
+   </tr>
+   <tr>
+   <td>2 (blk)</td>
+   <td>SERIAL4 TX (OUT)</td>
+   <td>+3.3V</td>
+   </tr>
+   <tr>
+   <td>3 (blk)</td>
+   <td>SERIAL4 RX (IN)</td>
+   <td>+3.3V</td>
+   </tr>
+   <tr>
+   <td>4 (blk)</td>
+   <td>SCL</td>
+   <td>+3.3 (pullups)</td>
+   </tr>
+   <tr>
+   <td>5 (blk)</td>
+   <td>SDA</td>
+   <td>+3.3 (pullups)</td>
+   </tr>
+   <tr>
+   <td>6 (blk)</td>
+   <td>GND</td>
+   <td>GND</td>
+   </tr>
+   </tbody>
+   </table>
+
+## CAN
+
+The Pixhawk4 has two independent CAN buses, with the following pinouts.
+
+### CAN1&2
+
+   <table border="1" class="docutils">
+   <tbody>
+   <tr>
+   <th>Pin</th>
+   <th>Signal</th>
+   <th>Volt</th>
+   </tr>
+   <tr>
+   <td>1 (red)</td>
+   <td>VCC</td>
+   <td>+5V</td>
+   </tr>
+   <tr>
+   <td>2 (blk)</td>
+   <td>CAN_H</td>
+   <td>+12V</td>
+   </tr>
+   <tr>
+   <td>3 (blk)</td>
+   <td>CAN_L</td>
+   <td>+12V</td>
+   </tr>
+   <tr>
+   <td>4 (blk)</td>
+   <td>GND</td>
+   <td>GND</td>
+   </tr>
+   </tbody>
+   </table>
+
+## Debug
+
+The Pixhawk4 supports SWD debugging on the debug port
+
+   <table border="1" class="docutils">
+   <tbody>
+   <tr>
+   <th>Pin </th>
+   <th>Signal </th>
+   <th>Volt </th>
+   </tr>
+   <tr>
+   <td>1 (red)</td>
+   <td>FMU VDD 3.3</td>
+   <td>+3.3V</td>
+   </tr>
+   <tr>
+   <td>2 (blk)</td>
+   <td>UART TX Debug (OUT)</td>
+   <td>+3.3V</td>
+   </tr>
+   <tr>
+   <td>3 (blk)</td>
+   <td>UART RX Debug (IN)</td>
+   <td>+3.3V</td>
+   </tr>
+   <tr>
+   <td>4 (blk)</td>
+   <td>SWDIO</td>
+   <td>+3.3V</td>
+   </tr>
+   <tr>
+   <td>5 (blk)</td>
+   <td>SWCLK</td>
+   <td>+3.3V</td>
+   </tr>
+   <tr>
+   <td>6 (blk)</td>
+   <td>GND</td>
+   <td>GND</td>
+   </tr>
+   </tbody>
+   </table>
+
+
+## Loading Firmware
+
+The board comes pre-installed with an ArduPilot compatible bootloader,
+allowing the loading of *.apj firmware files with any ArduPilot
+compatible ground station.
+
+## Acknowledgements
+
+Thanks to
+[PX4](https://docs.px4.io/en/flight_controller/pixhawk4.html) for
+images used under the [CC-BY 4.0 license](https://creativecommons.org/licenses/by/4.0/)

libraries/AP_HAL_ChibiOS/hwdef/zr-v4/defaults.parm

@@ -0,0 +1,325 @@
+
+# Pixhawk4 does not have hw flow control on radios
+#NOTE: 2020/8/26 18:26:32 Frame : PID 1.4.4
+ACRO_BAL_PITCH 1
+ACRO_BAL_ROLL 1
+ACRO_RP_EXPO 0.3
+ACRO_RP_P 4.5
+ACRO_THR_MID 0
+ACRO_TRAINER 2
+ACRO_Y_EXPO 0
+ACRO_YAW_P 3
+ARMING_CHECK 1
+ARMING_DELAY 5
+ARMING_MIS_ITEMS 0
+ARMING_RUDDER 2
+ARMING_VPERC 90
+ATC_ACCEL_P_MAX 110000
+ATC_ACCEL_R_MAX 110000
+ATC_ACCEL_Y_MAX 27000
+ATC_ANG_LIM_TC 1
+ATC_ANG_PIT_P 4.5
+ATC_ANG_RLL_P 4.5
+ATC_ANG_YAW_P 6
+ATC_ANGLE_BOOST 1
+ATC_INPUT_TC 0.15
+ATC_RAT_PIT_D 0.0036
+ATC_RAT_PIT_FF 0
+ATC_RAT_PIT_FLTD 20
+ATC_RAT_PIT_FLTE 0
+ATC_RAT_PIT_FLTT 20
+ATC_RAT_PIT_I 0.19
+ATC_RAT_PIT_IMAX 0.5
+ATC_RAT_PIT_P 0.22
+ATC_RAT_RLL_D 0.0036
+ATC_RAT_RLL_FF 0
+ATC_RAT_RLL_FLTD 20
+ATC_RAT_RLL_FLTE 0
+ATC_RAT_RLL_FLTT 20
+ATC_RAT_RLL_I 0.19
+ATC_RAT_RLL_IMAX 0.5
+ATC_RAT_RLL_P 0.22
+ATC_RAT_YAW_D 0
+ATC_RAT_YAW_FF 0
+ATC_RAT_YAW_FLTD 0
+ATC_RAT_YAW_FLTE 2.5
+ATC_RAT_YAW_FLTT 20
+ATC_RAT_YAW_I 0.04
+ATC_RAT_YAW_IMAX 0.5
+ATC_RAT_YAW_P 0.45
+ATC_RATE_FF_ENAB 1
+ATC_RATE_P_MAX 0
+ATC_RATE_R_MAX 0
+ATC_RATE_Y_MAX 0
+ATC_SLEW_YAW 6000
+ATC_THR_MIX_MAN 0.1
+ATC_THR_MIX_MAX 0.5
+ATC_THR_MIX_MIN 0.1
+AVOID_ANGLE_MAX 1000
+AVOID_BEHAVE 0
+AVOID_DIST_MAX 1
+AVOID_ENABLE 0
+AVOID_MARGIN 7
+BATT_AMP_OFFSET 0
+BATT_AMP_PERVLT 20.7
+BATT_ARM_MAH 0
+BATT_ARM_VOLT 0
+BATT_CAPACITY 16000
+BATT_CRT_MAH 0
+BATT_CRT_VOLT 0
+BATT_CURR_PIN 1
+BATT_FS_CRT_ACT 0
+BATT_FS_LOW_ACT 2
+BATT_FS_VOLTSRC 0
+BATT_LOW_MAH 0
+BATT_LOW_VOLT 20.5
+BATT_LOW_TIMER 10
+BATT_MONITOR 4
+BATT_SERIAL_NUM -1
+BATT_VOLT_PIN 0
+BATT2_MONITOR 13
+BRD_PWM_COUNT 0
+BRD_RTC_TYPES 1
+BRD_RTC_TZ_MIN 0
+BRD_SAFETYENABLE 0
+BRD_SAFETYOPTION 3
+BRD_SBUS_OUT 0
+BRD_SD_SLOWDOWN 0
+BRD_SER1_RTSCTS 0
+BRD_SER2_RTSCTS 0
+BRD_SERIAL_NUM 0
+BRD_TYPE 24
+BRD_VBUS_MIN 4.3
+BRD_VSERVO_MIN 0
+BTN_ENABLE 0
+CAM_AUTO_ONLY 1
+CAM_DURATION 3
+CAM_FEEDBACK_PIN -1
+CAM_FEEDBACK_POL 0
+CAM_MAX_ROLL 0
+CAM_MIN_INTERVAL 0
+CAM_RELAY_ON 1
+CAM_SERVO_OFF 1100
+CAM_SERVO_ON 1900
+CAM_TRIGG_DIST 0
+CAM_TRIGG_TYPE 2
+CAM_TYPE 0
+CAN_D1_PROTOCOL 1
+CAN_D1_UC_ESC_BM 0
+CAN_D1_UC_NODE 10
+CAN_D1_UC_SRV_BM 0
+CAN_D1_UC_SRV_RT 50
+CAN_D2_PROTOCOL 1
+CAN_P1_BITRATE 1000000
+CAN_P1_DRIVER 1
+CAN_P2_BITRATE 1000000
+CAN_P2_DRIVER 1
+CAN_SLCAN_CPORT 0
+CAN_SLCAN_SERNUM -1
+CAN_SLCAN_TIMOUT 0
+CHUTE_ENABLED 0
+CIRCLE_RADIUS 1000
+CIRCLE_RATE 20
+COMPASS_USE 1
+COMPASS_USE2 0
+COMPASS_USE3 0
+DEV_OPTIONS 0
+DISARM_DELAY 2
+EK2_MAG_I_GATE 800
+EK2_YAW_I_GATE 800
+FLTMODE_CH 5
+FLTMODE1 2
+FLTMODE2 2
+FLTMODE3 5
+FLTMODE4 5
+FLTMODE5 5
+FLTMODE6 5
+FRAME_CLASS 1
+FRAME_TYPE 1
+FS_CRASH_CHECK 1
+FS_EKF_ACTION 1
+FS_EKF_THRESH 0.8
+FS_GCS_ENABLE 0
+FS_OPTIONS 0
+FS_THR_ENABLE 0
+FS_THR_VALUE 975
+FS_VIBE_ENABLE 1
+GND_EFFECT_COMP 1
+GND_EXT_BUS -1
+GND_FLTR_RNG 0
+GND_PRIMARY 0
+GND_PROBE_EXT 0
+GND_TEMP 0
+GPS_AUTO_CONFIG 1
+GPS_AUTO_SWITCH 1
+GPS_BLEND_MASK 5
+GPS_BLEND_TC 10
+GPS_DELAY_MS 0
+GPS_DELAY_MS2 0
+GPS_GNSS_MODE 0
+GPS_GNSS_MODE2 0
+GPS_HDOP_GOOD 90
+GPS_INJECT_TO 127
+GPS_MIN_DGPS 100
+GPS_MIN_ELEV -100
+GPS_NAVFILTER 8
+GPS_NUM_SW 5
+GPS_POS1_X 0
+GPS_POS1_Y 0
+GPS_POS1_Z 0
+GPS_POS2_X 0
+GPS_POS2_Y 0
+GPS_POS2_Z 0
+GPS_RATE_MS 200
+GPS_RATE_MS2 200
+GPS_RAW_DATA 0
+GPS_SAVE_CFG 2
+GPS_SBAS_MODE 2
+GPS_SBP_LOGMASK -256
+GPS_TYPE 1
+GPS_TYPE2 5
+LAND_ALT_LOW 5000
+LAND_LOCK_ALT 40
+LAND_LOCK_RPY 1
+LAND_REPOSITION 1
+LAND_SL_2ND_ALT 1500
+LAND_SL_3ND_ALT 3000
+LAND_SPEED 150
+LAND_SPEED_2ND 50
+LAND_SPEED_3ND 100
+LAND_SPEED_HIGH 0
+MOT_BAT_CURR_MAX 0
+MOT_BAT_CURR_TC 5
+MOT_BAT_IDX 0
+MOT_BAT_VOLT_MAX 0
+MOT_BAT_VOLT_MIN 0
+MOT_BOOST_SCALE 0
+MOT_HOVER_LEARN 2
+MOT_PWM_MAX 0
+MOT_PWM_MIN 0
+MOT_PWM_TYPE 0
+MOT_SAFE_DISARM 0
+MOT_SAFE_TIME 1
+MOT_SLEW_DN_TIME 0
+MOT_SLEW_UP_TIME 0
+MOT_SPIN_ARM 0.1
+MOT_SPIN_MAX 0.95
+MOT_SPIN_MIN 0.15
+MOT_SPOOL_TIME 0.5
+MOT_THST_EXPO 0.65
+MOT_THST_HOVER 0.35
+MOT_YAW_HEADROOM 200
+NTF_BUZZ_ENABLE 1
+NTF_BUZZ_ON_LVL 1
+NTF_BUZZ_PIN 0
+NTF_BUZZ_VOLUME 100
+NTF_DISPLAY_TYPE 0
+NTF_LED_BRIGHT 3
+NTF_LED_OVERRIDE 4
+NTF_LED_TYPES 38
+NTF_OREO_THEME 0
+OA_TYPE 0
+PHLD_BRAKE_ANGLE 3000
+PHLD_BRAKE_RATE 8
+PILOT_ACCEL_Z 250
+PILOT_DN2_ALT 100
+PILOT_SPEED_DN 250
+PILOT_SPEED_DN2 100
+PILOT_SPEED_UP 250
+PILOT_THR_BHV 0
+PILOT_THR_FILT 0
+PILOT_TKOFF_ALT 0
+PLND_ENABLED 0
+PRX_TYPE 4
+PRX_YAW_CORR 0
+RALLY_INCL_HOME 1
+RALLY_LIMIT_KM 0.3
+RALLY_TOTAL 0
+RC7_OPTION 9
+RC8_OPTION 0
+RC9_OPTION 4
+RELAY_PIN 50
+RELAY_PIN2 53
+RELAY_PIN3 52
+RELAY_PIN4 54
+RELAY_PIN5 -1
+RELAY_PIN6 -1
+RNGFND_GAIN 0.8
+RNGFND1_ADDR 11
+RNGFND1_FUNCTION 0
+RNGFND1_GNDCLEAR 10
+RNGFND1_MAX_CM 500
+RNGFND1_MIN_CM 20
+RNGFND1_OFFSET 0
+RNGFND1_ORIENT 25
+RNGFND1_PIN -1
+RNGFND1_POS_X 0
+RNGFND1_POS_Y 0
+RNGFND1_POS_Z 0
+RNGFND1_PWRRNG 0
+RNGFND1_RMETRIC 1
+RNGFND1_SCALING 3
+RNGFND1_STOP_PIN -1
+RNGFND1_TYPE 24
+RNGFND2_ADDR 1
+RNGFND2_FUNCTION 0
+RNGFND2_GNDCLEAR 10
+RNGFND2_MAX_CM 3500
+RNGFND2_MIN_CM 50
+RNGFND2_OFFSET 0
+RNGFND2_ORIENT 0
+RNGFND2_PIN -1
+RNGFND2_POS_X 0
+RNGFND2_POS_Y 0
+RNGFND2_POS_Z 0
+RNGFND2_PWRRNG 0
+RNGFND2_RMETRIC 1
+RNGFND2_SCALING 3
+RNGFND2_STOP_PIN -1
+RNGFND2_TYPE 24
+RTL_ALT_FINAL 1500
+RTL_CLIMB_MIN 0
+RTL_CONE_SLOPE 3
+RTL_LOIT_TIME 5000
+RTL_SPEED 0
+SCHED_DEBUG 0
+SCHED_LOOP_RATE 400
+SCR_ENABLE 0
+SERIAL_PASS1 0
+SERIAL_PASS2 -1
+SERIAL_PASSTIMO 15
+SERIAL0_BAUD 115
+SERIAL0_PROTOCOL 2
+SERIAL1_BAUD 57
+SERIAL1_OPTIONS 0
+SERIAL1_PROTOCOL 1
+SERIAL2_BAUD 115
+SERIAL2_OPTIONS 0
+SERIAL2_PROTOCOL 24
+SERIAL3_BAUD 38
+SERIAL3_OPTIONS 0
+SERIAL3_PROTOCOL 5
+SERIAL4_BAUD 115
+SERIAL4_OPTIONS 0
+SERIAL4_PROTOCOL 5
+SERIAL5_BAUD 57
+SERIAL5_OPTIONS 0
+SERIAL5_PROTOCOL -1
+SERIAL6_BAUD 57
+SERIAL6_OPTIONS 0
+SERIAL6_PROTOCOL -1
+SERIAL7_BAUD 115200
+SERIAL7_OPTIONS 0
+SERIAL7_PROTOCOL 2
+WP_YAW_BEHAVIOR 1
+WPNAV_ACCEL 100
+WPNAV_ACCEL_Z 100
+WPNAV_FAST_WP 0
+WPNAV_RADIUS 200
+WPNAV_RFND_USE 1
+WPNAV_SPEED 800
+WPNAV_SPEED_DN 150
+WPNAV_SPEED_UP 350
+ZR_RTL_DELAY 15
+ZR_TK_DELAY 5
+ZR_TK_REQ_ALT 600

libraries/AP_HAL_ChibiOS/hwdef/zr-v4/hwdef-bl.dat

@@ -0,0 +1,4 @@
+# hw definition file for processing by chibios_hwdef.py
+# for Pixhawk4 bootloader
+
+include ../fmuv5/hwdef-bl.dat

libraries/AP_HAL_ChibiOS/hwdef/zr-v4/hwdef.dat

@@ -0,0 +1,11 @@
+# hw definition file for processing by chibios_hwdef.py
+# for Holybro Pixhawk4 hardware.
+
+include ../fmuv5/hwdef.dat
+
+# setup for supplied power brick
+undef HAL_BATT_VOLT_SCALE
+define HAL_BATT_VOLT_SCALE 18.182
+undef HAL_BATT_CURR_SCALE
+define HAL_BATT_CURR_SCALE 36.364
+

libraries/AP_Logger/LogFile.cpp

@@ -594,26 +594,30 @@ void AP_Logger::Write_CameraInfo(enum LogMessages msg, const Location &current_l
 
 void AP_Logger::Write_Underwater_Sonar(const Location &current_loc)
 {
-    int32_t altitude_gps;
-    const AP_GPS &gps = AP::gps();
-    if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
-        altitude_gps = gps.location().alt;
-    } else {
-        altitude_gps = 0;
+    
+    const RangeFinder *rangefinder = RangeFinder::get_singleton();
+    AP_Baro &baro = AP::baro();
+    // only log depth on boats with working downward facing range finders
+    if ( !rangefinder->has_data_orient(ROTATION_PITCH_270)) {
+        return;
     }
+    float deepthR = (float)(rangefinder->distance_cm_orient(ROTATION_PITCH_270) * 0.01f);
+    
+    const AP_GPS &gps = AP::gps();
 
-    const Underwater_Sonar sonar = AP::underwater_sonar();
     struct log_Underwater_Sonar pkt{
         LOG_PACKET_HEADER_INIT(LOG_UNDERWATER_SONAR_MSG),
         time_us         : AP_HAL::micros64(),
         latitude    : current_loc.lat,
         longitude   : current_loc.lng,
-        altitude    : altitude_gps,
+        altitude    : current_loc.alt,
+        temperature :   baro.get_temperature(0),
+        deepth      :   deepthR,
+        gps_lat:gps.location().lat,
+        gps_lng:gps.location().lng,
+        gps_alt:gps.location().alt,
     };
 
-        for (uint8_t i = 0; i < 8; i++) {
-            pkt.radar_data[i] = sonar.radar_data[i] ;
-        }
     WriteBlock(&pkt, sizeof(pkt));
 }
 

libraries/AP_Logger/LogStructure.h

@@ -732,12 +732,17 @@ struct PACKED log_Camera {
 
 
 struct PACKED log_Underwater_Sonar {
-    LOG_PACKET_HEADER;
-    uint64_t time_us;
-    int32_t  latitude;
-    int32_t  longitude;
-    int32_t  altitude;
-    float radar_data[8];
+  LOG_PACKET_HEADER;
+  uint64_t time_us;
+  int32_t latitude;
+  int32_t longitude;
+  int32_t altitude;
+  // float radar_data[8];
+  float temperature;
+  float deepth;
+  int32_t gps_lat;
+  int32_t gps_lng;
+  int32_t gps_alt;
 };
 
 struct PACKED log_Attitude {
@@ -1431,7 +1436,7 @@ struct PACKED log_Arm_Disarm {
     { LOG_OA_DIJKSTRA_MSG, sizeof(log_OADijkstra), \
       "OADJ","QBBBBLLLL","TimeUS,State,Err,CurrPoint,TotPoints,DLat,DLng,OALat,OALng", "sbbbbDUDU", "F----GGGG" }, \
     { LOG_UNDERWATER_SONAR_MSG, sizeof(log_Underwater_Sonar), \
-      "UDSR", "QLLeffffffff","TimeUS,Lat,Lng,Alt,C1,C2,C3,C4,C5,C6,C7,C8", "s--DUm--------", "F-----------" }
+      "UDSR", "QLLeffLLe","TimeUS,Lat,Lng,Alt,Temp,Deep,GLat,GLng,GAlt", "sDUm--DUm", "FGGB--GGB" }
     
 // messages for more advanced boards
 #define LOG_EXTRA_STRUCTURES \

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -569,7 +569,22 @@ void NavEKF2_core::UpdateFilter(bool predict)
 
         // Update states using  magnetometer data
         SelectMagFusion();
+        
+        static uint8_t first_in = 1;
+        if(!use_compass() ){
+            if(first_in){
+                recordYawReset(); // @Brown
+                first_in = 0;
+                // GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "[INFO]:EKF2 IMU%u yaw aligned to GPS ",(unsigned)imu_index);
+                // gcs().send_text(MAV_SEVERITY_INFO, "[INFO]:EKF2 IMU yaw aligned to GPS ");
+            }
+        }else if(first_in == 0)
+        {
+            first_in = 1;
+            // GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "[INFO]:EKF2 IMU%u yaw aligned to Compass ",(unsigned)imu_index);
+                // gcs().send_text(MAV_SEVERITY_INFO, "[INFO]:EKF2 IMU yaw aligned to Compass ");
 
+        }
         // Update states using GPS and altimeter data
         SelectVelPosFusion();
 

libraries/AP_Proximity/AP_Proximity.cpp

@@ -215,13 +215,15 @@ void AP_Proximity::init(void)
 // update Proximity state for all instances. This should be called at a high rate by the main loop
 void AP_Proximity::update(void)
 {
+    uint8_t cnt = 0;
     for (uint8_t i=0; i<num_instances; i++) {
         if (!valid_instance(i)) {
             continue;
         }
+        cnt += 1;
         drivers[i]->update();
     }
-
+    prx_init_cnt = cnt;
     // work out primary instance - first sensor returning good data
     for (int8_t i=num_instances-1; i>=0; i--) {
         if (drivers[i] != nullptr && (state[i].status == Status::Good)) {
@@ -230,6 +232,29 @@ void AP_Proximity::update(void)
     }
 }
 
+#include <GCS_MAVLink/GCS.h>
+bool AP_Proximity::re_init()
+{
+    bool ret = (prx_init_cnt != 0);
+    static uint32_t last_5s;
+    uint32_t tnow = AP_HAL::millis();
+    
+    if(!ret){
+        if (tnow - last_5s > 5000) {
+            last_5s = tnow;
+            init();
+            gcs().send_text(MAV_SEVERITY_NOTICE, "init [%d] prx: %ld",num_instances,tnow);
+        }
+        
+    }
+    if (tnow - last_5s > 5000) {
+            last_5s = tnow;
+            // init();
+            gcs().send_text(MAV_SEVERITY_NOTICE, " prx[%d] : %ld, %d",num_instances,tnow,ret);
+        }
+    return ret;
+}
+
 // return sensor orientation
 uint8_t AP_Proximity::get_orientation(uint8_t instance) const
 {
@@ -328,7 +353,12 @@ void AP_Proximity::detect_instance(uint8_t instance)
             return;
         }
         break;
-
+#if HAL_WITH_UAVCAN
+ case Type::UAVCAN:
+        state[instance].instance = instance;
+        drivers[instance] = AP_Proximity_UAVCAN::probe(*this, state[instance]);
+        return;
+#endif
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     case Type::SITL:
         state[instance].instance = instance;
@@ -345,10 +375,6 @@ void AP_Proximity::detect_instance(uint8_t instance)
         drivers[instance] = new AP_Proximity_AirSimSITL(*this, state[instance]);
         return;
 #endif
-    case Type::UAVCAN:
-        state[instance].instance = instance;
-        drivers[instance] = AP_Proximity_UAVCAN::probe(*this, state[instance]);
-        return;
     }
 }
 

libraries/AP_Proximity/AP_Proximity.h

@@ -136,6 +136,7 @@ public:
     bool sensor_enabled() const;
     bool sensor_failed() const;
 
+    bool re_init();
 private:
     static AP_Proximity *_singleton;
     Proximity_State state[PROXIMITY_MAX_INSTANCES];
@@ -158,6 +159,8 @@ private:
     AP_Int8 _ignore_width_deg[PROXIMITY_MAX_IGNORE];    // width of beam (in degrees) that should be ignored
 
     void detect_instance(uint8_t instance);
+    // bool prx_init;
+    uint8_t prx_init_cnt;
 };
 
 namespace AP {

libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp

@@ -8,6 +8,13 @@
 #include <AP_UAVCAN/AP_UAVCAN.h>
 
 #include <zrzk/equipment/range_sensor/Proximity.hpp>
+#define PRX_DEBUG 1
+#if PRX_DEBUG
+#include <GCS_MAVLink/GCS.h>
+    # define Prx_Debug(fmt, args ...)  gcs().send_text(MAV_SEVERITY_INFO, "prx-%d: " fmt "", __LINE__, ## args)
+#else
+    # define Prx_Debug(fmt, args ...)
+#endif
 
 extern const AP_HAL::HAL &hal;
 
@@ -16,16 +23,24 @@ extern const AP_HAL::HAL &hal;
     {                                                                      \
         if ((level_debug) <= AP::can().get_debug_level_driver(can_driver)) \
         {                                                                  \
-            printf(fmt, ##args);                                           \
+            hal.console->printf(fmt, ##args);                              \
         }                                                                  \
     } while (0)
 
 HAL_Semaphore AP_Proximity_UAVCAN::_sem_registry;
 UC_REGISTRY_BINDER(ProximityCb, zrzk::equipment::range_sensor::Proximity);
+
+uint8_t AP_Proximity_UAVCAN::_node_id = 0;
+AP_Proximity_UAVCAN* AP_Proximity_UAVCAN::_driver = nullptr;
+AP_UAVCAN* AP_Proximity_UAVCAN::_ap_uavcan = nullptr;
+
 AP_Proximity_UAVCAN::DetectedModules AP_Proximity_UAVCAN::_detected_modules[] = {0};
 
-AP_Proximity_UAVCAN::AP_Proximity_UAVCAN(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state) : AP_Proximity_Backend(_frontend, _state)
+AP_Proximity_UAVCAN::AP_Proximity_UAVCAN(AP_Proximity &_frontend,
+                                         AP_Proximity::Proximity_State &_state) : 
+                                         AP_Proximity_Backend(_frontend, _state)
 {
+    _driver = this;
 }
 
 void AP_Proximity_UAVCAN::subscribe_msgs(AP_UAVCAN *ap_uavcan)
@@ -54,7 +69,16 @@ AP_Proximity_Backend *AP_Proximity_UAVCAN::probe(AP_Proximity &_frontend,
     WITH_SEMAPHORE(_sem_registry);
 
     AP_Proximity_UAVCAN *backend = nullptr;
-
+    Prx_Debug("---------- AP_Proximity_UAVCAN ----------");
+    backend = new AP_Proximity_UAVCAN(_frontend, _state);
+    if (backend == nullptr)
+    {
+        Prx_Debug("backend:%d",(int)backend);
+    }else{
+        
+        Prx_Debug("faild,backend");
+    }
+    /*
     for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++)
     {
         if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_uavcan != nullptr)
@@ -62,6 +86,11 @@ AP_Proximity_Backend *AP_Proximity_UAVCAN::probe(AP_Proximity &_frontend,
             backend = new AP_Proximity_UAVCAN(_frontend, _state);
             if (backend == nullptr)
             {
+                Prx_Debug("------index:%d, Failed register Node %d on Bus %d\n",
+                                        _detected_modules[i].ap_uavcan->get_driver_index(),
+                                        _detected_modules[i].node_id,
+                                       _detected_modules[i].ap_uavcan->get_driver_index());
+
                 debug_proximity_uavcan(2,
                                        _detected_modules[i].ap_uavcan->get_driver_index(),
                                        "Failed register UAVCAN Proximity Node %d on Bus %d\n",
@@ -70,6 +99,11 @@ AP_Proximity_Backend *AP_Proximity_UAVCAN::probe(AP_Proximity &_frontend,
             }
             else
             {
+                Prx_Debug("-----index:%d, Registered Node %d on Bus %d\n",
+                                        _detected_modules[i].ap_uavcan->get_driver_index(),
+                                        _detected_modules[i].node_id,
+                                       _detected_modules[i].ap_uavcan->get_driver_index());
+
                 _detected_modules[i].driver = backend;
                 debug_proximity_uavcan(2,
                                        _detected_modules[i].ap_uavcan->get_driver_index(),
@@ -80,10 +114,12 @@ AP_Proximity_Backend *AP_Proximity_UAVCAN::probe(AP_Proximity &_frontend,
             break;
         }
     }
+    */
+    Prx_Debug("---------- AP_Proximity_UAVCAN ----------\n");
     return backend;
 }
 
-AP_Proximity_UAVCAN *AP_Proximity_UAVCAN::get_uavcan_backend(AP_UAVCAN *ap_uavcan)
+AP_Proximity_UAVCAN *AP_Proximity_UAVCAN::get_uavcan_backend(AP_UAVCAN *ap_uavcan,uint8_t node_id)
 {
     if (ap_uavcan == nullptr)
     {
@@ -93,7 +129,8 @@ AP_Proximity_UAVCAN *AP_Proximity_UAVCAN::get_uavcan_backend(AP_UAVCAN *ap_uavca
     for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++)
     {
         if (_detected_modules[i].driver != nullptr &&
-            _detected_modules[i].ap_uavcan == ap_uavcan)
+            _detected_modules[i].ap_uavcan == ap_uavcan&&
+            _detected_modules[i].node_id == node_id)    
         {
             return _detected_modules[i].driver;
         }
@@ -102,7 +139,8 @@ AP_Proximity_UAVCAN *AP_Proximity_UAVCAN::get_uavcan_backend(AP_UAVCAN *ap_uavca
     bool detected = false;
     for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++)
     {
-        if (_detected_modules[i].ap_uavcan == ap_uavcan)
+        if (_detected_modules[i].ap_uavcan == ap_uavcan &&
+            _detected_modules[i].node_id == node_id)
         {
             // detected
             detected = true;
@@ -117,6 +155,7 @@ AP_Proximity_UAVCAN *AP_Proximity_UAVCAN::get_uavcan_backend(AP_UAVCAN *ap_uavca
             if (_detected_modules[i].ap_uavcan == nullptr)
             {
                 _detected_modules[i].ap_uavcan = ap_uavcan;
+                _detected_modules[i].node_id = node_id;
                 break;
             }
         }
@@ -140,13 +179,13 @@ void AP_Proximity_UAVCAN::update(void)
     WITH_SEMAPHORE(_sem_proximity);
 
     update_sector_data(0, _interim_data.d1_cm);   // d1
-    update_sector_data(45, _interim_data.d8_cm);  // d8
-    update_sector_data(90, _interim_data.d7_cm);  // d7
-    update_sector_data(135, _interim_data.d6_cm); // d6
+    update_sector_data(45, _interim_data.d2_cm);  // d8
+    update_sector_data(90, _interim_data.d3_cm);  // d7
+    update_sector_data(135, _interim_data.d4_cm); // d6
     update_sector_data(180, _interim_data.d5_cm); // d5
-    update_sector_data(225, _interim_data.d4_cm); // d4
-    update_sector_data(270, _interim_data.d3_cm); // d3
-    update_sector_data(315, _interim_data.d2_cm); // d2
+    update_sector_data(225, _interim_data.d6_cm); // d4
+    update_sector_data(270, _interim_data.d7_cm); // d3
+    update_sector_data(315, _interim_data.d8_cm); // d2
 
     if ((_last_distance_received_ms == 0) || (AP_HAL::millis() - _last_distance_received_ms > PROXIMITY_UAVCAN_TIMEOUT_MS))
     {
@@ -161,14 +200,37 @@ void AP_Proximity_UAVCAN::update(void)
 
 void AP_Proximity_UAVCAN::handle_proximity_data_trampoline(AP_UAVCAN *ap_uavcan, uint8_t node_id, const ProximityCb &cb)
 {
-    WITH_SEMAPHORE(_sem_registry);
-
-    AP_Proximity_UAVCAN *driver = get_uavcan_backend(ap_uavcan);
+    
+    if (_driver == nullptr) {
+        return;
+    }
+    
+    if (_ap_uavcan == nullptr) {
+        _ap_uavcan = ap_uavcan;
+        _node_id = node_id;
+        Prx_Debug("---- uavcan: ap:%d,id:%d ----",(int)_ap_uavcan,_node_id);
+    }
+    
+    if (_ap_uavcan == ap_uavcan && _node_id == node_id) {
+        WITH_SEMAPHORE(_sem_registry);
+            _driver->_interim_data.d1_cm = cb.msg->d0;
+            _driver->_interim_data.d2_cm = cb.msg->d45;
+            _driver->_interim_data.d3_cm = cb.msg->d90;
+            _driver->_interim_data.d4_cm = cb.msg->d135;
+            _driver->_interim_data.d5_cm = cb.msg->d180;
+            _driver->_interim_data.d6_cm = cb.msg->d225;
+            _driver->_interim_data.d7_cm = cb.msg->d270;
+            _driver->_interim_data.d8_cm = cb.msg->d315;
+            _driver->_last_distance_received_ms = AP_HAL::millis();
+    }
+    /*
+    AP_Proximity_UAVCAN *driver = get_uavcan_backend(ap_uavcan,node_id);
     if (driver == nullptr)
     {
         return;
     }
     driver->handle_proximity_data(cb);
+    */
 }
 
 void AP_Proximity_UAVCAN::handle_proximity_data(const ProximityCb &cb)
@@ -176,39 +238,46 @@ void AP_Proximity_UAVCAN::handle_proximity_data(const ProximityCb &cb)
 
     WITH_SEMAPHORE(_sem_proximity);
 
-    _interim_data.d1_cm = handle_data_valid(cb.msg->d0);
-    _interim_data.d2_cm = handle_data_valid(cb.msg->d315);
-    _interim_data.d3_cm = handle_data_valid(cb.msg->d315);
-    _interim_data.d5_cm = handle_data_valid(cb.msg->d180);
-    _interim_data.d6_cm = handle_data_valid(cb.msg->d135);
-    _interim_data.d7_cm = handle_data_valid(cb.msg->d90);
-    _interim_data.d8_cm = handle_data_valid(cb.msg->d45);
-
+    _interim_data.d1_cm = cb.msg->d0;
+    _interim_data.d2_cm = cb.msg->d45;
+    _interim_data.d3_cm = cb.msg->d90;
+    _interim_data.d4_cm = cb.msg->d135;
+    _interim_data.d5_cm = cb.msg->d180;
+    _interim_data.d6_cm = cb.msg->d225;
+    _interim_data.d7_cm = cb.msg->d270;
+    _interim_data.d8_cm = cb.msg->d315;
     _last_distance_received_ms = AP_HAL::millis();
     // _last_sample_time_ms = AP_HAL::millis();
 }
 
-uint16_t AP_Proximity_UAVCAN::handle_data_valid(uint16_t data)
-{
-    if (data == 0xFFFF)
-    {
-        return 0;
-    }
-    else
-    {
-        return data / 10;
-    }
-}
+// uint16_t AP_Proximity_UAVCAN::handle_data_valid(uint16_t data)
+// {
+//     if (data == 0xFFFF)
+//     {
+//         return 0;
+//     }
+//     else
+//     {
+//         return data / 10;
+//     }
+// }
 
 void AP_Proximity_UAVCAN::update_sector_data(int16_t angle_deg, uint16_t distance_cm)
 {
+    
+    
     const uint8_t sector = convert_angle_to_sector(angle_deg);
-    _angle[sector] = angle_deg;
-    _distance[sector] = ((float)distance_cm) / 100;
-    _distance_valid[sector] = distance_cm != 0xffff;
-    //_last_distance_received_ms = AP_HAL::millis();
-    // update boundary used for avoidance
-    update_boundary_for_sector(sector, true);
+    // if(convert_angle_to_sector(angle_deg,sector)){
+        _angle[sector] = angle_deg;
+        _distance[sector] = ((float)distance_cm) / 1000;
+        // _distance_valid[sector] = distance_cm != 0xffff;
+        //_last_distance_received_ms = AP_HAL::millis();
+        // update boundary used for avoidance
+
+        _distance_valid[sector] = (_distance[sector] >= distance_min()) && (_distance[sector] <= distance_max());
+
+        update_boundary_for_sector(sector, true);
+    // }
 }
 
 #endif

libraries/AP_Proximity/AP_Proximity_UAVCAN.h

@@ -29,7 +29,9 @@ public:
   //AP_Proximity::Proximity_State _status;
 
 private:
-  static AP_Proximity_UAVCAN *get_uavcan_backend(AP_UAVCAN *ap_uavcan);
+  static AP_Proximity_UAVCAN *get_uavcan_backend(AP_UAVCAN *ap_uavcan,uint8_t node_id);
+  
+  static AP_Proximity_UAVCAN* _driver;
   // inline void register_sensor()
   // {
   //   //  instance = frontend.register_sensor();
@@ -61,5 +63,10 @@ private:
 
   static HAL_Semaphore _sem_registry;
   HAL_Semaphore _sem_proximity;
+  
+    static AP_UAVCAN* _ap_uavcan;
+    static uint8_t _node_id;
+
+
 };
 #endif //HAL_WITH_UAVCAN

libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp

@@ -18,6 +18,15 @@
 #include <AP_SerialManager/AP_SerialManager.h>
 #include <ctype.h>
 #include "AP_RangeFinder_NMEA.h"
+#include <GCS_MAVLink/GCS.h>
+
+#define BOAT_DEBUG 1
+#if BOAT_DEBUG
+#include <GCS_MAVLink/GCS.h>
+ # define Debug(fmt, args ...)  gcs().send_text(MAV_SEVERITY_INFO, "BOAT:%d: " fmt "\n", ## args)
+#else
+ # define Debug(fmt, args ...)
+#endif
 
 extern const AP_HAL::HAL& hal;
 
@@ -90,33 +99,34 @@ bool AP_RangeFinder_NMEA::get_reading(uint16_t &reading_cm)
 bool AP_RangeFinder_NMEA::decode(char c)
 {
     switch (c) {
-    case ',':
-        // end of a term, add to checksum
-        _checksum ^= c;
-        FALLTHROUGH;
-    case '\r':
-    case '\n':
-    case '*':
-    {
-        // null terminate and decode latest term
-        _term[_term_offset] = 0;
-        bool valid_sentence = decode_latest_term();
-
-        // move onto next term
-        _term_number++;
-        _term_offset = 0;
-        _term_is_checksum = (c == '*');
-        return valid_sentence;
-    }
+        case ',':
+            // end of a term, add to checksum
+            _checksum ^= c;
+            FALLTHROUGH;
+        case '\r':
+        case '\n':
+        case '*':
+        {
+            // null terminate and decode latest term
+            _term[_term_offset] = 0;
+            bool valid_sentence = decode_latest_term();
+
+            // move onto next term
+            _term_number++;
+            _term_offset = 0;
+            _term_is_checksum = (c == '*');
+            return valid_sentence;
+        }
 
-    case '$': // sentence begin
-        _sentence_type = SONAR_UNKNOWN;
-        _term_number = 0;
-        _term_offset = 0;
-        _checksum = 0;
-        _term_is_checksum = false;
-        _distance_m = -1.0f;
-        return false;
+        case '@': // sentence begin
+        case '$': // sentence begin
+            _sentence_type = SONAR_UNKNOWN;
+            _term_number = 0;
+            _term_offset = 0;
+            _checksum = 0;
+            _term_is_checksum = false;
+            _distance_m = -1.0f;
+            return false;
     }
 
     // ordinary characters are added to term
@@ -142,11 +152,14 @@ bool AP_RangeFinder_NMEA::decode_latest_term()
             return false;
         }
         const uint8_t checksum = (nibble_high << 4u) | nibble_low;
-        return ((checksum == _checksum) &&
-                !is_negative(_distance_m) &&
-                (_sentence_type == SONAR_DBT || _sentence_type == SONAR_DPT));
+
+        // Debug("red:%x,_checksum:%x\n",checksum,_checksum);
+        // gcs().send_text(MAV_SEVERITY_INFO, "[%d] read:%x,_checksum:%x,dist:%.2f\n",(checksum == _checksum),checksum,_checksum, _distance_m);
+        // return true;
+        return ((checksum == _checksum) && (_sentence_type == SONAR_SIC || _sentence_type == SONAR_DPT));
     }
 
+
     // the first term determines the sentence type
     if (_term_number == 0) {
         // the first two letters of the NMEA term are the talker ID.
@@ -157,17 +170,26 @@ bool AP_RangeFinder_NMEA::decode_latest_term()
             return false;
         }
         const char *term_type = &_term[2];
+        const char *term_sic = &_term[0];
         if (strcmp(term_type, "DBT") == 0) {
             _sentence_type = SONAR_DBT;
         } else if (strcmp(term_type, "DPT") == 0) {
             _sentence_type = SONAR_DPT;
+        }else if (strcmp(term_sic, "SIC") == 0) {
+            _sentence_type = SONAR_SIC;
         } else {
             _sentence_type = SONAR_UNKNOWN;
         }
         return false;
     }
+    if ((_sentence_type == SONAR_SIC )) {
 
-    if (_sentence_type == SONAR_DBT) {
+        if (_term_number == 5) {
+            _distance_m = atof(_term);
+            // Debug("dist:%.2f",_distance_m);
+            // cout << "type " << SONAR_SIC + 5 << ": " << _term << ", " << _term[1] <<", --- " << sonar_deep << endl;
+        }
+    }else if (_sentence_type == SONAR_DBT) {
         // parse DBT messages
         if (_term_number == 3) {
             _distance_m = atof(_term);
@@ -179,5 +201,6 @@ bool AP_RangeFinder_NMEA::decode_latest_term()
         }
     }
 
+    // cout << "term " << int(_term_number) << ": " << _term << endl;
     return false;
 }

libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h

@@ -45,6 +45,7 @@ private:
     enum sentence_types : uint8_t {
         SONAR_UNKNOWN = 0,
         SONAR_DBT,
+        SONAR_SIC,
         SONAR_DPT
     };
 
@@ -63,11 +64,27 @@ private:
     AP_HAL::UARTDriver *uart = nullptr;     // pointer to serial uart
 
     // message decoding related members
-    char _term[15];                         // buffer for the current term within the current sentence
-    uint8_t _term_offset;                   // offset within the _term buffer where the next character should be placed
+    // char _term[15];                         // buffer for the current term within the current sentence
+    // uint8_t _term_offset;                   // offset within the _term buffer where the next character should be placed
     uint8_t _term_number;                   // term index within the current sentence
     float _distance_m;                      // distance in meters parsed from a term, -1 if no distance
     uint8_t _checksum;                      // checksum accumulator
     bool _term_is_checksum;                 // current term is the checksum
     sentence_types _sentence_type;          // the sentence type currently being processed
+
+
+    float radar_data[8];
+
+    uint8_t     ucCRCHi;
+    uint8_t     ucCRCLo;
+    int         iIndex;
+    uint8_t     sonar_data[25];
+    char _term[10];                                                     ///< buffer for the current term within the current sentence
+    uint8_t _term_offset;                                       ///< character offset with the term being received
+    uint8_t _term_num;
+    uint32_t  data_index;
+    float _sonar_deep;
+    float _sonar_voltage;
+    uint8_t decode_step;
+
 };

libraries/GCS_MAVLink/GCS.cpp

@@ -126,7 +126,10 @@ void GCS::update_sensor_status_flags()
     if (compass.enabled() && compass.healthy() && ahrs.use_compass()) {
         control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG;
     }
-
+    const AP_GPS &gps = AP::gps();
+    if(gps.gps_heading_enable()){
+        control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG;
+    }
     const AP_Baro &barometer = AP::baro();
     control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
     control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -4311,9 +4311,9 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
     case MSG_RANGEFINDER:
         {
         CHECK_PAYLOAD_SIZE(RANGEFINDER);
-        //send_rangefinder();
-            Underwater_Sonar sonar = AP::underwater_sonar();
-            sonar.send_sonar_mavlink(chan);
+        send_rangefinder();
+            // Underwater_Sonar sonar = AP::underwater_sonar();
+            // sonar.send_sonar_mavlink(chan);
         }
         break;
 

px4v3.sh

@@ -0,0 +1,3 @@
+./waf configure --board fmuv3
+./waf  rover
+cp build/fmuv3/bin/ardurover.apj /mnt/f/_01-work/100=data/固件/zr-v4/wsl/fmuv3-rover.px4

zr-rover.sh

@@ -0,0 +1,3 @@
+./waf configure --board zr-v4
+./waf rover
+cp build/zr-v4/bin/ardurover.apj /mnt/f/_01-work/100=data/固件/zr-v4/wsl/zrrover-4.0.3-rc2.px4

zr-v4.sh

@@ -0,0 +1,3 @@
+./waf configure --board pixhawk4
+./waf rover
+cp ./build/pixhawk4/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/zr-rover4.1.1.px4

zrv4.sh

@@ -0,0 +1,3 @@
+./waf configure --board zr-v4
+./waf rover
+cp build/zr-v4/bin/ardurover.apj /mnt/f/_01-work/100=data/固件/zr-v4/wsl/zrv4rover2.px4