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

Copter: add SPORT mode

This commit is contained in:
Randy Mackay 2013-08-04 15:56:35 +09:00
parent a18d55a9d3
commit 39ab4f62ef
9 changed files with 157 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
ArduCopter/ArduCopter.pde
View File

@ -1637,6 +1637,7 @@ bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode)
case ROLL_PITCH_AUTO:
case ROLL_PITCH_STABLE_OF:
case ROLL_PITCH_TOY:
case ROLL_PITCH_SPORT:
roll_pitch_initialised = true;
break;
@ -1761,6 +1762,18 @@ void update_roll_pitch_mode(void)
get_stabilize_roll(nav_roll);
get_stabilize_pitch(nav_pitch);
break;
case ROLL_PITCH_SPORT:
// apply SIMPLE mode transform
if(ap.simple_mode && ap_system.new_radio_frame) {
update_simple_mode();
}
// copy user input for reporting purposes
control_roll = g.rc_1.control_in;
control_pitch = g.rc_2.control_in;
get_roll_rate_stabilized_ef(g.rc_1.control_in);
get_pitch_rate_stabilized_ef(g.rc_2.control_in);
break;
}
#if FRAME_CONFIG != HELI_FRAME

82
ArduCopter/Attitude.pde
View File

@ -233,6 +233,88 @@ get_yaw_rate_stabilized_bf(int32_t stick_angle)
set_yaw_rate_target(0, BODY_EARTH_FRAME);
}
// Roll with rate input and stabilized in the earth frame
static void
get_roll_rate_stabilized_ef(int32_t stick_angle)
{
int32_t angle_error = 0;
// convert the input to the desired roll rate
int32_t target_rate = stick_angle * g.acro_p - (roll_axis * g.acro_balance_roll)/100;
// convert the input to the desired roll rate
roll_axis += target_rate * G_Dt;
roll_axis = wrap_180_cd(roll_axis);
// ensure that we don't reach gimbal lock
if (labs(roll_axis) > 4500 && g.acro_trainer_enabled) {
roll_axis = constrain_int32(roll_axis, -4500, 4500);
angle_error = wrap_180_cd(roll_axis - ahrs.roll_sensor);
} else {
// angle error with maximum of +- max_angle_overshoot
angle_error = wrap_180_cd(roll_axis - ahrs.roll_sensor);
angle_error = constrain_int32(angle_error, -MAX_ROLL_OVERSHOOT, MAX_ROLL_OVERSHOOT);
}
#if FRAME_CONFIG == HELI_FRAME
if (!motors.motor_runup_complete) {
angle_error = 0;
}
#else
// reset target angle to current angle if motors not spinning
if (!motors.armed() || g.rc_3.servo_out == 0) {
angle_error = 0;
}
#endif // HELI_FRAME
// update roll_axis to be within max_angle_overshoot of our current heading
roll_axis = wrap_180_cd(angle_error + ahrs.roll_sensor);
// set earth frame targets for rate controller
set_roll_rate_target(g.pi_stabilize_roll.get_p(angle_error) + target_rate, EARTH_FRAME);
}
// Pitch with rate input and stabilized in the earth frame
static void
get_pitch_rate_stabilized_ef(int32_t stick_angle)
{
int32_t angle_error = 0;
// convert the input to the desired pitch rate
int32_t target_rate = stick_angle * g.acro_p - (pitch_axis * g.acro_balance_pitch)/100;
// convert the input to the desired pitch rate
pitch_axis += target_rate * G_Dt;
pitch_axis = wrap_180_cd(pitch_axis);
// ensure that we don't reach gimbal lock
if (labs(pitch_axis) > 4500) {
pitch_axis = constrain_int32(pitch_axis, -4500, 4500);
angle_error = wrap_180_cd(pitch_axis - ahrs.pitch_sensor);
} else {
// angle error with maximum of +- max_angle_overshoot
angle_error = wrap_180_cd(pitch_axis - ahrs.pitch_sensor);
angle_error = constrain_int32(angle_error, -MAX_PITCH_OVERSHOOT, MAX_PITCH_OVERSHOOT);
}
#if FRAME_CONFIG == HELI_FRAME
if (!motors.motor_runup_complete) {
angle_error = 0;
}
#else
// reset target angle to current angle if motors not spinning
if (!motors.armed() || g.rc_3.servo_out == 0) {
angle_error = 0;
}
#endif // HELI_FRAME
// update pitch_axis to be within max_angle_overshoot of our current heading
pitch_axis = wrap_180_cd(angle_error + ahrs.pitch_sensor);
// set earth frame targets for rate controller
set_pitch_rate_target(g.pi_stabilize_pitch.get_p(angle_error) + target_rate, EARTH_FRAME);
}
// Yaw with rate input and stabilized in the earth frame
static void
get_yaw_rate_stabilized_ef(int32_t stick_angle)

12
ArduCopter/Parameters.pde
View File

@ -308,42 +308,42 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Param: FLTMODE1
// @DisplayName: Flight Mode 1
// @Description: Flight mode when Channel 5 pwm is <= 1230
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM,13:Sport
// @User: Standard
GSCALAR(flight_mode1, "FLTMODE1", FLIGHT_MODE_1),
// @Param: FLTMODE2
// @DisplayName: Flight Mode 2
// @Description: Flight mode when Channel 5 pwm is >1230, <= 1360
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM,13:Sport
// @User: Standard
GSCALAR(flight_mode2, "FLTMODE2", FLIGHT_MODE_2),
// @Param: FLTMODE3
// @DisplayName: Flight Mode 3
// @Description: Flight mode when Channel 5 pwm is >1360, <= 1490
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM,13:Sport
// @User: Standard
GSCALAR(flight_mode3, "FLTMODE3", FLIGHT_MODE_3),
// @Param: FLTMODE4
// @DisplayName: Flight Mode 4
// @Description: Flight mode when Channel 5 pwm is >1490, <= 1620
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM,13:Sport
// @User: Standard
GSCALAR(flight_mode4, "FLTMODE4", FLIGHT_MODE_4),
// @Param: FLTMODE5
// @DisplayName: Flight Mode 5
// @Description: Flight mode when Channel 5 pwm is >1620, <= 1749
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM,13:Sport
// @User: Standard
GSCALAR(flight_mode5, "FLTMODE5", FLIGHT_MODE_5),
// @Param: FLTMODE6
// @DisplayName: Flight Mode 6
// @Description: Flight mode when Channel 5 pwm is >=1750
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM
// @Values: 0:Stabilize,1:Acro,2:AltHold,3:Auto,4:Guided,5:Loiter,6:RTL,7:Circle,8:Position,9:Land,10:OF_Loiter,11:ToyA,12:ToyM,13:Sport
// @User: Standard
GSCALAR(flight_mode6, "FLTMODE6", FLIGHT_MODE_6),

13
ArduCopter/config.h
View File

@ -585,6 +585,19 @@
# define ACRO_THR THROTTLE_MANUAL
#endif
// Sport Mode
#ifndef SPORT_YAW
# define SPORT_YAW YAW_ACRO
#endif
#ifndef SPORT_RP
# define SPORT_RP ROLL_PITCH_SPORT
#endif
#ifndef SPORT_THR
# define SPORT_THR THROTTLE_MANUAL
#endif
// Alt Hold Mode
#ifndef ALT_HOLD_YAW
# define ALT_HOLD_YAW YAW_HOLD

9
ArduCopter/defines.h
View File

@ -27,11 +27,12 @@
#define YAW_RESETTOARMEDYAW 9 // point towards heading at time motors were armed
#define ROLL_PITCH_STABLE 0 // pilot input roll, pitch angles
#define ROLL_PITCH_ACRO 1 // pilot inputs roll, pitch rotation rates
#define ROLL_PITCH_ACRO 1 // pilot inputs roll, pitch rotation rates in body frame
#define ROLL_PITCH_AUTO 2 // no pilot input. autopilot roll, pitch is sent to stabilize controller inputs
#define ROLL_PITCH_STABLE_OF 3 // pilot inputs roll, pitch angles which are mixed with optical flow based position controller lean anbles
#define ROLL_PITCH_TOY 4 // THOR This is the Roll and Pitch mode
#define ROLL_PITCH_LOITER 5 // pilot inputs the desired horizontal velocities
#define ROLL_PITCH_SPORT 6 // pilot inputs roll, pitch rotation rates in earth frame
#define THROTTLE_MANUAL 0 // manual throttle mode - pilot input goes directly to motors
#define THROTTLE_MANUAL_TILT_COMPENSATED 1 // mostly manual throttle but with some tilt compensation
@ -135,11 +136,11 @@
#define CIRCLE 7 // AUTO control
#define POSITION 8 // AUTO control
#define LAND 9 // AUTO control
#define OF_LOITER 10 // Hold a single location using optical flow
// sensor
#define OF_LOITER 10 // Hold a single location using optical flow sensor
#define TOY_A 11 // THOR Enum for Toy mode
#define TOY_M 12 // THOR Enum for Toy mode
#define NUM_MODES 13
#define SPORT 13 // earth frame rate control
#define NUM_MODES 14
// CH_6 Tuning
// -----------

3
ArduCopter/events.pde
View File

@ -15,6 +15,7 @@ static void failsafe_radio_on_event()
switch(control_mode) {
case STABILIZE:
case ACRO:
case SPORT:
// if throttle is zero disarm motors
if (g.rc_3.control_in == 0) {
init_disarm_motors();
@ -89,6 +90,7 @@ static void low_battery_event(void)
switch(control_mode) {
case STABILIZE:
case ACRO:
case SPORT:
// if throttle is zero disarm motors
if (g.rc_3.control_in == 0) {
init_disarm_motors();
@ -217,6 +219,7 @@ static void failsafe_gcs_check()
switch(control_mode) {
case STABILIZE:
case ACRO:
case SPORT:
// if throttle is zero disarm motors
if (g.rc_3.control_in == 0) {
init_disarm_motors();

2
ArduCopter/fence.pde
View File

@ -32,7 +32,7 @@ void fence_check()
// disarm immediately if we think we are on the ground
// don't disarm if the high-altitude fence has been broken because it's likely the user has pulled their throttle to zero to bring it down
if(control_mode <= ACRO && g.rc_3.control_in == 0 && !ap.failsafe_radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0)){
if(manual_flight_mode(control_mode) && g.rc_3.control_in == 0 && !ap.failsafe_radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0)){
init_disarm_motors();
}else{
// if we have a GPS

6
ArduCopter/motors.pde
View File

@ -18,8 +18,8 @@ static void arm_motors_check()
return;
}
// allow arming/disarming in ACRO, STABILIZE and TOY flight modes
if (control_mode == ACRO || control_mode == STABILIZE || control_mode == TOY_A || control_mode == TOY_M) {
// allow arming/disarming in fully manual flight modes ACRO, STABILIZE, SPORT and TOY
if (manual_flight_mode(control_mode)) {
allow_arming = true;
}
@ -97,7 +97,7 @@ static void auto_disarm_check()
{
static uint8_t auto_disarming_counter;
if((control_mode <= ACRO) && (g.rc_3.control_in == 0) && motors.armed()) {
if(manual_flight_mode(control_mode) && (g.rc_3.control_in == 0) && motors.armed()) {
auto_disarming_counter++;
if(auto_disarming_counter == AUTO_DISARMING_DELAY) {

33
ArduCopter/system.pde
View File

@ -342,9 +342,25 @@ static bool mode_requires_GPS(uint8_t mode) {
return false;
}
// manual_flight_mode - returns true if flight mode is completely manual (i.e. roll, pitch and yaw controlled by pilot)
static bool manual_flight_mode(uint8_t mode) {
switch(mode) {
case ACRO:
case STABILIZE:
case TOY_A:
case TOY_M:
case SPORT:
return true;
default:
return false;
}
return false;
}
// set_mode - change flight mode and perform any necessary initialisation
// returns true if mode was succesfully set
// STABILIZE, ACRO and LAND can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately
// STABILIZE, ACRO, SPORT and LAND can always be set successfully but the return state of other flight modes should be checked and the caller should deal with failures appropriately
static bool set_mode(uint8_t mode)
{
// boolean to record if flight mode could be set
@ -505,6 +521,16 @@ static bool set_mode(uint8_t mode)
set_throttle_mode(THROTTLE_HOLD);
break;
case SPORT:
success = true;
ap.manual_throttle = true;
ap.manual_attitude = true;
set_yaw_mode(SPORT_YAW);
set_roll_pitch_mode(SPORT_RP);
set_throttle_mode(SPORT_THR);
set_nav_mode(NAV_NONE);
break;
default:
success = false;
break;
@ -547,7 +573,7 @@ static void update_auto_armed()
return;
}
// if in stabilize or acro flight mode and throttle is zero, auto-armed should become false
if(control_mode <= ACRO && g.rc_3.control_in == 0 && !ap.failsafe_radio) {
if(manual_flight_mode(control_mode) && g.rc_3.control_in == 0 && !ap.failsafe_radio) {
set_auto_armed(false);
}
}else{
@ -668,6 +694,9 @@ print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
case TOY_A:
port->print_P(PSTR("TOY_A"));
break;
case SPORT:
port->print_P(PSTR("SPORT"));
break;
default:
port->printf_P(PSTR("Mode(%u)"), (unsigned)mode);
break;