aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

CLI and magneto initializations

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@692 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jphelirc 2010-10-19 15:22:58 +00:00
parent ede3a46cf1
commit f5cd4e45f9
8 changed files with 456 additions and 278 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

184
ArducopterNG/ArduUser.h
View File

@ -47,10 +47,17 @@ TODO:
/* Will be moved in future to AN extension ports */ /* Will be moved in future to AN extension ports */
/* due need to have PWM pins free for sonars and servos */ /* due need to have PWM pins free for sonars and servos */
/*
#define FR_LED 3 // Mega PE4 pin, OUT7 #define FR_LED 3 // Mega PE4 pin, OUT7
#define RE_LED 2 // Mega PE5 pin, OUT6 #define RE_LED 2 // Mega PE5 pin, OUT6
#define RI_LED 7 // Mega PH4 pin, OUT5 #define RI_LED 7 // Mega PH4 pin, OUT5
#define LE_LED 8 // Mega PH5 pin, OUT4 #define LE_LED 8 // Mega PH5 pin, OUT4
*/
#define FR_LED AN12 // Mega PE4 pin, OUT7
#define RE_LED AN14 // Mega PE5 pin, OUT6
#define RI_LED AN10 // Mega PH4 pin, OUT5
#define LE_LED AN8 // Mega PH5 pin, OUT4
/* AM PIN Definitions - END */ /* AM PIN Definitions - END */
@ -76,7 +83,7 @@ TODO:
#define AUX_MID 1500 #define AUX_MID 1500
#define CHANN_CENTER 1500 // Channel center, legacy #define CHANN_CENTER 1500 // Channel center, legacy
#define MIN_THROTTLE 1040 // Throttle pulse width at minimun... #define MIN_THROTTLE 1080 // Throttle pulse width at minimun...
/*************************************************************/ /*************************************************************/
// General definitions // General definitions
@ -102,180 +109,5 @@ TODO:
#define ACCELZ 5 #define ACCELZ 5
#define LASTSENSOR 6 #define LASTSENSOR 6
// Following variables stored in EEPROM
float KP_QUAD_ROLL;
float KI_QUAD_ROLL;
float STABLE_MODE_KP_RATE_ROLL;
float KP_QUAD_PITCH;
float KI_QUAD_PITCH;
float STABLE_MODE_KP_RATE_PITCH;
float KP_QUAD_YAW;
float KI_QUAD_YAW;
float STABLE_MODE_KP_RATE_YAW;
float STABLE_MODE_KP_RATE; //NOT USED NOW
float KP_GPS_ROLL;
float KI_GPS_ROLL;
float KD_GPS_ROLL;
float KP_GPS_PITCH;
float KI_GPS_PITCH;
float KD_GPS_PITCH;
float GPS_MAX_ANGLE;
float KP_ALTITUDE;
float KI_ALTITUDE;
float KD_ALTITUDE;
int acc_offset_x;
int acc_offset_y;
int acc_offset_z;
int gyro_offset_roll;
int gyro_offset_pitch;
int gyro_offset_yaw;
float Kp_ROLLPITCH;
float Ki_ROLLPITCH;
float Kp_YAW;
float Ki_YAW;
float GEOG_CORRECTION_FACTOR;
int MAGNETOMETER;
float Kp_RateRoll;
float Ki_RateRoll;
float Kd_RateRoll;
float Kp_RatePitch;
float Ki_RatePitch;
float Kd_RatePitch;
float Kp_RateYaw;
float Ki_RateYaw;
float Kd_RateYaw;
float xmitFactor;
float ch_roll_slope = 1;
float ch_pitch_slope = 1;
float ch_throttle_slope = 1;
float ch_yaw_slope = 1;
float ch_aux_slope = 1;
float ch_aux2_slope = 1;
float ch_roll_offset = 0;
float ch_pitch_offset = 0;
float ch_throttle_offset = 0;
float ch_yaw_offset = 0;
float ch_aux_offset = 0;
float ch_aux2_offset = 0;
// This function call contains the default values that are set to the ArduCopter
// when a "Default EEPROM Value" command is sent through serial interface
void defaultUserConfig() {
KP_QUAD_ROLL = 4.0;
KI_QUAD_ROLL = 0.15;
STABLE_MODE_KP_RATE_ROLL = 1.2;
KP_QUAD_PITCH = 4.0;
KI_QUAD_PITCH = 0.15;
STABLE_MODE_KP_RATE_PITCH = 1.2;
KP_QUAD_YAW = 3.0;
KI_QUAD_YAW = 0.15;
STABLE_MODE_KP_RATE_YAW = 2.4;
STABLE_MODE_KP_RATE = 0.2; // NOT USED NOW
KP_GPS_ROLL = 0.015;
KI_GPS_ROLL = 0.005;
KD_GPS_ROLL = 0.01;
KP_GPS_PITCH = 0.015;
KI_GPS_PITCH = 0.005;
KD_GPS_PITCH = 0.01;
GPS_MAX_ANGLE = 22;
KP_ALTITUDE = 0.8;
KI_ALTITUDE = 0.2;
KD_ALTITUDE = 0.2;
acc_offset_x = 2048;
acc_offset_y = 2048;
acc_offset_z = 2048;
gyro_offset_roll = 1659;
gyro_offset_pitch = 1650;
gyro_offset_yaw = 1650;
Kp_ROLLPITCH = 0.0014;
Ki_ROLLPITCH = 0.00000015;
Kp_YAW = 1.0;
Ki_YAW = 0.00002;
GEOG_CORRECTION_FACTOR = 0.87;
MAGNETOMETER = 0;
Kp_RateRoll = 1.95;
Ki_RateRoll = 0.0;
Kd_RateRoll = 0.0;
Kp_RatePitch = 1.95;
Ki_RatePitch = 0.0;
Kd_RatePitch = 0.0;
Kp_RateYaw = 3.2;
Ki_RateYaw = 0.0;
Kd_RateYaw = 0.0;
xmitFactor = 0.32;
roll_mid = 1500;
pitch_mid = 1500;
yaw_mid = 1500;
ch_roll_slope = 1;
ch_pitch_slope = 1;
ch_throttle_slope = 1;
ch_yaw_slope = 1;
ch_aux_slope = 1;
ch_aux2_slope = 1;
ch_roll_offset = 0;
ch_pitch_offset = 0;
ch_throttle_offset = 0;
ch_yaw_offset = 0;
ch_aux_offset = 0;
ch_aux2_offset = 0;
}
// EEPROM storage addresses
#define KP_QUAD_ROLL_ADR 0
#define KI_QUAD_ROLL_ADR 8
#define STABLE_MODE_KP_RATE_ROLL_ADR 4
#define KP_QUAD_PITCH_ADR 12
#define KI_QUAD_PITCH_ADR 20
#define STABLE_MODE_KP_RATE_PITCH_ADR 16
#define KP_QUAD_YAW_ADR 24
#define KI_QUAD_YAW_ADR 32
#define STABLE_MODE_KP_RATE_YAW_ADR 28
#define STABLE_MODE_KP_RATE_ADR 36 // NOT USED NOW
#define KP_GPS_ROLL_ADR 40
#define KI_GPS_ROLL_ADR 48
#define KD_GPS_ROLL_ADR 44
#define KP_GPS_PITCH_ADR 52
#define KI_GPS_PITCH_ADR 60
#define KD_GPS_PITCH_ADR 56
#define GPS_MAX_ANGLE_ADR 64
#define KP_ALTITUDE_ADR 68
#define KI_ALTITUDE_ADR 76
#define KD_ALTITUDE_ADR 72
#define acc_offset_x_ADR 80
#define acc_offset_y_ADR 84
#define acc_offset_z_ADR 88
#define gyro_offset_roll_ADR 92
#define gyro_offset_pitch_ADR 96
#define gyro_offset_yaw_ADR 100
#define Kp_ROLLPITCH_ADR 104
#define Ki_ROLLPITCH_ADR 108
#define Kp_YAW_ADR 112
#define Ki_YAW_ADR 116
#define GEOG_CORRECTION_FACTOR_ADR 120
#define MAGNETOMETER_ADR 124
#define XMITFACTOR_ADR 128
#define KP_RATEROLL_ADR 132
#define KI_RATEROLL_ADR 136
#define KD_RATEROLL_ADR 140
#define KP_RATEPITCH_ADR 144
#define KI_RATEPITCH_ADR 148
#define KD_RATEPITCH_ADR 152
#define KP_RATEYAW_ADR 156
#define KI_RATEYAW_ADR 160
#define KD_RATEYAW_ADR 164
#define CHROLL_MID 168
#define CHPITCH_MID 172
#define CHYAW_MID 176
#define ch_roll_slope_ADR 180
#define ch_pitch_slope_ADR 184
#define ch_throttle_slope_ADR 188
#define ch_yaw_slope_ADR 192
#define ch_aux_slope_ADR 196
#define ch_aux2_slope_ADR 200
#define ch_roll_offset_ADR 204
#define ch_pitch_offset_ADR 208
#define ch_throttle_offset_ADR 212
#define ch_yaw_offset_ADR 216
#define ch_aux_offset_ADR 220
#define ch_aux2_offset_ADR 224

176
ArducopterNG/Arducopter.h
View File

@ -428,4 +428,180 @@ unsigned long elapsedTime = 0; // for doing custom events
// SEVERITY_HIGH // SEVERITY_HIGH
// SEVERITY_CRITICAL // SEVERITY_CRITICAL
// Following variables stored in EEPROM
float KP_QUAD_ROLL;
float KI_QUAD_ROLL;
float STABLE_MODE_KP_RATE_ROLL;
float KP_QUAD_PITCH;
float KI_QUAD_PITCH;
float STABLE_MODE_KP_RATE_PITCH;
float KP_QUAD_YAW;
float KI_QUAD_YAW;
float STABLE_MODE_KP_RATE_YAW;
float STABLE_MODE_KP_RATE; //NOT USED NOW
float KP_GPS_ROLL;
float KI_GPS_ROLL;
float KD_GPS_ROLL;
float KP_GPS_PITCH;
float KI_GPS_PITCH;
float KD_GPS_PITCH;
float GPS_MAX_ANGLE;
float KP_ALTITUDE;
float KI_ALTITUDE;
float KD_ALTITUDE;
int acc_offset_x;
int acc_offset_y;
int acc_offset_z;
int gyro_offset_roll;
int gyro_offset_pitch;
int gyro_offset_yaw;
float Kp_ROLLPITCH;
float Ki_ROLLPITCH;
float Kp_YAW;
float Ki_YAW;
float GEOG_CORRECTION_FACTOR;
int MAGNETOMETER;
float Kp_RateRoll;
float Ki_RateRoll;
float Kd_RateRoll;
float Kp_RatePitch;
float Ki_RatePitch;
float Kd_RatePitch;
float Kp_RateYaw;
float Ki_RateYaw;
float Kd_RateYaw;
float xmitFactor;
float ch_roll_slope = 1;
float ch_pitch_slope = 1;
float ch_throttle_slope = 1;
float ch_yaw_slope = 1;
float ch_aux_slope = 1;
float ch_aux2_slope = 1;
float ch_roll_offset = 0;
float ch_pitch_offset = 0;
float ch_throttle_offset = 0;
float ch_yaw_offset = 0;
float ch_aux_offset = 0;
float ch_aux2_offset = 0;
// This function call contains the default values that are set to the ArduCopter
// when a "Default EEPROM Value" command is sent through serial interface
void defaultUserConfig() {
KP_QUAD_ROLL = 4.0;
KI_QUAD_ROLL = 0.15;
STABLE_MODE_KP_RATE_ROLL = 1.2;
KP_QUAD_PITCH = 4.0;
KI_QUAD_PITCH = 0.15;
STABLE_MODE_KP_RATE_PITCH = 1.2;
KP_QUAD_YAW = 3.0;
KI_QUAD_YAW = 0.15;
STABLE_MODE_KP_RATE_YAW = 2.4;
STABLE_MODE_KP_RATE = 0.2; // NOT USED NOW
KP_GPS_ROLL = 0.015;
KI_GPS_ROLL = 0.005;
KD_GPS_ROLL = 0.01;
KP_GPS_PITCH = 0.015;
KI_GPS_PITCH = 0.005;
KD_GPS_PITCH = 0.01;
GPS_MAX_ANGLE = 22;
KP_ALTITUDE = 0.8;
KI_ALTITUDE = 0.2;
KD_ALTITUDE = 0.2;
acc_offset_x = 2048;
acc_offset_y = 2048;
acc_offset_z = 2048;
gyro_offset_roll = 1659;
gyro_offset_pitch = 1650;
gyro_offset_yaw = 1650;
Kp_ROLLPITCH = 0.0014;
Ki_ROLLPITCH = 0.00000015;
Kp_YAW = 1.0;
Ki_YAW = 0.00002;
GEOG_CORRECTION_FACTOR = 0.87;
MAGNETOMETER = 0;
Kp_RateRoll = 1.95;
Ki_RateRoll = 0.0;
Kd_RateRoll = 0.0;
Kp_RatePitch = 1.95;
Ki_RatePitch = 0.0;
Kd_RatePitch = 0.0;
Kp_RateYaw = 3.2;
Ki_RateYaw = 0.0;
Kd_RateYaw = 0.0;
xmitFactor = 0.32;
roll_mid = 1500;
pitch_mid = 1500;
yaw_mid = 1500;
ch_roll_slope = 1;
ch_pitch_slope = 1;
ch_throttle_slope = 1;
ch_yaw_slope = 1;
ch_aux_slope = 1;
ch_aux2_slope = 1;
ch_roll_offset = 0;
ch_pitch_offset = 0;
ch_throttle_offset = 0;
ch_yaw_offset = 0;
ch_aux_offset = 0;
ch_aux2_offset = 0;
}
// EEPROM storage addresses
#define KP_QUAD_ROLL_ADR 0
#define KI_QUAD_ROLL_ADR 8
#define STABLE_MODE_KP_RATE_ROLL_ADR 4
#define KP_QUAD_PITCH_ADR 12
#define KI_QUAD_PITCH_ADR 20
#define STABLE_MODE_KP_RATE_PITCH_ADR 16
#define KP_QUAD_YAW_ADR 24
#define KI_QUAD_YAW_ADR 32
#define STABLE_MODE_KP_RATE_YAW_ADR 28
#define STABLE_MODE_KP_RATE_ADR 36 // NOT USED NOW
#define KP_GPS_ROLL_ADR 40
#define KI_GPS_ROLL_ADR 48
#define KD_GPS_ROLL_ADR 44
#define KP_GPS_PITCH_ADR 52
#define KI_GPS_PITCH_ADR 60
#define KD_GPS_PITCH_ADR 56
#define GPS_MAX_ANGLE_ADR 64
#define KP_ALTITUDE_ADR 68
#define KI_ALTITUDE_ADR 76
#define KD_ALTITUDE_ADR 72
#define acc_offset_x_ADR 80
#define acc_offset_y_ADR 84
#define acc_offset_z_ADR 88
#define gyro_offset_roll_ADR 92
#define gyro_offset_pitch_ADR 96
#define gyro_offset_yaw_ADR 100
#define Kp_ROLLPITCH_ADR 104
#define Ki_ROLLPITCH_ADR 108
#define Kp_YAW_ADR 112
#define Ki_YAW_ADR 116
#define GEOG_CORRECTION_FACTOR_ADR 120
#define MAGNETOMETER_ADR 124
#define XMITFACTOR_ADR 128
#define KP_RATEROLL_ADR 132
#define KI_RATEROLL_ADR 136
#define KD_RATEROLL_ADR 140
#define KP_RATEPITCH_ADR 144
#define KI_RATEPITCH_ADR 148
#define KD_RATEPITCH_ADR 152
#define KP_RATEYAW_ADR 156
#define KI_RATEYAW_ADR 160
#define KD_RATEYAW_ADR 164
#define CHROLL_MID 168
#define CHPITCH_MID 172
#define CHYAW_MID 176
#define ch_roll_slope_ADR 180
#define ch_pitch_slope_ADR 184
#define ch_throttle_slope_ADR 188
#define ch_yaw_slope_ADR 192
#define ch_aux_slope_ADR 196
#define ch_aux2_slope_ADR 200
#define ch_roll_offset_ADR 204
#define ch_pitch_offset_ADR 208
#define ch_throttle_offset_ADR 212
#define ch_yaw_offset_ADR 216
#define ch_aux_offset_ADR 220
#define ch_aux2_offset_ADR 224

70
ArducopterNG/ArducopterNG.pde
View File

@ -39,44 +39,88 @@
/* **************** MAIN PROGRAM - MODULES ******************** */ /* **************** MAIN PROGRAM - MODULES ******************** */
/* ************************************************************ */ /* ************************************************************ */
/* User definable modules */ /* ************************************************************ */
// Comment out with // modules that you are not using // User definable modules, check them every time you have new
// software version at your hand.
// Comment out with // modules that you are not using
//#define IsGPS // Do we have a GPS connected //#define IsGPS // Do we have a GPS connected
//#define IsNEWMTEK // Do we have MTEK with new firmware //#define IsNEWMTEK // Do we have MTEK with new firmware
//#define IsMAG // Do we have a Magnetometer connected, if have remember to activate it from Configurator //#define IsMAG // Do we have a Magnetometer connected, if have remember to activate it from Configurator
//LEGACY-jp #define IsTEL // Do we have a telemetry connected, eg. XBee connected on Telemetry port //#define IsAM // Do we have motormount LED's. AM = Atraction Mode
#define IsAM // Do we have motormount LED's. AM = Atraction Mode
//#define UseAirspeed
//#define UseAirspeed // Quads don't use AirSpeed... Legacy, jp 19-10-10
//#define UseBMP //#define UseBMP
//#define BATTERY_EVENT 1 // (boolean) 0 = don't read battery, 1 = read battery voltage (only if you have it _wired_ up!) //#define BATTERY_EVENT 1 // (boolean) 0 = don't read battery, 1 = read battery voltage (only if you have it _wired_ up!)
#define CONFIGURATOR #define CONFIGURATOR
//////////////////// ////////////////////
// Telemetry // Serial ports & speeds
// Serial data, do we have FTDI cable or Xbee on Telemetry port as our primary command link // Serial data, do we have FTDI cable or Xbee on Telemetry port as our primary command link
// If we are using normal FTDI/USB port as our telemetry/configuration, disable next // If we are using normal FTDI/USB port as our telemetry/configuration, keep next line disabled
//#define SerXbee //#define SerXbee
// Telemetry port speed, default is 115200 // Telemetry port speed, default is 115200
//#define SerBau 19200
//#define SerBau 38400
//#define SerBau 57600
#define SerBau 115200 #define SerBau 115200
// For future use, for now don't activate any!
// Serial1 speed for GPS, mostly 38.4k, done from libraries
//#define GpsBau 19200
//#define GpsBau 38400
//#define GpsBau 57600
//#define GpsBau 115200
//////////////////////
// Flight orientation
/* ************************************************* */
// Flight & Electronics orientation
// Frame build condiguration // Frame build condiguration
#define FLIGHT_MODE_+ // Traditional "one arm as nose" frame configuration #define FLIGHT_MODE_+ // Traditional "one arm as nose" frame configuration
//#define FLIGHT_MODE_X // Frame orientation 45 deg to CCW, nose between two arms //#define FLIGHT_MODE_X // Frame orientation 45 deg to CCW, nose between two arms
// Magneto orientation and corrections.
// If you don't have magneto actiavted, It is safe to ignore these
#ifdef IsMAG
#define MAGORIENTATION APM_COMPASS_COMPONENTS_UP_PINS_FORWARD // This is default solution for ArduCopter
//#define MAGORIENTATION APM_COMPASS_COMPONENTS_UP_PINS_BACK // Alternative orientation for ArduCopter
//#define MAGORIENTATION APM_COMPASS_COMPONENTS_DOWN_PINS_FORWARD // If you have soldered Magneto to IMU shield in WIki pictures shows
// To get Magneto offsets, switch to CLI mode and run offset calibration. During calibration
// you need to roll/bank/tilt/yaw/shake etc your ArduCoptet. Don't kick like Jani always does :)
#define MAGOFFSET 0,0,0
// 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/
#define DECLINATION 0.0
// And remember result from NOAA website is in form of DEGREES°MINUTES'. Degrees you can use directly but Minutes you need to
// recalculate due they one degree is 60 minutes.. For example Jani's real declination is 0.61, correct way to calculate this is
// 37 / 60 = 0.61 and for Helsinki it would be 7°44' eg 7. and then 44/60 = 0.73 so declination for Helsinki/South Finland would be 7.73
// East values are positive
// West values are negative
// Some of devel team's Declinations and their Cities
//#define DECLINATION 0.61 // Jani, Bangkok, 0°37' E (Due I live almost at Equator, my Declination is rather small)
//#define DECLINATION 7.73 // Jani, Helsinki,7°44' E (My "summer" home back at Finland)
//#define DECLINATION -20.68 // Sandro, Belo Horizonte, 22°08' W (Whoah... Sandro is really DECLINED)
//#define DECLINATION 7.03 // Randy, Tokyo, 7°02'E
//#define DECLINATION 8.91 // Doug, Denver, 8°55'E
//#define DECLINATION -6.08 // Jose, Canary Islands, 6°5'W
//#define DECLINATION 0.73 // Tony, Minneapolis, 0°44'E
#endif
@ -106,7 +150,7 @@
//#include <GPS_NMEA.h> // ArduPilot NMEA GPS library //#include <GPS_NMEA.h> // ArduPilot NMEA GPS library
/* Software version */ /* Software version */
#define VER 1.5 // Current software version (only numeric values) #define VER 1.51 // Current software version (only numeric values)
/* ************************************************************ */ /* ************************************************************ */

124
ArducopterNG/CLI.pde
View File

@ -25,31 +25,143 @@
* ************************************************************** * * ************************************************************** *
ChangeLog: ChangeLog:
- 19-10-10 Initial CLI
* ************************************************************** * * ************************************************************** *
TODO: TODO:
- Full menu systems, debug, settings
* ************************************************************** */ * ************************************************************** */
boolean ShowMainMenu;
// CLI Functions // CLI Functions
// This can be moved later to CLI.pde // This can be moved later to CLI.pde
void RunCLI () { void RunCLI () {
ShowMainMenu = TRUE;
// We need to initialize Serial again due it was not initialized during startup. // We need to initialize Serial again due it was not initialized during startup.
SerBeg(SerBau); SerBeg(SerBau);
SerPri("Welcome to ArduCopter CLI"); SerPrln();
SerPrln("Welcome to ArduCopter CLI");
SerPri("Firmware: "); SerPri("Firmware: ");
SerPri(VER); SerPrln(VER);
SerPrln();
if(ShowMainMenu) Show_MainMenu();
// Our main loop that never ends. Only way to get away from here is to reboot APM // Our main loop that never ends. Only way to get away from here is to reboot APM
for (;;) { for (;;) {
if(ShowMainMenu) Show_MainMenu();
if (SerAva()) {
ShowMainMenu = TRUE;
queryType = SerRea();
switch (queryType) {
case 'c':
CALIB_CompassOffset();
break;
}
}
} // Mainloop ends } // Mainloop ends
} }
void Show_MainMenu() {
ShowMainMenu = FALSE;
SerPrln("CLI Menu - Type your command on command prompt");
SerPrln("----------------------------------------------");
SerPrln(" c - Show compass offsets (no return, reboot)");
SerPrln(" ");
}
/* ************************************************************** */
// 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....");
APM_Compass.Init(); // Initialization
APM_Compass.SetOrientation(MAGORIENTATION); // set compass's orientation on aircraft
APM_Compass.SetOffsets(0,0,0); // set offsets to account for surrounding interference
APM_Compass.SetDeclination(ToRad(DECLINATION)); // set local difference between magnetic north and true north
int counter = 0;
for(;;) {
static float min[3], max[3], offset[3];
if((millis()- timer) > 100) {
timer = millis();
APM_Compass.Read();
APM_Compass.Calculate(0,0); // roll = 0, pitch = 0 for this example
// capture min
if( APM_Compass.Mag_X < min[0] ) min[0] = APM_Compass.Mag_X;
if( APM_Compass.Mag_Y < min[1] ) min[1] = APM_Compass.Mag_Y;
if( APM_Compass.Mag_Z < min[2] ) min[2] = APM_Compass.Mag_Z;
// capture max
if( APM_Compass.Mag_X > max[0] ) max[0] = APM_Compass.Mag_X;
if( APM_Compass.Mag_Y > max[1] ) max[1] = APM_Compass.Mag_Y;
if( APM_Compass.Mag_Z > max[2] ) max[2] = APM_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(APM_Compass.Heading));
SerPri(" \t(");
SerPri(APM_Compass.Mag_X);
SerPri(",");
SerPri(APM_Compass.Mag_Y);
SerPri(",");
SerPri(APM_Compass.Mag_Z);
SerPri(")");
// display offsets
SerPri("\t offsets(");
SerPri(offset[0]);
SerPri(",");
SerPri(offset[1]);
SerPri(",");
SerPri(offset[2]);
SerPri(")");
SerPrln();
if(counter == 50) {
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++;
}
}
#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
}

4
ArducopterNG/GCS.pde
View File

@ -145,6 +145,10 @@ void readSerialCommand() {
ch_aux_offset = readFloatSerial(); ch_aux_offset = readFloatSerial();
ch_aux2_slope = readFloatSerial(); ch_aux2_slope = readFloatSerial();
ch_aux2_offset = readFloatSerial(); ch_aux2_offset = readFloatSerial();
break;
case '5': // Special debug features
break; break;
} }
} }

52
ArducopterNG/Motors.pde
View File

@ -32,3 +32,55 @@ TODO:
* ************************************************************** */ * ************************************************************** */
// Send output commands to ESC´s
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
digitalWrite(FR_LED, HIGH); // AM-Mode
#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
digitalWrite(FR_LED, LOW); // AM-Mode
#endif
digitalWrite(LED_Green,HIGH); // Ready LED on
rightMotor = MIN_THROTTLE;
leftMotor = MIN_THROTTLE;
frontMotor = MIN_THROTTLE;
backMotor = MIN_THROTTLE;
// Reset_I_Terms();
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);
// InstantPWM => Force inmediate output on PWM signals
APM_RC.Force_Out0_Out1();
APM_RC.Force_Out2_Out3();
}

49
ArducopterNG/Radio.pde
View File

@ -123,52 +123,3 @@ void read_radio()
} }
// Send output commands to ESC´s
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
digitalWrite(FR_LED, HIGH); // AM-Mode
#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
digitalWrite(FR_LED, LOW); // AM-Mode
#endif
digitalWrite(LED_Green,HIGH); // Ready LED on
rightMotor = MIN_THROTTLE;
leftMotor = MIN_THROTTLE;
frontMotor = MIN_THROTTLE;
backMotor = MIN_THROTTLE;
// Reset_I_Terms();
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);
// InstantPWM => Force inmediate output on PWM signals
APM_RC.Force_Out0_Out1();
APM_RC.Force_Out2_Out3();
}

51
ArducopterNG/System.pde
View File

@ -6,9 +6,9 @@
File : System.pde File : System.pde
Version : v1.0, Aug 27, 2010 Version : v1.0, Aug 27, 2010
Author(s): ArduCopter Team Author(s): ArduCopter Team
Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz, Ted Carancho (aeroquad), Jose Julio, Jordi Muñoz,
Jani Hirvinen, Ken McEwans, Roberto Navoni, Jani Hirvinen, Ken McEwans, Roberto Navoni,
Sandro Benigno, Chris Anderson Sandro Benigno, Chris Anderson
This program is free software: you can redistribute it and/or modify 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 it under the terms of the GNU General Public License as published by
@ -23,15 +23,15 @@
You should have received a copy of the GNU General Public License You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>. along with this program. If not, see <http://www.gnu.org/licenses/>.
* ************************************************************** * * ************************************************************** *
ChangeLog: ChangeLog:
* ************************************************************** * * ************************************************************** *
TODO: TODO:
* ************************************************************** */ * ************************************************************** */
// General Initialization for all APM electronics // General Initialization for all APM electronics
void APM_Init() { void APM_Init() {
@ -46,19 +46,19 @@ void APM_Init() {
// Because DDRE and DDRL Ports are not included to normal Arduino Libraries, we need to // Because DDRE and DDRL Ports are not included to normal Arduino Libraries, we need to
// initialize them with a special command // initialize them with a special command
APMPinMode(DDRE,7,INPUT); // DIP1, (PE7), Closest to sliding SW2 switch APMPinMode(DDRE,7,INPUT); // DIP1, (PE7), Closest DIP to sliding SW2 switch
APMPinMode(DDRE,6,INPUT); // DIP2, (PE6) APMPinMode(DDRE,6,INPUT); // DIP2, (PE6)
APMPinMode(DDRL,6,INPUT); // DIP3, (PL6) APMPinMode(DDRL,6,INPUT); // DIP3, (PL6)
APMPinMode(DDRL,7,INPUT); // DIP4, (PL7) 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. // Is CLI mode active or not, if it is fire it up and never return.
if(digitalRead(SW2)) { if(digitalRead(SW2)) {
SerPrln("Entering CLI Mode.."); RunCLI();
RunCLI(); // Btw.. We never return from this....
} }
/* ********************************************************* */ /* ********************************************************* */
///////////////////////////////////////////////////////// /////////////////////////////////////////////////////////
// Normal Initialization sequence starts from here. // Normal Initialization sequence starts from here.
APM_RC.Init(); // APM Radio initialization APM_RC.Init(); // APM Radio initialization
@ -106,12 +106,18 @@ void APM_Init() {
APM_RC.OutputCh(2,MIN_THROTTLE); APM_RC.OutputCh(2,MIN_THROTTLE);
APM_RC.OutputCh(3,MIN_THROTTLE); APM_RC.OutputCh(3,MIN_THROTTLE);
if (MAGNETOMETER == 1) #ifdef IsMag
if (MAGNETOMETER == 1) {
APM_Compass.Init(); // I2C initialization APM_Compass.Init(); // I2C initialization
APM_Compass.SetOrientation(MAGORIENTATION);
APM_Compass.SetOffsets(MAGOFFSET);
APM_Compass.SetDeclination(ToRad(DECLINATION));
}
#endif
DataFlash.StartWrite(1); // Start a write session on page 1 DataFlash.StartWrite(1); // Start a write session on page 1
//Serial.begin(115200); // Old mode serial begin, remove soon, by jp/17-10-10 //Serial.begin(115200); // Old mode serial begin, remove soon, by jp/17-10-10
//Serial.println("ArduCopter Quadcopter v1.0"); //Serial.println("ArduCopter Quadcopter v1.0");
@ -157,12 +163,12 @@ void APM_Init() {
SW_DIP4 = APMPinRead(PINL, 7); SW_DIP4 = APMPinRead(PINL, 7);
/* Works, tested 18-10-10 JP /* Works, tested 18-10-10 JP
if(SW_DIP1) { if(SW_DIP1) {
SerPrln("+ mode"); SerPrln("+ mode");
} else { } else {
SerPrln("x mode"); SerPrln("x mode");
} }
*/ */
DataFlash.StartWrite(1); // Start a write session on page 1 DataFlash.StartWrite(1); // Start a write session on page 1
@ -177,3 +183,4 @@ void APM_Init() {
} }