From d716d9a2589974cb7ff97ff886e47734756a7a48 Mon Sep 17 00:00:00 2001 From: z Date: Sat, 1 Aug 2020 17:35:42 +0800 Subject: [PATCH] GPS change, Flow rotation --- ArduCopter/UserCode.cpp | 10 +--------- ArduCopter/mode_brake.cpp | 2 +- ArduCopter/mode_rtl.cpp | 2 +- ArduCopter/version.h | 2 +- libraries/AP_GPS/AP_GPS.cpp | 9 ++++++--- libraries/AP_GPS/AP_GPS.h | 1 + libraries/AP_GPS/AP_GPS_UBLOX.cpp | 12 ++++++++---- libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp | 9 +++++++-- zr_version | 10 ++++++++++ 9 files changed, 36 insertions(+), 21 deletions(-) create mode 100644 zr_version diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index 3f7d6c4b2a..4abfb10608 100644 --- a/ArduCopter/UserCode.cpp +++ b/ArduCopter/UserCode.cpp @@ -99,16 +99,8 @@ void Copter::userhook_SuperSlowLoop() break; } cacl_volt_pst = 100 - cnt; - battery.reset_remaining(0, cacl_volt_pst); + battery.reset_remaining(1, cacl_volt_pst); } - - // // check AHRS and GPS are within 10m of each other - // const Location gps_loc = gps.location(); - // Location ahrs_loc; - // if (AP::ahrs().get_position(ahrs_loc)) { - // const float distance = gps_loc.get_distance(ahrs_loc); - // gcs().send_text(MAV_SEVERITY_INFO, "GPS and AHRS differ by %4.1fm", (double)distance); - // } } #endif diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index 550e80dd42..e4ebef37bb 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -91,7 +91,7 @@ void ModeBrake::run() } else if(countdown<=0){ gcs().send_text(MAV_SEVERITY_INFO," 执行继续任务"); - copter.updownStatus == UpDown_ContinueClimb; + copter.updownStatus = UpDown_ContinueClimb; set_mode(Mode::Number::AUTO, ModeReason::REQUEST_CMD); } } diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 65973331bd..dbd23ffa63 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -96,7 +96,7 @@ void ModeRTL::run(bool disarm_on_land) } else if(countdown<=0){ gcs().send_text(MAV_SEVERITY_INFO,"开始着陆"); - copter.updownStatus == UpDown_ContinueDescent; + copter.updownStatus = UpDown_ContinueDescent; _state = RTL_Land; } } diff --git a/ArduCopter/version.h b/ArduCopter/version.h index 2b904e1cdf..c8cf2d296d 100644 --- a/ArduCopter/version.h +++ b/ArduCopter/version.h @@ -6,7 +6,7 @@ #include "ap_version.h" -#define THISFIRMWARE "ZRUAV v4.0.4" //"ArduCopter V4.0.0" +#define THISFIRMWARE "ZRUAV v4.0.4-rc1.0" //"ArduCopter V4.0.0" // the following line is parsed by the autotest scripts #define FIRMWARE_VERSION 4,0,4,FIRMWARE_VERSION_TYPE_OFFICIAL diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 5333cf29ab..35d92b1e96 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -282,6 +282,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("NUM_SW", 22, AP_GPS, _nunber_to_switch, 5), + #endif AP_GROUPEND @@ -782,7 +785,7 @@ void AP_GPS::update(void) for (uint8_t i=1; i state[primary_instance].status) || - ((state[i].status == state[primary_instance].status) && (state[i].num_sats > state[primary_instance].num_sats))) { + ((state[i].status == state[primary_instance].status) && (state[i].num_sats > state[primary_instance].num_sats + _nunber_to_switch))) { primary_instance = i; _last_instance_swap_ms = now; } @@ -800,11 +803,11 @@ void AP_GPS::update(void) continue; } - bool another_gps_has_1_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 1); + bool another_gps_has_1_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + _nunber_to_switch); if (state[i].status == state[primary_instance].status && another_gps_has_1_or_more_sats) { - bool another_gps_has_2_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + 2); + bool another_gps_has_2_or_more_sats = (state[i].num_sats >= state[primary_instance].num_sats + _nunber_to_switch + 2); if ((another_gps_has_1_or_more_sats && (now - _last_instance_swap_ms) >= 20000) || (another_gps_has_2_or_more_sats && (now - _last_instance_swap_ms) >= 5000)) { diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 1185e9b2bf..b01a09857b 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -458,6 +458,7 @@ protected: AP_Int16 _delay_ms[GPS_MAX_RECEIVERS]; AP_Int8 _blend_mask; AP_Float _blend_tc; + AP_Int8 _nunber_to_switch; uint32_t _log_gps_bit = -1; diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index d045179a86..43e0b4c17e 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -1024,7 +1024,8 @@ AP_GPS_UBLOX::_parse_gps(void) if (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) { if( (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) && (_buffer.status.fix_status & AP_GPS_UBLOX::NAV_STATUS_DGPS_USED)) { - next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS; + // next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS; + next_fix = AP_GPS::GPS_OK_FIX_3D; }else if( _buffer.status.fix_type == AP_GPS_UBLOX::FIX_3D) { next_fix = AP_GPS::GPS_OK_FIX_3D; }else if (_buffer.status.fix_type == AP_GPS_UBLOX::FIX_2D) { @@ -1065,7 +1066,8 @@ AP_GPS_UBLOX::_parse_gps(void) if (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) { if( (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) && (_buffer.solution.fix_status & AP_GPS_UBLOX::NAV_STATUS_DGPS_USED)) { - next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS; + // next_fix = AP_GPS::GPS_OK_FIX_3D_DGPS; + next_fix = AP_GPS::GPS_OK_FIX_3D; }else if( _buffer.solution.fix_type == AP_GPS_UBLOX::FIX_3D) { next_fix = AP_GPS::GPS_OK_FIX_3D; }else if (_buffer.solution.fix_type == AP_GPS_UBLOX::FIX_2D) { @@ -1166,8 +1168,10 @@ AP_GPS_UBLOX::_parse_gps(void) break; case 3: state.status = AP_GPS::GPS_OK_FIX_3D; - if (_buffer.pvt.flags & 0b00000010) // diffsoln - state.status = AP_GPS::GPS_OK_FIX_3D_DGPS; + if (_buffer.pvt.flags & 0b00000010){ // diffsoln + // state.status = AP_GPS::GPS_OK_FIX_3D_DGPS; + state.status = AP_GPS::GPS_OK_FIX_3D; + } if (_buffer.pvt.flags & 0b01000000) // carrsoln - float state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT; if (_buffer.pvt.flags & 0b10000000) // carrsoln - fixed diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp index bf01addbd6..6d762c4e32 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp @@ -66,8 +66,13 @@ void AP_OpticalFlow_HereFlow::handle_measurement(AP_UAVCAN* ap_uavcan, uint8_t n if (_ap_uavcan == ap_uavcan && _node_id == node_id) { WITH_SEMAPHORE(_driver->_sem); _driver->new_data = true; - _driver->flowRate = Vector2f(cb.msg->flow_integral[0], cb.msg->flow_integral[1]); - _driver->bodyRate = Vector2f(cb.msg->rate_gyro_integral[0], cb.msg->rate_gyro_integral[1]); + if(0){ + _driver->flowRate = Vector2f(cb.msg->flow_integral[0], cb.msg->flow_integral[1]); + _driver->bodyRate = Vector2f(cb.msg->rate_gyro_integral[0], cb.msg->rate_gyro_integral[1]); + }else{ + _driver->flowRate = Vector2f(-cb.msg->flow_integral[1], cb.msg->flow_integral[0]); + _driver->bodyRate = Vector2f(-cb.msg->rate_gyro_integral[1], cb.msg->rate_gyro_integral[0]); + } _driver->integral_time = cb.msg->integration_interval; _driver->surface_quality = cb.msg->quality; } diff --git a/zr_version b/zr_version new file mode 100644 index 0000000000..9af16d8c74 --- /dev/null +++ b/zr_version @@ -0,0 +1,10 @@ +zr-v4.0: 经过飞行测试，稳定版本 +zr-v4.0-dev: 新添加的功能，并入主分支前先验证 + +zr-v4.0.1: + +zr-dev-4.0.2: NMEA 时间修复，日志目录名 + +v4.0.3: 快速解锁 + +v4.0.4： 分阶段降落，光流门限，限制GPS切换 _nunber_to_switch,