Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 15:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: allow taking off in Guided mode's attitude control sub mode

Browse Source
This commit is contained in:
Randy Mackay 2016-08-02 18:25:28 +09:00
parent 4e92f08bf1
commit 40db19549e
1 changed files with 6 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduCopter/control_guided.cpp
View File

@ -278,6 +278,11 @@ void Copter::guided_set_angle(const Quaternion &q, float climb_rate_cms)
guided_angle_state.climb_rate_cms = climb_rate_cms;
guided_angle_state.update_time_ms = millis();
// interpret positive climb rate as triggering take-off
if (motors.armed() && !ap.auto_armed && (guided_angle_state.climb_rate_cms > 0.0f)) {
set_auto_armed(true);
}
// log target
Log_Write_GuidedTarget(guided_mode,
Vector3f(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd),
@ -536,7 +541,7 @@ void Copter::guided_posvel_control_run()
void Copter::guided_angle_control_run()
{
// if not auto armed or motors not enabled set throttle to zero and exit immediately
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || (ap.land_complete && guided_angle_state.climb_rate_cms <= 0.0f)) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control.set_yaw_target_to_current_heading();