ardupilot/ArduCopter/toy.pde

171 lines
4.9 KiB
Plaintext

////////////////////////////////////////////////////////////////////////////////
// Toy Mode - THOR
////////////////////////////////////////////////////////////////////////////////
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
//called at 10hz
void update_toy_throttle()
{
// 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;
}
}
#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;
}
}
// 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()
{
#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_system.yaw_stopped = false;
yaw_timer = 150;
}else if (!ap_system.yaw_stopped) {
get_acro_yaw(0);
yaw_timer--;
if((yaw_timer == 0) || (fabsf(omega.z) < 0.17f)) {
ap_system.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;
//cliSerial->printf("roll_rate: %d\n",roll_rate);
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
}