diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 5a148f2b28..7a6d88ae0b 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -684,24 +684,24 @@ void Mode::land_run_horizontal_control() void Mode::land_lock_att_control(float *p_roll,float *p_pitch,float *p_yaw_rate) { - float *target_roll = p_roll; - float *target_pitch = p_pitch; - float *target_yaw_rate = p_yaw_rate; static uint8_t prt_once; #if CONFIG_HAL_BOARD == HAL_BOARD_SITL bool land_lock_attitude = (get_alt_above_ground_cm() < g.land_lock_alt * 2) && \ g.land_lock_alt && \ - (copter.control_mode == Mode::Number::LAND || copter.control_mode == Mode::Number::RTL || copter.control_mode == Mode::Number::AUTO); + (copter.control_mode == Mode::Number::LOITER || 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); + (copter.control_mode == Mode::Number::LOITER || copter.control_mode == Mode::Number::LAND || copter.control_mode == Mode::Number::RTL || copter.control_mode == Mode::Number::AUTO); #endif if(land_lock_attitude) { + float *target_roll = p_roll; + float *target_pitch = p_pitch; + float *target_yaw_rate = p_yaw_rate; if(prt_once == 0) { gcs().send_text(MAV_SEVERITY_INFO, "Land ready to enter attitude lock mode"); diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index 2295d07e56..eed4c02e98 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -95,12 +95,12 @@ void ModeLoiter::run() // 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's desired yaw rate + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); // 提前到降落锁定判断前 + land_lock_att_control(&target_roll,&target_pitch,&target_yaw_rate); //降落锁定判断 // process pilot's roll and pitch input loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt); - // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); - // get pilot desired climb rate target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); // target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up); diff --git a/hexa-zrv4.sh b/hexa-zrv4.sh index 3644e8ebcf..76d0ab11ce 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-rc12.px4 +cp ./build/zr-hexa/bin/arducopter.apj /mnt/f/_01-work/100=data/固件/zr-v4/六轴_v4.0.16-rc2.px4