mirror of https://github.com/ArduPilot/ardupilot
CLI and magneto initializations
git-svn-id: https://arducopter.googlecode.com/svn/trunk@692 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jphelirc
parent
ede3a46cf1
commit
f5cd4e45f9
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -39,44 +39,88 @@
|
|||
/* **************** MAIN PROGRAM - MODULES ******************** */
|
||||
/* ************************************************************ */
|
||||
|
||||
/* User definable modules */
|
||||
/* ************************************************************ */
|
||||
// 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 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 <GPS_NMEA.h> // 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)
|
||||
|
||||
|
||||
/* ************************************************************ */
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -145,6 +145,10 @@ void readSerialCommand() {
|
|||
ch_aux_offset = readFloatSerial();
|
||||
ch_aux2_slope = readFloatSerial();
|
||||
ch_aux2_offset = readFloatSerial();
|
||||
break;
|
||||
case '5': // Special debug features
|
||||
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -23,15 +23,15 @@
|
|||
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:
|
||||
* ************************************************************** *
|
||||
ChangeLog:
|
||||
|
||||
|
||||
* ************************************************************** *
|
||||
TODO:
|
||||
* ************************************************************** *
|
||||
TODO:
|
||||
|
||||
|
||||
* ************************************************************** */
|
||||
* ************************************************************** */
|
||||
|
||||
// General Initialization for all APM electronics
|
||||
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
|
||||
// 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();
|
||||
// Btw.. We never return from this....
|
||||
}
|
||||
|
||||
/* ********************************************************* */
|
||||
/////////////////////////////////////////////////////////
|
||||
/* ********************************************************* */
|
||||
/////////////////////////////////////////////////////////
|
||||
// Normal Initialization sequence starts from here.
|
||||
|
||||
APM_RC.Init(); // APM Radio initialization
|
||||
|
|
@ -106,8 +106,14 @@ 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
|
||||
|
||||
|
|
@ -177,3 +183,4 @@ void APM_Init() {
|
|||
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue