ESC/Motor tester on CLI

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1100 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jphelirc 2010-12-10 05:22:52 +00:00
parent d8642aeaab
commit b82ba72ab2
2 changed files with 86 additions and 10 deletions

View File

@ -150,10 +150,11 @@
// To get Magneto offsets, switch to CLI mode and run offset calibration. During calibration
// you need to roll/bank/tilt/yaw/shake etc your ArduCopter. Don't kick like Jani always does :)
#define MAGOFFSET 0,0,0
//#define MAGOFFSET 0,0,0
//#define MAGOFFSET -27.50,23.00,81.00
// Obsolete, Magnetometer offset are moved to CLI
// Declination is a correction factor between North Pole and real magnetic North. This is different on every location
// IF you want to use really accurate headholding and future navigation features, you should update this
// You can check Declination to your location from http://www.magnetic-declination.com/
@ -216,7 +217,7 @@
#endif
/* Software version */
#define VER 1.53 // Current software version (only numeric values)
#define VER 1.54 // Current software version (only numeric values)
// Sensors - declare one global instance
AP_ADC_ADS7844 adc;

View File

@ -81,6 +81,9 @@ void RunCLI () {
case 's':
Show_Settings();
break;
case 'm':
RUN_Motors();
break;
}
SerFlu();
}
@ -102,9 +105,10 @@ void Show_MainMenu() {
SerPrln();
SerPrln("CLI Menu - Type your command on command prompt");
SerPrln("----------------------------------------------");
SerPrln(" c - Show compass offsets");
SerPrln(" c - Show/Save compass offsets");
SerPrln(" e - ESC Throttle calibration (Works with official ArduCopter ESCs)");
SerPrln(" i - Initialize and calibrate Accel offsets");
SerPrln(" m - Motor tester with AIL/ELE stick");
SerPrln(" t - Calibrate MIN Throttle value");
SerPrln(" s - Show settings");
SerPrln(" ");
@ -370,6 +374,75 @@ void CALIB_Esc() {
}
void RUN_Motors() {
long run_timer;
byte motor;
SerPrln("Move your ROLL/PITCH Stick to up/down, left/right to start");
SerPrln("corresponding motor. Motor will pulse slowly! (20% Throttle)");
SerPrln("SAFETY!! Remove all propellers before doing stick movements");
SerPrln();
SerPrln("Exit from test by hiting Enter");
SerPrln();
SerFlu();
while(1) {
ch_roll = APM_RC.InputCh(CH_ROLL);
ch_pitch = APM_RC.InputCh(CH_PITCH);
if(ch_roll < 1400) {
SerPriln("Left Motor");
OutMotor(1);
delay(500);
}
if(ch_roll > 1600) {
SerPriln("Right Motor");
OutMotor(0);
delay(500);
}
if(ch_pitch < 1400) {
SerPriln("Front Motor");
OutMotor(2);
delay(500);
}
if(ch_pitch > 1600) {
SerPriln("Rear Motor");
OutMotor(3);
delay(500);
}
// Shuting down all motors
APM_RC.OutputCh(0, 900);
APM_RC.OutputCh(1, 900);
APM_RC.OutputCh(2, 900);
APM_RC.OutputCh(3, 900);
APM_RC.Force_Out0_Out1();
APM_RC.Force_Out2_Out3();
delay(100);
// delay(20);
if(SerAva() > 0){
SerFlu();
SerPriln("Exiting motor/esc tester...");
break;
}
}
}
// Just a small ESC/Motor commander
void OutMotor(byte motor_id) {
APM_RC.OutputCh(motor_id, 1200);
APM_RC.Force_Out0_Out1();
APM_RC.Force_Out2_Out3();
}
void Show_Settings() {
// Reading current EEPROM values
@ -411,7 +484,8 @@ void Show_Settings() {
SerPri("Flight orientation: ");
if(SW_DIP1) {
SerPriln("x mode");
} else {
}
else {
SerPriln("+ mode");
}
@ -442,3 +516,4 @@ void WaitSerialEnter() {