Browse Source

land lock modify

master
zbr3550 5 years ago
parent
commit
dea7ec5acc
  1. 2
      ArduCopter/UserCode.cpp
  2. 14
      ArduCopter/mode.cpp
  3. 2
      libraries/AP_Arming/AP_Arming.cpp
  4. 2
      libraries/RC_Channel/RC_Channel.h

2
ArduCopter/UserCode.cpp

@ -88,7 +88,7 @@ void Copter::userhook_SuperSlowLoop() @@ -88,7 +88,7 @@ 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){
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 || \

14
ArduCopter/mode.cpp

@ -642,8 +642,18 @@ void Mode::land_run_horizontal_control() @@ -642,8 +642,18 @@ void Mode::land_run_horizontal_control()
}
#endif
static uint8_t prt_once;
bool land_lock_attitude = (get_alt_above_ground_cm() < g.land_lock_alt * 2) && copter.rangefinder_alt_ok() && g.land_lock_alt && (copter.control_mode == Mode::Number::LAND);
// bool land_lock_attitude = (get_alt_above_ground_cm() < g.land_lock_alt * 4) && g.land_lock_alt && (copter.control_mode == Mode::Number::LAND);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
bool land_lock_attitude = (get_alt_above_ground_cm() < g.land_lock_alt * 4) && \
g.land_lock_alt && \
(copter.control_mode == Mode::Number::LAND || copter.control_mode == Mode::Number::RTL || copter.control_mode == Mode::Number::AUTO);
if(!prt_once && land_lock_attitude)
gcs().send_text(MAV_SEVERITY_INFO, "land lock sitl mode");
#else
bool land_lock_attitude = (get_alt_above_ground_cm() < g.land_lock_alt * 2) && \
copter.rangefinder_alt_ok() && g.land_lock_alt && \
(copter.control_mode == Mode::Number::LAND || copter.control_mode == Mode::Number::RTL || copter.control_mode == Mode::Number::AUTO);
#endif
if(land_lock_attitude)
{
if(prt_once == 0)

2
libraries/AP_Arming/AP_Arming.cpp

@ -743,7 +743,7 @@ bool AP_Arming::proximity_checks(bool report) const @@ -743,7 +743,7 @@ bool AP_Arming::proximity_checks(bool report) const
// return false if proximity sensor unhealthy
if (proximity->get_status() < AP_Proximity::Status::Good) {
check_failed(report, "检查接近传感器");//check proximity sensor
check_failed(report, "避障传感器不健康");//check proximity sensor
return false;
}

2
libraries/RC_Channel/RC_Channel.h

@ -364,7 +364,7 @@ protected: @@ -364,7 +364,7 @@ protected:
IGNORE_RECEIVER = (1 << 0), // RC receiver modules
IGNORE_OVERRIDES = (1 << 1), // MAVLink overrides
IGNORE_FAILSAFE = (1 << 2), // ignore RC failsafe bits
IGNORE_CTRLCHAN = (1 << 3), // ignore RC failsafe bits
IGNORE_CTRLCHAN = (1 << 3), // ignore RC before ch6
};
void new_override_received() {

Loading…
Cancel
Save