Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/archive/ArducopterNG/CLI.pde

742 lines
18 KiB
Plaintext
Raw Blame History

/*
www.ArduCopter.com - www.DIYDrones.com
Copyright (c) 2010. All rights reserved.
An Open Source Arduino based multicopter.
File : CLI.pde
Version : v1.0, Oct 18, 2010
Author(s): ArduCopter Team
Jani Hirvinen, Jose Julio, Jordi Muñoz,
Ken McEwans, Roberto Navoni, Sandro Benigno, Chris Anderson, Randy McEvans
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
* ************************************************************** *
ChangeLog:
- 19-10-10 Initial CLI
* ************************************************************** *
TODO:
- Full menu systems, debug, settings
* ************************************************************** */
boolean ShowMainMenu;
// CLI Functions
// This can be moved later to CLI.pde
void RunCLI () {
// APM_RC.Init(); // APM Radio initialization
readUserConfig(); // Read memory values from EEPROM
ShowMainMenu = TRUE;
// We need to initialize Serial again due it was not initialized during startup.
SerBeg(SerBau);
SerPrln();
SerPrln("Welcome to ArduCopter CLI");
SerPri("Firmware: ");
SerPrln(VER);
SerPrln();
SerPrln("Make sure that you have Carriage Return active");
if(ShowMainMenu) Show_MainMenu();
// Our main loop that never ends. Only way to get away from here is to reboot APM
for (;;) {
if(ShowMainMenu) Show_MainMenu();
delay(50);
if (SerAva()) {
ShowMainMenu = TRUE;
queryType = SerRea();
switch (queryType) {
case 'a':
Flip_MAG();
break;
case 'c':
CALIB_CompassOffset();
break;
case 'd':
Log_Read(1,4000);
break;
case 'i':
CALIB_AccOffset();
break;
case 't':
CALIB_Throttle();
break;
case 'e':
CALIB_Esc();
break;
case 'o':
Set_SonarAndObstacleAvoidance_PIDs();
break;
case 's':
Show_Settings();
break;
case 'r':
Reset_Settings();
break;
case 'm':
RUN_Motors();
break;
case 'z':
Log_Erase();
break;
}
SerFlu();
}
// Changing LED statuses to inform that we are in CLI mode
// Blinking Red, Yellow, Green when in CLI mode
if(millis() - cli_timer > 1000) {
cli_timer = millis();
CLILedStep();
}
} // Mainloop ends
}
void Show_MainMenu() {
ShowMainMenu = FALSE;
SerPrln();
SerPrln("CLI Menu - Type your command on command prompt");
SerPrln("----------------------------------------------");
SerPrln(" a - Activate/Deactivate compass (check #IsMag define too)");
SerPrln(" c - Show/Save compass offsets");
SerPrln(" d - dump logs to serial");
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(" o - Show/Save sonar & obstacle avoidance PIDs");
SerPrln(" r - Reset to factory settings");
SerPrln(" t - Calibrate MIN Throttle value");
SerPrln(" s - Show settings");
SerPrln(" z - clear all logs");
SerPrln(" ");
SerFlu();
}
/* ************************************************************** */
// Compass/Magnetometer Offset Calibration
void CALIB_CompassOffset() {
#ifdef IsMAG
SerPrln("Rotate/Pitch/Roll your ArduCopter untill offset variables are not");
SerPrln("anymore changing, write down your offset values and update them ");
SerPrln("to their correct location. Starting in..");
SerPrln("2 secs.");
delay(1000);
SerPrln("1 secs.");
delay(1000);
SerPrln("starting now....");
AP_Compass.init(); // Initialization
AP_Compass.set_orientation(MAGORIENTATION); // set compass's orientation on aircraft
AP_Compass.set_offsets(0,0,0); // set offsets to account for surrounding interference
AP_Compass.set_declination(ToRad(DECLINATION)); // set local difference between magnetic north and true north
int counter = 0;
SerFlu();
while(1) {
static float min[3], max[3], offset[3];
if((millis()- timer) > 100) {
timer = millis();
AP_Compass.read();
AP_Compass.calculate(0,0); // roll = 0, pitch = 0 for this example
// capture min
if( AP_Compass.mag_x < min[0] ) min[0] = AP_Compass.mag_x;
if( AP_Compass.mag_y < min[1] ) min[1] = AP_Compass.mag_y;
if( AP_Compass.mag_z < min[2] ) min[2] = AP_Compass.mag_z;
// capture max
if( AP_Compass.mag_x > max[0] ) max[0] = AP_Compass.mag_x;
if( AP_Compass.mag_y > max[1] ) max[1] = AP_Compass.mag_y;
if( AP_Compass.mag_z > max[2] ) max[2] = AP_Compass.mag_z;
// calculate offsets
offset[0] = -(max[0]+min[0])/2;
offset[1] = -(max[1]+min[1])/2;
offset[2] = -(max[2]+min[2])/2;
// display all to user
SerPri("Heading:");
SerPri(ToDeg(AP_Compass.heading));
SerPri(" \t(");
SerPri(AP_Compass.mag_x);
SerPri(",");
SerPri(AP_Compass.mag_y);
SerPri(",");
SerPri(AP_Compass.mag_z);
SerPri(") ");
// display offsets
SerPri("\t offsets(");
SerPri(offset[0]);
SerPri(",");
SerPri(offset[1]);
SerPri(",");
SerPri(offset[2]);
SerPri(")");
SerPrln();
if(counter == 200) {
counter = 0;
SerPrln();
SerPrln("Roll and Rotate your quad untill offsets are not changing!");
// SerPrln("to exit from this loop, reboot your APM");
SerPrln();
delay(500);
}
counter++;
}
if(SerAva() > 0){
mag_offset_x = offset[0];
mag_offset_y = offset[1];
mag_offset_z = offset[2];
SerPrln("Saving Offsets to EEPROM");
writeEEPROM(mag_offset_x, mag_offset_x_ADR);
writeEEPROM(mag_offset_y, mag_offset_y_ADR);
writeEEPROM(mag_offset_z, mag_offset_z_ADR);
delay(500);
SerPrln("Saved...");
SerPrln();
break;
}
}
#else
SerPrln("Magneto module is not activated on Arducopter.pde");
SerPrln("Check your program #definitions, upload firmware and try again!!");
// SerPrln("Now reboot your APM");
// for(;;)
// delay(10);
#endif
}
/* ************************************************************** */
// Accell calibration
void CALIB_AccOffset() {
int loopy;
long xx = 0, xy = 0, xz = 0;
SerPrln("Initializing Accelerometers..");
adc.Init(); // APM ADC library initialization
// delay(250); // Giving small moment before starting
calibrateSensors(); // Calibrate neutral values of gyros (in Sensors.pde)
SerPrln();
SerPrln("Sampling 50 samples from Accelerometers, don't move your ArduCopter!");
SerPrln("Sample:\tAcc-X\tAxx-Y\tAcc-Z");
for(loopy = 1; loopy <= 50; loopy++) {
SerPri(" ");
SerPri(loopy);
SerPri(":");
tab();
SerPri(xx += adc.Ch(4));
tab();
SerPri(xy += adc.Ch(5));
tab();
SerPrln(xz += adc.Ch(6));
delay(20);
}
xx = xx / (loopy - 1);
xy = xy / (loopy - 1);
xz = xz / (loopy - 1);
xz = xz - 407; // Z-Axis correction
// xx += 42;
acc_offset_y = xy;
acc_offset_x = xx;
acc_offset_z = xz;
AN_OFFSET[3] = acc_offset_x;
AN_OFFSET[4] = acc_offset_y;
AN_OFFSET[5] = acc_offset_z;
SerPrln();
SerPrln("Offsets as follows: ");
SerPri(" ");
tab();
SerPri(acc_offset_x);
tab();
SerPri(acc_offset_y);
tab();
SerPrln(acc_offset_z);
SerPrln("Final results as follows: ");
SerPri(" ");
tab();
SerPri(adc.Ch(4) - acc_offset_x);
tab();
SerPri(adc.Ch(5) - acc_offset_y);
tab();
SerPrln(adc.Ch(6) - acc_offset_z);
SerPrln();
SerPrln("Final results should be close to 0, 0, 408 if they are far (-+10) from ");
SerPrln("those values, redo initialization or use Configurator for finetuning");
SerPrln();
SerPrln("Saving values to EEPROM");
writeEEPROM(acc_offset_x, acc_offset_x_ADR);
writeEEPROM(acc_offset_y, acc_offset_y_ADR);
writeEEPROM(acc_offset_z, acc_offset_z_ADR);
delay(200);
SerPrln("Saved..");
SerPrln();
}
void Flip_MAG() {
SerPrln("Activate/Deactivate Magentometer!");
SerPri("Magnetometer is now: ");
delay(500);
if(MAGNETOMETER == 0) {
MAGNETOMETER = 1;
writeEEPROM(MAGNETOMETER, MAGNETOMETER_ADR);
SerPrln("Activated");
} else {
MAGNETOMETER = 0;
writeEEPROM(MAGNETOMETER, MAGNETOMETER_ADR);
SerPrln("Deactivated");
}
delay(1000);
SerPrln("State... saved");
}
void CALIB_Throttle() {
int tmpThrottle = 1200;
SerPrln("Move your throttle to MIN, reading starts in 3 seconds");
delay(1000);
SerPrln("2. ");
delay(1000);
SerPrln("1. ");
delay(1000);
SerPrln("Reading Throttle value, hit enter to exit");
SerFlu();
while(1) {
ch_throttle = APM_RC.InputCh(CH_THROTTLE);
SerPri("Throttle channel: ");
SerPrln(ch_throttle);
if(tmpThrottle > ch_throttle) tmpThrottle = ch_throttle;
delay(50);
if(SerAva() > 0){
break;
}
}
SerPrln();
SerPri("Lowest throttle value: ");
SerPrln(tmpThrottle);
SerPrln();
SerPrln("Saving MIN_THROTTLE to EEPROM");
writeEEPROM(tmpThrottle, MIN_THROTTLE_ADR);
delay(200);
SerPrln("Saved..");
SerPrln();
}
void CALIB_Esc() {
SerPrln("Disconnect your battery if you have it connected, keep your USB cable/Xbee connected!");
SerPrln("After battery is disconnected hit enter key and wait more instructions:");
SerPrln("As safety measure, unmount all your propellers before continuing!!");
WaitSerialEnter();
SerPrln("Move your Throttle to maximum and connect your battery. ");
SerPrln("after you hear beep beep tone, move your throttle to minimum and");
SerPrln("hit enter after you are ready to disarm motors.");
SerPrln("Arming now all motors");
delay(500);
SerPrln("Motors: ARMED");
delay(200);
SerPrln("Connect your battery and let ESCs to reboot!");
while(1) {
ch_throttle = APM_RC.InputCh(CH_THROTTLE);
APM_RC.OutputCh(0, ch_throttle);
APM_RC.OutputCh(1, ch_throttle);
APM_RC.OutputCh(2, ch_throttle);
APM_RC.OutputCh(3, ch_throttle);
// InstantPWM => Force inmediate output on PWM signals
APM_RC.Force_Out0_Out1();
APM_RC.Force_Out2_Out3();
delay(20);
if(SerAva() > 0){
break;
}
}
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();
SerPrln("Motors: DISARMED");
SerPrln();
}
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) {
SerPrln("Left Motor");
OutMotor(1);
delay(500);
}
if(ch_roll > 1600) {
SerPrln("Right Motor");
OutMotor(0);
delay(500);
}
if(ch_pitch < 1400) {
SerPrln("Front Motor");
OutMotor(2);
delay(500);
}
if(ch_pitch > 1600) {
SerPrln("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();
SerPrln("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();
}
byte Reset_Settings() {
int c;
SerPrln("Reseting EEPROM to default!");
delay(500);
SerFlu();
delay(500);
SerPrln("Hit 'Y' to reset factory settings, any other and you will return to main menu!");
do {
c = SerRea();
}
while (-1 == c);
if (('y' != c) && ('Y' != c)) {
SerPrln("EEPROM has not reseted!");
SerPrln("Returning to main menu.");
return(-1);
}
SerPrln("Reseting to factory settings!");
defaultUserConfig();
delay(200);
SerPrln("Saving to EEPROM");
writeUserConfig();
SerPrln("Done..");
}
void Show_Settings() {
// Reading current EEPROM values
SerPrln("ArduCopter - Current settings");
SerPrln("-----------------------------");
SerPri("Firmware: ");
SerPri(VER);
SerPrln();
SerPrln();
readUserConfig();
delay(50);
SerPri("Magnetom. offsets (x,y,z): ");
SerPri(mag_offset_x);
cspc();
SerPri(mag_offset_y);
cspc();
SerPri(mag_offset_z);
SerPrln();
SerPri("Accel offsets (x,y,z): ");
SerPri(acc_offset_x);
cspc();
SerPri(acc_offset_y);
cspc();
SerPri(acc_offset_z);
SerPrln();
SerPri("Min Throttle: ");
SerPrln(MIN_THROTTLE);
SerPri("Magnetometer 1-ena/0-dis: ");
SerPrln(MAGNETOMETER, DEC);
SerPri("Camera mode: ");
SerPrln(cam_mode, DEC);
SerPri("Flight orientation: ");
if(SW_DIP1) {
SerPrln("x mode");
}
else {
SerPrln("+ mode");
}
Show_SonarAndObstacleAvoidance_PIDs();
SerPrln();
}
// Display obstacle avoidance pids
void Show_SonarAndObstacleAvoidance_PIDs() {
SerPri("\tSonar PID: ");
SerPri(KP_SONAR_ALTITUDE); cspc();
SerPri(KI_SONAR_ALTITUDE); cspc();
SerPrln(KD_SONAR_ALTITUDE);
SerPri("\tObstacle SafetyZone: ");
SerPrln(RF_SAFETY_ZONE);
SerPri("\tRoll PID: ");
SerPri(KP_RF_ROLL); cspc();
SerPri(KI_RF_ROLL); cspc();
SerPrln(KD_RF_ROLL);
SerPri("\tPitch PID: ");
SerPri(KP_RF_PITCH); cspc();
SerPri(KI_RF_PITCH); cspc();
SerPri(KD_RF_PITCH);
SerPrln();
SerPri("\tMaxAngle: ");
SerPri(RF_MAX_ANGLE);
SerPrln();
}
// save RF pids to eeprom
void Save_SonarAndObstacleAvoidancePIDs_toEEPROM() {
writeEEPROM(KP_RF_ROLL,KP_RF_ROLL_ADR);
writeEEPROM(KI_RF_ROLL,KI_RF_ROLL_ADR);
writeEEPROM(KD_RF_ROLL,KD_RF_ROLL_ADR);
writeEEPROM(KP_RF_PITCH,KP_RF_PITCH_ADR);
writeEEPROM(KI_RF_PITCH,KI_RF_PITCH_ADR);
writeEEPROM(KD_RF_PITCH,KD_RF_PITCH_ADR);
writeEEPROM(RF_MAX_ANGLE,RF_MAX_ANGLE_ADR);
writeEEPROM(RF_SAFETY_ZONE,RF_SAFETY_ZONE_ADR);
writeEEPROM(KP_SONAR_ALTITUDE,KP_SONAR_ALTITUDE_ADR);
writeEEPROM(KI_SONAR_ALTITUDE,KI_SONAR_ALTITUDE_ADR);
writeEEPROM(KD_SONAR_ALTITUDE,KD_SONAR_ALTITUDE_ADR);
}
//
void Set_SonarAndObstacleAvoidance_PIDs() {
float tempVal1, tempVal2, tempVal3;
int saveToEeprom = 0;
// Display current PID values
SerPrln("Sonar and Obstacle Avoidance:");
Show_SonarAndObstacleAvoidance_PIDs();
SerPrln();
// SONAR PIDs
SerFlu();
SerPri("Enter Sonar P;I;D; values or 0 to skip: ");
while( !SerAva() ); // wait until user presses a key
tempVal1 = readFloatSerial();
tempVal2 = readFloatSerial();
tempVal3 = readFloatSerial();
if( tempVal1 != 0 || tempVal2 != 0 || tempVal3 != 0 ) {
KP_SONAR_ALTITUDE = tempVal1;
KI_SONAR_ALTITUDE = tempVal2;
KD_SONAR_ALTITUDE = tempVal3;
SerPrln();
SerPri("P:");
SerPri(KP_SONAR_ALTITUDE);
SerPri("\tI:");
SerPri(KI_SONAR_ALTITUDE);
SerPri("\tD:");
SerPri(KD_SONAR_ALTITUDE);
saveToEeprom = 1;
}
SerPrln();
// SAFETY ZONE
SerFlu();
SerPri("Enter Safety Zone (in cm) or 0 to skip: ");
while( !SerAva() ); // wait until user presses a key
tempVal1 = readFloatSerial();
if( tempVal1 >= 20 && tempVal1 <= 700 ) {
RF_SAFETY_ZONE = tempVal1;
SerPri("SafetyZone: ");
SerPri(RF_SAFETY_ZONE);
saveToEeprom = 1;
}
SerPrln();
// ROLL PIDs
SerFlu();
SerPri("Enter Roll P;I;D; values or 0 to skip: ");
while( !SerAva() ); // wait until user presses a key
tempVal1 = readFloatSerial();
tempVal2 = readFloatSerial();
tempVal3 = readFloatSerial();
if( tempVal1 != 0 || tempVal2 != 0 || tempVal3 != 0 ) {
KP_RF_ROLL = tempVal1;
KI_RF_ROLL = tempVal2;
KD_RF_ROLL = tempVal3;
SerPrln();
SerPri("P:");
SerPri(KP_RF_ROLL);
SerPri("\tI:");
SerPri(KI_RF_ROLL);
SerPri("\tD:");
SerPri(KD_RF_ROLL);
saveToEeprom = 1;
}
SerPrln();
// PITCH PIDs
SerFlu();
SerPri("Enter Pitch P;I;D; values or 0 to skip: ");
while( !SerAva() ); // wait until user presses a key
tempVal1 = readFloatSerial();
tempVal2 = readFloatSerial();
tempVal3 = readFloatSerial();
if( tempVal1 != 0 || tempVal2 != 0 || tempVal3 != 0 ) {
KP_RF_PITCH = tempVal1;
KI_RF_PITCH = tempVal2;
KD_RF_PITCH = tempVal3;
SerPrln();
SerPri("P:");
SerPri(KP_RF_PITCH);
SerPri("\tI:");
SerPri(KI_RF_PITCH);
SerPri("\tD:");
SerPri(KD_RF_PITCH);
saveToEeprom = 1;
}
SerPrln();
// Max Angle
SerFlu();
SerPri("Enter Max Angle or 0 to skip: ");
while( !SerAva() ); // wait until user presses a key
tempVal1 = readFloatSerial();
SerPrln(tempVal1);
if( tempVal1 > 0 && tempVal1 < 90 ) {
RF_MAX_ANGLE = tempVal1;
SerPrln();
SerPri("MaxAngle: ");
SerPri(RF_MAX_ANGLE);
saveToEeprom = 1;
}
SerPrln();
// save to eeprom
if( saveToEeprom == 1 ) {
SerPrln("Obstacle Avoidance:");
Show_SonarAndObstacleAvoidance_PIDs();
SerPrln();
Save_SonarAndObstacleAvoidancePIDs_toEEPROM();
SerPrln("Saved to EEPROM");
SerPrln();
}else{
SerPrln("No changes. Nothing saved to EEPROM");
SerPrln();
}
}
void cspc() {
SerPri(", ");
}
void WaitSerialEnter() {
// Flush serials
SerFlu();
delay(50);
while(1) {
if(SerAva() > 0){
break;
}
delay(20);
}
delay(250);
SerFlu();
}
Reference in New Issue View Git Blame Copy Permalink