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

TradHeli: add THROTTLE_MANUAL_HELI

Browse Source 
Move check_dynamic_flight to run as scheduled task
This commit is contained in:
Randy Mackay 2013-11-06 14:51:28 +09:00
parent 537b4aef38
commit 20b2fb7680
5 changed files with 47 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
ArduCopter/ArduCopter.pde
View File

@ -869,6 +869,9 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ three_hz_loop, 33, 90 },
{ compass_accumulate, 2, 420 },
{ barometer_accumulate, 2, 250 },
#if FRAME_CONFIG == HELI_FRAME
{ check_dynamic_flight, 2, 100 },
#endif
{ update_notify, 2, 100 },
{ one_hz_loop, 100, 420 },
{ crash_check, 10, 20 },
@ -1805,6 +1808,12 @@ void update_super_simple_bearing(bool force_update)
}
}
// throttle_mode_manual - returns true if the throttle is directly controlled by the pilot
bool throttle_mode_manual(uint8_t thr_mode)
{
return (thr_mode == THROTTLE_MANUAL || thr_mode == THROTTLE_MANUAL_TILT_COMPENSATED || thr_mode == THROTTLE_MANUAL_HELI);
}
// set_throttle_mode - sets the throttle mode and initialises any variables as required
bool set_throttle_mode( uint8_t new_throttle_mode )
{
@ -1829,7 +1838,7 @@ bool set_throttle_mode( uint8_t new_throttle_mode )
case THROTTLE_AUTO:
controller_desired_alt = get_initial_alt_hold(current_loc.alt, climb_rate); // reset controller desired altitude to current altitude
wp_nav.set_desired_alt(controller_desired_alt); // same as above but for loiter controller
if ( throttle_mode <= THROTTLE_MANUAL_TILT_COMPENSATED ) { // reset the alt hold I terms if previous throttle mode was manual
if (throttle_mode_manual(throttle_mode)) { // reset the alt hold I terms if previous throttle mode was manual
reset_throttle_I();
set_accel_throttle_I_from_pilot_throttle(get_pilot_desired_throttle(g.rc_3.control_in));
}
@ -1841,6 +1850,14 @@ bool set_throttle_mode( uint8_t new_throttle_mode )
controller_desired_alt = get_initial_alt_hold(current_loc.alt, climb_rate); // reset controller desired altitude to current altitude
throttle_initialised = true;
break;
#if FRAME_CONFIG == HELI_FRAME
case THROTTLE_MANUAL_HELI:
throttle_accel_deactivate(); // this controller does not use accel based throttle controller
altitude_error = 0; // clear altitude error reported to GCS
throttle_initialised = true;
break;
#endif
}
// update the throttle mode
@ -1850,11 +1867,6 @@ bool set_throttle_mode( uint8_t new_throttle_mode )
// reset some variables used for logging
desired_climb_rate = 0;
nav_throttle = 0;
#if FRAME_CONFIG == HELI_FRAME
// use reduced collective range if in manual throttle mode
motors.set_collective_for_manual_control(throttle_mode == THROTTLE_MANUAL || throttle_mode == THROTTLE_MANUAL_TILT_COMPENSATED);
#endif
}
// return success or failure
@ -1871,28 +1883,15 @@ void update_throttle_mode(void)
if(ap.do_flip) // this is pretty bad but needed to flip in AP modes.
return;
#if FRAME_CONFIG == HELI_FRAME
// check if we are flying
check_dynamic_flight();
// allow swash collective to move if we are in manual throttle modes, even if disarmed
if( !motors.armed() ) {
if ( !(throttle_mode == THROTTLE_MANUAL) && !(throttle_mode == THROTTLE_MANUAL_TILT_COMPENSATED)){
throttle_accel_deactivate(); // do not allow the accel based throttle to override our command
return;
}
}
#else // HELI_FRAME
// do not run throttle controllers if motors disarmed
#if FRAME_CONFIG != HELI_FRAME
// do not run throttle controllers if motors disarmed
if( !motors.armed() ) {
set_throttle_out(0, false);
throttle_accel_deactivate(); // do not allow the accel based throttle to override our command
set_target_alt_for_reporting(0);
return;
}
#endif // HELI_FRAME
#endif // FRAME_CONFIG != HELI_FRAME
switch(throttle_mode) {
@ -2013,6 +2012,20 @@ void update_throttle_mode(void)
get_throttle_land();
set_target_alt_for_reporting(0);
break;
#if FRAME_CONFIG == HELI_FRAME
case THROTTLE_MANUAL_HELI:
// trad heli manual throttle controller
// send pilot's output directly to swash plate
pilot_throttle_scaled = motors.get_pilot_desired_collective(g.rc_3.control_in);
set_throttle_out(pilot_throttle_scaled, false);
// update estimate of throttle cruise
update_throttle_cruise(motors.get_collective_out());
set_target_alt_for_reporting(0);
break;
#endif // HELI_FRAME
}
}

7
ArduCopter/config.h
View File

@ -111,7 +111,7 @@
# define HELI_PITCH_FF 0
# define HELI_ROLL_FF 0
# define HELI_YAW_FF 0
# define STABILIZE_THROTTLE THROTTLE_MANUAL
# define STABILIZE_THR THROTTLE_MANUAL_HELI
# define MPU6K_FILTER 10
#endif
@ -531,6 +531,11 @@
// Flight mode roll, pitch, yaw, throttle and navigation definitions
// Stabilize Mode
#ifndef STABILIZE_THR
# define STABILIZE_THR THROTTLE_MANUAL_TILT_COMPENSATED
#endif
// Acro Mode
#ifndef ACRO_YAW
# define ACRO_YAW YAW_ACRO

1
ArduCopter/defines.h
View File

@ -40,6 +40,7 @@
#define THROTTLE_HOLD 2 // alt hold plus pilot input of climb rate
#define THROTTLE_AUTO 3 // auto pilot altitude controller with target altitude held in next_WP.alt
#define THROTTLE_LAND 4 // landing throttle controller
#define THROTTLE_MANUAL_HELI 5 // pilot manually controlled throttle for traditional helicopters
// sonar - for use with CONFIG_SONAR_SOURCE

6
ArduCopter/flip.pde
View File

@ -35,7 +35,7 @@ void roll_flip()
if (roll < 4500) {
// Roll control
roll_rate_target_bf = 40000 * flip_dir;
if(throttle_mode == THROTTLE_MANUAL || throttle_mode == THROTTLE_MANUAL_TILT_COMPENSATED){
if (throttle_mode_manual(throttle_mode)){
// increase throttle right before flip
set_throttle_out(g.rc_3.control_in + AAP_THR_INC, false);
}
@ -52,7 +52,7 @@ void roll_flip()
roll_rate_target_bf = 40000 * flip_dir;
#endif
// decrease throttle while inverted
if(throttle_mode == THROTTLE_MANUAL || throttle_mode == THROTTLE_MANUAL_TILT_COMPENSATED){
if (throttle_mode_manual(throttle_mode)){
set_throttle_out(g.rc_3.control_in - AAP_THR_DEC, false);
}
}else{
@ -68,7 +68,7 @@ void roll_flip()
// It will be handled by normal flight control loops
// increase throttle to gain any lost alitude
if(throttle_mode == THROTTLE_MANUAL || throttle_mode == THROTTLE_MANUAL_TILT_COMPENSATED){
if (throttle_mode_manual(throttle_mode)){
set_throttle_out(g.rc_3.control_in + AAP_THR_INC, false);
}
flip_timer++;

3
ArduCopter/system.pde
View File

@ -327,6 +327,7 @@ static bool manual_flight_mode(uint8_t mode) {
}
// set_mode - change flight mode and perform any necessary initialisation
// optional force parameter used to force the flight mode change (used only first time mode is set)
// returns true if mode was succesfully set
// 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)
@ -353,7 +354,7 @@ static bool set_mode(uint8_t mode)
success = true;
set_yaw_mode(YAW_HOLD);
set_roll_pitch_mode(ROLL_PITCH_STABLE);
set_throttle_mode(THROTTLE_MANUAL_TILT_COMPENSATED);
set_throttle_mode(STABILIZE_THR);
set_nav_mode(NAV_NONE);
break;