Browse Source

修正起飞高度(类型)

celiu-4.0.17-rc8
yaozb 5 years ago
parent
commit
7711b40c13
  1. 2
      ArduCopter/Parameters.h
  2. 12
      ArduCopter/mode_auto.cpp
  3. 2
      modules/mavlink

2
ArduCopter/Parameters.h

@ -497,7 +497,7 @@ public: @@ -497,7 +497,7 @@ public:
AP_Float acro_balance_pitch;
AP_Int8 acro_trainer;
AP_Float acro_rp_expo;
AP_Int16 zr_tk_req_alt;
AP_Int32 zr_tk_req_alt;
AP_Int16 zr_tk_delay;
AP_Int16 zr_rtl_delay;
// Note: keep initializers here in the same order as they are declared

12
ArduCopter/mode_auto.cpp

@ -765,7 +765,10 @@ void ModeAuto::takeoff_run() @@ -765,7 +765,10 @@ void ModeAuto::takeoff_run()
{
if (motors->armed() || copter.ap.auto_armed)
{
if (g.zr_tk_req_alt > 0 && copter.current_loc.alt >= g.zr_tk_req_alt
int32_t alt_rev = 0;
if (copter.current_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_rev))
{
if (g.zr_tk_req_alt > 0 && alt_rev >= g.zr_tk_req_alt
&& copter.updownStatus < UpDown_RequestClimb)
{
@ -783,8 +786,8 @@ void ModeAuto::takeoff_run() @@ -783,8 +786,8 @@ void ModeAuto::takeoff_run()
}
set_mode(Mode::Number::BRAKE, ModeReason::REQUEST_CMD);
}
else if (g.zr_tk_req_alt > 0 && copter.current_loc.alt < g.zr_tk_req_alt
&& copter.current_loc.alt > 0
else if (g.zr_tk_req_alt > 0 && alt_rev < g.zr_tk_req_alt
&& alt_rev > 0
&&(AP_HAL::millis()-last_time_send_updownload>500))
{
last_time_send_updownload = AP_HAL::millis();
@ -793,7 +796,8 @@ void ModeAuto::takeoff_run() @@ -793,7 +796,8 @@ void ModeAuto::takeoff_run()
zr_flying_status_t.updown_status = copter.updownStatus;
zr_flying_status_t.countdown = 255;
gcs().update_zr_fly_status(&zr_flying_status_t);
}
}
}
}
auto_takeoff_run();

2
modules/mavlink

@ -1 +1 @@ @@ -1 +1 @@
Subproject commit 18cab6950986cd7b9b7a507ec35f3501d4a7e019
Subproject commit 4a6577557847fcb13f8bd9b7ebbe54f4a5a9f00e
Loading…
Cancel
Save