mirror of https://github.com/ArduPilot/ardupilot
preparing for hexa-x and hexa-+ frames, code updates
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1875 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
478af67099
commit
dd16a000cb
|
@ -20,8 +20,9 @@
|
|||
#define PLUS_FRAME 0
|
||||
#define X_FRAME 1
|
||||
#define TRI_FRAME 2
|
||||
#define HEXA_FRAME 3
|
||||
#define HEXAX_FRAME 3
|
||||
#define Y6_FRAME 4
|
||||
#define HEXAP_FRAME 5
|
||||
|
||||
// Internal defines, don't edit and expect things to work
|
||||
// -------------------------------------------------------
|
||||
|
|
|
@ -116,7 +116,7 @@ set_servos_4()
|
|||
APM_RC.OutputCh(CH_7, g.rc_4.radio_out);
|
||||
|
||||
|
||||
}else if (g.frame_type == HEXA_FRAME) {
|
||||
}else if (g.frame_type == HEXAX_FRAME) {
|
||||
//Serial.println("6_FRAME");
|
||||
|
||||
int roll_out = (float)g.rc_1.pwm_out * .866;
|
||||
|
@ -180,7 +180,7 @@ set_servos_4()
|
|||
motor_out[CH_3] = constrain(motor_out[CH_3], out_min, g.rc_3.radio_max.get());
|
||||
motor_out[CH_4] = constrain(motor_out[CH_4], out_min, g.rc_3.radio_max.get());
|
||||
|
||||
if ((g.frame_type == HEXA_FRAME) || (g.frame_type == Y6_FRAME)) {
|
||||
if ((g.frame_type == HEXAX_FRAME) || (g.frame_type == Y6_FRAME)) {
|
||||
motor_out[CH_7] = constrain(motor_out[CH_7], out_min, g.rc_3.radio_max.get());
|
||||
motor_out[CH_8] = constrain(motor_out[CH_8], out_min, g.rc_3.radio_max.get());
|
||||
}
|
||||
|
@ -273,7 +273,7 @@ set_servos_4()
|
|||
APM_RC.Force_Out0_Out1();
|
||||
APM_RC.Force_Out2_Out3();
|
||||
|
||||
if ((g.frame_type == HEXA_FRAME) || (g.frame_type == Y6_FRAME)) {
|
||||
if ((g.frame_type == HEXAX_FRAME) || (g.frame_type == Y6_FRAME)) {
|
||||
APM_RC.OutputCh(CH_7, motor_out[CH_7]);
|
||||
APM_RC.OutputCh(CH_8, motor_out[CH_8]);
|
||||
APM_RC.Force_Out6_Out7();
|
||||
|
@ -289,7 +289,7 @@ set_servos_4()
|
|||
APM_RC.Force_Out0_Out1();
|
||||
APM_RC.Force_Out2_Out3();
|
||||
|
||||
if ((g.frame_type == HEXA_FRAME) || (g.frame_type == Y6_FRAME)) {
|
||||
if ((g.frame_type == HEXAX_FRAME) || (g.frame_type == Y6_FRAME)) {
|
||||
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
|
||||
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
|
||||
APM_RC.Force_Out6_Out7();
|
||||
|
@ -318,7 +318,7 @@ set_servos_4()
|
|||
APM_RC.OutputCh(CH_4, motor_out[CH_4]);
|
||||
|
||||
|
||||
if ((g.frame_type == HEXA_FRAME) || (g.frame_type == Y6_FRAME)){
|
||||
if ((g.frame_type == HEXAX_FRAME) || (g.frame_type == Y6_FRAME)){
|
||||
APM_RC.OutputCh(CH_7, motor_out[CH_7]);
|
||||
APM_RC.OutputCh(CH_8, motor_out[CH_8]);
|
||||
}
|
||||
|
|
|
@ -474,12 +474,15 @@ setup_frame(uint8_t argc, const Menu::arg *argv)
|
|||
} else if (!strcmp_P(argv[1].str, PSTR("tri"))) {
|
||||
g.frame_type = TRI_FRAME;
|
||||
|
||||
} else if (!strcmp_P(argv[1].str, PSTR("hexa"))) {
|
||||
g.frame_type = HEXA_FRAME;
|
||||
} else if (!strcmp_P(argv[1].str, PSTR("hexax"))) {
|
||||
g.frame_type = HEXAX_FRAME;
|
||||
|
||||
} else if (!strcmp_P(argv[1].str, PSTR("y6"))) {
|
||||
g.frame_type = Y6_FRAME;
|
||||
|
||||
} else if (!strcmp_P(argv[1].str, PSTR("hexa+"))) {
|
||||
g.frame_type = HEXAP_FRAME;
|
||||
|
||||
}else{
|
||||
Serial.printf_P(PSTR("\nOptions:[+, x, tri, hexa, y6]\n"));
|
||||
report_frame();
|
||||
|
@ -845,10 +848,12 @@ void report_frame()
|
|||
Serial.printf_P(PSTR("Plus "));
|
||||
else if(g.frame_type == TRI_FRAME)
|
||||
Serial.printf_P(PSTR("TRI "));
|
||||
else if(g.frame_type == HEXA_FRAME)
|
||||
Serial.printf_P(PSTR("HEXA "));
|
||||
else if(g.frame_type == HEXAX_FRAME)
|
||||
Serial.printf_P(PSTR("HEXA X"));
|
||||
else if(g.frame_type == Y6_FRAME)
|
||||
Serial.printf_P(PSTR("Y6 "));
|
||||
else if(g.frame_type == HEXAP_FRAME)
|
||||
Serial.printf_P(PSTR("HEXA +"));
|
||||
|
||||
Serial.printf_P(PSTR("frame (%d)"), (int)g.frame_type);
|
||||
print_blanks(2);
|
||||
|
|
Loading…
Reference in New Issue