aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: added new TECS altitude controller

Browse Source 
Includes improvements to takeoff/launch detect logic to support hand
launching for X-8 flight
This commit is contained in:
Paul Riseborough 2013-06-26 18:36:53 +10:00 committed by Andrew Tridgell
parent 5b0129e02b
commit d62636d309
8 changed files with 127 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
ArduPlane/ArduPlane.pde
View File

@ -59,6 +59,9 @@
#include <AP_L1_Control.h>
#include <AP_RCMapper.h> // RC input mapping library
#include <AP_SpdHgtControl.h>
#include <AP_TECS.h>
// Pre-AP_HAL compatibility
#include "compat.h"
@ -232,6 +235,7 @@ AP_InertialSensor_Oilpan ins( &apm1_adc );
AP_AHRS_DCM ahrs(&ins, g_gps);
static AP_L1_Control L1_controller(&ahrs);
static AP_TECS TECS_controller(&ahrs, &barometer);
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
SITL sitl;
@ -250,6 +254,9 @@ static GCS_MAVLINK gcs3;
// selected navigation controller
static AP_Navigation *nav_controller = &L1_controller;
// selected navigation controller
static AP_SpdHgtControl *SpdHgt_Controller = &TECS_controller;
////////////////////////////////////////////////////////////////////////////////
// Analog Inputs
////////////////////////////////////////////////////////////////////////////////
@ -342,6 +349,14 @@ static uint32_t last_heartbeat_ms;
// A timer used to track how long we have been in a "short failsafe" condition due to loss of RC signal
static uint32_t ch3_failsafe_timer = 0;
////////////////////////////////////////////////////////////////////////////////
// AUTO takeoff
////////////////////////////////////////////////////////////////////////////////
// States used for delay from launch detect to engine start
static bool launchCountStarted;
static uint32_t last_tkoff_arm_time;
////////////////////////////////////////////////////////////////////////////////
// LED output
////////////////////////////////////////////////////////////////////////////////
@ -456,7 +471,7 @@ static bool takeoff_complete = true;
//Set land_complete if we are within 2 seconds distance or within 3 meters altitude of touchdown
static bool land_complete;
// Altitude threshold to complete a takeoff command in autonomous modes. Centimeters
static int32_t takeoff_altitude;
static int32_t takeoff_altitude_cm;
// Minimum pitch to hold during takeoff command execution. Hundredths of a degree
static int16_t takeoff_pitch_cd;
@ -684,7 +699,7 @@ void setup() {
batt_volt_pin = hal.analogin->channel(g.battery_volt_pin);
batt_curr_pin = hal.analogin->channel(g.battery_curr_pin);
init_ardupilot();
// initialise the main loop scheduler
@ -761,6 +776,11 @@ static void fast_loop()
inertialNavigation();
#endif
if (g.alt_control_algorithm == ALT_CONTROL_TECS) {
// Call TECS 50Hz update
SpdHgt_Controller->update_50hz();
}
// custom code/exceptions for flight modes
// calculates roll, pitch and yaw demands from navigation demands
// --------------------------------------------------------------
@ -1215,6 +1235,15 @@ static void update_alt()
// Calculate new climb rate
//if(medium_loopCounter == 0 && slow_loopCounter == 0)
// add_altitude_data(millis() / 100, g_gps->altitude / 10);
// Update the speed & height controller states
if (g.alt_control_algorithm == ALT_CONTROL_TECS) {
SpdHgt_Controller->update_pitch_throttle(target_altitude_cm - home.alt, target_airspeed_cm,
nav_command_ID == MAV_CMD_NAV_TAKEOFF, takeoff_pitch_cd);
if (g.log_bitmask & MASK_LOG_TECS) {
Log_Write_TECS_Tuning();
}
}
}
AP_HAL_MAIN();

57
ArduPlane/Attitude.pde
View File

@ -273,7 +273,9 @@ static void calc_throttle()
return;
}
if (!alt_control_airspeed()) {
if (g.alt_control_algorithm == ALT_CONTROL_TECS) {
channel_throttle->servo_out = SpdHgt_Controller->get_throttle_demand();
} else if (!alt_control_airspeed()) {
int16_t throttle_target = g.throttle_cruise + throttle_nudge;
// TODO: think up an elegant way to bump throttle when
@ -303,6 +305,7 @@ static void calc_throttle()
g.throttle_min.get(), g.throttle_max.get());
}
}
/*****************************************
@ -336,7 +339,9 @@ static void calc_nav_pitch()
{
// Calculate the Pitch of the plane
// --------------------------------
if (alt_control_airspeed()) {
if (g.alt_control_algorithm == ALT_CONTROL_TECS) {
nav_pitch_cd = SpdHgt_Controller->get_pitch_demand();
} else if (alt_control_airspeed()) {
nav_pitch_cd = -g.pidNavPitchAirspeed.get_pid(airspeed_error_cm);
} else {
nav_pitch_cd = g.pidNavPitchAltitude.get_pid(altitude_error_cm);
@ -381,11 +386,13 @@ static void throttle_slew_limit(int16_t last_throttle)
}
}
/*
check for automatic takeoff conditions being met
*/
static bool auto_takeoff_check(void)
{
#if 1
if (g_gps == NULL || g_gps->status() != GPS::GPS_OK_FIX_3D) {
// no auto takeoff without GPS lock
return false;
@ -411,6 +418,52 @@ static bool auto_takeoff_check(void)
// we're good for takeoff
return true;
#else
// this is a more advanced check that relies on TECS
uint32_t now = hal.scheduler->micros();
if (g_gps == NULL || g_gps->status() != GPS::GPS_OK_FIX_3D)
{
// no auto takeoff without GPS lock
return false;
}
if (SpdHgt_Controller->get_VXdot() >= g.takeoff_throttle_min_accel || g.takeoff_throttle_min_accel == 0.0 || launchCountStarted)
{
if (!launchCountStarted)
{
launchCountStarted = true;
last_tkoff_arm_time = now;
gcs_send_text_fmt(PSTR("Armed AUTO, xaccel = %.1f m/s/s, waiting %.1f sec"), SpdHgt_Controller->get_VXdot(), 0.1f*float(min(g.takeoff_throttle_delay,15)));
}
if ((now - last_tkoff_arm_time) <= 2500000)
{
if ((g_gps->ground_speed > g.takeoff_throttle_min_speed*100.0f || g.takeoff_throttle_min_speed == 0.0) && ((now -last_tkoff_arm_time) >= min(uint32_t(g.takeoff_throttle_delay*100000),1500000)))
{
gcs_send_text_fmt(PSTR("Triggered AUTO, GPSspd = %.1f"), g_gps->ground_speed*100.0f);
launchCountStarted = false;
last_tkoff_arm_time = 0;
return true;
}
else
{
launchCountStarted = true;
return false;
}
}
else
{
gcs_send_text_fmt(PSTR("Timeout AUTO"));
launchCountStarted = false;
last_tkoff_arm_time = 0;
return false;
}
}
launchCountStarted = false;
last_tkoff_arm_time = 0;
return false;
#endif
}

9
ArduPlane/Log.pde
View File

@ -49,6 +49,7 @@ print_log_menu(void)
PLOG(CMD);
PLOG(CURRENT);
PLOG(COMPASS);
PLOG(TECS);
#undef PLOG
}
@ -317,6 +318,12 @@ static void Log_Write_Control_Tuning()
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
// Write a TECS tuning packet
static void Log_Write_TECS_Tuning(void)
{
SpdHgt_Controller->log_data(DataFlash, LOG_TECS_MSG);
}
struct PACKED log_Nav_Tuning {
LOG_PACKET_HEADER;
uint16_t yaw;
@ -447,6 +454,7 @@ static const struct LogStructure log_structure[] PROGMEM = {
"CURR", "hhhHf", "Thr,Volt,Curr,Vcc,CurrTot" },
{ LOG_COMPASS_MSG, sizeof(log_Compass),
"MAG", "hhhhhhhhh", "MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" },
TECS_LOG_FORMAT(LOG_TECS_MSG),
};
// Read the DataFlash.log memory : Packet Parser
@ -478,6 +486,7 @@ static void Log_Write_Startup(uint8_t type) {}
static void Log_Write_Cmd(uint8_t num, const struct Location *wp) {}
static void Log_Write_Current() {}
static void Log_Write_Nav_Tuning() {}
static void Log_Write_TECS_Tuning() {}
static void Log_Write_Performance() {}
static void Log_Write_Attitude() {}
static void Log_Write_Control_Tuning() {}

6
ArduPlane/Parameters.h
View File

@ -90,6 +90,7 @@ public:
k_param_mixing_gain,
k_param_scheduler,
k_param_relay,
k_param_takeoff_throttle_delay,
// 110: Telemetry control
//
@ -231,6 +232,7 @@ public:
k_param_yawController,
k_param_L1_controller,
k_param_rcmap,
k_param_TECS_controller,
//
// 240: PID Controllers
@ -269,6 +271,9 @@ public:
// navigation controller type. See AP_Navigation::ControllerType
AP_Int8 nav_controller;
// attitude controller type.
AP_Int8 att_controller;
// Estimation
//
AP_Float altitude_mix;
@ -376,6 +381,7 @@ public:
AP_Int8 stick_mixing;
AP_Float takeoff_throttle_min_speed;
AP_Float takeoff_throttle_min_accel;
AP_Int8 takeoff_throttle_delay;
AP_Int8 level_roll_limit;
// RC channels

17
ArduPlane/Parameters.pde
View File

@ -87,7 +87,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: TKOFF_THR_MINSPD
// @DisplayName: Takeoff throttle min speed
// @Description: Minimum GPS ground speed in m/s before un-suppressing throttle in auto-takeoff. This is meant to be used for catapult launches where you want the motor to engage only after the plane leaves the catapult. Note that the GPS velocity will lag the real velocity by about 0.5seconds.
// @Description: Minimum GPS ground speed in m/s used by the speed check that un-suppresses throttle in auto-takeoff. This can be be used for catapult launches where you want the motor to engage only after the plane leaves the catapult, but it is preferable to use the TKOFF_THR_MINACC and TKOFF_THR_DELAY parameters for cvatapult launches due to the errors associated with GPS measurements. For hand launches with a pusher prop it is strongly advised that this parameter be set to a value no less than 4 m/s to provide additional protection against premature motor start. Note that the GPS velocity will lag the real velocity by about 0.5 seconds. The ground speed check is delayed by the TKOFF_THR_DELAY parameter.
// @Units: m/s
// @Range: 0 30
// @Increment: 0.1
@ -96,13 +96,22 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: TKOFF_THR_MINACC
// @DisplayName: Takeoff throttle min acceleration
// @Description: Minimum forward acceleration in m/s/s before un-suppressing throttle in auto-takeoff. This is meant to be used for hand launches with a tractor style (front engine) plane. If this is set then the auto takeoff will only trigger if the pitch of the plane is between -30 and +45 degrees, and the roll is less than 30 degrees. This makes it less likely it will trigger due to carrying the plane with the nose down.
// @Description: Minimum forward acceleration in m/s/s before arming the ground speed check in auto-takeoff. This is meant to be used for hand launches. Setting this value to 0 disables the acceleration test which means the ground speed check will always be armed which could allow GPS velocity jumps to start the engine. For hand launches this should be set to 15.
// @Units: m/s/s
// @Range: 0 30
// @Increment: 0.1
// @User: User
GSCALAR(takeoff_throttle_min_accel, "TKOFF_THR_MINACC", 0),
// @Param: TKOFF_THR_DELAY
// @DisplayName: Takeoff throttle delay
// @Description: This parameter sets the time delay (in 1/10ths of a second) that the ground speed check is delayed after the forward acceleration check controlled by TKOFF_THR_MINACC has passed. For hand launches with pusher propellers it is essential that this is set to a value of no less than 2 (0.2 seconds) to ensure that the aircraft is safely clear of the throwers arm before the motor can start.
// @Units: 0.1 seconds
// @Range: 0 15
// @Increment: 1
// @User: User
GSCALAR(takeoff_throttle_delay, "TKOFF_THR_DELAY", 0),
// @Param: LEVEL_ROLL_LIMIT
// @DisplayName: Level flight roll limit
// @Description: This controls the maximum bank angle in degrees during flight modes where level flight is desired, such as in the final stages of landing, and during auto takeoff. This should be a small angle (such as 5 degrees) to prevent a wing hitting the runway during takeoff or landing. Setting this to zero will completely disable heading hold on auto takeoff and final landing approach.
@ -813,6 +822,10 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Path: ../libraries/AP_L1_Control/AP_L1_Control.cpp
GOBJECT(L1_controller, "NAVL1_", AP_L1_Control),
// @Group: TECS_
// @Path: ../libraries/AP_TECS/AP_TECS.cpp
GOBJECT(TECS_controller, "TECS_", AP_TECS),
#if MOUNT == ENABLED
// @Group: MNT_
// @Path: ../libraries/AP_Mount/AP_Mount.cpp

5
ArduPlane/commands_logic.pde
View File

@ -254,7 +254,7 @@ static void do_takeoff()
set_next_WP(&next_nav_command);
// pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
takeoff_pitch_cd = (int)next_nav_command.p1 * 100;
takeoff_altitude = next_nav_command.alt;
takeoff_altitude_cm = next_nav_command.alt;
next_WP.lat = home.lat + 1000; // so we don't have bad calcs
next_WP.lng = home.lng + 1000; // so we don't have bad calcs
takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction
@ -322,7 +322,8 @@ static bool verify_takeoff()
nav_controller->update_level_flight();
}
if (adjusted_altitude_cm() > takeoff_altitude) {
// see if we have reached takeoff altitude
if (adjusted_altitude_cm() > takeoff_altitude) {
hold_course_cd = -1;
takeoff_complete = true;
next_WP = prev_WP = current_loc;

3
ArduPlane/config.h
View File

@ -646,7 +646,8 @@
MASK_LOG_MODE | \
MASK_LOG_CMD | \
MASK_LOG_COMPASS | \
MASK_LOG_CURRENT
MASK_LOG_CURRENT | \
MASK_LOG_TECS

9
ArduPlane/defines.h
View File

@ -157,7 +157,8 @@ enum log_messages {
LOG_ATTITUDE_MSG,
LOG_MODE_MSG,
LOG_COMPASS_MSG,
MAX_NUM_LOGS
LOG_TECS_MSG,
MAX_NUM_LOGS // always at the end
};
#define MASK_LOG_ATTITUDE_FAST (1<<0)
@ -171,6 +172,7 @@ enum log_messages {
#define MASK_LOG_CMD (1<<8)
#define MASK_LOG_CURRENT (1<<9)
#define MASK_LOG_COMPASS (1<<10)
#define MASK_LOG_TECS (1<<11)
// Waypoint Modes
// ----------------
@ -250,8 +252,9 @@ enum log_messages {
// altitude control algorithms
enum {
ALT_CONTROL_DEFAULT=0,
ALT_CONTROL_NON_AIRSPEED=1
ALT_CONTROL_DEFAULT = 0,
ALT_CONTROL_NON_AIRSPEED = 1,
ALT_CONTROL_TECS = 2
};
// attitude controller choice