From 8b4204f03aaefe394e3f30f103f527800f870fe2 Mon Sep 17 00:00:00 2001 From: z Date: Mon, 13 Jul 2020 18:08:35 +0800 Subject: [PATCH] =?UTF-8?q?=E5=90=88=E5=B9=B6=E5=85=AD=E8=BD=B4=EF=BC=8C?= =?UTF-8?q?=E5=A4=87=E4=BB=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/Parameters.cpp | 6 +-- ArduCopter/UserCode.cpp | 50 +------------------ ArduCopter/events.cpp | 14 ++++++ ArduCopter/mode_rtl.cpp | 2 + ArduCopter/motors.cpp | 13 +++-- .../AP_BattMonitor_Serial_BattGo.cpp | 7 ++- libraries/AP_GPS/GPS_Backend.cpp | 2 +- .../AP_HAL_ChibiOS/hwdef/fmuv5/hwdef.dat | 22 ++++---- libraries/AP_Notify/UAVCAN_RGB_LED.cpp | 22 +++++++- libraries/AP_Notify/UAVCAN_RGB_LED.h | 5 ++ 10 files changed, 71 insertions(+), 72 deletions(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 418e8ad91f..891f53509f 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1629,11 +1629,11 @@ const char* Copter::get_sysid_board_id(void) static char buf[50]; // snprintf(buf, sizeof(buf), "Version: v3.5.6 ,Board ID: ZRZK.19QT3.%d",(int)g.sysid_board_id); - // int32_t nameValue1 =(int32_t) g.sysid_board_name_1st; + int32_t nameValue1 =(int32_t) g.sysid_board_name_1st; int32_t nameValue2 = (int32_t)g.sysid_board_name_2nd; - snprintf(buf, sizeof(buf), "Version: zr-v4.0.3 ,Board ID: ZRZK.20QT2.%d",(int)nameValue2); - // snprintf(buf, sizeof(buf), "Version: zr-v4.0.3 ,ID: RS100%04d%05d",(int)nameValue1,(int)nameValue2); + // snprintf(buf, sizeof(buf), "Version: zr-v4.0.3 ,Board ID: ZRZK.20QT2.%d",(int)nameValue2); + snprintf(buf, sizeof(buf), "Version: zr-v4.0.3 ,ID: RS100%04d%05d",(int)nameValue1,(int)nameValue2); AP::logger().Write_Message(buf); return buf; } diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index 48a87585b4..f16effdcd6 100644 --- a/ArduCopter/UserCode.cpp +++ b/ArduCopter/UserCode.cpp @@ -8,23 +8,7 @@ void Copter::userhook_init() { // put your initialisation code here // this will be called once at start-up - if (x30t != nullptr) - { - x30t->init(); - } - else - { - if (x30t == nullptr) - { - gcs().send_text(MAV_SEVERITY_INFO,"x30t is null"); - x30t = new AP_Camera_Serial_X30T(); - if (x30t == nullptr || !x30t->init()) - { - delete x30t; - x30t = nullptr; - } - } - } + } #endif @@ -46,38 +30,6 @@ void Copter::userhook_50Hz() void Copter::userhook_MediumLoop() { // put your 10Hz code here - if (x30t != nullptr) - { - int32_t lat = AP::gps().location().lat; - int32_t lng = AP::gps().location().lat; - int32_t alt = AP::gps().location().alt; - - uint8_t posdata[12]; - posdata[0] = lat & 0xff; - posdata[1] = (lat >> 8) & 0xff; - posdata[2] = (lat >> 16) & 0xff; - posdata[3] = (lat >> 24) & 0xff; - - posdata[4] = lng & 0xff; - posdata[5] = (lng >> 8) & 0xff; - posdata[6] = (lng >> 16) & 0xff; - posdata[7] = (lng >> 24) & 0xff; - - posdata[8] = alt& 0xff; - posdata[9] = (alt>> 8) & 0xff; - posdata[10] =(alt >> 16) & 0xff; - posdata[11] =(alt >> 24) & 0xff; - - x30t->sendPos(&posdata[0],X30T_MSG_TYPE_POS); - - - - - - - - } - } #endif diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index 2a56baece3..0e11c7c371 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -102,6 +102,20 @@ void Copter::handle_battery_failsafe(const char *type_str, const int8_t action) } else { gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe"); } + const AP_AHRS &_ahrs = AP::ahrs(); + float home = 0.0; + Location loc; + if (_ahrs.get_position(loc)) { + const Location &home_loc = _ahrs.get_home(); + if (home_loc.lat != 0 || home_loc.lng != 0) { + // distance between vehicle and home_loc in meters + home = home_loc.get_distance(loc); + if(home < 10) + { + desired_action = Failsafe_Action_None; + } + } + } // Battery FS options already use the Failsafe_Options enum. So use them directly. do_failsafe_action(desired_action, ModeReason::BATTERY_FAILSAFE); diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 857a3edb55..b52903deb4 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -66,6 +66,8 @@ void ModeRTL::run(bool disarm_on_land) } break; case RTL_FinalDescent: + gcs().send_text(MAV_SEVERITY_INFO,"complete:%d,_state:%d",_state_complete,_state); + set_mode(Mode::Number::BRAKE,ModeReason::MISSION_END); // do nothing break; case RTL_Land: diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 49774ca370..07f0704260 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -26,9 +26,10 @@ void Copter::arm_motors_check() } #endif + int16_t yaw_in = channel_yaw->get_control_in(); - int16_t pitch_in = channel_pitch->get_control_in(); - int16_t roll_in = channel_roll->get_control_in(); + // int16_t pitch_in = channel_pitch->get_control_in(); + // int16_t roll_in = channel_roll->get_control_in(); int16_t throttle_in = channel_throttle->get_control_in(); static const uint16_t trim_trg_value = arming.get_rudder_arm_value()*40; static const int8_t con_arm_delay = arming.get_rudder_arm_time(); @@ -45,7 +46,8 @@ void Copter::arm_motors_check() // full right // if (yaw_in > 4000) { - if(yaw_in>trim_trg_value&&pitch_in>trim_trg_value&&roll_in< -trim_trg_value){ + // if(yaw_in>trim_trg_value&&pitch_in>trim_trg_value&&roll_in< -trim_trg_value){ + if(yaw_in>trim_trg_value){ // increase the arming counter to a maximum of 1 beyond the auto trim counter if (arming_counter <= AUTO_TRIM_DELAY) { arming_counter++; @@ -69,11 +71,12 @@ void Copter::arm_motors_check() // full left and rudder disarming is enabled // } else if ((yaw_in < -4000) && (arming_rudder == AP_Arming::RudderArming::ARMDISARM)) { - } else if((yaw_in<-trim_trg_value&&pitch_in>trim_trg_value&&roll_in>trim_trg_value)&&(arming_rudder==AP_Arming::RudderArming::ARMDISARM)){ + // } else if((yaw_in<-trim_trg_value&&pitch_in>trim_trg_value&&roll_in>trim_trg_value)&&(arming_rudder==AP_Arming::RudderArming::ARMDISARM)){ + } else if((yaw_in<-trim_trg_value)&&(arming_rudder==AP_Arming::RudderArming::ARMDISARM)){ if (!flightmode->has_manual_throttle() && !ap.land_complete) { arming_counter = 0; return; - } + } // increase the counter to a maximum of 1 beyond the disarm delay // if (arming_counter <= DISARM_DELAY) { diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp index eead3bba55..0cd7dbb18d 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp @@ -235,6 +235,7 @@ void AP_BattMonitor_Serial_BattGo::read() { requestBattData(BATTGO_CMD_REQUEST_ACTUAL_DATA); uint32_t tnow = AP_HAL::micros(); + static uint32_t last_prt; if (get_reading()) { if (parseBattGo()) @@ -261,15 +262,17 @@ void AP_BattMonitor_Serial_BattGo::read() //... _state.healthy = _interim_state.healthy; }else{ - if(tnow-last_rev_batt_time>5000000){ + if(tnow-last_rev_batt_time>5000000 && tnow - last_prt > 20000000 ){ _interim_state.healthy = false; gcs().send_text(MAV_SEVERITY_INFO,"BATT_ERR:已经%d秒没有接收到智能电池 的串口数据！",(tnow-last_rev_batt_time)/1000000); + last_prt = tnow; } } }else{ - if(tnow-last_rev_batt_time>5000000){ + if(tnow-last_rev_batt_time>5000000 && tnow - last_prt > 20000000 ){ _interim_state.healthy = false; gcs().send_text(MAV_SEVERITY_INFO,"BATT_ERR:已经%d秒没有接收到智能电池 的串口数据！",(tnow-last_rev_batt_time)/1000000); + last_prt = tnow; } } } diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index c51c7d5c14..fe9770f566 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -110,7 +110,7 @@ void AP_GPS_Backend::make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds) //convert to time since Unix epoch uint32_t ret =mkgmtime(timeinfo); //GPS epoch - ret -= 315964800; //315964800 1980-1-6 - 1970 1- 1 + ret -= 315964800 + 18; //315964800 1980-1-6 - 1970 1- 1,+18s tiaomiao // // get time in seconds since unix epoch // uint32_t ret = (year/4) - (GPS_LEAPSECONDS_MILLIS / 1000UL) + 367*rmon/12 + day; diff --git a/libraries/AP_HAL_ChibiOS/hwdef/fmuv5/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/fmuv5/hwdef.dat index 08629bdceb..0f989d96a7 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/fmuv5/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/fmuv5/hwdef.dat @@ -160,20 +160,20 @@ PD0 UART4_RX UART4 NODMA PD1 UART4_TX UART4 NODMA # # USART6 is telem3 -# PG9 USART6_RX USART6 NODMA -# # we leave PG14 as an input to prevent it acting as a pullup -# # on the IOMCU SBUS input -# # PG14 USART6_TX USART6 NODMA -# PG15 USART6_CTS USART6 -# PG8 USART6_RTS USART6 - -# # USART6 is telem3 for cube PG9 USART6_RX USART6 NODMA # we leave PG14 as an input to prevent it acting as a pullup # on the IOMCU SBUS input -PG14 USART6_TX USART6 NODMA -# PG15 USART6_CTS USART6 -# PG8 USART6_RTS USART6 +# PG14 USART6_TX USART6 NODMA +PG15 USART6_CTS USART6 +PG8 USART6_RTS USART6 + +# # USART6 is telem3 for cube +# PG9 USART6_RX USART6 NODMA +# # we leave PG14 as an input to prevent it acting as a pullup +# # on the IOMCU SBUS input +# PG14 USART6_TX USART6 NODMA +# # PG15 USART6_CTS USART6 +# # PG8 USART6_RTS USART6 # UART7 is debug PF6 UART7_RX UART7 NODMA diff --git a/libraries/AP_Notify/UAVCAN_RGB_LED.cpp b/libraries/AP_Notify/UAVCAN_RGB_LED.cpp index 84c4a24749..c505e5ef61 100644 --- a/libraries/AP_Notify/UAVCAN_RGB_LED.cpp +++ b/libraries/AP_Notify/UAVCAN_RGB_LED.cpp @@ -16,8 +16,9 @@ */ #include #include - + #if HAL_WITH_UAVCAN +// #if 1 #include "UAVCAN_RGB_LED.h" #include @@ -54,10 +55,29 @@ bool UAVCAN_RGB_LED::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) bool success = false; uint8_t can_num_drivers = AP::can().get_num_drivers(); + static uint8_t cnt; + static uint8_t l_mode,l_red,l_green,l_blue; + rgb = {red, green, blue}; + + cnt++; + + if(l_mode != rgb_mode || l_red != rgb.r || l_green != rgb.g || l_blue != rgb.b || cnt > 50) + { + cnt =0; + _need_update = true; + l_mode = rgb_mode ; + l_red = rgb.r ; + l_green = rgb.g ; + l_blue = rgb.b; + }else{ + return success; + } + for (uint8_t i = 0; i < can_num_drivers; i++) { AP_UAVCAN *uavcan = AP_UAVCAN::get_uavcan(i); if (uavcan != nullptr) { // success = uavcan->led_write(_led_index, red, green, blue) || success; + // success = uavcan->send_zr_rgb_led(rgb_mode,red, green, blue) || success;//(_led_index, red, green, blue) || success success = uavcan->send_zr_rgb_led(rgb_mode,red, green, blue) || success;//(_led_index, red, green, blue) || success } } diff --git a/libraries/AP_Notify/UAVCAN_RGB_LED.h b/libraries/AP_Notify/UAVCAN_RGB_LED.h index 4b176a4397..898f032160 100644 --- a/libraries/AP_Notify/UAVCAN_RGB_LED.h +++ b/libraries/AP_Notify/UAVCAN_RGB_LED.h @@ -13,7 +13,12 @@ protected: virtual bool hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) override; void set_rgb_mode(uint8_t mode) override; private: + void _timer(void); uint8_t _led_index; uint8_t rgb_mode; + bool _need_update; + struct { + uint8_t r, g, b; + } rgb; };