Jason Short
11 years ago
committed by
Randy Mackay
9 changed files with 51 additions and 255 deletions
@ -1,175 +1,31 @@
@@ -1,175 +1,31 @@
|
||||
//////////////////////////////////////////////////////////////////////////////// |
||||
// Toy Mode - THOR |
||||
// Toy Mode |
||||
//////////////////////////////////////////////////////////////////////////////// |
||||
static bool CH7_toy_flag; |
||||
|
||||
#if TOY_MIXER == TOY_LOOKUP_TABLE |
||||
static const int16_t toy_lookup[] = { |
||||
186, 373, 558, 745, |
||||
372, 745, 1117, 1490, |
||||
558, 1118, 1675, 2235, |
||||
743, 1490, 2233, 2980, |
||||
929, 1863, 2792, 3725, |
||||
1115, 2235, 3350, 4470, |
||||
1301, 2608, 3908, 4500, |
||||
1487, 2980, 4467, 4500, |
||||
1673, 3353, 4500, 4500 |
||||
}; |
||||
#endif |
||||
static float toy_gain; |
||||
|
||||
//called at 10hz |
||||
void update_toy_throttle() |
||||
static void |
||||
get_yaw_toy() |
||||
{ |
||||
if(control_mode == TOY_A) { |
||||
// look for a change in throttle position to exit throttle hold |
||||
if(abs(g.rc_3.control_in - saved_toy_throttle) > 40) { |
||||
throttle_mode = THROTTLE_MANUAL; |
||||
} |
||||
|
||||
if(throttle_mode == THROTTLE_AUTO) { |
||||
update_toy_altitude(); |
||||
} |
||||
} |
||||
} |
||||
|
||||
#define TOY_ALT_SMALL 25 |
||||
#define TOY_ALT_LARGE 100 |
||||
|
||||
//called at 10hz |
||||
void update_toy_altitude() |
||||
{ |
||||
int16_t input = g.rc_3.radio_in; // throttle |
||||
float current_target_alt = wp_nav.get_desired_alt(); |
||||
//int16_t input = g.rc_7.radio_in; |
||||
|
||||
// Trigger upward alt change |
||||
if(false == CH7_toy_flag && input > 1666) { |
||||
CH7_toy_flag = true; |
||||
// go up |
||||
if(current_target_alt >= 400) { |
||||
wp_nav.set_desired_alt(current_target_alt + TOY_ALT_LARGE); |
||||
}else{ |
||||
wp_nav.set_desired_alt(current_target_alt + TOY_ALT_SMALL); |
||||
} |
||||
|
||||
// Trigger downward alt change |
||||
}else if(false == CH7_toy_flag && input < 1333) { |
||||
CH7_toy_flag = true; |
||||
// go down |
||||
if(current_target_alt >= (400 + TOY_ALT_LARGE)) { |
||||
wp_nav.set_desired_alt(current_target_alt - TOY_ALT_LARGE); |
||||
}else if(current_target_alt >= TOY_ALT_SMALL) { |
||||
wp_nav.set_desired_alt(current_target_alt - TOY_ALT_SMALL); |
||||
}else if(current_target_alt < TOY_ALT_SMALL) { |
||||
wp_nav.set_desired_alt(0); |
||||
} |
||||
|
||||
// clear flag |
||||
}else if (CH7_toy_flag && ((input < 1666) && (input > 1333))) { |
||||
CH7_toy_flag = false; |
||||
} |
||||
// convert pilot input to lean angles |
||||
// moved to Yaw since it is called before get_roll_pitch_toy(); |
||||
get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, control_roll, control_pitch); |
||||
|
||||
// Gain scheduling for Yaw input - |
||||
// we reduce the yaw input based on the velocity of the copter |
||||
// 0 = full control, 600cm/s = 20% control |
||||
toy_gain = min(inertial_nav.get_velocity_xy(), 600); |
||||
toy_gain = 1.0 - (toy_gain / 750.0); |
||||
get_yaw_rate_stabilized_ef((float)control_roll * toy_gain); |
||||
} |
||||
|
||||
// called at 50 hz from all flight modes |
||||
#if TOY_EDF == ENABLED |
||||
void edf_toy() |
||||
{ |
||||
// EDF control: |
||||
g.rc_8.radio_out = 1000 + ((abs(g.rc_2.control_in) << 1) / 9); |
||||
if(g.rc_8.radio_out < 1050) |
||||
g.rc_8.radio_out = 1000; |
||||
|
||||
// output throttle to EDF |
||||
if(motors.armed()) { |
||||
APM_RC.OutputCh(CH_8, g.rc_8.radio_out); |
||||
}else{ |
||||
APM_RC.OutputCh(CH_8, 1000); |
||||
} |
||||
|
||||
// output Servo direction |
||||
if(g.rc_2.control_in > 0) { |
||||
APM_RC.OutputCh(CH_6, 1000); // 1000 : 2000 |
||||
}else{ |
||||
APM_RC.OutputCh(CH_6, 2000); // less than 1450 |
||||
} |
||||
} |
||||
#endif |
||||
|
||||
// The function call for managing the flight mode Toy |
||||
void roll_pitch_toy() |
||||
static void |
||||
get_roll_pitch_toy() |
||||
{ |
||||
#if TOY_MIXER == TOY_LOOKUP_TABLE || TOY_MIXER == TOY_LINEAR_MIXER |
||||
int16_t yaw_rate = g.rc_1.control_in / g.toy_yaw_rate; |
||||
|
||||
if(g.rc_1.control_in != 0) { // roll |
||||
get_acro_yaw(yaw_rate/2); |
||||
ap.yaw_stopped = false; |
||||
yaw_timer = 150; |
||||
|
||||
}else if (!ap.yaw_stopped) { |
||||
get_acro_yaw(0); |
||||
yaw_timer--; |
||||
|
||||
if((yaw_timer == 0) || (fabsf(omega.z) < 0.17f)) { |
||||
ap.yaw_stopped = true; |
||||
nav_yaw = ahrs.yaw_sensor; |
||||
} |
||||
}else{ |
||||
if(motors.armed() == false || g.rc_3.control_in == 0) |
||||
nav_yaw = ahrs.yaw_sensor; |
||||
|
||||
get_stabilize_yaw(nav_yaw); |
||||
} |
||||
#endif |
||||
|
||||
// roll_rate is the outcome of the linear equation or lookup table |
||||
// based on speed and Yaw rate |
||||
int16_t roll_rate = 0; |
||||
|
||||
#if TOY_MIXER == TOY_LOOKUP_TABLE |
||||
uint8_t xx, yy; |
||||
// Lookup value |
||||
//xx = g_gps->ground_speed / 200; |
||||
xx = abs(g.rc_2.control_in / 1000); |
||||
yy = abs(yaw_rate / 500); |
||||
|
||||
// constrain to lookup Array range |
||||
xx = constrain_int16(xx, 0, 3); |
||||
yy = constrain_int16(yy, 0, 8); |
||||
|
||||
roll_rate = toy_lookup[yy * 4 + xx]; |
||||
|
||||
if(yaw_rate == 0) { |
||||
roll_rate = 0; |
||||
}else if(yaw_rate < 0) { |
||||
roll_rate = -roll_rate; |
||||
} |
||||
|
||||
int16_t roll_limit = 4500 / g.toy_yaw_rate; |
||||
roll_rate = constrain_int16(roll_rate, -roll_limit, roll_limit); |
||||
|
||||
#elif TOY_MIXER == TOY_LINEAR_MIXER |
||||
roll_rate = -((int32_t)g.rc_2.control_in * (yaw_rate/100)) /30; |
||||
roll_rate = constrain_int32(roll_rate, -2000, 2000); |
||||
|
||||
#elif TOY_MIXER == TOY_EXTERNAL_MIXER |
||||
// JKR update to allow external roll/yaw mixing |
||||
roll_rate = g.rc_1.control_in; |
||||
#endif |
||||
|
||||
#if TOY_EDF == ENABLED |
||||
// Output the attitude |
||||
//g.rc_1.servo_out = get_stabilize_roll(roll_rate); |
||||
//g.rc_2.servo_out = get_stabilize_pitch(g.rc_6.control_in); // use CH6 to trim pitch |
||||
get_stabilize_roll(roll_rate); |
||||
get_stabilize_pitch(g.rc_6.control_in); // use CH6 to trim pitch |
||||
|
||||
#else |
||||
// Output the attitude |
||||
//g.rc_1.servo_out = get_stabilize_roll(roll_rate); |
||||
//g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in); |
||||
get_stabilize_roll(roll_rate); |
||||
get_stabilize_pitch(g.rc_2.control_in); |
||||
#endif |
||||
|
||||
// pass desired roll, pitch to stabilize attitude controllers |
||||
get_stabilize_roll((float)control_roll * (1.0 - toy_gain)); |
||||
get_stabilize_pitch(control_pitch); |
||||
} |
||||
|
||||
|
||||
Loading…
Reference in new issue