diff --git a/ArducopterNG/ArduUser.h b/ArducopterNG/ArduUser.h index f50a861609..7fd5b34073 100644 --- a/ArducopterNG/ArduUser.h +++ b/ArducopterNG/ArduUser.h @@ -47,10 +47,17 @@ TODO: /* Will be moved in future to AN extension ports */ /* due need to have PWM pins free for sonars and servos */ +/* #define FR_LED 3 // Mega PE4 pin, OUT7 #define RE_LED 2 // Mega PE5 pin, OUT6 #define RI_LED 7 // Mega PH4 pin, OUT5 #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 */ @@ -76,7 +83,7 @@ TODO: #define AUX_MID 1500 #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 @@ -102,180 +109,5 @@ TODO: #define ACCELZ 5 #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 - diff --git a/ArducopterNG/Arducopter.h b/ArducopterNG/Arducopter.h index 484d36b11a..eca11c866b 100644 --- a/ArducopterNG/Arducopter.h +++ b/ArducopterNG/Arducopter.h @@ -428,4 +428,180 @@ unsigned long elapsedTime = 0; // for doing custom events // SEVERITY_HIGH // 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 diff --git a/ArducopterNG/ArducopterNG.pde b/ArducopterNG/ArducopterNG.pde index ab09dd6ad6..b71a133c50 100644 --- a/ArducopterNG/ArducopterNG.pde +++ b/ArducopterNG/ArducopterNG.pde @@ -39,44 +39,88 @@ /* **************** 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 IsNEWMTEK // Do we have MTEK with new firmware -//#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 UseAirspeed +//#define IsMAG // Do we have a Magnetometer connected, if have remember to activate it from Configurator +//#define IsAM // Do we have motormount LED's. AM = Atraction Mode +//#define UseAirspeed // Quads don't use AirSpeed... Legacy, jp 19-10-10 //#define UseBMP - //#define BATTERY_EVENT 1 // (boolean) 0 = don't read battery, 1 = read battery voltage (only if you have it _wired_ up!) #define CONFIGURATOR //////////////////// -// Telemetry +// Serial ports & speeds // 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 // Telemetry port speed, default is 115200 +//#define SerBau 19200 +//#define SerBau 38400 +//#define SerBau 57600 #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 #define FLIGHT_MODE_+ // Traditional "one arm as nose" frame configuration //#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 // ArduPilot NMEA GPS library /* Software version */ -#define VER 1.5 // Current software version (only numeric values) +#define VER 1.51 // Current software version (only numeric values) /* ************************************************************ */ diff --git a/ArducopterNG/CLI.pde b/ArducopterNG/CLI.pde index 3e51a5d09d..55c6b23e37 100644 --- a/ArducopterNG/CLI.pde +++ b/ArducopterNG/CLI.pde @@ -25,31 +25,143 @@ * ************************************************************** * 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 () { + ShowMainMenu = TRUE; // We need to initialize Serial again due it was not initialized during startup. SerBeg(SerBau); - SerPri("Welcome to ArduCopter CLI"); + SerPrln(); + SerPrln("Welcome to ArduCopter CLI"); 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 for (;;) { + if(ShowMainMenu) Show_MainMenu(); + if (SerAva()) { + ShowMainMenu = TRUE; + queryType = SerRea(); + switch (queryType) { + case 'c': + CALIB_CompassOffset(); + break; - - + } + } } // 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 + +} + + + + diff --git a/ArducopterNG/GCS.pde b/ArducopterNG/GCS.pde index ed2467b6d6..9c8574a6a6 100644 --- a/ArducopterNG/GCS.pde +++ b/ArducopterNG/GCS.pde @@ -146,6 +146,10 @@ void readSerialCommand() { ch_aux2_slope = readFloatSerial(); ch_aux2_offset = readFloatSerial(); break; + case '5': // Special debug features + + + break; } } } diff --git a/ArducopterNG/Motors.pde b/ArducopterNG/Motors.pde index 8f6386fcd2..8fdec5912a 100644 --- a/ArducopterNG/Motors.pde +++ b/ArducopterNG/Motors.pde @@ -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(); +} + diff --git a/ArducopterNG/Radio.pde b/ArducopterNG/Radio.pde index 5683210ba8..9eb2d7debb 100644 --- a/ArducopterNG/Radio.pde +++ b/ArducopterNG/Radio.pde @@ -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(); -} - diff --git a/ArducopterNG/System.pde b/ArducopterNG/System.pde index a3fa318bae..2d99f2bdeb 100644 --- a/ArducopterNG/System.pde +++ b/ArducopterNG/System.pde @@ -6,9 +6,9 @@ File : System.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 . - -* ************************************************************** * -ChangeLog: - - -* ************************************************************** * -TODO: - - -* ************************************************************** */ + + * ************************************************************** * + ChangeLog: + + + * ************************************************************** * + TODO: + + + * ************************************************************** */ // General Initialization for all APM electronics void APM_Init() { @@ -43,24 +43,24 @@ void APM_Init() { pinMode(RELAY,OUTPUT); // Relay output (PL2) pinMode(SW1,INPUT); // Switch SW1 (PG0) pinMode(SW2,INPUT); // Switch SW2 (PG1) - + // Because DDRE and DDRL Ports are not included to normal Arduino Libraries, we need to // 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(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. if(digitalRead(SW2)) { - SerPrln("Entering CLI Mode.."); - RunCLI(); + RunCLI(); + // Btw.. We never return from this.... } -/* ********************************************************* */ -///////////////////////////////////////////////////////// + /* ********************************************************* */ + ///////////////////////////////////////////////////////// // Normal Initialization sequence starts from here. - + APM_RC.Init(); // APM Radio initialization // RC channels Initialization (Quad motors) @@ -106,19 +106,25 @@ void APM_Init() { APM_RC.OutputCh(2,MIN_THROTTLE); APM_RC.OutputCh(3,MIN_THROTTLE); - if (MAGNETOMETER == 1) +#ifdef IsMag + if (MAGNETOMETER == 1) { 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 - //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"); - + // Proper Serial port/baud are defined on main .pde and then Arducopter.h with // Choises of Xbee or normal serial port SerBeg(SerBau); - + // 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) @@ -128,7 +134,7 @@ void APM_Init() { delay(30000); } - + calibrateSensors(); // Calibrate neutral values of gyros (in Sensors.pde) // Neutro_yaw = APM_RC.InputCh(3); // Take yaw neutral radio value @@ -157,12 +163,12 @@ void APM_Init() { SW_DIP4 = APMPinRead(PINL, 7); /* Works, tested 18-10-10 JP - if(SW_DIP1) { - SerPrln("+ mode"); - } else { - SerPrln("x mode"); - } - */ + if(SW_DIP1) { + SerPrln("+ mode"); + } else { + SerPrln("x mode"); + } + */ DataFlash.StartWrite(1); // Start a write session on page 1 @@ -177,3 +183,4 @@ void APM_Init() { } +