mirror of https://github.com/ArduPilot/ardupilot
preparing for automatic calibration via CLI
git-svn-id: https://arducopter.googlecode.com/svn/trunk@752 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jphelirc
parent
7f4137569b
commit
4a07a3c09d
|
|
@ -59,6 +59,11 @@ TODO:
|
|||
#define RI_LED AN10 // Mega PH4 pin, OUT5
|
||||
#define LE_LED AN8 // Mega PH5 pin, OUT4
|
||||
|
||||
#define GPS_AM_PAT1 L\0x00\0x00\0x05
|
||||
#define GPS_AM_PAT2 L\0xFF\0xFF\0x05
|
||||
#define GPS_AM_PAT3 L\0xF0\0xF0\0x05
|
||||
|
||||
|
||||
/* AM PIN Definitions - END */
|
||||
|
||||
/*************************************************************/
|
||||
|
|
@ -86,6 +91,7 @@ TODO:
|
|||
#define MIN_THROTTLE 1040 // Throttle pulse width at minimun...
|
||||
|
||||
#define CAM_CENT 1500 // Camera center
|
||||
#define CAM_SMOOTHING 1000 // Camera movement smoothing
|
||||
|
||||
/*************************************************************/
|
||||
// General definitions
|
||||
|
|
|
|||
|
|
@ -110,6 +110,7 @@ TODO:
|
|||
//#define SerBau 115200 // Baud setting moved close next to port selection
|
||||
#define SerPri Serial3.print
|
||||
#define SerPrln Serial3.println
|
||||
#define SerPriln Serial3.println
|
||||
#define SerRea Serial3.read
|
||||
#define SerAva Serial3.available
|
||||
#define SerRea Serial3.read
|
||||
|
|
@ -122,6 +123,7 @@ TODO:
|
|||
//#define SerBau 115200 // Baud setting moved close next to port selection
|
||||
#define SerPri Serial.print
|
||||
#define SerPrln Serial.println
|
||||
#define SerPriln Serial.println
|
||||
#define SerRea Serial.read
|
||||
#define SerAva Serial.available
|
||||
#define SerRea Serial.read
|
||||
|
|
@ -156,6 +158,10 @@ int SENSOR_SIGN[]={
|
|||
#define GRAVITY 408 //this equivalent to 1G in the raw data coming from the accelerometer
|
||||
#define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square
|
||||
|
||||
#define ROLL_DEF 0 // Level values for roll, used to calculate roll_acc_offset
|
||||
#define PITCH_DEF 0 // Level values for pitch, used to calculate pitch_acc_offset
|
||||
#define Z_DEF GRAVITY // Stable level value for Z, used to calculate z_acc_offset, same as GRAVITY
|
||||
|
||||
#define ToRad(x) (x*0.01745329252) // *pi/180
|
||||
#define ToDeg(x) (x*57.2957795131) // *180/pi
|
||||
|
||||
|
|
|
|||
|
|
@ -251,7 +251,8 @@ void loop()
|
|||
// Send output commands to motor ESCs...
|
||||
motor_output();
|
||||
|
||||
// camera_output();
|
||||
// Do we have cameras stabilization connected and in use?
|
||||
if(!SW_DIP2) camera_output();
|
||||
|
||||
// Autopilot mode functions
|
||||
if (AP_mode == AP_AUTOMATIC_MODE)
|
||||
|
|
|
|||
|
|
@ -7,8 +7,7 @@
|
|||
Version : v1.0, Oct 18, 2010
|
||||
Author(s): ArduCopter Team
|
||||
Jani Hirvinen, Jose Julio, Jordi Muñoz,
|
||||
Ted Carancho (aeroquad), Ken McEwans, Roberto Navoni,
|
||||
Sandro Benigno, Chris Anderson, Randy McEvans
|
||||
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
|
||||
|
|
@ -40,6 +39,10 @@ boolean ShowMainMenu;
|
|||
// 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);
|
||||
|
|
@ -61,7 +64,9 @@ void RunCLI () {
|
|||
case 'c':
|
||||
CALIB_CompassOffset();
|
||||
break;
|
||||
|
||||
case 'i':
|
||||
CALIB_AccOffset();
|
||||
break;
|
||||
}
|
||||
}
|
||||
} // Mainloop ends
|
||||
|
|
@ -73,6 +78,7 @@ void Show_MainMenu() {
|
|||
SerPrln("CLI Menu - Type your command on command prompt");
|
||||
SerPrln("----------------------------------------------");
|
||||
SerPrln(" c - Show compass offsets (no return, reboot)");
|
||||
SerPrln(" i - Initialize and calibrate Accel offsets");
|
||||
SerPrln(" ");
|
||||
}
|
||||
|
||||
|
|
@ -162,6 +168,59 @@ void CALIB_CompassOffset() {
|
|||
}
|
||||
|
||||
|
||||
/* ************************************************************** */
|
||||
// Accell calibration
|
||||
void CALIB_AccOffset() {
|
||||
|
||||
uint8_t loopy;
|
||||
uint16_t xx = 0, xy = 0, xz = 0;
|
||||
|
||||
APM_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 10 samples from Accelerometers, don't move your ArduCopter!");
|
||||
SerPrln("Sample:\tAcc-X\tAxx-Y\tAcc-Z");
|
||||
|
||||
for(loopy = 1; loopy <= 5; loopy++) {
|
||||
SerPri(" ");
|
||||
SerPri(loopy);
|
||||
SerPri(":");
|
||||
tab();
|
||||
/* SerPri(xx += read_adc(4));
|
||||
tab();
|
||||
SerPri(xy += -read_adc(3));
|
||||
tab();
|
||||
SerPrln(xz += read_adc(5));
|
||||
*/
|
||||
SerPri(xx += APM_ADC.Ch(4));
|
||||
tab();
|
||||
SerPri(xy += APM_ADC.Ch(5));
|
||||
tab();
|
||||
SerPrln(xz += APM_ADC.Ch(3));
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
xx = xx / (loopy - 1);
|
||||
xy = xy / (loopy - 1);
|
||||
xz = xz / (loopy - 1) ;
|
||||
|
||||
SerPriln("Averages as follows");
|
||||
SerPri(" ");
|
||||
tab();
|
||||
SerPri(xx);
|
||||
tab();
|
||||
SerPri(xy);
|
||||
tab();
|
||||
SerPri(xz);
|
||||
SerPriln();
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -435,6 +435,9 @@ void comma() {
|
|||
SerPri(',');
|
||||
}
|
||||
|
||||
void tab() {
|
||||
SerPri("\t");
|
||||
}
|
||||
|
||||
// Used to read floating point values from the serial port
|
||||
float readFloatSerial() {
|
||||
|
|
|
|||
|
|
@ -6,9 +6,9 @@
|
|||
File : Motors.pde
|
||||
Version : v1.0, Aug 27, 2010
|
||||
Author(s): ArduCopter Team
|
||||
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
|
||||
Jani Hirvinen, Ken McEwans, Roberto Navoni,
|
||||
Sandro Benigno, Chris Anderson
|
||||
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
|
||||
Jani Hirvinen, Ken McEwans, Roberto Navoni,
|
||||
Sandro Benigno, Chris Anderson
|
||||
|
||||
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
|
||||
|
|
@ -22,16 +22,16 @@
|
|||
|
||||
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:
|
||||
|
||||
|
||||
* ************************************************************** *
|
||||
TODO:
|
||||
|
||||
|
||||
* ************************************************************** */
|
||||
|
||||
* ************************************************************** *
|
||||
ChangeLog:
|
||||
|
||||
|
||||
* ************************************************************** *
|
||||
TODO:
|
||||
|
||||
|
||||
* ************************************************************** */
|
||||
|
||||
|
||||
|
||||
|
|
@ -40,24 +40,24 @@ void motor_output()
|
|||
{
|
||||
if (ch_throttle < (MIN_THROTTLE + 100)) // If throttle is low we disable yaw (neccesary to arm/disarm motors safely)
|
||||
control_yaw = 0;
|
||||
|
||||
|
||||
// Quadcopter mix
|
||||
if (motorArmed == 1)
|
||||
{
|
||||
#ifdef IsAM
|
||||
if (motorArmed == 1) {
|
||||
#ifdef IsAM
|
||||
digitalWrite(FR_LED, HIGH); // AM-Mode
|
||||
#endif
|
||||
#endif
|
||||
|
||||
// Quadcopter output mix
|
||||
rightMotor = constrain(ch_throttle - control_roll + control_yaw, minThrottle, 2000);
|
||||
leftMotor = constrain(ch_throttle + control_roll + control_yaw, minThrottle, 2000);
|
||||
frontMotor = constrain(ch_throttle + control_pitch - control_yaw, minThrottle, 2000);
|
||||
backMotor = constrain(ch_throttle - control_pitch - control_yaw, minThrottle, 2000);
|
||||
}
|
||||
else // MOTORS DISARMED
|
||||
{
|
||||
#ifdef IsAM
|
||||
|
||||
} else { // MOTORS DISARMED
|
||||
|
||||
#ifdef IsAM
|
||||
digitalWrite(FR_LED, LOW); // AM-Mode
|
||||
#endif
|
||||
#endif
|
||||
digitalWrite(LED_Green,HIGH); // Ready LED on
|
||||
|
||||
rightMotor = MIN_THROTTLE;
|
||||
|
|
@ -69,22 +69,21 @@ void motor_output()
|
|||
roll_I = 0; // reset I terms of PID controls
|
||||
pitch_I = 0;
|
||||
yaw_I = 0;
|
||||
|
||||
// Initialize yaw command to actual yaw when throttle is down...
|
||||
command_rx_yaw = ToDeg(yaw);
|
||||
}
|
||||
}
|
||||
|
||||
// Send commands to motors
|
||||
APM_RC.OutputCh(0, rightMotor);
|
||||
APM_RC.OutputCh(1, leftMotor);
|
||||
APM_RC.OutputCh(2, frontMotor);
|
||||
APM_RC.OutputCh(3, backMotor);
|
||||
|
||||
// Do we have cameras stabilization connected and in use?
|
||||
if(SW_DIP2) camera_output();
|
||||
|
||||
|
||||
// InstantPWM => Force inmediate output on PWM signals
|
||||
APM_RC.Force_Out0_Out1();
|
||||
APM_RC.Force_Out2_Out3();
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -66,9 +66,11 @@ void calibrateSensors(void) {
|
|||
{
|
||||
Read_adc_raw(); // Read sensors
|
||||
for(gyro=GYROZ; gyro<=GYROY; gyro++)
|
||||
aux_float[gyro]=aux_float[gyro]*0.8 + AN[gyro]*0.2; // Filtering
|
||||
Log_Write_Sensor(AN[0],AN[1],AN[2],AN[3],AN[4],AN[5],0);
|
||||
aux_float[gyro] = aux_float[gyro] * 0.8 + AN[gyro] * 0.2; // Filtering
|
||||
Log_Write_Sensor(AN[0], AN[1], AN[2], AN[3], AN[4], AN[5], 0);
|
||||
|
||||
delay(5);
|
||||
|
||||
RunningLights(j); // (in Functions.pde)
|
||||
// Runnings lights effect to let user know that we are taking mesurements
|
||||
if((i % 5) == 0) j++;
|
||||
|
|
@ -78,8 +80,8 @@ void calibrateSensors(void) {
|
|||
// Switch off all ABC lights
|
||||
LightsOff();
|
||||
|
||||
for(gyro=GYROZ; gyro<=GYROY; gyro++)
|
||||
AN_OFFSET[gyro]=aux_float[gyro]; // Update sensor OFFSETs from values read
|
||||
for(gyro = GYROZ; gyro <= GYROY; gyro++)
|
||||
AN_OFFSET[gyro] = aux_float[gyro]; // Update sensor OFFSETs from values read
|
||||
}
|
||||
|
||||
#ifdef UseBMP
|
||||
|
|
@ -91,10 +93,10 @@ void read_baro(void)
|
|||
//tempPresAlt = pow(tempPresAlt, 0.190284);
|
||||
//press_alt = (1.0 - tempPresAlt) * 145366.45;
|
||||
tempPresAlt = pow(tempPresAlt, 0.190295);
|
||||
if (press_alt==0)
|
||||
if (press_alt == 0)
|
||||
press_alt = (1.0 - tempPresAlt) * 4433000; // Altitude in cm
|
||||
else
|
||||
press_alt = press_alt*0.9 + ((1.0 - tempPresAlt) * 443300); // Altitude in cm (filtered)
|
||||
press_alt = press_alt * 0.9 + ((1.0 - tempPresAlt) * 443300); // Altitude in cm (filtered)
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -51,11 +51,6 @@ void APM_Init() {
|
|||
APMPinMode(DDRL,6,INPUT); // DIP3, (PL6)
|
||||
APMPinMode(DDRL,7,INPUT); // DIP4, (PL7), Furthest DIP from sliding SW2 switch
|
||||
|
||||
// Is CLI mode active or not, if it is fire it up and never return.
|
||||
if(digitalRead(SW2)) {
|
||||
RunCLI();
|
||||
// Btw.. We never return from this....
|
||||
}
|
||||
|
||||
/* ********************************************************* */
|
||||
/////////////////////////////////////////////////////////
|
||||
|
|
@ -93,6 +88,14 @@ void APM_Init() {
|
|||
#endif
|
||||
#endif
|
||||
|
||||
|
||||
// Is CLI mode active or not, if it is fire it up and never return.
|
||||
if(digitalRead(SW2)) {
|
||||
RunCLI();
|
||||
// Btw.. We never return from this....
|
||||
}
|
||||
|
||||
|
||||
// Read DIP Switches and other important values. DIP switches needs special functions to
|
||||
// read due they are not defined as normal pins like other GPIO's are.
|
||||
SW_DIP1 = APMPinRead(PINE, 7);
|
||||
|
|
@ -143,7 +146,7 @@ void APM_Init() {
|
|||
|
||||
// Check if we enable the DataFlash log Read Mode (switch)
|
||||
// If we press switch 1 at startup we read the Dataflash eeprom
|
||||
while (digitalRead(SW1)==0)
|
||||
while (digitalRead(SW1)==0) // LEGACY remove soon by jp, 30-10-10
|
||||
{
|
||||
Serial.println("Entering Log Read Mode..."); // This will be obsole soon due moving to CLI system
|
||||
Log_Read(1,1000);
|
||||
|
|
|
|||
Loading…
Reference in New Issue