Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-29 12:08:32 -04:00
Code Issues Packages Projects Releases Wiki Activity

uncrustify ArduCopter/control_modes.pde

Browse Source
This commit is contained in:
uncrustify 2012-08-16 17:50:02 -07:00 committed by Pat Hickey
parent 5d29d5e5a1
commit 0c0652b6d8
1 changed files with 170 additions and 170 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

102
ArduCopter/control_modes.pde
View File

@ -6,14 +6,14 @@ static void read_control_switch()
byte switchPosition = readSwitch(); byte switchPosition = readSwitch();
if (oldSwitchPosition != switchPosition){ if (oldSwitchPosition != switchPosition) {
if(switch_debouncer){ if(switch_debouncer) {
oldSwitchPosition = switchPosition; oldSwitchPosition = switchPosition;
switch_debouncer = false; switch_debouncer = false;
set_mode(flight_modes[switchPosition]); set_mode(flight_modes[switchPosition]);
if(g.ch7_option != CH7_SIMPLE_MODE){ if(g.ch7_option != CH7_SIMPLE_MODE) {
// set Simple mode using stored paramters from Mission planner // set Simple mode using stored paramters from Mission planner
// rather than by the control switch // rather than by the control switch
do_simple = (g.simple_modes & (1 << switchPosition)); do_simple = (g.simple_modes & (1 << switchPosition));
@ -47,10 +47,10 @@ static void read_trim_switch()
{ {
int8_t option; int8_t option;
if(g.ch7_option == CH7_MULTI_MODE){ if(g.ch7_option == CH7_MULTI_MODE) {
if (g.rc_6.radio_in < CH_6_PWM_TRIGGER_LOW){ if (g.rc_6.radio_in < CH_6_PWM_TRIGGER_LOW) {
option = CH7_FLIP; option = CH7_FLIP;
}else if (g.rc_6.radio_in > CH_6_PWM_TRIGGER_HIGH){ }else if (g.rc_6.radio_in > CH_6_PWM_TRIGGER_HIGH) {
option = CH7_SAVE_WP; option = CH7_SAVE_WP;
}else{ }else{
option = CH7_RTL; option = CH7_RTL;
@ -59,44 +59,44 @@ static void read_trim_switch()
option = g.ch7_option; option = g.ch7_option;
} }
if(option == CH7_SIMPLE_MODE){ if(option == CH7_SIMPLE_MODE) {
do_simple = (g.rc_7.radio_in > CH_7_PWM_TRIGGER); do_simple = (g.rc_7.radio_in > CH_7_PWM_TRIGGER);
}else if (option == CH7_FLIP){ }else if (option == CH7_FLIP) {
if (CH7_flag == false && g.rc_7.radio_in > CH_7_PWM_TRIGGER){ if (CH7_flag == false && g.rc_7.radio_in > CH_7_PWM_TRIGGER) {
CH7_flag = true; CH7_flag = true;
// don't flip if we accidentally engaged flip, but didn't notice and tried to takeoff // don't flip if we accidentally engaged flip, but didn't notice and tried to takeoff
if(g.rc_3.control_in != 0 && takeoff_complete){ if(g.rc_3.control_in != 0 && takeoff_complete) {
init_flip(); init_flip();
} }
} }
if (CH7_flag == true && g.rc_7.control_in < 800){ if (CH7_flag == true && g.rc_7.control_in < 800) {
CH7_flag = false; CH7_flag = false;
} }
}else if (option == CH7_RTL){ }else if (option == CH7_RTL) {
if (CH7_flag == false && g.rc_7.radio_in > CH_7_PWM_TRIGGER){ if (CH7_flag == false && g.rc_7.radio_in > CH_7_PWM_TRIGGER) {
CH7_flag = true; CH7_flag = true;
set_mode(RTL); set_mode(RTL);
} }
if (CH7_flag == true && g.rc_7.control_in < 800){ if (CH7_flag == true && g.rc_7.control_in < 800) {
CH7_flag = false; CH7_flag = false;
if (control_mode == RTL || control_mode == LOITER){ if (control_mode == RTL || control_mode == LOITER) {
reset_control_switch(); reset_control_switch();
} }
} }
}else if (option == CH7_SAVE_WP){ }else if (option == CH7_SAVE_WP) {
if (g.rc_7.radio_in > CH_7_PWM_TRIGGER){ // switch is engaged if (g.rc_7.radio_in > CH_7_PWM_TRIGGER) { // switch is engaged
CH7_flag = true; CH7_flag = true;
}else{ // switch is disengaged }else{ // switch is disengaged
if(CH7_flag){ if(CH7_flag) {
CH7_flag = false; CH7_flag = false;
if(control_mode == AUTO){ if(control_mode == AUTO) {
// reset the mission // reset the mission
CH7_wp_index = 0; CH7_wp_index = 0;
g.command_total.set_and_save(1); g.command_total.set_and_save(1);
@ -104,7 +104,7 @@ static void read_trim_switch()
return; return;
} }
if(CH7_wp_index == 0){ if(CH7_wp_index == 0) {
// this is our first WP, let's save WP 1 as a takeoff // this is our first WP, let's save WP 1 as a takeoff
// increment index to WP index of 1 (home is stored at 0) // increment index to WP index of 1 (home is stored at 0)
CH7_wp_index = 1; CH7_wp_index = 1;
@ -128,7 +128,7 @@ static void read_trim_switch()
// max out at 100 since I think we need to stay under the EEPROM limit // max out at 100 since I think we need to stay under the EEPROM limit
CH7_wp_index = constrain(CH7_wp_index, 1, 100); CH7_wp_index = constrain(CH7_wp_index, 1, 100);
if(g.rc_3.control_in > 0){ if(g.rc_3.control_in > 0) {
// set our location ID to 16, MAV_CMD_NAV_WAYPOINT // set our location ID to 16, MAV_CMD_NAV_WAYPOINT
current_loc.id = MAV_CMD_NAV_WAYPOINT; current_loc.id = MAV_CMD_NAV_WAYPOINT;
}else{ }else{
@ -147,8 +147,8 @@ static void read_trim_switch()
// 3 = command total // 3 = command total
} }
} }
}else if (option == CH7_AUTO_TRIM){ }else if (option == CH7_AUTO_TRIM) {
if (g.rc_7.radio_in > CH_7_PWM_TRIGGER){ if (g.rc_7.radio_in > CH_7_PWM_TRIGGER) {
auto_level_counter = 10; auto_level_counter = 10;
} }
} }
@ -156,7 +156,7 @@ static void read_trim_switch()
static void auto_trim() static void auto_trim()
{ {
if(auto_level_counter > 0){ if(auto_level_counter > 0) {
//g.rc_1.dead_zone = 60; // 60 = .6 degrees //g.rc_1.dead_zone = 60; // 60 = .6 degrees
//g.rc_2.dead_zone = 60; //g.rc_2.dead_zone = 60;
@ -165,7 +165,7 @@ static void auto_trim()
led_mode = AUTO_TRIM_LEDS; led_mode = AUTO_TRIM_LEDS;
do_simple = false; do_simple = false;
if(auto_level_counter == 1){ if(auto_level_counter == 1) {
//g.rc_1.dead_zone = 0; // 60 = .6 degrees //g.rc_1.dead_zone = 0; // 60 = .6 degrees
//g.rc_2.dead_zone = 0; //g.rc_2.dead_zone = 0;
led_mode = NORMAL_LEDS; led_mode = NORMAL_LEDS;
@ -182,22 +182,22 @@ static void auto_trim()
/* /*
How this works: * How this works:
Level Example: * Level Example:
A_off: -14.00, -20.59, -30.80 * A_off: -14.00, -20.59, -30.80
*
Right roll Example: * Right roll Example:
A_off: -6.73, 89.05, -46.02 * A_off: -6.73, 89.05, -46.02
*
Left Roll Example: * Left Roll Example:
A_off: -18.11, -160.31, -56.42 * A_off: -18.11, -160.31, -56.42
*
Pitch Forward: * Pitch Forward:
A_off: -127.00, -22.16, -50.09 * A_off: -127.00, -22.16, -50.09
*
Pitch Back: * Pitch Back:
A_off: 201.95, -24.00, -88.56 * A_off: 201.95, -24.00, -88.56
*/ */
static void trim_accel() static void trim_accel()
{ {
@ -209,24 +209,24 @@ static void trim_accel()
trim_roll = constrain(trim_roll, -1.5, 1.5); trim_roll = constrain(trim_roll, -1.5, 1.5);
trim_pitch = constrain(trim_pitch, -1.5, 1.5); trim_pitch = constrain(trim_pitch, -1.5, 1.5);
if(g.rc_1.control_in > 200){ // Roll Right if(g.rc_1.control_in > 200) { // Roll Right
imu.ay(imu.ay() - trim_roll); imu.ay(imu.ay() - trim_roll);
}else if (g.rc_1.control_in < -200){ }else if (g.rc_1.control_in < -200) {
imu.ay(imu.ay() - trim_roll); imu.ay(imu.ay() - trim_roll);
} }
if(g.rc_2.control_in > 200){ // Pitch Back if(g.rc_2.control_in > 200) { // Pitch Back
imu.ax(imu.ax() + trim_pitch); imu.ax(imu.ax() + trim_pitch);
}else if (g.rc_2.control_in < -200){ }else if (g.rc_2.control_in < -200) {
imu.ax(imu.ax() + trim_pitch); imu.ax(imu.ax() + trim_pitch);
} }
/* /*
Serial.printf_P(PSTR("r:%1.2f %1.2f \t| p:%1.2f %1.2f\n"), * Serial.printf_P(PSTR("r:%1.2f %1.2f \t| p:%1.2f %1.2f\n"),
trim_roll, * trim_roll,
(float)imu.ay(), * (float)imu.ay(),
trim_pitch, * trim_pitch,
(float)imu.ax()); * (float)imu.ax());
//*/ * //*/
} }