git-svn-id: https://arducopter.googlecode.com/svn/trunk@1596 f9c3cf11-9bcb-44bc-f272-b75c42450872

This commit is contained in:
jasonshort 2011-02-06 07:02:25 +00:00
parent 9ec8e29b7b
commit 76f0d1739b
1 changed files with 52 additions and 21 deletions

View File

@ -217,29 +217,29 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
while(1){ while(1){
delay(20); delay(20);
read_radio(); read_radio();
motor_out[LEFT] = rc_3.radio_min; motor_out[CH_1] = rc_3.radio_min;
motor_out[BACK] = rc_3.radio_min; motor_out[CH_2] = rc_3.radio_min;
motor_out[FRONT] = rc_3.radio_min; motor_out[CH_3] = rc_3.radio_min;
motor_out[RIGHT] = rc_3.radio_min; motor_out[CH_4] = rc_3.radio_min;
if(frame_type == PLUS_FRAME){ if(frame_type == PLUS_FRAME){
if(rc_1.control_in > 0){ if(rc_1.control_in > 0){
motor_out[RIGHT] = out_min; motor_out[CH_1] = out_min;
Serial.println("0"); Serial.println("0");
}else if(rc_1.control_in < 0){ }else if(rc_1.control_in < 0){
motor_out[LEFT] = out_min; motor_out[CH_2] = out_min;
Serial.println("1"); Serial.println("1");
} }
if(rc_2.control_in > 0){ if(rc_2.control_in > 0){
motor_out[BACK] = out_min; motor_out[CH_4] = out_min;
Serial.println("3"); Serial.println("3");
}else if(rc_2.control_in < 0){ }else if(rc_2.control_in < 0){
motor_out[FRONT] = out_min; motor_out[CH_3] = out_min;
Serial.println("2"); Serial.println("2");
} }
@ -247,35 +247,35 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
// lower right // lower right
if((rc_1.control_in > 0) && (rc_2.control_in > 0)){ if((rc_1.control_in > 0) && (rc_2.control_in > 0)){
motor_out[BACK] = out_min; motor_out[CH_4] = out_min;
Serial.println("3"); Serial.println("3");
// lower left // lower left
}else if((rc_1.control_in < 0) && (rc_2.control_in > 0)){ }else if((rc_1.control_in < 0) && (rc_2.control_in > 0)){
motor_out[LEFT] = out_min; motor_out[CH_2] = out_min;
Serial.println("1"); Serial.println("1");
// upper left // upper left
}else if((rc_1.control_in < 0) && (rc_2.control_in < 0)){ }else if((rc_1.control_in < 0) && (rc_2.control_in < 0)){
motor_out[FRONT] = out_min; motor_out[CH_3] = out_min;
Serial.println("2"); Serial.println("2");
// upper right // upper right
}else if((rc_1.control_in > 0) && (rc_2.control_in < 0)){ }else if((rc_1.control_in > 0) && (rc_2.control_in < 0)){
motor_out[RIGHT] = out_min; motor_out[CH_1] = out_min;
Serial.println("0"); Serial.println("0");
} }
}else if(frame_type == TRI_FRAME){ }else if(frame_type == TRI_FRAME){
if(rc_1.control_in > 0){ if(rc_1.control_in > 0){
motor_out[RIGHT] = out_min; motor_out[CH_1] = out_min;
}else if(rc_1.control_in < 0){ }else if(rc_1.control_in < 0){
motor_out[LEFT] = out_min; motor_out[CH_2] = out_min;
} }
if(rc_2.control_in > 0){ if(rc_2.control_in > 0){
motor_out[BACK] = out_min; motor_out[CH_4] = out_min;
} }
if(rc_4.control_in > 0){ if(rc_4.control_in > 0){
@ -286,7 +286,7 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
} }
rc_4.calc_pwm(); rc_4.calc_pwm();
motor_out[FRONT] = rc_4.radio_out; motor_out[CH_3] = rc_4.radio_out;
} }
if(rc_3.control_in > 0){ if(rc_3.control_in > 0){
@ -296,10 +296,10 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
if(frame_type != TRI_FRAME) if(frame_type != TRI_FRAME)
APM_RC.OutputCh(CH_4, rc_3.radio_in); APM_RC.OutputCh(CH_4, rc_3.radio_in);
}else{ }else{
APM_RC.OutputCh(CH_1, motor_out[RIGHT]); APM_RC.OutputCh(CH_1, motor_out[CH_1]);
APM_RC.OutputCh(CH_2, motor_out[LEFT]); APM_RC.OutputCh(CH_2, motor_out[CH_2]);
APM_RC.OutputCh(CH_3, motor_out[FRONT]); APM_RC.OutputCh(CH_3, motor_out[CH_3]);
APM_RC.OutputCh(CH_4, motor_out[BACK]); APM_RC.OutputCh(CH_4, motor_out[CH_4]);
} }
if(Serial.available() > 0){ if(Serial.available() > 0){
@ -323,7 +323,38 @@ setup_accel(uint8_t argc, const Menu::arg *argv)
static int8_t static int8_t
setup_pid(uint8_t argc, const Menu::arg *argv) setup_pid(uint8_t argc, const Menu::arg *argv)
{ {
default_gains(); if (!strcmp_P(argv[1].str, PSTR("default"))) {
default_gains();
}else if (!strcmp_P(argv[1].str, PSTR("s_kp"))) {
pid_stabilize_roll.kP(argv[2].f);
pid_stabilize_pitch.kP(argv[2].f);
save_EEPROM_PID();
}else if (!strcmp_P(argv[1].str, PSTR("s_kd"))) {
stabilize_dampener = argv[2].f;
save_EEPROM_PID();
}else if (!strcmp_P(argv[1].str, PSTR("y_kp"))) {
pid_yaw.kP(argv[2].f);
save_EEPROM_PID();
}else if (!strcmp_P(argv[1].str, PSTR("s_kd"))) {
pid_yaw.kD(argv[2].f);
save_EEPROM_PID();
}else if (!strcmp_P(argv[1].str, PSTR("t_kp"))) {
pid_baro_throttle.kP(argv[2].f);
save_EEPROM_PID();
}else if (!strcmp_P(argv[1].str, PSTR("t_kd"))) {
pid_baro_throttle.kD(argv[2].f);
save_EEPROM_PID();
}else{
default_gains();
}
report_gains(); report_gains();
} }