Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: allow TAKEOFF mode on board with no compass

Browse Source
This commit is contained in:
Andrew Tridgell 2019-10-10 13:46:46 +11:00
parent e92ef64fe5
commit 3194059408
2 changed files with 14 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduPlane/mode.h
View File

@ -1,6 +1,7 @@
#pragma once
#include <AP_Param/AP_Param.h>
#include <AP_Common/Location.h>
#include <stdint.h>
class Mode
@ -474,8 +475,8 @@ protected:
AP_Int16 level_alt;
AP_Int8 level_pitch;
int32_t initial_alt_cm;
bool takeoff_started;
Location start_loc;
bool _enter() override;
};

17
ArduPlane/mode_takeoff.cpp
View File

@ -81,13 +81,12 @@ void ModeTakeoff::update()
const float alt = target_alt;
const float direction = degrees(plane.ahrs.yaw);
start_loc = plane.current_loc;
plane.prev_WP_loc = plane.current_loc;
plane.next_WP_loc = plane.current_loc;
plane.next_WP_loc.alt += alt*100.0;
plane.next_WP_loc.offset_bearing(direction, dist);
initial_alt_cm = plane.current_loc.alt;
plane.crash_state.is_crashed = false;
plane.auto_state.takeoff_pitch_cd = level_pitch * 100;
@ -105,9 +104,17 @@ void ModeTakeoff::update()
// TKOFF_LVL_ALT or we pass the target location. The check for
// target location prevents us flying forever if we can't climb
if (plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF &&
(plane.current_loc.alt - initial_alt_cm >= level_alt*100 ||
plane.current_loc.past_interval_finish_line(plane.prev_WP_loc, plane.next_WP_loc))) {
// reached level alt
(plane.current_loc.alt - start_loc.alt >= level_alt*100 ||
start_loc.get_distance(plane.current_loc) >= target_dist)) {
// reached level alt, re-calculate bearing to cope with systems with no compass
// or with poor initial compass
float direction = start_loc.get_bearing_to(plane.current_loc) * 0.01;
float dist_done = start_loc.get_distance(plane.current_loc);
const float dist = target_dist;
plane.next_WP_loc = plane.current_loc;
plane.next_WP_loc.offset_bearing(direction, MAX(dist-dist_done, 0));
plane.set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
plane.complete_auto_takeoff();
}