From 76ab50e8a0627da261bade359c82ee492eb514cb Mon Sep 17 00:00:00 2001 From: zbr Date: Tue, 15 Dec 2020 15:25:24 +0800 Subject: [PATCH] =?UTF-8?q?=E5=85=AD=E8=BD=B4=E4=BD=BF=E8=83=BD=E9=81=A5?= =?UTF-8?q?=E6=8E=A7=E5=99=A8RTL=E9=98=B6=E6=AE=B5=E6=8E=A7=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/Parameters.cpp | 9 ++++----- ArduCopter/mode.cpp | 4 ++-- ArduCopter/mode_rtl.cpp | 10 +++++----- ArduCopter/version.h | 6 +++--- ArduCopter/zr_flight.cpp | 7 +++---- hexa-zrv4.sh | 4 ++-- .../AP_HAL_ChibiOS/hwdef/zr-hexa/defaults.parm | 16 ++++++++++++++-- 7 files changed, 33 insertions(+), 23 deletions(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 346f3d62a4..b10858c0b4 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1665,22 +1665,21 @@ const char* Copter::get_sysid_board_id(void) switch (type) { case 0: - snprintf(buf, sizeof(buf), "Version: zr-v4.0.8 ,ID: RS100%04d%05d",(int)nameValue1,(int)nameValue2); + snprintf(buf, sizeof(buf), "Version: zr-v4.0.9 ,ID: RS100%04d%05d",(int)nameValue1,(int)nameValue2); break; case 1: - snprintf(buf, sizeof(buf), "Version: zr-v4.0.8 ,ID: M660%04d%05d",(int)nameValue1,(int)nameValue2); + snprintf(buf, sizeof(buf), "Version: zr-v4.0.9 ,ID: M660%04d%05d",(int)nameValue1,(int)nameValue2); break; case 2: - snprintf(buf, sizeof(buf), "Version: zr-v4.0.8 ,ID: M610%04d%05d",(int)nameValue1,(int)nameValue2); + snprintf(buf, sizeof(buf), "Version: zr-v4.0.9 ,ID: M610%04d%05d",(int)nameValue1,(int)nameValue2); break; case 3: - snprintf(buf, sizeof(buf), "Version: zr-v4.0.8 ,Board ID: ZRZK.20QT2.%d",(int)nameValue2); + snprintf(buf, sizeof(buf), "Version: zr-v4.0.9 ,Board ID: ZRZK.20QT2.%d",(int)nameValue2); break; default: break; } - AP::logger().Write_Message(buf); return buf; } diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index b6db105f8a..7d4110d95a 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -609,7 +609,7 @@ void Mode::land_run_horizontal_control() update_simple_mode(); // convert pilot input to lean angles - // get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); + get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); // record if pilot has overridden roll or pitch if (!is_zero(target_roll) || !is_zero(target_pitch)) { @@ -621,7 +621,7 @@ void Mode::land_run_horizontal_control() } // get pilot's desired yaw rate - // target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { auto_yaw.set_mode(AUTO_YAW_HOLD); } diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index d2eaf86734..dc914e2cd8 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -60,7 +60,7 @@ void ModeRTL::run(bool disarm_on_land) break; case RTL_LoiterAtHome: // if (rtl_path.land || copter.failsafe.radio) { - if (g.rtl_alt_final == 0) { + if (g.rtl_alt_final <= 0) { land_start(); }else{ descent_start(); @@ -192,7 +192,7 @@ void ModeRTL::climb_return_run() float target_yaw_rate = 0; if (!copter.failsafe.radio) { // get pilot's desired yaw rate - // target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { auto_yaw.set_mode(AUTO_YAW_HOLD); } @@ -249,7 +249,7 @@ void ModeRTL::loiterathome_run() float target_yaw_rate = 0; if (!copter.failsafe.radio) { // get pilot's desired yaw rate - // target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { auto_yaw.set_mode(AUTO_YAW_HOLD); } @@ -334,7 +334,7 @@ void ModeRTL::descent_run() update_simple_mode(); // convert pilot input to lean angles - // get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); + get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); // record if pilot has overridden roll or pitch if (!is_zero(target_roll) || !is_zero(target_pitch)) { @@ -346,7 +346,7 @@ void ModeRTL::descent_run() } // get pilot's desired yaw rate - // target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); } // set motors to full range diff --git a/ArduCopter/version.h b/ArduCopter/version.h index a64f9288ad..5c2da1eb8b 100644 --- a/ArduCopter/version.h +++ b/ArduCopter/version.h @@ -6,12 +6,12 @@ #include "ap_version.h" -#define THISFIRMWARE "ZRUAV v4.0.8-RC9" //"ArduCopter V4.0.0" +#define THISFIRMWARE "ZRUAV v4.0.9-RC10" //"ArduCopter V4.0.0" // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,0,8,FIRMWARE_VERSION_TYPE_OFFICIAL +#define FIRMWARE_VERSION 4,0,9,FIRMWARE_VERSION_TYPE_OFFICIAL #define FW_MAJOR 4 #define FW_MINOR 0 -#define FW_PATCH 8 +#define FW_PATCH 9 #define FW_TYPE FIRMWARE_VERSION_TYPE_OFFICIAL diff --git a/ArduCopter/zr_flight.cpp b/ArduCopter/zr_flight.cpp index 0fc97237b9..79f7b1be4e 100644 --- a/ArduCopter/zr_flight.cpp +++ b/ArduCopter/zr_flight.cpp @@ -25,9 +25,7 @@ bool Copter::zr_radio_valid(){ if (!rc().ignore_ctrl_channel() && (control_mode == Mode::Number::AUTO || control_mode == Mode::Number::RTL || control_mode == Mode::Number::LAND)){ if(abs(channel_roll->get_control_in())>750 || \ abs(channel_pitch->get_control_in())>750 || \ - abs(channel_yaw->get_control_in())>750 || \ - channel_throttle->get_control_in()<380 || \ - channel_throttle->get_control_in()>590 ) + abs(channel_yaw->get_control_in())>750) { // rc().set_disable_ch(); gcs().send_text(MAV_SEVERITY_INFO,"注意遥控通道是否在中位"); @@ -103,8 +101,10 @@ void Copter::zr_SuperSlowLoop(){ if(before_fly){ before_fly = false; camera.create_new_folder(); + gcs().send_text(MAV_SEVERITY_INFO,"%s",get_sysid_board_id()); } relay.on(3); + gcs().send_message(MSG_ZR_FLYING_STATUS); // camera.set_in_arm_mode(true); }else{ @@ -138,7 +138,6 @@ void Copter::zr_SuperSlowLoop(){ } } - gcs().send_message(MSG_ZR_FLYING_STATUS); } #endif \ No newline at end of file diff --git a/hexa-zrv4.sh b/hexa-zrv4.sh index 2e81fd4b28..f5fe211efc 100755 --- a/hexa-zrv4.sh +++ b/hexa-zrv4.sh @@ -1,3 +1,3 @@ ./waf configure --board zr-hexa -./waf --targets bin/arducopter --upload -cp ./build/zr-hexa/bin/arducopter.apj /mnt/f/_01-work/100=data/固件/zr-v4/dev-六轴_v4.0.8-rc8.px4 +./waf copter +cp ./build/zr-hexa/bin/arducopter.apj /mnt/f/_01-work/100=data/固件/zr-v4/六轴_v4.0.9-rc10.px4 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/zr-hexa/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/zr-hexa/defaults.parm index c8af8abcd1..1437c29cc1 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/zr-hexa/defaults.parm +++ b/libraries/AP_HAL_ChibiOS/hwdef/zr-hexa/defaults.parm @@ -71,7 +71,7 @@ BATT_MONITOR 4 BATT_SERIAL_NUM -1 BATT_VOLT_PIN 0 -BRD_BOOT_DELAY 1000 +BRD_BOOT_DELAY 2000 BRD_PWM_COUNT 0 BRD_RTC_TYPES 1 BRD_RTC_TZ_MIN 0 @@ -99,7 +99,19 @@ CAM_SERVO_ON 1900 CAM_TRIGG_DIST 0 CAM_TRIGG_TYPE 1 CAM_TYPE 0 - +CAN_D1_PROTOCOL 1 +CAN_D1_UC_ESC_BM 0 +CAN_D1_UC_NODE 10 +CAN_D1_UC_SRV_BM 0 +CAN_D1_UC_SRV_RT 50 +CAN_D2_PROTOCOL 1 +CAN_P1_BITRATE 1000000 +CAN_P1_DRIVER 1 +CAN_P2_BITRATE 1000000 +CAN_P2_DRIVER 1 +CAN_SLCAN_CPORT 0 +CAN_SLCAN_SERNUM -1 +CAN_SLCAN_TIMOUT 0 CHUTE_ENABLED 0 CIRCLE_RADIUS 1000 CIRCLE_RATE 20