diff --git a/APMrover2/Rover.cpp b/APMrover2/Rover.cpp index ba84dbd805..7348d42305 100644 --- a/APMrover2/Rover.cpp +++ b/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(); - uint8_t ch_flag = rc6_in>1500?1:0; + 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: { - gps.set_gps_heading_enable(1); - compass.set_use_for_yaw(0,0); - gcs().send_text(MAV_SEVERITY_INFO, "[INFO]:Yaw Switch to GPS"); + 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: { - gps.set_gps_heading_enable(0); - compass.set_use_for_yaw(0,1); - gcs().send_text(MAV_SEVERITY_INFO, "[INFO]:Yaw Switch to Compass"); + if(last_type != 0){ + // gcs().send_text(MAV_SEVERITY_INFO, "[INFO]:Yaw Switch auto"); + type_yaw = 0; + last_type = type_yaw; + } break; } } diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 6d745365d6..10b45d6619 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -457,6 +457,8 @@ public: // Simple mode float simple_sin_yaw; + + uint8_t type_yaw; }; extern Rover rover; diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index aa548ecd73..cfbceef94d 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/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 } diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h index 37b2f98e7f..7e2888831a 100644 --- a/libraries/AP_Compass/AP_Compass.h +++ b/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 { diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 8055e54474..d76119cc97 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/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 }; diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index ad7c87302f..ff4b41bb13 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/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 ); - // state[i].heading_offset = _heading_offset; + // 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; diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 7b3ac3fd75..f7be7da349 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/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 diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index 06f1a88f47..974f864850 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/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(); diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp index 979464fcd0..4614ecaf86 100644 --- a/libraries/GCS_MAVLink/GCS.cpp +++ b/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;