Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 00:13:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: mavlink initiated takeoff in alt-hold modes

Browse Source 
adds PILOT_TKOFF_ALT for target altitude above home in cm for pilot initiated takeoff
This commit is contained in:
Jonathan Challinger 2015-04-30 15:06:55 +09:00 committed by Randy Mackay
parent b10cf0f38a
commit 73d961cebc
12 changed files with 138 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduCopter/ArduCopter.pde
View File

@ -374,6 +374,13 @@ static struct {
uint32_t last_edge_time_ms; // system time that switch position was last changed
} control_switch_state;
static struct {
bool running;
float speed;
uint32_t start_ms;
uint32_t time_ms;
} takeoff_state;
static RCMapper rcmap;
// board specific config

14
ArduCopter/GCS_Mavlink.pde
View File

@ -1044,22 +1044,22 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
switch(packet.command) {
case MAV_CMD_NAV_TAKEOFF:
case MAV_CMD_NAV_TAKEOFF: {
// param4 : yaw angle (not supported)
// param5 : latitude (not supported)
// param6 : longitude (not supported)
// param7 : altitude [metres]
if (motors.armed() && control_mode == GUIDED) {
set_auto_armed(true);
float takeoff_alt = packet.param7 * 100; // Convert m to cm
takeoff_alt = max(takeoff_alt,current_loc.alt);
takeoff_alt = max(takeoff_alt,100.0f);
guided_takeoff_start(takeoff_alt);
float takeoff_alt = packet.param7 * 100; // Convert m to cm
if(do_user_takeoff(takeoff_alt, !packet.param3)) {
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
break;
}
case MAV_CMD_NAV_LOITER_UNLIM:
if (set_mode(LOITER)) {

2
ArduCopter/Parameters.h
View File

@ -130,6 +130,7 @@ public:
k_param_cli_enabled,
k_param_throttle_filt,
k_param_throttle_behavior,
k_param_pilot_takeoff_alt, // 64
// 65: AP_Limits Library
k_param_limits = 65, // deprecated - remove
@ -345,6 +346,7 @@ public:
AP_Float throttle_filt;
AP_Int16 throttle_behavior;
AP_Int16 takeoff_trigger_dz;
AP_Float pilot_takeoff_alt;
AP_Int16 rtl_altitude;
AP_Float sonar_gain;

9
ArduCopter/Parameters.pde
View File

@ -71,6 +71,15 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Increment: .5
GSCALAR(throttle_filt, "PILOT_THR_FILT", 0),
// @Param: PILOT_TKOFF_ALT
// @DisplayName: Pilot takeoff altitude
// @Description: Altitude that altitude control modes will climb to when a takeoff is triggered with the throttle stick.
// @User: Standard
// @Units: Centimeters
// @Range: 0.0 1000.0
// @Increment: 10
GSCALAR(pilot_takeoff_alt, "PILOT_TKOFF_ALT", 0.0f),
// @Param: PILOT_TKOFF_DZ
// @DisplayName: Takeoff trigger deadzone
// @Description: Offset from mid stick at which takeoff is triggered

12
ArduCopter/control_althold.pde
View File

@ -23,7 +23,7 @@ static void althold_run()
{
float target_roll, target_pitch;
float target_yaw_rate;
int16_t target_climb_rate;
float target_climb_rate;
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) {
@ -44,9 +44,14 @@ static void althold_run()
// get pilot desired climb rate
target_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max-tkoff_get_speed(), g.pilot_velocity_z_max-tkoff_get_speed());
// check for take-off
if (ap.land_complete && (takeoff_state.running || g.rc_3.control_in > get_takeoff_trigger_throttle())) {
if (!takeoff_state.running) {
tkoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
}
// check for pilot requested take-off
if (ap.land_complete && target_climb_rate > 0) {
// indicate we are taking off
set_land_complete(false);
// clear i term when we're taking off
@ -70,6 +75,7 @@ static void althold_run()
// call position controller
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
tkoff_increment_alt_target(G_Dt);
pos_control.update_z_controller();
}
}

10
ArduCopter/control_loiter.pde
View File

@ -53,9 +53,14 @@ static void loiter_run()
// get pilot desired climb rate
target_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max-tkoff_get_speed(), g.pilot_velocity_z_max-tkoff_get_speed());
// check for take-off
if (ap.land_complete && (takeoff_state.running || g.rc_3.control_in > get_takeoff_trigger_throttle())) {
if (!takeoff_state.running) {
tkoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
}
// check for pilot requested take-off
if (ap.land_complete && target_climb_rate > 0) {
// indicate we are taking off
set_land_complete(false);
// clear i term when we're taking off
@ -94,6 +99,7 @@ static void loiter_run()
// update altitude target and call position controller
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
tkoff_increment_alt_target(G_Dt);
pos_control.update_z_controller();
}
}

10
ArduCopter/control_poshold.pde
View File

@ -171,9 +171,14 @@ static void poshold_run()
// get pilot desired climb rate (for alt-hold mode and take-off)
target_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max-tkoff_get_speed(), g.pilot_velocity_z_max-tkoff_get_speed());
// check for take-off
if (ap.land_complete && (takeoff_state.running || g.rc_3.control_in > get_takeoff_trigger_throttle())) {
if (!takeoff_state.running) {
tkoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
}
// check for pilot requested take-off
if (ap.land_complete && target_climb_rate > 0) {
// indicate we are taking off
set_land_complete(false);
// clear i term when we're taking off
@ -521,6 +526,7 @@ static void poshold_run()
}
// update altitude target and call position controller
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
tkoff_increment_alt_target(G_Dt);
pos_control.update_z_controller();
}
}

10
ArduCopter/control_sport.pde
View File

@ -64,9 +64,14 @@ static void sport_run()
// get pilot desired climb rate
target_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max-tkoff_get_speed(), g.pilot_velocity_z_max-tkoff_get_speed());
// check for take-off
if (ap.land_complete && (takeoff_state.running || g.rc_3.control_in > get_takeoff_trigger_throttle())) {
if (!takeoff_state.running) {
tkoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f));
}
// check for pilot requested take-off
if (ap.land_complete && target_climb_rate > 0) {
// indicate we are taking off
set_land_complete(false);
// clear i term when we're taking off
@ -91,6 +96,7 @@ static void sport_run()
// call position controller
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
tkoff_increment_alt_target(G_Dt);
pos_control.update_z_controller();
}
}

3
ArduCopter/defines.h
View File

@ -402,4 +402,7 @@ enum FlipState {
#define MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE (1<<10)
#define MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE (1<<11)
// for PILOT_THR_BHV parameter
#define THR_BEHAVE_FEEDBACK_FROM_MID_STICK (1<<0)
#endif // _DEFINES_H

2
ArduCopter/flight_mode.pde
View File

@ -129,6 +129,8 @@ static bool set_mode(uint8_t mode)
// called at 100hz or more
static void update_flight_mode()
{
tkoff_timer_update();
#if AP_AHRS_NAVEKF_AVAILABLE
// Update EKF speed limit - used to limit speed when we are using optical flow
ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);

2
ArduCopter/motors.pde
View File

@ -646,7 +646,7 @@ static bool arm_checks(bool display_failure, bool arming_from_gcs)
// check throttle is not too high - skips checks if arming from GCS in Guided
if (!(arming_from_gcs && control_mode == GUIDED)) {
// above top of deadband is too always high
if (g.rc_3.control_in > (g.rc_3.get_control_mid() + g.throttle_deadzone)) {
if (g.rc_3.control_in > get_takeoff_trigger_throttle()) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Throttle too high"));
}

74
ArduCopter/takeoff.pde Normal file
View File

@ -0,0 +1,74 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
bool current_mode_has_user_takeoff(bool with_navigation)
{
switch (control_mode) {
case GUIDED:
case LOITER:
case POSHOLD:
return true;
case ALT_HOLD:
case SPORT:
return !with_navigation;
default:
return false;
}
}
static bool do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
{
if (motors.armed() && ap.land_complete && current_mode_has_user_takeoff(must_navigate) && takeoff_alt_cm > current_loc.alt) {
switch(control_mode) {
case GUIDED:
set_auto_armed(true);
guided_takeoff_start(takeoff_alt_cm);
return true;
case LOITER:
case POSHOLD:
case ALT_HOLD:
case SPORT:
set_auto_armed(true);
tkoff_timer_start(pv_alt_above_origin(takeoff_alt_cm)-pos_control.get_pos_target().z);
return true;
}
}
return false;
}
static void tkoff_timer_start(float alt)
{
float speed = min(wp_nav.get_speed_up(), max(g.pilot_velocity_z_max*2.0f/3.0f, g.pilot_velocity_z_max-50.0f));
if (takeoff_state.running || speed <= 0.0f || alt <= 0.0f) {
return;
}
takeoff_state.running = true;
takeoff_state.speed = speed;
takeoff_state.start_ms = millis();
takeoff_state.time_ms = (alt/takeoff_state.speed) * 1.0e3f;
}
static void tkoff_timer_update()
{
if (millis()-takeoff_state.start_ms >= takeoff_state.time_ms) {
takeoff_state.running = false;
}
}
static void tkoff_increment_alt_target(float dt)
{
if (takeoff_state.running) {
Vector3f pos_target = pos_control.get_pos_target();
pos_target.z += takeoff_state.speed*dt;
pos_control.set_pos_target(pos_target);
}
}
static float tkoff_get_speed()
{
return takeoff_state.running?takeoff_state.speed:0.0f;
}