Browse Source

Copter: use simplified precland interface

mission-4.1.18
Jonathan Challinger 9 years ago committed by Randy Mackay
parent
commit
91a5b26725
  1. 2
      ArduCopter/Copter.h
  2. 9
      ArduCopter/control_auto.cpp
  3. 8
      ArduCopter/control_land.cpp

2
ArduCopter/Copter.h

@ -269,6 +269,8 @@ private: @@ -269,6 +269,8 @@ private:
uint32_t start_ms;
} takeoff_state;
uint32_t precland_last_update_ms;
RCMapper rcmap;
// board specific config

9
ArduCopter/control_auto.cpp

@ -420,10 +420,15 @@ void Copter::auto_land_run() @@ -420,10 +420,15 @@ void Copter::auto_land_run()
// process roll, pitch inputs
wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);
#if PRECISION_LANDING == ENABLED
// run precision landing
if (!ap.land_repo_active) {
wp_nav.shift_loiter_target(precland.get_target_shift(wp_nav.get_loiter_target()));
if (!ap.land_repo_active && precland.target_acquired() && precland_last_update_ms != precland.last_update_ms()) {
Vector3f target_pos;
precland.get_target_position(target_pos);
pos_control.set_xy_target(target_pos.x, target_pos.y);
pos_control.freeze_ff_xy();
precland_last_update_ms = precland.last_update_ms();
}
#endif

8
ArduCopter/control_land.cpp

@ -123,8 +123,12 @@ void Copter::land_gps_run() @@ -123,8 +123,12 @@ void Copter::land_gps_run()
#if PRECISION_LANDING == ENABLED
// run precision landing
if (!ap.land_repo_active) {
wp_nav.shift_loiter_target(precland.get_target_shift(wp_nav.get_loiter_target()));
if (!ap.land_repo_active && precland.target_acquired() && precland_last_update_ms != precland.last_update_ms()) {
Vector3f target_pos;
precland.get_target_position(target_pos);
pos_control.set_xy_target(target_pos.x, target_pos.y);
pos_control.freeze_ff_xy();
precland_last_update_ms = precland.last_update_ms();
}
#endif

Loading…
Cancel
Save