Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/ArduPlane/ArduPlane.cpp

817 lines
26 KiB
C++
Raw Blame History

/*
Lead developer: Andrew Tridgell
Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler, Amilcar Lucas, Gregory Fletcher, Paul Riseborough, Brandon Jones, Jon Challinger, Tom Pittenger
Thanks to: Chris Anderson, Michael Oborne, Paul Mather, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi, Yury Smirnov, Sandro Benigno, Max Levine, Roberto Navoni, Lorenz Meier, Yury MonZon
Please contribute your ideas! See https://ardupilot.org/dev for details
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "Plane.h"
#define SCHED_TASK(func, rate_hz, max_time_micros, priority) SCHED_TASK_CLASS(Plane, &plane, func, rate_hz, max_time_micros, priority)
/*
scheduler table - all regular tasks should be listed here.
All entries in this table must be ordered by priority.
This table is interleaved with the table presnet in each of the
vehicles to determine the order in which tasks are run. Convenience
methods SCHED_TASK and SCHED_TASK_CLASS are provided to build
entries in this structure:
SCHED_TASK arguments:
- name of static function to call
- rate (in Hertz) at which the function should be called
- expected time (in MicroSeconds) that the function should take to run
- priority (0 through 255, lower number meaning higher priority)
SCHED_TASK_CLASS arguments:
- class name of method to be called
- instance on which to call the method
- method to call on that instance
- rate (in Hertz) at which the method should be called
- expected time (in MicroSeconds) that the method should take to run
- priority (0 through 255, lower number meaning higher priority)
*/
const AP_Scheduler::Task Plane::scheduler_tasks[] = {
// Units: Hz us
SCHED_TASK(ahrs_update, 400, 400, 3),
SCHED_TASK(read_radio, 50, 100, 6),
SCHED_TASK(check_short_failsafe, 50, 100, 9),
SCHED_TASK(update_speed_height, 50, 200, 12),
SCHED_TASK(update_control_mode, 400, 100, 15),
SCHED_TASK(stabilize, 400, 100, 18),
SCHED_TASK(set_servos, 400, 100, 21),
SCHED_TASK(update_throttle_hover, 100, 90, 24),
SCHED_TASK(read_control_switch, 7, 100, 27),
SCHED_TASK(update_GPS_50Hz, 50, 300, 30),
SCHED_TASK(update_GPS_10Hz, 10, 400, 33),
SCHED_TASK(navigate, 10, 150, 36),
SCHED_TASK(update_compass, 10, 200, 39),
SCHED_TASK(calc_airspeed_errors, 10, 100, 42),
SCHED_TASK(update_alt, 10, 200, 45),
SCHED_TASK(adjust_altitude_target, 10, 200, 48),
#if ADVANCED_FAILSAFE == ENABLED
SCHED_TASK(afs_fs_check, 10, 100, 51),
#endif
SCHED_TASK(ekf_check, 10, 75, 54),
SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_receive, 300, 500, 57),
SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_send, 300, 750, 60),
SCHED_TASK_CLASS(AP_ServoRelayEvents, &plane.ServoRelayEvents, update_events, 50, 150, 63),
SCHED_TASK_CLASS(AP_BattMonitor, &plane.battery, read, 10, 300, 66),
SCHED_TASK_CLASS(AP_Baro, &plane.barometer, accumulate, 50, 150, 69),
SCHED_TASK_CLASS(AP_Notify, &plane.notify, update, 50, 300, 72),
SCHED_TASK(read_rangefinder, 50, 100, 78),
#if AP_ICENGINE_ENABLED
SCHED_TASK_CLASS(AP_ICEngine, &plane.g2.ice_control, update, 10, 100, 81),
#endif
#if AP_OPTICALFLOW_ENABLED
SCHED_TASK_CLASS(AP_OpticalFlow, &plane.optflow, update, 50, 50, 87),
#endif
SCHED_TASK(one_second_loop, 1, 400, 90),
SCHED_TASK(three_hz_loop, 3, 75, 93),
SCHED_TASK(check_long_failsafe, 3, 400, 96),
SCHED_TASK_CLASS(AP_RPM, &plane.rpm_sensor, update, 10, 100, 99),
#if AP_AIRSPEED_AUTOCAL_ENABLE
SCHED_TASK(airspeed_ratio_update, 1, 100, 102),
#endif // AP_AIRSPEED_AUTOCAL_ENABLE
#if HAL_MOUNT_ENABLED
SCHED_TASK_CLASS(AP_Mount, &plane.camera_mount, update, 50, 100, 105),
#endif // HAL_MOUNT_ENABLED
#if CAMERA == ENABLED
SCHED_TASK_CLASS(AP_Camera, &plane.camera, update, 50, 100, 108),
#endif // CAMERA == ENABLED
SCHED_TASK_CLASS(AP_Scheduler, &plane.scheduler, update_logging, 0.2, 100, 111),
SCHED_TASK(compass_save, 0.1, 200, 114),
SCHED_TASK(Log_Write_FullRate, 400, 300, 117),
SCHED_TASK(update_logging10, 10, 300, 120),
SCHED_TASK(update_logging25, 25, 300, 123),
#if HAL_SOARING_ENABLED
SCHED_TASK(update_soaring, 50, 400, 126),
#endif
SCHED_TASK(parachute_check, 10, 200, 129),
#if AP_TERRAIN_AVAILABLE
SCHED_TASK_CLASS(AP_Terrain, &plane.terrain, update, 10, 200, 132),
#endif // AP_TERRAIN_AVAILABLE
SCHED_TASK(update_is_flying_5Hz, 5, 100, 135),
#if LOGGING_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Logger, &plane.logger, periodic_tasks, 50, 400, 138),
#endif
SCHED_TASK_CLASS(AP_InertialSensor, &plane.ins, periodic, 50, 50, 141),
#if HAL_ADSB_ENABLED
SCHED_TASK(avoidance_adsb_update, 10, 100, 144),
#endif
SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&plane.g2.rc_channels, read_aux_all, 10, 200, 147),
#if HAL_BUTTON_ENABLED
SCHED_TASK_CLASS(AP_Button, &plane.button, update, 5, 100, 150),
#endif
#if STATS_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Stats, &plane.g2.stats, update, 1, 100, 153),
#endif
#if GRIPPER_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Gripper, &plane.g2.gripper, update, 10, 75, 156),
#endif
#if LANDING_GEAR_ENABLED == ENABLED
SCHED_TASK(landing_gear_update, 5, 50, 159),
#endif
};
void Plane::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
uint8_t &task_count,
uint32_t &log_bit)
{
tasks = &scheduler_tasks[0];
task_count = ARRAY_SIZE(scheduler_tasks);
log_bit = MASK_LOG_PM;
}
#if HAL_QUADPLANE_ENABLED
constexpr int8_t Plane::_failsafe_priorities[7];
#else
constexpr int8_t Plane::_failsafe_priorities[6];
#endif
// update AHRS system
void Plane::ahrs_update()
{
arming.update_soft_armed();
ahrs.update();
if (should_log(MASK_LOG_IMU)) {
AP::ins().Write_IMU();
}
// calculate a scaled roll limit based on current pitch
roll_limit_cd = aparm.roll_limit_cd;
pitch_limit_min_cd = aparm.pitch_limit_min_cd;
bool rotate_limits = true;
#if HAL_QUADPLANE_ENABLED
if (quadplane.tailsitter.active()) {
rotate_limits = false;
}
#endif
if (rotate_limits) {
roll_limit_cd *= ahrs.cos_pitch();
pitch_limit_min_cd *= fabsf(ahrs.cos_roll());
}
// updated the summed gyro used for ground steering and
// auto-takeoff. Dot product of DCM.c with gyro vector gives earth
// frame yaw rate
steer_state.locked_course_err += ahrs.get_yaw_rate_earth() * G_Dt;
steer_state.locked_course_err = wrap_PI(steer_state.locked_course_err);
#if HAL_QUADPLANE_ENABLED
// check if we have had a yaw reset from the EKF
quadplane.check_yaw_reset();
// update inertial_nav for quadplane
quadplane.inertial_nav.update();
#endif
if (should_log(MASK_LOG_VIDEO_STABILISATION)) {
ahrs.write_video_stabilisation();
}
}
/*
update 50Hz speed/height controller
*/
void Plane::update_speed_height(void)
{
if (control_mode->does_auto_throttle()) {
// Call TECS 50Hz update. Note that we call this regardless of
// throttle suppressed, as this needs to be running for
// takeoff detection
TECS_controller.update_50hz();
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_mode() ||
quadplane.in_assisted_flight()) {
quadplane.update_throttle_mix();
}
#endif
}
/*
read and update compass
*/
void Plane::update_compass(void)
{
compass.read();
}
/*
do 10Hz logging
*/
void Plane::update_logging10(void)
{
bool log_faster = (should_log(MASK_LOG_ATTITUDE_FULLRATE) || should_log(MASK_LOG_ATTITUDE_FAST));
if (should_log(MASK_LOG_ATTITUDE_MED) && !log_faster) {
Log_Write_Attitude();
ahrs.Write_AOA_SSA();
} else if (log_faster) {
ahrs.Write_AOA_SSA();
}
}
/*
do 25Hz logging
*/
void Plane::update_logging25(void)
{
// MASK_LOG_ATTITUDE_FULLRATE logs at 400Hz, MASK_LOG_ATTITUDE_FAST at 25Hz, MASK_LOG_ATTIUDE_MED logs at 10Hz
// highest rate selected wins
bool log_faster = should_log(MASK_LOG_ATTITUDE_FULLRATE);
if (should_log(MASK_LOG_ATTITUDE_FAST) && !log_faster) {
Log_Write_Attitude();
}
if (should_log(MASK_LOG_CTUN)) {
Log_Write_Control_Tuning();
AP::ins().write_notch_log_messages();
#if HAL_GYROFFT_ENABLED
gyro_fft.write_log_messages();
#endif
}
if (should_log(MASK_LOG_NTUN)) {
Log_Write_Nav_Tuning();
Log_Write_Guided();
}
if (should_log(MASK_LOG_RC))
Log_Write_RC();
if (should_log(MASK_LOG_IMU))
AP::ins().Write_Vibration();
}
/*
check for AFS failsafe check
*/
#if ADVANCED_FAILSAFE == ENABLED
void Plane::afs_fs_check(void)
{
// perform AFS failsafe checks
#if AP_FENCE_ENABLED
const bool fence_breached = fence.get_breaches() != 0;
#else
const bool fence_breached = false;
#endif
afs.check(fence_breached, failsafe.AFS_last_valid_rc_ms);
}
#endif
#if HAL_WITH_IO_MCU
#include <AP_IOMCU/AP_IOMCU.h>
extern AP_IOMCU iomcu;
#endif
void Plane::one_second_loop()
{
// make it possible to change control channel ordering at runtime
set_control_channels();
#if HAL_WITH_IO_MCU
iomcu.setup_mixing(&rcmap, g.override_channel.get(), g.mixing_gain, g2.manual_rc_mask);
#endif
#if HAL_ADSB_ENABLED
adsb.set_stall_speed_cm(aparm.airspeed_min * 100); // convert m/s to cm/s
adsb.set_max_speed(aparm.airspeed_max);
#endif
if (g2.flight_options & FlightOptions::ENABLE_DEFAULT_AIRSPEED) {
// use average of min and max airspeed as default airspeed fusion with high variance
ahrs.writeDefaultAirSpeed((float)((aparm.airspeed_min + aparm.airspeed_max)/2),
(float)((aparm.airspeed_max - aparm.airspeed_min)/2));
}
// sync MAVLink system ID
mavlink_system.sysid = g.sysid_this_mav;
SRV_Channels::enable_aux_servos();
// update notify flags
AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);
AP_Notify::flags.pre_arm_gps_check = true;
AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::Required::NO;
#if AP_TERRAIN_AVAILABLE
if (should_log(MASK_LOG_GPS)) {
terrain.log_terrain_data();
}
#endif
// update home position if NOT armed and gps position has
// changed. Update every 5s at most
if (!arming.is_armed() &&
gps.last_message_time_ms() - last_home_update_ms > 5000 &&
gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
last_home_update_ms = gps.last_message_time_ms();
update_home();
// reset the landing altitude correction
landing.alt_offset = 0;
}
// this ensures G_Dt is correct, catching startup issues with constructors
// calling the scheduler methods
if (!is_equal(1.0f/scheduler.get_loop_rate_hz(), scheduler.get_loop_period_s()) ||
!is_equal(G_Dt, scheduler.get_loop_period_s())) {
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
}
void Plane::three_hz_loop()
{
#if AP_FENCE_ENABLED
fence_check();
#endif
}
void Plane::compass_save()
{
if (AP::compass().available() &&
compass.get_learn_type() >= Compass::LEARN_INTERNAL &&
!hal.util->get_soft_armed()) {
/*
only save offsets when disarmed
*/
compass.save_offsets();
}
}
#if AP_AIRSPEED_AUTOCAL_ENABLE
/*
once a second update the airspeed calibration ratio
*/
void Plane::airspeed_ratio_update(void)
{
if (!airspeed.enabled() ||
gps.status() < AP_GPS::GPS_OK_FIX_3D ||
gps.ground_speed() < 4) {
// don't calibrate when not moving
return;
}
if (airspeed.get_airspeed() < aparm.airspeed_min &&
gps.ground_speed() < (uint32_t)aparm.airspeed_min) {
// don't calibrate when flying below the minimum airspeed. We
// check both airspeed and ground speed to catch cases where
// the airspeed ratio is way too low, which could lead to it
// never coming up again
return;
}
if (labs(ahrs.roll_sensor) > roll_limit_cd ||
ahrs.pitch_sensor > aparm.pitch_limit_max_cd ||
ahrs.pitch_sensor < pitch_limit_min_cd) {
// don't calibrate when going beyond normal flight envelope
return;
}
const Vector3f &vg = gps.velocity();
airspeed.update_calibration(vg, aparm.airspeed_max);
}
#endif // AP_AIRSPEED_AUTOCAL_ENABLE
/*
read the GPS and update position
*/
void Plane::update_GPS_50Hz(void)
{
gps.update();
// get position from AHRS
have_position = ahrs.get_location(current_loc);
ahrs.get_relative_position_D_home(relative_altitude);
relative_altitude *= -1.0f;
}
/*
read update GPS position - 10Hz update
*/
void Plane::update_GPS_10Hz(void)
{
static uint32_t last_gps_msg_ms;
if (gps.last_message_time_ms() != last_gps_msg_ms && gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
last_gps_msg_ms = gps.last_message_time_ms();
if (ground_start_count > 1) {
ground_start_count--;
} else if (ground_start_count == 1) {
// We countdown N number of good GPS fixes
// so that the altitude is more accurate
// -------------------------------------
if (current_loc.lat == 0 && current_loc.lng == 0) {
ground_start_count = 5;
} else if (!hal.util->was_watchdog_reset()) {
if (!set_home_persistently(gps.location())) {
// silently ignore failure...
}
next_WP_loc = prev_WP_loc = home;
ground_start_count = 0;
}
}
// update wind estimate
ahrs.estimate_wind();
} else if (gps.status() < AP_GPS::GPS_OK_FIX_3D && ground_start_count != 0) {
// lost 3D fix, start again
ground_start_count = 5;
}
calc_gndspeed_undershoot();
}
/*
main control mode dependent update code
*/
void Plane::update_control_mode(void)
{
if (control_mode != &mode_auto) {
// hold_course is only used in takeoff and landing
steer_state.hold_course_cd = -1;
}
update_fly_forward();
control_mode->update();
}
void Plane::update_fly_forward(void)
{
// ensure we are fly-forward when we are flying as a pure fixed
// wing aircraft. This helps the EKF produce better state
// estimates as it can make stronger assumptions
#if HAL_QUADPLANE_ENABLED
if (quadplane.available() &&
quadplane.tailsitter.is_in_fw_flight()) {
ahrs.set_fly_forward(true);
return;
}
if (quadplane.in_vtol_mode() ||
quadplane.in_assisted_flight()) {
ahrs.set_fly_forward(false);
return;
}
#endif
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
ahrs.set_fly_forward(landing.is_flying_forward());
return;
}
ahrs.set_fly_forward(true);
}
/*
set the flight stage
*/
void Plane::set_flight_stage(AP_Vehicle::FixedWing::FlightStage fs)
{
if (fs == flight_stage) {
return;
}
landing.handle_flight_stage_change(fs == AP_Vehicle::FixedWing::FLIGHT_LAND);
if (fs == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
gcs().send_text(MAV_SEVERITY_NOTICE, "Landing aborted, climbing to %dm",
int(auto_state.takeoff_altitude_rel_cm/100));
}
flight_stage = fs;
Log_Write_Status();
}
void Plane::update_alt()
{
barometer.update();
#if HAL_QUADPLANE_ENABLED
if (quadplane.available()) {
quadplane.motors->set_air_density_ratio(barometer.get_air_density_ratio());
}
#endif
// calculate the sink rate.
float sink_rate;
Vector3f vel;
if (ahrs.get_velocity_NED(vel)) {
sink_rate = vel.z;
} else if (gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.have_vertical_velocity()) {
sink_rate = gps.velocity().z;
} else {
sink_rate = -barometer.get_climb_rate();
}
// low pass the sink rate to take some of the noise out
auto_state.sink_rate = 0.8f * auto_state.sink_rate + 0.2f*sink_rate;
#if PARACHUTE == ENABLED
parachute.set_sink_rate(auto_state.sink_rate);
#endif
update_flight_stage();
if (control_mode->does_auto_throttle() && !throttle_suppressed) {
float distance_beyond_land_wp = 0;
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND && current_loc.past_interval_finish_line(prev_WP_loc, next_WP_loc)) {
distance_beyond_land_wp = current_loc.get_distance(next_WP_loc);
}
tecs_target_alt_cm = relative_target_altitude_cm();
if (control_mode == &mode_rtl && !rtl.done_climb && (g2.rtl_climb_min > 0 || (plane.g2.flight_options & FlightOptions::CLIMB_BEFORE_TURN))) {
// ensure we do the initial climb in RTL. We add an extra
// 10m in the demanded height to push TECS to climb
// quickly
tecs_target_alt_cm = MAX(tecs_target_alt_cm, prev_WP_loc.alt - home.alt) + (g2.rtl_climb_min+10)*100;
}
TECS_controller.update_pitch_throttle(tecs_target_alt_cm,
target_airspeed_cm,
flight_stage,
distance_beyond_land_wp,
get_takeoff_pitch_min_cd(),
throttle_nudge,
tecs_hgt_afe(),
aerodynamic_load_factor);
}
}
/*
recalculate the flight_stage
*/
void Plane::update_flight_stage(void)
{
// Update the speed & height controller states
if (control_mode->does_auto_throttle() && !throttle_suppressed) {
if (control_mode == &mode_auto) {
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_auto()) {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL);
return;
}
#endif
if (auto_state.takeoff_complete == false) {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_TAKEOFF);
return;
} else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) {
if (landing.is_commanded_go_around() || flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
// abort mode is sticky, it must complete while executing NAV_LAND
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND);
} else if (landing.get_abort_throttle_enable() && get_throttle_input() >= 90 &&
landing.request_go_around()) {
gcs().send_text(MAV_SEVERITY_INFO,"Landing aborted via throttle");
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND);
} else {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND);
}
return;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_assisted_flight()) {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL);
return;
}
#endif
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
} else if (control_mode != &mode_takeoff) {
// If not in AUTO then assume normal operation for normal TECS operation.
// This prevents TECS from being stuck in the wrong stage if you switch from
// AUTO to, say, FBWB during a landing, an aborted landing or takeoff.
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
}
return;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_mode() ||
quadplane.in_assisted_flight()) {
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL);
return;
}
#endif
set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
}
/*
If land_DisarmDelay is enabled (non-zero), check for a landing then auto-disarm after time expires
only called from AP_Landing, when the landing library is ready to disarm
*/
void Plane::disarm_if_autoland_complete()
{
if (landing.get_disarm_delay() > 0 &&
!is_flying() &&
arming.arming_required() != AP_Arming::Required::NO &&
arming.is_armed()) {
/* we have auto disarm enabled. See if enough time has passed */
if (millis() - auto_state.last_flying_ms >= landing.get_disarm_delay()*1000UL) {
if (arming.disarm(AP_Arming::Method::AUTOLANDED)) {
gcs().send_text(MAV_SEVERITY_INFO,"Auto disarmed");
}
}
}
}
/*
the height above field elevation that we pass to TECS
*/
float Plane::tecs_hgt_afe(void)
{
/*
pass the height above field elevation as the height above
the ground when in landing, which means that TECS gets the
rangefinder information and thus can know when the flare is
coming.
*/
float hgt_afe;
if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
hgt_afe = height_above_target();
hgt_afe -= rangefinder_correction();
} else {
// when in normal flight we pass the hgt_afe as relative
// altitude to home
hgt_afe = relative_altitude;
}
return hgt_afe;
}
// vehicle specific waypoint info helpers
bool Plane::get_wp_distance_m(float &distance) const
{
// see GCS_MAVLINK_Plane::send_nav_controller_output()
if (control_mode == &mode_manual) {
return false;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_mode()) {
distance = quadplane.using_wp_nav() ? quadplane.wp_nav->get_wp_distance_to_destination() * 0.01 : 0;
return true;
}
#endif
distance = auto_state.wp_distance;
return true;
}
bool Plane::get_wp_bearing_deg(float &bearing) const
{
// see GCS_MAVLINK_Plane::send_nav_controller_output()
if (control_mode == &mode_manual) {
return false;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_mode()) {
bearing = quadplane.using_wp_nav() ? quadplane.wp_nav->get_wp_bearing_to_destination() : 0;
return true;
}
#endif
bearing = nav_controller->target_bearing_cd() * 0.01;
return true;
}
bool Plane::get_wp_crosstrack_error_m(float &xtrack_error) const
{
// see GCS_MAVLINK_Plane::send_nav_controller_output()
if (control_mode == &mode_manual) {
return false;
}
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_mode()) {
xtrack_error = quadplane.using_wp_nav() ? quadplane.wp_nav->crosstrack_error() : 0;
return true;
}
#endif
xtrack_error = nav_controller->crosstrack_error();
return true;
}
#if AP_SCRIPTING_ENABLED
// set target location (for use by scripting)
bool Plane::set_target_location(const Location &target_loc)
{
Location loc{target_loc};
if (plane.control_mode != &plane.mode_guided) {
// only accept position updates when in GUIDED mode
return false;
}
// add home alt if needed
if (loc.relative_alt) {
loc.alt += plane.home.alt;
loc.relative_alt = 0;
}
plane.set_guided_WP(loc);
return true;
}
// set target location (for use by scripting)
bool Plane::get_target_location(Location& target_loc)
{
switch (control_mode->mode_number()) {
case Mode::Number::RTL:
case Mode::Number::AVOID_ADSB:
case Mode::Number::GUIDED:
case Mode::Number::AUTO:
case Mode::Number::LOITER:
case Mode::Number::TAKEOFF:
#if HAL_QUADPLANE_ENABLED
case Mode::Number::QLOITER:
case Mode::Number::QLAND:
case Mode::Number::QRTL:
#endif
target_loc = next_WP_loc;
return true;
break;
default:
break;
}
return false;
}
/*
update_target_location() works in all auto navigation modes
*/
bool Plane::update_target_location(const Location &old_loc, const Location &new_loc)
{
if (!old_loc.same_latlon_as(next_WP_loc)) {
return false;
}
ftype alt_diff;
if (!old_loc.get_alt_distance(next_WP_loc, alt_diff) ||
!is_zero(alt_diff)) {
return false;
}
next_WP_loc = new_loc;
next_WP_loc.change_alt_frame(old_loc.get_alt_frame());
return true;
}
// allow for velocity matching in VTOL
bool Plane::set_velocity_match(const Vector2f &velocity)
{
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_mode() || quadplane.in_vtol_land_sequence()) {
quadplane.poscontrol.velocity_match = velocity;
quadplane.poscontrol.last_velocity_match_ms = AP_HAL::millis();
return true;
}
#endif
return false;
}
#endif // AP_SCRIPTING_ENABLED
// correct AHRS pitch for TRIM_PITCH_CD in non-VTOL modes, and return VTOL view in VTOL
void Plane::get_osd_roll_pitch_rad(float &roll, float &pitch) const
{
#if HAL_QUADPLANE_ENABLED
if (quadplane.show_vtol_view()) {
pitch = quadplane.ahrs_view->pitch;
roll = quadplane.ahrs_view->roll;
return;
}
#endif
pitch = ahrs.pitch;
roll = ahrs.roll;
if (!(g2.flight_options & FlightOptions::OSD_REMOVE_TRIM_PITCH_CD)) { // correct for TRIM_PITCH_CD
pitch -= g.pitch_trim_cd * 0.01 * DEG_TO_RAD;
}
}
AP_HAL_MAIN_CALLBACKS(&plane);
Reference in New Issue View Git Blame Copy Permalink