From 71aca00ca763af01ad251ad9fef629e513ae2ec0 Mon Sep 17 00:00:00 2001 From: zbr Date: Mon, 23 Nov 2020 10:35:09 +0800 Subject: [PATCH 01/10] =?UTF-8?q?rangefinder=20=E4=B8=80=E4=BA=9B=E4=BF=AE?= =?UTF-8?q?=E6=94=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../AP_RangeFinder_Benewake.cpp | 5 -- libraries/AP_RangeFinder/RangeFinder.cpp | 51 ++++++++----------- 2 files changed, 22 insertions(+), 34 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp index 825991ccb3..f02a845988 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp @@ -120,11 +120,6 @@ bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm) } // if checksum matches extract contents if (checksum == linebuf[BENEWAKE_FRAME_LENGTH-1]) { - - uint16_t strength = ((uint16_t)linebuf[5] << 8) | linebuf[4]; - if(strength < 100 || strength == 0xFFFF){ - return false; - } // calculate distance uint16_t dist = ((uint16_t)linebuf[3] << 8) | linebuf[2]; if (dist >= BENEWAKE_DIST_MAX_CM) { diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index 477f22b6a9..10d5da45a2 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -88,7 +88,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = { // @Group: 4_ // @Path: AP_RangeFinder_Wasp.cpp - AP_SUBGROUPVARPTR(drivers[0], "4_", 60, RangeFinder, backend_var_info[3]), + AP_SUBGROUPVARPTR(drivers[3], "4_", 60, RangeFinder, backend_var_info[3]), #endif #if RANGEFINDER_MAX_INSTANCES > 4 @@ -556,15 +556,22 @@ bool RangeFinder::has_orientation(enum Rotation orientation) const // find first range finder instance with the specified orientation AP_RangeFinder_Backend *RangeFinder::find_instance(enum Rotation orientation) const { + // first try for a rangefinder that is in range for (uint8_t i=0; iorientation() == orientation && + backend->status() == RangeFinder_Good) { + return backend; } - if (backend->orientation() != orientation) { - continue; + } + // if none in range then return first with correct orientation + for (uint8_t i=0; iorientation() == orientation) { + return backend; } - return backend; } return nullptr; } @@ -676,30 +683,16 @@ void RangeFinder::Log_RFND() if (s == nullptr) { continue; } - if (i == 0) - { - const struct log_RFND pkt = { + + const struct log_RFND pkt = { LOG_PACKET_HEADER_INIT(LOG_RFND_MSG), - time_us : AP_HAL::micros64(), - instance : i, - dist : s->distance_cm(), - status : (uint8_t)s->status(), - orient : s->orientation(), - }; - AP::logger().WriteBlock(&pkt, sizeof(pkt)); - } - else - { - const struct log_RFND pkt2 = { - LOG_PACKET_HEADER_INIT(LOG_RFND2_MSG), - time_us : AP_HAL::micros64(), - instance : i, - dist : s->distance_cm(), - status : (uint8_t)s->status(), - orient : s->orientation(), - }; - AP::logger().WriteBlock(&pkt2, sizeof(pkt2)); - } + time_us : AP_HAL::micros64(), + instance : i, + dist : s->distance_cm(), + status : (uint8_t)s->status(), + orient : s->orientation(), + }; + AP::logger().WriteBlock(&pkt, sizeof(pkt)); } } From 9cb5a786331a46cfcc5112aa347278fef9fb54fa Mon Sep 17 00:00:00 2001 From: zbr Date: Thu, 26 Nov 2020 14:21:00 +0800 Subject: [PATCH 02/10] =?UTF-8?q?=E6=B0=94=E5=8E=8B=E8=AE=A1=E6=B8=A9?= =?UTF-8?q?=E5=BA=A6=E6=A0=A1=E5=87=86=E6=B5=8B=E8=AF=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/Parameters.cpp | 2 +- ArduCopter/version.h | 2 +- hexa-zrv4.sh | 2 +- .../AP_HAL_ChibiOS/hwdef/zr-hexa/defaults.parm | 2 ++ .../AP_RangeFinder/AP_RangeFinder_Benewake.cpp | 5 +++++ .../AP_TempCalibration/AP_TempCalibration.cpp | 18 +++++++++++++++++- .../AP_TempCalibration/AP_TempCalibration.h | 5 +++-- 7 files changed, 30 insertions(+), 6 deletions(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index f193ad7fe1..346f3d62a4 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1671,7 +1671,7 @@ const char* Copter::get_sysid_board_id(void) snprintf(buf, sizeof(buf), "Version: zr-v4.0.8 ,ID: M660%04d%05d",(int)nameValue1,(int)nameValue2); break; case 2: - snprintf(buf, sizeof(buf), "Version: zr-v4.0.8 ,ID: RF610%04d%05d",(int)nameValue1,(int)nameValue2); + snprintf(buf, sizeof(buf), "Version: zr-v4.0.8 ,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); diff --git a/ArduCopter/version.h b/ArduCopter/version.h index 9b38784bbd..a64f9288ad 100644 --- a/ArduCopter/version.h +++ b/ArduCopter/version.h @@ -6,7 +6,7 @@ #include "ap_version.h" -#define THISFIRMWARE "ZRUAV v4.0.8-RC7" //"ArduCopter V4.0.0" +#define THISFIRMWARE "ZRUAV v4.0.8-RC9" //"ArduCopter V4.0.0" // the following line is parsed by the autotest scripts #define FIRMWARE_VERSION 4,0,8,FIRMWARE_VERSION_TYPE_OFFICIAL diff --git a/hexa-zrv4.sh b/hexa-zrv4.sh index 9c38465324..2e81fd4b28 100755 --- a/hexa-zrv4.sh +++ b/hexa-zrv4.sh @@ -1,3 +1,3 @@ ./waf configure --board zr-hexa ./waf --targets bin/arducopter --upload -sudo cp ./build/zr-hexa/bin/arducopter.apj /mnt/f/_01-work/100=data/固件/zr-v4/六轴_v4.0.8-rc8.px4 +cp ./build/zr-hexa/bin/arducopter.apj /mnt/f/_01-work/100=data/固件/zr-v4/dev-六轴_v4.0.8-rc8.px4 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/zr-hexa/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/zr-hexa/defaults.parm index 4086523c92..c8af8abcd1 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/zr-hexa/defaults.parm +++ b/libraries/AP_HAL_ChibiOS/hwdef/zr-hexa/defaults.parm @@ -71,6 +71,7 @@ BATT_MONITOR 4 BATT_SERIAL_NUM -1 BATT_VOLT_PIN 0 +BRD_BOOT_DELAY 1000 BRD_PWM_COUNT 0 BRD_RTC_TYPES 1 BRD_RTC_TZ_MIN 0 @@ -268,6 +269,7 @@ SERIAL6_PROTOCOL -1 SERIAL7_BAUD 115200 SERIAL7_OPTIONS 0 SERIAL7_PROTOCOL 2 +SYSID_TYPE 2 WP_YAW_BEHAVIOR 1 WPNAV_ACCEL 100 WPNAV_ACCEL_Z 100 diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp index f02a845988..8bb975d75d 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp @@ -120,6 +120,11 @@ bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm) } // if checksum matches extract contents if (checksum == linebuf[BENEWAKE_FRAME_LENGTH-1]) { + + // uint16_t strength = ((uint16_t)linebuf[5] << 8) | linebuf[4]; + // if(strength < 100 || strength == 0xFFFF){ + // return false; + // } // calculate distance uint16_t dist = ((uint16_t)linebuf[3] << 8) | linebuf[2]; if (dist >= BENEWAKE_DIST_MAX_CM) { diff --git a/libraries/AP_TempCalibration/AP_TempCalibration.cpp b/libraries/AP_TempCalibration/AP_TempCalibration.cpp index 25679a01d2..ec9d11c6dd 100644 --- a/libraries/AP_TempCalibration/AP_TempCalibration.cpp +++ b/libraries/AP_TempCalibration/AP_TempCalibration.cpp @@ -19,6 +19,7 @@ #include "AP_TempCalibration.h" #include #include +#include extern const AP_HAL::HAL& hal; @@ -135,6 +136,7 @@ void AP_TempCalibration::calculate_calibration(void) float current_err = calculate_p_range(baro_exponent); float test_exponent = baro_exponent + learn_delta; float test_err = calculate_p_range(test_exponent); + static float last_err = 0; if (test_err >= current_err) { test_exponent = baro_exponent - learn_delta; test_err = calculate_p_range(test_exponent); @@ -149,6 +151,12 @@ void AP_TempCalibration::calculate_calibration(void) } temp_min.set_and_save_ifchanged(learn_temp_start); temp_max.set_and_save_ifchanged(learn_temp_start + learn_i*learn_temp_step); + + gcs().send_text(MAV_SEVERITY_INFO, "---CAL: %.2f, %.2f\n", test_exponent,baro_exponent); + } + if(fabs(last_err - current_err) > 0.01){ + gcs().send_text(MAV_SEVERITY_INFO, "CAL: %.2f, %.2f,%d, %d,%.2f,%.2f\n", test_exponent,baro_exponent,temp_min,temp_max,current_err,test_err); + last_err = current_err; } } @@ -176,7 +184,15 @@ void AP_TempCalibration::learn_calibration(void) } float temp = baro.get_temperature(0); float P = baro.get_pressure(0); + static float last_temp; + static uint16_t last_idx; uint16_t idx = (temp - learn_temp_start) / learn_temp_step; + // gcs().send_text(MAV_SEVERITY_INFO, "learning %u %.2f at %.2f", idx, learn_values[idx], temp); + if((last_idx != idx || fabs(last_temp - temp)> 1.0) && !is_zero(learn_values[idx]) ){ + gcs().send_text(MAV_SEVERITY_INFO, "learning, %u, %.2f, at, %.2f", idx, learn_values[idx], temp); + last_idx = idx; + last_temp = temp; + } if (idx >= learn_count) { // could change learn_temp_step here return; @@ -191,7 +207,7 @@ void AP_TempCalibration::learn_calibration(void) learn_i = MAX(learn_i, idx); uint32_t now = AP_HAL::millis(); - if (now - last_learn_ms > 100 && + if (now - last_learn_ms > 500 && idx*learn_temp_step > min_learn_temp_range && temp - learn_temp_start > temp_max - temp_min) { last_learn_ms = now; diff --git a/libraries/AP_TempCalibration/AP_TempCalibration.h b/libraries/AP_TempCalibration/AP_TempCalibration.h index 3730ecd8b3..897a2da722 100644 --- a/libraries/AP_TempCalibration/AP_TempCalibration.h +++ b/libraries/AP_TempCalibration/AP_TempCalibration.h @@ -62,7 +62,7 @@ private: uint32_t last_learn_ms; // temperature at which baro correction starts - const float Tzero = 25; + const float Tzero = 0; const float exp_limit_max = 2; const float exp_limit_min = 0; @@ -70,7 +70,8 @@ private: // require observation of at least 5 degrees of temp range to // start learning - const float min_learn_temp_range = 7; + // const float min_learn_temp_range = 7; + const float min_learn_temp_range = 1; void setup_learning(void); void learn_calibration(void); From 79f8615601984bab7ee4a9e7bc88845fdd67e379 Mon Sep 17 00:00:00 2001 From: zbr Date: Thu, 26 Nov 2020 14:22:06 +0800 Subject: [PATCH 03/10] =?UTF-8?q?=E6=B0=94=E5=8E=8B=E8=AE=A1=E6=B8=A9?= =?UTF-8?q?=E5=BA=A6=E6=A0=A1=E5=87=86=E6=B5=8B=E8=AF=95=EF=BC=8C=E8=BF=98?= =?UTF-8?q?=E5=8E=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../AP_TempCalibration/AP_TempCalibration.cpp | 18 +----------------- .../AP_TempCalibration/AP_TempCalibration.h | 5 ++--- 2 files changed, 3 insertions(+), 20 deletions(-) diff --git a/libraries/AP_TempCalibration/AP_TempCalibration.cpp b/libraries/AP_TempCalibration/AP_TempCalibration.cpp index ec9d11c6dd..25679a01d2 100644 --- a/libraries/AP_TempCalibration/AP_TempCalibration.cpp +++ b/libraries/AP_TempCalibration/AP_TempCalibration.cpp @@ -19,7 +19,6 @@ #include "AP_TempCalibration.h" #include #include -#include extern const AP_HAL::HAL& hal; @@ -136,7 +135,6 @@ void AP_TempCalibration::calculate_calibration(void) float current_err = calculate_p_range(baro_exponent); float test_exponent = baro_exponent + learn_delta; float test_err = calculate_p_range(test_exponent); - static float last_err = 0; if (test_err >= current_err) { test_exponent = baro_exponent - learn_delta; test_err = calculate_p_range(test_exponent); @@ -151,12 +149,6 @@ void AP_TempCalibration::calculate_calibration(void) } temp_min.set_and_save_ifchanged(learn_temp_start); temp_max.set_and_save_ifchanged(learn_temp_start + learn_i*learn_temp_step); - - gcs().send_text(MAV_SEVERITY_INFO, "---CAL: %.2f, %.2f\n", test_exponent,baro_exponent); - } - if(fabs(last_err - current_err) > 0.01){ - gcs().send_text(MAV_SEVERITY_INFO, "CAL: %.2f, %.2f,%d, %d,%.2f,%.2f\n", test_exponent,baro_exponent,temp_min,temp_max,current_err,test_err); - last_err = current_err; } } @@ -184,15 +176,7 @@ void AP_TempCalibration::learn_calibration(void) } float temp = baro.get_temperature(0); float P = baro.get_pressure(0); - static float last_temp; - static uint16_t last_idx; uint16_t idx = (temp - learn_temp_start) / learn_temp_step; - // gcs().send_text(MAV_SEVERITY_INFO, "learning %u %.2f at %.2f", idx, learn_values[idx], temp); - if((last_idx != idx || fabs(last_temp - temp)> 1.0) && !is_zero(learn_values[idx]) ){ - gcs().send_text(MAV_SEVERITY_INFO, "learning, %u, %.2f, at, %.2f", idx, learn_values[idx], temp); - last_idx = idx; - last_temp = temp; - } if (idx >= learn_count) { // could change learn_temp_step here return; @@ -207,7 +191,7 @@ void AP_TempCalibration::learn_calibration(void) learn_i = MAX(learn_i, idx); uint32_t now = AP_HAL::millis(); - if (now - last_learn_ms > 500 && + if (now - last_learn_ms > 100 && idx*learn_temp_step > min_learn_temp_range && temp - learn_temp_start > temp_max - temp_min) { last_learn_ms = now; diff --git a/libraries/AP_TempCalibration/AP_TempCalibration.h b/libraries/AP_TempCalibration/AP_TempCalibration.h index 897a2da722..3730ecd8b3 100644 --- a/libraries/AP_TempCalibration/AP_TempCalibration.h +++ b/libraries/AP_TempCalibration/AP_TempCalibration.h @@ -62,7 +62,7 @@ private: uint32_t last_learn_ms; // temperature at which baro correction starts - const float Tzero = 0; + const float Tzero = 25; const float exp_limit_max = 2; const float exp_limit_min = 0; @@ -70,8 +70,7 @@ private: // require observation of at least 5 degrees of temp range to // start learning - // const float min_learn_temp_range = 7; - const float min_learn_temp_range = 1; + const float min_learn_temp_range = 7; void setup_learning(void); void learn_calibration(void); From 76ab50e8a0627da261bade359c82ee492eb514cb Mon Sep 17 00:00:00 2001 From: zbr Date: Tue, 15 Dec 2020 15:25:24 +0800 Subject: [PATCH 04/10] =?UTF-8?q?=E5=85=AD=E8=BD=B4=E4=BD=BF=E8=83=BD?= =?UTF-8?q?=E9=81=A5=E6=8E=A7=E5=99=A8RTL=E9=98=B6=E6=AE=B5=E6=8E=A7?= =?UTF-8?q?=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 From 2566717c5f70ce90230c7eefcbc7327ba4f5b188 Mon Sep 17 00:00:00 2001 From: zbr Date: Thu, 31 Dec 2020 10:12:47 +0800 Subject: [PATCH 05/10] =?UTF-8?q?hexa=20=E5=A2=9E=E5=8A=A0=E6=97=A7ID?= =?UTF-8?q?=E6=94=AF=E6=8C=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/Parameters.cpp | 19 ++++++++++++++++++- ArduCopter/Parameters.h | 4 +++- ArduCopter/version.h | 2 +- 3 files changed, 22 insertions(+), 3 deletions(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index b10858c0b4..735b4aef1c 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -57,6 +57,8 @@ const AP_Param::Info Copter::var_info[] = { GSCALAR(sysid_board_name_2nd, "SYSID_BDNAME_P2", 1001), // 0:rs100, 1:M66, 2:RF610 3:zrzk.20qt2 GSCALAR(sysid_type, "SYSID_TYPE", 0 ), + + GSCALAR(sysid_board_id, "SYSID_BOARD_ID", 100), // @Param: SYSID_THISMAV // @DisplayName: MAVLink system ID of this vehicle @@ -1662,6 +1664,16 @@ const char* Copter::get_sysid_board_id(void) int32_t nameValue1 =(int32_t) g.sysid_board_name_1st; int32_t nameValue2 = (int32_t)g.sysid_board_name_2nd; int8_t type = g.sysid_type; + char name[6] = {' ',' ',' ',' ',' '}; + // memset(name,0,6); + nameValue2 = nameValue2 & 0xffffff; + // name[5] = "a"; + name[4] = nameValue2&0xFF; + name[3] = nameValue2>>8&0xFF; + name[2] = nameValue2>>16& 0xFF; + name[1] = nameValue1&0xFF; + name[0] = nameValue1>>8 & 0xFF; + switch (type) { case 0: @@ -1674,7 +1686,12 @@ const char* Copter::get_sysid_board_id(void) 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.9 ,Board ID: ZRZK.20QT2.%d",(int)nameValue2); + // snprintf(buf, sizeof(buf), "Version: zr-v4.0.9 ,Board ID: ZRZK.20QT2.%d",(int)nameValue2); + snprintf(buf, sizeof(buf), "Version: zr-v4.0.9 ,Board ID: ZRZK.%5s.%03d",name,(int)g.sysid_board_id); + + break; + case 4: + snprintf(buf, sizeof(buf), "Version: zr-v4.0.9 ,Board ID: ZRZK.19QT2.%d",(int)nameValue2); break; default: diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 95d810b430..304749114b 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -205,6 +205,7 @@ public: k_param_sysid_board_name_1st =106, //add by @Binsir k_param_sysid_board_name_2nd=107, //add by @Binsir k_param_sysid_type=108, // modify by @Brown + k_param_sysid_board_id=109, // modify by @Brown // 110: Telemetry control // @@ -399,7 +400,8 @@ public: AP_Int8 sysid_type; // modify by @Brown AP_Int16 sysid_board_name_1st;// modify by @Binsir - AP_Int16 sysid_board_name_2nd;// modify by @Binsir + AP_Int32 sysid_board_name_2nd;// modify by @Binsir + AP_Int16 sysid_board_id; // modify by @Brown // Telemetry control // diff --git a/ArduCopter/version.h b/ArduCopter/version.h index 5c2da1eb8b..9a2c897fbb 100644 --- a/ArduCopter/version.h +++ b/ArduCopter/version.h @@ -6,7 +6,7 @@ #include "ap_version.h" -#define THISFIRMWARE "ZRUAV v4.0.9-RC10" //"ArduCopter V4.0.0" +#define THISFIRMWARE "ZRUAV v4.0.9-RC11" //"ArduCopter V4.0.0" // the following line is parsed by the autotest scripts #define FIRMWARE_VERSION 4,0,9,FIRMWARE_VERSION_TYPE_OFFICIAL From d752d1495b827940ce6400d7c0e65b6acb874a46 Mon Sep 17 00:00:00 2001 From: zbr Date: Thu, 7 Jan 2021 14:51:03 +0800 Subject: [PATCH 06/10] =?UTF-8?q?=E7=9B=B8=E6=9C=BA=E7=85=A7=E7=89=87?= =?UTF-8?q?=E5=B7=AE=E5=BC=82=EF=BC=8C=E6=94=B9=E6=88=90=E4=B8=80=E5=8F=B0?= =?UTF-8?q?=E7=9B=B8=E6=9C=BA2=E5=BC=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- libraries/AP_Camera/AP_Camera.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index a80fdcb446..9a1f5e86c1 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -602,11 +602,15 @@ void AP_Camera::send_camera_zr_status(mavlink_channel_t chan) delt_pic_num += abs(num - camera_state.num[i]); // 判断各个相机照片数是否相等 } + if(delt_pic_num == 0){ + delt_cnt = 0; + last_delt = 0; + } if(last_delt != delt_pic_num){ delt_cnt+=1; if(delt_cnt > 5){ // Mavlink一次可能会发两三个chan，取5确保是两次计算差值满足条件 // gcs().send_text(MAV_SEVERITY_INFO, "照片数差异(%d):%d,%d,%d,%d,%d",delt_pic_num,camera_state.num[0],camera_state.num[1],camera_state.num[2],camera_state.num[3],camera_state.num[4]); - if(delt_pic_num > 3){ + if(delt_pic_num > 5){ gcs().send_text(MAV_SEVERITY_CRITICAL, "%d:照片数差异大,总差值：%d",camera_state.num[0],delt_pic_num); feedback_gimbal = 20; }else @@ -616,7 +620,6 @@ void AP_Camera::send_camera_zr_status(mavlink_channel_t chan) last_delt = delt_pic_num; delt_cnt = 0; } - }else{ } mavlink_msg_camera_zr_status_send( chan, From a0b8dcb33e77ce4f0387f74cb642b667adb278ec Mon Sep 17 00:00:00 2001 From: zbr Date: Thu, 7 Jan 2021 14:53:51 +0800 Subject: [PATCH 07/10] =?UTF-8?q?=E7=9B=B8=E6=9C=BA=E7=85=A7=E7=89=87?= =?UTF-8?q?=E5=B7=AE=E5=BC=82=EF=BC=8C=E6=94=B9=E6=88=90=E4=B8=80=E5=8F=B0?= =?UTF-8?q?=E7=9B=B8=E6=9C=BA2=E5=BC=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- libraries/AP_Camera/AP_Camera.cpp | 7 +++++-- libraries/AP_GPS/GPS_Backend.cpp | 6 ------ 2 files changed, 5 insertions(+), 8 deletions(-) diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index a80fdcb446..9a1f5e86c1 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -602,11 +602,15 @@ void AP_Camera::send_camera_zr_status(mavlink_channel_t chan) delt_pic_num += abs(num - camera_state.num[i]); // 判断各个相机照片数是否相等 } + if(delt_pic_num == 0){ + delt_cnt = 0; + last_delt = 0; + } if(last_delt != delt_pic_num){ delt_cnt+=1; if(delt_cnt > 5){ // Mavlink一次可能会发两三个chan，取5确保是两次计算差值满足条件 // gcs().send_text(MAV_SEVERITY_INFO, "照片数差异(%d):%d,%d,%d,%d,%d",delt_pic_num,camera_state.num[0],camera_state.num[1],camera_state.num[2],camera_state.num[3],camera_state.num[4]); - if(delt_pic_num > 3){ + if(delt_pic_num > 5){ gcs().send_text(MAV_SEVERITY_CRITICAL, "%d:照片数差异大,总差值：%d",camera_state.num[0],delt_pic_num); feedback_gimbal = 20; }else @@ -616,7 +620,6 @@ void AP_Camera::send_camera_zr_status(mavlink_channel_t chan) last_delt = delt_pic_num; delt_cnt = 0; } - }else{ } mavlink_msg_camera_zr_status_send( chan, diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index fe9770f566..b00e33576f 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -90,12 +90,6 @@ void AP_GPS_Backend::make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds) min = v % 100; v /= 100; hour = v % 100; - int8_t rmon = mon - 2; - if (0 >= rmon) { - rmon += 12; - year -= 1; - } - time_t rawtime; struct tm* timeinfo; time(&rawtime); From 730ba25a6ebeae6f688be3dd85e01835f17d051c Mon Sep 17 00:00:00 2001 From: zbr Date: Fri, 8 Jan 2021 13:49:43 +0800 Subject: [PATCH 08/10] zr-hex rc12 --- ArduCopter/version.h | 2 +- hexa-zrv4.sh | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/version.h b/ArduCopter/version.h index 9a2c897fbb..adf9dbd0a5 100644 --- a/ArduCopter/version.h +++ b/ArduCopter/version.h @@ -6,7 +6,7 @@ #include "ap_version.h" -#define THISFIRMWARE "ZRUAV v4.0.9-RC11" //"ArduCopter V4.0.0" +#define THISFIRMWARE "ZRUAV v4.0.9-RC12" //"ArduCopter V4.0.0" // the following line is parsed by the autotest scripts #define FIRMWARE_VERSION 4,0,9,FIRMWARE_VERSION_TYPE_OFFICIAL diff --git a/hexa-zrv4.sh b/hexa-zrv4.sh index f5fe211efc..3644e8ebcf 100755 --- a/hexa-zrv4.sh +++ b/hexa-zrv4.sh @@ -1,3 +1,3 @@ ./waf configure --board zr-hexa ./waf copter -cp ./build/zr-hexa/bin/arducopter.apj /mnt/f/_01-work/100=data/固件/zr-v4/六轴_v4.0.9-rc10.px4 +cp ./build/zr-hexa/bin/arducopter.apj /mnt/f/_01-work/100=data/固件/zr-v4/六轴_v4.0.9-rc12.px4 From 240f060f004ba67c513d4fbbf588ce76167fd7a1 Mon Sep 17 00:00:00 2001 From: zbr Date: Tue, 12 Jan 2021 15:14:33 +0800 Subject: [PATCH 09/10] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E5=85=A8=E6=95=B0?= =?UTF-8?q?=E5=AD=97ID?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/Parameters.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 735b4aef1c..54987d1c2f 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1693,6 +1693,9 @@ const char* Copter::get_sysid_board_id(void) case 4: snprintf(buf, sizeof(buf), "Version: zr-v4.0.9 ,Board ID: ZRZK.19QT2.%d",(int)nameValue2); break; + case 5: + snprintf(buf, sizeof(buf), "Version: zr-v4.0.9 ,Board ID: %04d%04d",(int)nameValue1,(int)nameValue2); + break; default: break; From 68e9814f263384d0b214c4e383da84ecd117c77e Mon Sep 17 00:00:00 2001 From: zbr Date: Thu, 4 Feb 2021 09:46:08 +0800 Subject: [PATCH 10/10] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E8=87=AA=E5=8A=A8?= =?UTF-8?q?=E6=A8=A1=E5=BC=8F=E6=98=AF=E5=90=A6=E5=90=AF=E7=94=A8=E6=8E=A7?= =?UTF-8?q?=E5=88=B6=E5=88=A4=E6=96=AD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/Parameters.cpp | 1 + ArduCopter/Parameters.h | 3 +++ ArduCopter/mode.cpp | 6 ++++-- ArduCopter/mode_rtl.cpp | 10 +++++++--- 4 files changed, 15 insertions(+), 5 deletions(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index d31d874b6e..af99e74a46 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -143,6 +143,7 @@ const AP_Param::Info Copter::var_info[] = { // @Increment: 1 // @User: Standard GSCALAR(zr_tk_delay, "ZR_TK_DELAY", ZR_TK_DELAY), + GSCALAR(zr_use_rc, "ZR_USE_RC", 1), // @Param: ZR_RTL_DELAY // @DisplayName: rtl Altitude when at final decent alt diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 304749114b..c52e732ea3 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -392,6 +392,7 @@ public: k_param_zr_tk_req_alt, k_param_zr_tk_delay, k_param_zr_rtl_delay, + k_param_zr_use_rc, // the k_param_* space is 9-bits in size // 511: reserved }; @@ -506,6 +507,8 @@ public: AP_Int32 zr_tk_req_alt; AP_Int16 zr_tk_delay; AP_Int16 zr_rtl_delay; + AP_Int8 zr_use_rc; + // Note: keep initializers here in the same order as they are declared // above. Parameters() diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 7d4110d95a..3a5775c540 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -609,7 +609,8 @@ 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()); + if(g.zr_use_rc) + 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 +622,8 @@ 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()); + if(g.zr_use_rc) + 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 dc914e2cd8..4fd16425b2 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -192,7 +192,8 @@ 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()); + if(g.zr_use_rc) + 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 +250,8 @@ 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()); + if(g.zr_use_rc) + 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,6 +336,7 @@ void ModeRTL::descent_run() update_simple_mode(); // convert pilot input to lean angles + if(g.zr_use_rc) 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 @@ -346,7 +349,8 @@ void ModeRTL::descent_run() } // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + if(g.zr_use_rc) + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); } // set motors to full range