uncrustify ArduCopter/toy.pde

This commit is contained in:
uncrustify 2012-08-16 17:50:03 -07:00 committed by Pat Hickey
parent 1e2c01d8f6
commit c7ff6a6335
1 changed files with 125 additions and 124 deletions

View File

@ -5,38 +5,39 @@ static boolean CH7_toy_flag;
//static boolean CH6_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};
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()
{
/*
// Disabled, now handled by TOY_A (Alt hold) and TOY_M (Manual throttle)
if (false == CH6_toy_flag && g.rc_6.radio_in >= CH_6_PWM_TRIGGER){
CH6_toy_flag = true;
throttle_mode = THROTTLE_MANUAL;
/*
* // Disabled, now handled by TOY_A (Alt hold) and TOY_M (Manual throttle)
* if (false == CH6_toy_flag && g.rc_6.radio_in >= CH_6_PWM_TRIGGER){
* CH6_toy_flag = true;
* throttle_mode = THROTTLE_MANUAL;
*
* }else if (CH6_toy_flag && g.rc_6.radio_in < CH_6_PWM_TRIGGER){
* CH6_toy_flag = false;
* throttle_mode = THROTTLE_AUTO;
* set_new_altitude(current_loc.alt);
* saved_toy_throttle = g.rc_3.control_in;
* }*/
}else if (CH6_toy_flag && g.rc_6.radio_in < CH_6_PWM_TRIGGER){
CH6_toy_flag = false;
throttle_mode = THROTTLE_AUTO;
set_new_altitude(current_loc.alt);
saved_toy_throttle = g.rc_3.control_in;
}*/
// 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;
}
// 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
@ -45,134 +46,134 @@ void update_toy_throttle()
//called at 10hz
void update_toy_altitude()
{
int16_t input = g.rc_3.radio_in; // throttle
//int16_t input = g.rc_7.radio_in;
int16_t input = g.rc_3.radio_in; // throttle
//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(next_WP.alt >= 400){
force_new_altitude(next_WP.alt + TOY_ALT_LARGE);
}else{
force_new_altitude(next_WP.alt + TOY_ALT_SMALL);
}
// Trigger upward alt change
if(false == CH7_toy_flag && input > 1666) {
CH7_toy_flag = true;
// go up
if(next_WP.alt >= 400) {
force_new_altitude(next_WP.alt + TOY_ALT_LARGE);
}else{
force_new_altitude(next_WP.alt + TOY_ALT_SMALL);
}
// Trigger downward alt change
}else if(false == CH7_toy_flag && input < 1333){
CH7_toy_flag = true;
// go down
if(next_WP.alt >= (400 + TOY_ALT_LARGE)){
force_new_altitude(next_WP.alt - TOY_ALT_LARGE);
}else if(next_WP.alt >= TOY_ALT_SMALL){
force_new_altitude(next_WP.alt - TOY_ALT_SMALL);
}else if(next_WP.alt < TOY_ALT_SMALL){
force_new_altitude(0);
}
// Trigger downward alt change
}else if(false == CH7_toy_flag && input < 1333) {
CH7_toy_flag = true;
// go down
if(next_WP.alt >= (400 + TOY_ALT_LARGE)) {
force_new_altitude(next_WP.alt - TOY_ALT_LARGE);
}else if(next_WP.alt >= TOY_ALT_SMALL) {
force_new_altitude(next_WP.alt - TOY_ALT_SMALL);
}else if(next_WP.alt < TOY_ALT_SMALL) {
force_new_altitude(0);
}
// clear flag
}else if (CH7_toy_flag && ((input < 1666) && (input > 1333))){
CH7_toy_flag = false;
}
// 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;
// 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 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
}
// 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 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
g.rc_4.servo_out = get_acro_yaw(yaw_rate/2);
yaw_stopped = false;
yaw_timer = 150;
if(g.rc_1.control_in != 0) { // roll
g.rc_4.servo_out = get_acro_yaw(yaw_rate/2);
yaw_stopped = false;
yaw_timer = 150;
}else if (!yaw_stopped){
g.rc_4.servo_out = get_acro_yaw(0);
yaw_timer--;
}else if (!yaw_stopped) {
g.rc_4.servo_out = get_acro_yaw(0);
yaw_timer--;
if((yaw_timer == 0) || (fabs(omega.z) < .17)){
yaw_stopped = true;
nav_yaw = ahrs.yaw_sensor;
}
}else{
if(motors.armed() == false || g.rc_3.control_in == 0)
nav_yaw = ahrs.yaw_sensor;
if((yaw_timer == 0) || (fabs(omega.z) < .17)) {
yaw_stopped = true;
nav_yaw = ahrs.yaw_sensor;
}
}else{
if(motors.armed() == false || g.rc_3.control_in == 0)
nav_yaw = ahrs.yaw_sensor;
g.rc_4.servo_out = get_stabilize_yaw(nav_yaw);
}
#endif
g.rc_4.servo_out = 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;
// 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);
#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(xx, 0, 3);
yy = constrain(yy, 0, 8);
// constrain to lookup Array range
xx = constrain(xx, 0, 3);
yy = constrain(yy, 0, 8);
roll_rate = toy_lookup[yy * 4 + xx];
roll_rate = toy_lookup[yy * 4 + xx];
if(yaw_rate == 0){
roll_rate = 0;
}else if(yaw_rate < 0){
roll_rate = -roll_rate;
}
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(roll_rate, -roll_limit, roll_limit);
int16_t roll_limit = 4500 / g.toy_yaw_rate;
roll_rate = constrain(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;
//Serial.printf("roll_rate: %d\n",roll_rate);
roll_rate = constrain(roll_rate, -2000, 2000);
#elif TOY_MIXER == TOY_LINEAR_MIXER
roll_rate = -((int32_t)g.rc_2.control_in * (yaw_rate/100)) /30;
//Serial.printf("roll_rate: %d\n",roll_rate);
roll_rate = constrain(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
#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
#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
#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);
#endif
#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);
#endif
}