diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index ecda0b0886..c26c851233 100644 --- a/ArduCopter/UserCode.cpp +++ b/ArduCopter/UserCode.cpp @@ -85,9 +85,15 @@ void Copter::userhook_SuperSlowLoop() if(motors->armed()){ before_fly = false; relay.on(3); + if (control_mode == Mode::Number::AUTO || control_mode == Mode::Number::RTL || control_mode == Mode::Number::LAND){ + rc().set_disable_ch(); + }else{ + rc().set_enable_ch(); + } }else{ relay.off(3); + rc().set_enable_ch(); updownStatus =UpDown_TakeOffStart; mavlink_zr_flying_status_t zr_flying_status_t; zr_flying_status_t.updown_status = updownStatus; diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index d9172e61fe..dc27d36a2d 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -597,7 +597,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)) { @@ -609,7 +609,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_auto.cpp b/ArduCopter/mode_auto.cpp index bb38f5fb0e..442945ce54 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -1203,7 +1203,8 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd) break; case MAV_CMD_NAV_RETURN_TO_LAUNCH: // do not stop for RTL - fast_waypoint = true; + // fast_waypoint = true; + fast_waypoint = false; break; case MAV_CMD_NAV_TAKEOFF: default: diff --git a/ArduCopter/version.h b/ArduCopter/version.h index c8cf2d296d..b89b74dd2a 100644 --- a/ArduCopter/version.h +++ b/ArduCopter/version.h @@ -6,7 +6,7 @@ #include "ap_version.h" -#define THISFIRMWARE "ZRUAV v4.0.4-rc1.0" //"ArduCopter V4.0.0" +#define THISFIRMWARE "ZRUAV v4.0.4-rc1.1" //"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_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 7b0d79eb5f..16f83f0492 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -1100,7 +1100,7 @@ bool AP_AHRS_DCM::set_home(const Location &loc) // send new home and ekf origin to GCS gcs().send_message(MSG_HOME); gcs().send_message(MSG_ORIGIN); - + gcs().send_text(MAV_SEVERITY_INFO, "NOTICE: Home点更新"); AP_HAL::Util::PersistentData &pd = hal.util->persistent_data; pd.home_lat = loc.lat; pd.home_lon = loc.lng; diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 9a70b4c87f..b94784c009 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -136,7 +136,7 @@ RC_Channel::update(void) if (has_override() && !rc().ignore_overrides()) { radio_in = override_value; } else if (!rc().ignore_receiver()) { - if(rc().ignore_ctrl_channel() && ch_in < 6){ + if(rc().ignore_ctrl_channel() && !rc().is_enable_ch(ch_in)){ return false; }else{ radio_in = hal.rcin->read(ch_in); diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index f194586f19..2374f76cb4 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -343,6 +343,17 @@ public: return _options & uint32_t(Option::IGNORE_CTRLCHAN); } + bool is_enable_ch(uint8_t ch) const { + return (_enable_ch == (ch+1) ); + } + + void set_disable_ch() { + _options = uint32_t(Option::IGNORE_CTRLCHAN); + } + void set_enable_ch() { + _options = 0; + } + float override_timeout_ms() const { return _override_timeout.get() * 1e3f; } @@ -369,6 +380,7 @@ private: AP_Float _override_timeout; AP_Int32 _options; + AP_Int8 _enable_ch; // flight_mode_channel_number must be overridden: virtual int8_t flight_mode_channel_number() const = 0; diff --git a/libraries/RC_Channel/RC_Channels_VarInfo.h b/libraries/RC_Channel/RC_Channels_VarInfo.h index b1e8dbd9d8..685cac3a44 100644 --- a/libraries/RC_Channel/RC_Channels_VarInfo.h +++ b/libraries/RC_Channel/RC_Channels_VarInfo.h @@ -89,5 +89,9 @@ const AP_Param::GroupInfo RC_Channels::var_info[] = { // @Bitmask: 0:Ignore RC Receiver, 1:Ignore MAVLink Overrides, 2:Ignore Receiver Failsafe AP_GROUPINFO("_OPTIONS", 33, RC_CHANNELS_SUBCLASS, _options, 0), + + AP_GROUPINFO("_ENABLE_CH", 34, RC_CHANNELS_SUBCLASS, _enable_ch, 9), + + AP_GROUPEND }; diff --git a/zr_version b/zr_version index 9af16d8c74..def40d0934 100644 --- a/zr_version +++ b/zr_version @@ -8,3 +8,15 @@ zr-dev-4.0.2: NMEA 时间修复,日志目录名 v4.0.3: 快速解锁 v4.0.4: 分阶段降落,光流门限,限制GPS切换 _nunber_to_switch, + rc1.1 降落倒计时,状态码刷新,测距日志增加,智能电池压差 + + + + + + + + + + +