diff --git a/ArducopterNG/ArduUser.h b/ArducopterNG/ArduUser.h index 1f51472f95..7b8b9d4a79 100644 --- a/ArducopterNG/ArduUser.h +++ b/ArducopterNG/ArduUser.h @@ -116,9 +116,9 @@ TODO: #define CAM_SMOOTHING 1000 // Camera movement smoothing on pitch axis #define CAM_SMOOTHING_ROLL -400 // Camera movement smoothing on roll axis -#define CAM_TILT_OUT 6 // OUTx pin for Tilt servo -#define CAM_ROLL_OUT 7 // OUTx pin for Roll servo -#define CAM_YAW_OUT 7 // OUTx pin for Yaw servo (often same as Roll) +#define CAM_TILT_OUT 4 // OUTx pin for Tilt servo +#define CAM_ROLL_OUT 5 // OUTx pin for Roll servo +#define CAM_YAW_OUT 5 // OUTx pin for Yaw servo (often same as Roll) #define CAM_TILT_CH CH_7 // Channel for radio knob to controll tilt "zerolevel" @@ -147,4 +147,4 @@ TODO: #define ACCELY 4 #define ACCELZ 5 #define LASTSENSOR 6 - + diff --git a/ArducopterNG/Arducopter.h b/ArducopterNG/Arducopter.h index ebad25a00c..5c393a4f93 100644 --- a/ArducopterNG/Arducopter.h +++ b/ArducopterNG/Arducopter.h @@ -325,6 +325,9 @@ byte gled_status = HIGH; long gled_timer; int gled_speed; +long cli_timer; +byte cli_status = LOW; + long t0; int num_iter; float aux_debug; @@ -661,4 +664,4 @@ void defaultUserConfig() { -// end of file +// end of file diff --git a/ArducopterNG/ArducopterNG.pde b/ArducopterNG/ArducopterNG.pde index 2e54fcb36d..0c2f25cd12 100644 --- a/ArducopterNG/ArducopterNG.pde +++ b/ArducopterNG/ArducopterNG.pde @@ -119,18 +119,19 @@ // Magneto orientation and corrections. // If you don't have magneto activated, 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_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 +#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 ArduCopter. Don't kick like Jani always does :) -#define MAGOFFSET 0,0,0 +//#define MAGOFFSET 0,0,0 +#define MAGOFFSET 19.00,-72.00,-79.50 // 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 +#define DECLINATION 0.61 // 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 @@ -221,6 +222,19 @@ void setup() { GEOG_CORRECTION_FACTOR = 0; // Geographic correction factor will be automatically calculated Read_adc_raw(); // Initialize ADC readings... + +#ifdef SerXbee + Serial.begin(SerBau); + Serial.print("ArduCopter v"); + Serial.println(VER); + Serial.println("Serial data on Telemetry port"); + Serial.println("No commands or output on this serial, check your Arducopter.pde if needed to change."); + Serial.println(); + Serial.println("General info:"); + if(!SW_DIP1) Serial.println("Flight mode: + "); + if(SW_DIP1) Serial.println("Flight mode: x "); +#endif + delay(10); digitalWrite(LED_Green,HIGH); // Ready to go... @@ -461,4 +475,4 @@ void loop() } - + diff --git a/ArducopterNG/CLI.pde b/ArducopterNG/CLI.pde index a89a341941..4a3158696d 100644 --- a/ArducopterNG/CLI.pde +++ b/ArducopterNG/CLI.pde @@ -39,10 +39,10 @@ boolean ShowMainMenu; // This can be moved later to CLI.pde void RunCLI () { -// APM_RC.Init(); // APM Radio initialization - + // 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); @@ -69,6 +69,22 @@ void RunCLI () { break; } } + + // Changing LED statuses to inform that we are in CLI mode + // Blinking Red, Yellow, Green when in CLI mode + if(millis() - cli_timer > 1000) { + cli_timer = millis(); + if(cli_status == HIGH) { + LEDAllOFF(); + cli_status = LOW; + } + else { + LEDAllON(); + cli_status = HIGH; + } + } + + } // Mainloop ends } @@ -78,7 +94,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(" i - Initialize and calibrate Accel offsets (under work)"); SerPrln(" "); } @@ -176,7 +192,7 @@ void CALIB_AccOffset() { uint16_t xx = 0, xy = 0, xz = 0; adc.Init(); // APM ADC library initialization -// delay(250); // Giving small moment before starting + // delay(250); // Giving small moment before starting calibrateSensors(); // Calibrate neutral values of gyros (in Sensors.pde) @@ -189,12 +205,12 @@ void CALIB_AccOffset() { SerPri(loopy); SerPri(":"); tab(); -/* SerPri(xx += read_adc(4)); - tab(); - SerPri(xy += -read_adc(3)); - tab(); - SerPrln(xz += read_adc(5)); -*/ + /* SerPri(xx += read_adc(4)); + tab(); + SerPri(xy += -read_adc(3)); + tab(); + SerPrln(xz += read_adc(5)); + */ SerPri(xx += adc.Ch(4)); tab(); SerPri(xy += adc.Ch(5)); @@ -204,11 +220,11 @@ void CALIB_AccOffset() { } - + xx = xx / (loopy - 1); xy = xy / (loopy - 1); xz = xz / (loopy - 1) ; - + SerPriln("Averages as follows"); SerPri(" "); tab(); @@ -224,3 +240,5 @@ void CALIB_AccOffset() { } + + diff --git a/ArducopterNG/Functions.pde b/ArducopterNG/Functions.pde index 0b58ef6c36..421d97a055 100644 --- a/ArducopterNG/Functions.pde +++ b/ArducopterNG/Functions.pde @@ -139,6 +139,25 @@ int limitRange(int data, int minLimit, int maxLimit) { } + +void CLILeds (byte ledstep) { + + +} + +void LEDAllON() { + digitalWrite(LED_Green, HIGH); + digitalWrite(LED_Red, HIGH); + digitalWrite(LED_Yellow, HIGH); +} + +void LEDAllOFF() { + digitalWrite(LED_Green, LOW); + digitalWrite(LED_Red, LOW); + digitalWrite(LED_Yellow, LOW); +} + + // // Camera functions moved to event due it's and event 31-10-10, jp diff --git a/ArducopterNG/GCS.pde b/ArducopterNG/GCS.pde index efd34b96dc..197d1e69d8 100644 --- a/ArducopterNG/GCS.pde +++ b/ArducopterNG/GCS.pde @@ -364,7 +364,8 @@ void sendSerialTelemetry() { SerPri(read_adc(3)); comma(); SerPri(read_adc(5)); - /* comma(); + + comma(); SerPri(APM_Compass.Heading, 4); comma(); SerPri(APM_Compass.Heading_X, 4); @@ -377,7 +378,7 @@ void sendSerialTelemetry() { comma(); SerPri(APM_Compass.Mag_Z); comma(); - */ + SerPriln(); break; case 'T': // Spare @@ -458,6 +459,30 @@ void sendSerialTelemetry() { tab(); SerPriln(); break; +#ifdef IsGPS + case '4': // Jani's debugs +// Log_Write_GPS(GPS.Time, GPS.Lattitude, GPS.Longitude, GPS.Altitude, GPS.Altitude, GPS.Ground_Speed, GPS.Ground_Course, GPS.Fix, GPS.NumSats); + + SerPri(GPS.Time); + tab(); + SerPri(GPS.Lattitude); + tab(); + SerPri(GPS.Longitude); + tab(); + SerPri(GPS.Altitude); + tab(); + SerPri(GPS.Ground_Speed); + tab(); + SerPri(GPS.Ground_Course); + tab(); + SerPri(GPS.Fix); + tab(); + SerPri(GPS.NumSats); + + tab(); + SerPriln(); + break; +#endif case '.': // Modify GPS settings, print directly to GPS Port Serial1.print("$PGCMD,16,0,0,0,0,0*6A\r\n"); break; @@ -493,4 +518,4 @@ float readFloatSerial() { return atof(data); } - + diff --git a/ArducopterNG/Radio.pde b/ArducopterNG/Radio.pde index ef37a30f00..86e8211a3d 100644 --- a/ArducopterNG/Radio.pde +++ b/ArducopterNG/Radio.pde @@ -76,7 +76,7 @@ void read_radio() ch_throttle = channel_filter(tempThrottle, ch_throttle); // Flight mode - if(ch_aux2 > 1200) + if(ch_aux2 > 1300) flightMode = ACRO_MODE; // Force to Acro mode from radio else flightMode = STABLE_MODE; // Stable mode (default) @@ -84,7 +84,7 @@ void read_radio() // Autopilot mode (only works on Stable mode) if (flightMode == STABLE_MODE) { - if(ch_aux < 1200) + if(ch_aux < 1300) AP_mode = AP_AUTOMATIC_MODE; // Automatic mode : GPS position hold mode + altitude hold else AP_mode = AP_NORMAL_MODE; // Normal mode @@ -153,4 +153,4 @@ void read_radio() } } - +