定向磁力计报错修复，offset修改

APMrover2/Rover.cpp

@@ -312,30 +312,59 @@ void Rover::one_second_loop(void)
 #if DUAL_ANT_HEADING
     static uint8_t prt_cnt;
     prt_cnt++;
-    if(prt_cnt > 3 && gps.gps_heading_enable()){
+    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, "[Nova]:heading:%3.2f , heading_rad:%3.2f  ", heading, gps_heading_rad);
+        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 rc6_in = RC_Channels::rc_channel(CH_6)->get_radio_in();
+    uint16_t rc14_in = RC_Channels::rc_channel(CH_14)->get_radio_in();
-    uint8_t ch_flag = rc6_in>1500?1:0;
+    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: {
-            gps.set_gps_heading_enable(1);
+            if(last_type != 1){
-            compass.set_use_for_yaw(0,0);
+                gps.set_gps_heading_enable(0);
-            gcs().send_text(MAV_SEVERITY_INFO, "[INFO]:Yaw Switch to GPS");
+                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: {
-            gps.set_gps_heading_enable(0);
+            if(last_type != 0){
-            compass.set_use_for_yaw(0,1);
+            // gcs().send_text(MAV_SEVERITY_INFO, "[INFO]:Yaw Switch auto");
-            gcs().send_text(MAV_SEVERITY_INFO, "[INFO]:Yaw Switch to Compass");
+                type_yaw = 0;
+                last_type = type_yaw;
+            }
             break;
         }
     }

APMrover2/Rover.h

@@ -457,6 +457,8 @@ public:
 
     // Simple mode
     float simple_sin_yaw;
+    
+    uint8_t type_yaw;
 };
 
 extern Rover rover;

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

@@ -289,8 +289,6 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
     // @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
@@ -302,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

@@ -481,17 +481,19 @@ public:
     }
 
 
-    // @Brown
+// @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 >  _heading_offset )?(state[instance].heading - _heading_offset):(state[instance].heading + 360.0 - _heading_offset  );
-        // state[i].heading_offset = _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;

libraries/AP_GPS/AP_GPS_NMEA.cpp

@@ -302,6 +302,8 @@ bool AP_GPS_NMEA::_term_complete()
                     }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.gps_yaw = state.heading;
                     // state.have_gps_yaw = true;
                     // remember that we are setup to provide yaw. With

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/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;