增加双天线定向

APMrover2/Rover.cpp

@@ -270,6 +270,7 @@ void Rover::update_logging2(void)
 
 }
 
+#define DUAL_ANT_HEADING  1
 
 /*
   once a second events
@@ -307,6 +308,40 @@ void Rover::one_second_loop(void)
 
     // gcs().send_text(MAV_SEVERITY_CRITICAL, "test");
 
+
+#if DUAL_ANT_HEADING
+    static uint8_t prt_cnt;
+    prt_cnt++;
+    if(prt_cnt > 3 && 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, "[Nova]:heading:%3.2f , heading_rad:%3.2f  ", heading, gps_heading_rad);
+    }
+#endif
+
+#if DUAL_ANT_HEADING
+
+    uint16_t rc6_in = RC_Channels::rc_channel(CH_6)->get_radio_in();
+    uint8_t ch_flag = rc6_in>1500?1:0;
+    switch (ch_flag) {
+        case 1: {
+            gps.set_gps_heading_enable(1);
+            compass.set_use_for_yaw(0,0);
+            gcs().send_text(MAV_SEVERITY_INFO, "[INFO]:Yaw Switch to GPS");
+            break;
+        }
+
+        case 0: {
+            gps.set_gps_heading_enable(0);
+            compass.set_use_for_yaw(0,1);
+            gcs().send_text(MAV_SEVERITY_INFO, "[INFO]:Yaw Switch to Compass");
+            break;
+        }
+    }
+
+#endif
+
 }
 
 void Rover::update_GPS(void)

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_GPS/AP_GPS.cpp

@@ -288,6 +288,9 @@ 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),
+
+    AP_GROUPINFO("HEAD_OFFSET", 22, AP_GPS, _heading_offset, 0.0f),
+    AP_GROUPINFO("HEAD_ENABLE", 23, AP_GPS, _heading_enable, 0),
 #endif
 
 #if GPS_UBLOX_MOVING_BASELINE

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,33 @@ 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  );
+        // state[i].heading_offset = _heading_offset;
+    }
+
+    int32_t gps_heading() const {
+        return gps_heading(primary_instance)*100;
+    }
+
+    //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 +531,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,14 @@ 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.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