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

Plane: added a soaring controller to Arduplane

This commit is contained in:
Andrey Kolobov 2017-02-27 10:18:38 +11:00 committed by Andrew Tridgell
parent a1fa2a9de3
commit 35d406aeab
11 changed files with 145 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduPlane/ArduPlane.cpp
View File

@ -76,6 +76,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
SCHED_TASK(Log_Write_Fast, 25, 300),
SCHED_TASK(update_logging1, 10, 300),
SCHED_TASK(update_logging2, 10, 300),
SCHED_TASK(update_soaring, 50, 400),
SCHED_TASK(parachute_check, 10, 200),
SCHED_TASK(terrain_update, 10, 200),
SCHED_TASK(update_is_flying_5Hz, 5, 100),

2
ArduPlane/GCS_Mavlink.cpp
View File

@ -252,7 +252,7 @@ void Plane::send_vfr_hud(mavlink_channel_t chan)
(ahrs.yaw_sensor / 100) % 360,
abs(throttle_percentage()),
current_loc.alt / 100.0f,
barometer.get_climb_rate());
(g2.soaring_controller.is_active() ? g2.soaring_controller.get_vario_reading() : barometer.get_climb_rate()));
}
/*

7
ArduPlane/Parameters.cpp
View File

@ -1213,11 +1213,16 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Path: ../libraries/RC_Channel/RC_Channels.cpp
AP_SUBGROUPINFO(rc_channels, "RC", 7, ParametersG2, RC_Channels),
// @Group: SOAR_
// @Path: ../libraries/AP_Soaring/AP_Soaring.cpp
AP_SUBGROUPINFO(soaring_controller, "SOAR_", 8, ParametersG2, SoaringController),
AP_GROUPEND
};
ParametersG2::ParametersG2(void) :
ice_control(plane.rpm_sensor, plane.ahrs)
ice_control(plane.rpm_sensor, plane.ahrs),
soaring_controller(plane.ahrs, plane.TECS_controller, plane.aparm)
{
AP_Param::setup_object_defaults(this, var_info);
}

3
ArduPlane/Parameters.h
View File

@ -542,6 +542,9 @@ public:
// whether to enforce acceptance of packets only from sysid_my_gcs
AP_Int8 sysid_enforce;
// ArduSoar parameters
SoaringController soaring_controller;
};
extern const AP_Param::Info var_info[];

6
ArduPlane/Plane.h
View File

@ -76,6 +76,7 @@
#include <AP_NavEKF3/AP_NavEKF3.h>
#include <AP_Mission/AP_Mission.h> // Mission command library
#include <AP_Soaring/AP_Soaring.h>
#include <AP_Notify/AP_Notify.h> // Notify library
#include <AP_BattMonitor/AP_BattMonitor.h> // Battery monitor library
@ -216,7 +217,7 @@ private:
#endif
AP_L1_Control L1_controller {ahrs};
AP_TECS TECS_controller {ahrs, aparm, landing};
AP_TECS TECS_controller {ahrs, aparm, landing, g2.soaring_controller};
// Attitude to servo controllers
AP_RollController rollController {ahrs, aparm, DataFlash};
@ -410,7 +411,7 @@ private:
} acro_state;
// CRUISE controller state
struct {
struct CruiseState {
bool locked_heading;
int32_t locked_heading_cd;
uint32_t lock_timer_ms;
@ -1091,6 +1092,7 @@ private:
#endif
void accel_cal_update(void);
void update_soft_armed();
void update_soaring();
// support for AP_Avoidance custom flight mode, AVOID_ADSB
bool avoid_adsb_init(bool ignore_checks);

8
ArduPlane/defines.h
View File

@ -80,6 +80,10 @@ enum mode_reason_t {
MODE_REASON_FENCE_BREACH,
MODE_REASON_AVOIDANCE,
MODE_REASON_AVOIDANCE_RECOVERY,
MODE_REASON_SOARING_FBW_B_WITH_MOTOR_RUNNING,
MODE_REASON_SOARING_THERMAL_DETECTED,
MODE_REASON_SOARING_IN_THERMAL,
MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED
};
// type of stick mixing enabled
@ -135,7 +139,9 @@ enum log_messages {
LOG_STATUS_MSG,
LOG_OPTFLOW_MSG,
LOG_QTUN_MSG,
LOG_PARAMTUNE_MSG
LOG_PARAMTUNE_MSG,
LOG_THERMAL_MSG,
LOG_VARIO_MSG
};
#define MASK_LOG_ATTITUDE_FAST (1<<0)

1
ArduPlane/make.inc
View File

@ -63,3 +63,4 @@ LIBRARIES += AP_Stats
LIBRARIES += AP_Landing
LIBRARIES += AP_Beacon
LIBRARIES += PID
LIBRARIES += AP_Soaring

8
ArduPlane/servos.cpp
View File

@ -477,6 +477,14 @@ void Plane::set_servos_controlled(void)
// ask quadplane code for forward throttle
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, quadplane.forward_throttle_pct());
}
// suppress throttle when soaring is active
if ((control_mode == FLY_BY_WIRE_B || control_mode == CRUISE ||
control_mode == AUTO || control_mode == LOITER) &&
g2.soaring_controller.is_active() &&
g2.soaring_controller.get_throttle_suppressed()) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
}
}
/*

96
ArduPlane/soaring.cpp Normal file
View File

@ -0,0 +1,96 @@
#include "Plane.h"
/*
* ArduSoar support function
*
* Peter Braswell, Samuel Tabor, Andrey Kolobov, and Iain Guilliard
*/
void Plane::update_soaring() {
if (!g2.soaring_controller.is_active()) {
return;
}
g2.soaring_controller.update_vario();
// Check for throttle suppression change.
switch (control_mode){
case AUTO:
g2.soaring_controller.suppress_throttle();
break;
case FLY_BY_WIRE_B:
case CRUISE:
if (!g2.soaring_controller.suppress_throttle()) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Soaring: forcing RTL");
set_mode(RTL, MODE_REASON_SOARING_FBW_B_WITH_MOTOR_RUNNING);
}
break;
case LOITER:
// Do nothing. We will switch back to auto/rtl before enabling throttle.
break;
default:
// This does not affect the throttle since suppressed is only checked in the above three modes.
// It ensures that the soaring always starts with throttle suppressed though.
g2.soaring_controller.set_throttle_suppressed(true);
break;
}
// Nothing to do if we are in powered flight
if (!g2.soaring_controller.get_throttle_suppressed() && aparm.throttle_max > 0) {
return;
}
switch (control_mode){
case AUTO:
case FLY_BY_WIRE_B:
case CRUISE:
// Test for switch into thermalling mode
g2.soaring_controller.update_cruising();
if (g2.soaring_controller.check_thermal_criteria()) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Soaring: Thermal detected, entering loiter");
set_mode(LOITER, MODE_REASON_SOARING_THERMAL_DETECTED);
}
break;
case LOITER:
// Update thermal estimate and check for switch back to AUTO
g2.soaring_controller.update_thermalling(); // Update estimate
if (g2.soaring_controller.check_cruise_criteria()) {
// Exit as soon as thermal state estimate deteriorates
switch (previous_mode) {
case FLY_BY_WIRE_B:
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Soaring: Thermal ended, entering RTL");
set_mode(RTL, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED);
break;
case CRUISE: {
// return to cruise with old ground course
CruiseState cruise = cruise_state;
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Soaring: Thermal ended, restoring CRUISE");
set_mode(CRUISE, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED);
cruise_state = cruise;
set_target_altitude_current();
break;
}
case AUTO:
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Soaring: Thermal ended, restoring AUTO");
set_mode(AUTO, MODE_REASON_SOARING_THERMAL_ESTIMATE_DETERIORATED);
break;
default:
break;
}
} else {
// still in thermal - need to update the wp location
g2.soaring_controller.get_target(next_WP_loc);
}
break;
default:
// nothing to do
break;
}
}

17
ArduPlane/system.cpp
View File

@ -417,12 +417,20 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
auto_navigation_mode = false;
cruise_state.locked_heading = false;
cruise_state.lock_timer_ms = 0;
// for ArduSoar soaring_controller
g2.soaring_controller.init_cruising();
set_target_altitude_current();
break;
case FLY_BY_WIRE_B:
auto_throttle_mode = true;
auto_navigation_mode = false;
// for ArduSoar soaring_controller
g2.soaring_controller.init_cruising();
set_target_altitude_current();
break;
@ -444,6 +452,8 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
next_WP_loc = prev_WP_loc = current_loc;
// start or resume the mission, based on MIS_AUTORESET
mission.start_or_resume();
g2.soaring_controller.init_cruising();
break;
case RTL:
@ -457,6 +467,13 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
auto_throttle_mode = true;
auto_navigation_mode = true;
do_loiter_at_location();
if (g2.soaring_controller.is_active() &&
g2.soaring_controller.suppress_throttle()) {
g2.soaring_controller.init_thermalling();
g2.soaring_controller.get_target(next_WP_loc); // ahead on flight path
}
break;
case AVOID_ADSB:

1
ArduPlane/wscript
View File

@ -34,6 +34,7 @@ def build(bld):
'AP_Landing',
'AP_Beacon',
'PID',
'AP_Soaring'
],
)