diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index a1e3857942..9f447f9170 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduCopter V2.0.48 Beta" +#define THISFIRMWARE "ArduCopter V2.0.49 Beta" /* ArduCopter Version 2.0 Beta Authors: Jason Short @@ -425,13 +425,13 @@ static int event_repeat; // how many times to fire : 0 = forever, 1 = do static int event_value; // per command value, such as PWM for servos static int event_undo_value; // the value used to undo commands //static byte repeat_forever; -static byte undo_event; // counter for timing the undo +//static byte undo_event; // counter for timing the undo // delay command // -------------- static long condition_value; // used in condition commands (eg delay, change alt, etc.) static long condition_start; -static int condition_rate; +//static int condition_rate; // land command // ------------ @@ -445,12 +445,10 @@ static struct Location prev_WP; // last waypoint static struct Location current_loc; // current location static struct Location next_WP; // next waypoint static struct Location target_WP; // where do we want to you towards? -static struct Location simple_WP; // static struct Location next_command; // command preloaded static struct Location guided_WP; // guided mode waypoint static long target_altitude; // used for static boolean home_is_set; // Flag for if we have g_gps lock and have set the home location -static boolean new_location; // flag to tell us if location has been updated // IMU variables // ------------- @@ -1047,9 +1045,14 @@ static void update_navigation() case GUIDED: wp_control = WP_MODE; // check if we are close to point > loiter - verify_nav_wp(); + wp_verify_byte = 0; + verify_nav_wp(); - update_auto_yaw(); + if (wp_control == WP_MODE) { + update_auto_yaw(); + } else { + set_mode(LOITER); + } update_nav_wp(); break; diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index ba03570201..d7d11ffa54 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -42,7 +42,9 @@ static void reset_control_switch() read_control_switch(); } +#if CH7_OPTION == CH7_SET_HOVER static boolean trim_flag; +#endif // read at 10 hz // set this to your trainer switch diff --git a/ArduCopter/heli.pde b/ArduCopter/heli.pde index fea259b164..873212e119 100644 --- a/ArduCopter/heli.pde +++ b/ArduCopter/heli.pde @@ -20,30 +20,30 @@ static void heli_init_swash() int i; int tilt_max[CH_3+1]; int total_tilt_max = 0; - + // swash servo initialisation g.heli_servo_1.set_range(0,1000); g.heli_servo_2.set_range(0,1000); g.heli_servo_3.set_range(0,1000); g.heli_servo_4.set_angle(4500); - + // pitch factors heli_pitchFactor[CH_1] = cos(radians(g.heli_servo1_pos)); heli_pitchFactor[CH_2] = cos(radians(g.heli_servo2_pos)); heli_pitchFactor[CH_3] = cos(radians(g.heli_servo3_pos)); - + // roll factors heli_rollFactor[CH_1] = cos(radians(g.heli_servo1_pos + 90)); heli_rollFactor[CH_2] = cos(radians(g.heli_servo2_pos + 90)); heli_rollFactor[CH_3] = cos(radians(g.heli_servo3_pos + 90)); - + // collective min / max total_tilt_max = 0; for( i=CH_1; i<=CH_3; i++ ) { tilt_max[i] = max(abs(heli_rollFactor[i]*g.heli_roll_max), abs(heli_pitchFactor[i]*g.heli_pitch_max))/100; total_tilt_max = max(total_tilt_max,tilt_max[i]); } - + // servo min/max values - or should I use set_range? g.heli_servo_1.radio_min = g.heli_coll_min - tilt_max[CH_1]; g.heli_servo_1.radio_max = g.heli_coll_max + tilt_max[CH_1]; @@ -51,11 +51,11 @@ static void heli_init_swash() g.heli_servo_2.radio_max = g.heli_coll_max + tilt_max[CH_2]; g.heli_servo_3.radio_min = g.heli_coll_min - tilt_max[CH_3]; g.heli_servo_3.radio_max = g.heli_coll_max + tilt_max[CH_3]; - + // reset the servo averaging for( i=0; i<=3; i++ ) heli_servo_out[i] = 0; - + // double check heli_servo_averaging is reasonable if( g.heli_servo_averaging < 0 || g.heli_servo_averaging < 0 > 5 ) { g.heli_servo_averaging = 0; @@ -87,36 +87,36 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o g.heli_servo_1.servo_out = (heli_rollFactor[CH_1] * roll_out + heli_pitchFactor[CH_1] * pitch_out)/10 + coll_out + (g.heli_servo_1.radio_trim-1500); if( g.heli_servo_1.get_reverse() ) g.heli_servo_1.servo_out = 3000 - g.heli_servo_1.servo_out; - + g.heli_servo_2.servo_out = (heli_rollFactor[CH_2] * roll_out + heli_pitchFactor[CH_2] * pitch_out)/10 + coll_out + (g.heli_servo_2.radio_trim-1500); if( g.heli_servo_2.get_reverse() ) g.heli_servo_2.servo_out = 3000 - g.heli_servo_2.servo_out; - + g.heli_servo_3.servo_out = (heli_rollFactor[CH_3] * roll_out + heli_pitchFactor[CH_3] * pitch_out)/10 + coll_out + (g.heli_servo_3.radio_trim-1500); if( g.heli_servo_3.get_reverse() ) g.heli_servo_3.servo_out = 3000 - g.heli_servo_3.servo_out; - + if( g.heli_servo_4.get_reverse() ) g.heli_servo_4.servo_out = -yaw_out; // should probably just use rc_4 directly like we do for a tricopter else g.heli_servo_4.servo_out = yaw_out; - + // use servo_out to calculate pwm_out and radio_out g.heli_servo_1.calc_pwm(); g.heli_servo_2.calc_pwm(); g.heli_servo_3.calc_pwm(); - g.heli_servo_4.calc_pwm(); - + g.heli_servo_4.calc_pwm(); + // add the servo values to the averaging heli_servo_out[0] += g.heli_servo_1.servo_out; heli_servo_out[1] += g.heli_servo_2.servo_out; heli_servo_out[2] += g.heli_servo_3.servo_out; heli_servo_out[3] += g.heli_servo_4.radio_out; heli_servo_out_count++; - + // is it time to move the servos? if( heli_servo_out_count >= g.heli_servo_averaging ) { - + // average the values if necessary if( g.heli_servo_averaging >= 2 ) { heli_servo_out[0] /= g.heli_servo_averaging; @@ -124,22 +124,24 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o heli_servo_out[2] /= g.heli_servo_averaging; heli_servo_out[3] /= g.heli_servo_averaging; } - + // actually move the servos APM_RC.OutputCh(CH_1, heli_servo_out[0]); APM_RC.OutputCh(CH_2, heli_servo_out[1]); APM_RC.OutputCh(CH_3, heli_servo_out[2]); APM_RC.OutputCh(CH_4, heli_servo_out[3]); - + // output gyro value if( g.heli_ext_gyro_enabled ) { APM_RC.OutputCh(CH_7, g.heli_ext_gyro_gain); } - // InstantPWM - force message to the servos + #if INSTANT_PWM == 1 + // InstantPWM APM_RC.Force_Out0_Out1(); APM_RC.Force_Out2_Out3(); - + #endif + // reset the averaging heli_servo_out_count = 0; heli_servo_out[0] = 0; @@ -149,6 +151,15 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o } } +static void init_motors_out() +{ + #if INSTANT_PWM == 0 + ICR5 = 5000; // 400 hz output CH 1, 2, 9 + ICR1 = 5000; // 400 hz output CH 3, 4, 10 + ICR3 = 40000; // 50 hz output CH 7, 8, 11 + #endif +} + // these are not really motors, they're servos but we don't rename the function because it fits with the rest of the code better static void output_motors_armed() { diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 144b498243..b38be7d787 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -95,9 +95,16 @@ static void calc_loiter(int x_error, int y_error) // nav_roll, nav_pitch static void calc_loiter_pitch_roll() { + + float temp = radians((float)(9000 - (dcm.yaw_sensor))/100.0); + float _cos_yaw_x = cos(temp); + float _sin_yaw_y = sin(temp); + + Serial.printf("ys %ld, cyx %1.4f, _cyx %1.4f\n", dcm.yaw_sensor, cos_yaw_x, _cos_yaw_x); + // rotate the vector - nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x; - nav_pitch = (float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y; + nav_roll = (float)nav_lon * _sin_yaw_y - (float)nav_lat * _cos_yaw_x; + nav_pitch = (float)nav_lon * _cos_yaw_x + (float)nav_lat * _sin_yaw_y; // flip pitch because forward is negative nav_pitch = -nav_pitch; @@ -232,7 +239,7 @@ static void reset_crosstrack() crosstrack_bearing = get_bearing(¤t_loc, &next_WP); // Used for track following } */ -static long get_altitude_above_home(void) +/*static long get_altitude_above_home(void) { // This is the altitude above the home location // The GPS gives us altitude at Sea Level @@ -240,7 +247,7 @@ static long get_altitude_above_home(void) // ------------------------------------------------------------- return current_loc.alt - home.alt; } - +*/ // distance is returned in meters static long get_distance(struct Location *loc1, struct Location *loc2) { @@ -252,12 +259,12 @@ static long get_distance(struct Location *loc1, struct Location *loc2) float dlong = ((float)(loc2->lng - loc1->lng)) * scaleLongDown; return sqrt(sq(dlat) + sq(dlong)) * .01113195; } - +/* static long get_alt_distance(struct Location *loc1, struct Location *loc2) { return abs(loc1->alt - loc2->alt); } - +*/ static long get_bearing(struct Location *loc1, struct Location *loc2) { long off_x = loc2->lng - loc1->lng; diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index 34f5f7ddc5..faca5889c5 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -184,11 +184,3 @@ static void trim_radio() g.rc_4.save_eeprom(); } -static void trim_yaw() -{ - for (byte i = 0; i < 30; i++){ - read_radio(); - } - g.rc_4.trim(); // yaw -} - diff --git a/ArduCopter/read_commands.pde b/ArduCopter/read_commands.pde deleted file mode 100644 index baad2778a2..0000000000 --- a/ArduCopter/read_commands.pde +++ /dev/null @@ -1,112 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#if 0 - -#define INPUT_BUF_LEN 40 -char input_buffer[INPUT_BUF_LEN]; - -static void readCommands(void) -{ - static byte header[2]; - const byte read_GS_header[] = {0x21, 0x21}; //!! Used to verify the payload msg header - - if(Serial.available()){ - //Serial.println("Serial.available"); - byte bufferPointer; - - header[0] = Serial.read(); - header[1] = Serial.read(); - - if((header[0] == read_GS_header[0]) && (header[1] == read_GS_header[1])){ - - // Block until we read full command - // -------------------------------- - delay(20); - byte incoming_val = 0; - - // Ground Station communication - // ---------------------------- - while(Serial.available() > 0) - { - incoming_val = Serial.read(); - - if (incoming_val != 13 && incoming_val != 10 ) { - input_buffer[bufferPointer++] = incoming_val; - } - - if(bufferPointer >= INPUT_BUF_LEN){ - Serial.println("Big buffer overrun"); - bufferPointer = 0; - input_buffer[0] = 1; - Serial.flush(); - memset(input_buffer,0,sizeof(input_buffer)); - return; - } - } - parseCommand(input_buffer); - - // clear buffer of old data - // ------------------------ - memset(input_buffer,0,sizeof(input_buffer)); - - }else{ - Serial.flush(); - } - } -} - -// Commands can be sent as !!a:100|b:200|c:1 -// ----------------------------------------- -static void parseCommand(char *buffer) -{ - Serial.println("got cmd "); - Serial.println(buffer); - char *token, *saveptr1, *saveptr2; - - for (int j = 1;; j++, buffer = NULL) { - token = strtok_r(buffer, "|", &saveptr1); - if (token == NULL) break; - - char * cmd = strtok_r(token, ":", &saveptr2); - long value = strtol(strtok_r (NULL,":", &saveptr2), NULL,0); - - ///* - Serial.print("cmd "); - Serial.print(cmd[0]); - Serial.print("\tval "); - Serial.println(value); - Serial.println(""); - //*/ - ///* - switch(cmd[0]){ - case 'P': - g.pi_stabilize_roll.kP((float)value / 1000); - g.pi_stabilize_pitch.kP((float)value / 1000); - g.pi_stabilize_pitch.save_gains(); - break; - - case 'I': - g.pi_stabilize_roll.kI((float)value / 1000); - g.pi_stabilize_pitch.kI((float)value / 1000); - g.pi_stabilize_pitch.save_gains(); - break; - - case 'D': - //g.pi_stabilize_roll.kD((float)value / 1000); - //g.pi_stabilize_pitch.kD((float)value / 1000); - break; - - case 'X': - g.pi_stabilize_roll.imax(value * 100); - g.pi_stabilize_pitch.imax(value * 100); - g.pi_stabilize_pitch.save_gains(); - break; - - case 'R': - //g.stabilize_dampener.set_and_save((float)value / 1000); - break; - } - //*/ - } -} - -#endif diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 460b3302b2..c6f7e397d5 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -83,7 +83,6 @@ static void init_ardupilot() // report_version(); - // setup IO pins pinMode(C_LED_PIN, OUTPUT); // GPS status LED pinMode(A_LED_PIN, OUTPUT); // GPS status LED @@ -143,6 +142,7 @@ static void init_ardupilot() } }else{ + // Load all auto-loaded EEPROM variables AP_Var::load_all(); } @@ -157,7 +157,6 @@ static void init_ardupilot() // Serial3.begin(map_baudrate(g.serial3_baud,SERIAL3_BAUD), 128, 128); - #ifdef RADIO_OVERRIDE_DEFAULTS { int16_t rc_override[8] = RADIO_OVERRIDE_DEFAULTS; @@ -216,11 +215,7 @@ static void init_ardupilot() // if (check_startup_for_CLI()) { digitalWrite(A_LED_PIN,HIGH); // turn on setup-mode LED - Serial.printf_P(PSTR("\n" - "Entering interactive setup mode...\n" - "\n" - "Type 'help' to list commands, 'exit' to leave a submenu.\n" - "Visit the 'setup' menu for first-time configuration.\n\n")); + Serial.printf_P(PSTR("\nCLI:\n\n")); for (;;) { //Serial.println_P(PSTR("\nMove the slide switch and reset to FLY.\n")); main_menu.run(); @@ -544,6 +539,6 @@ static uint32_t map_baudrate(int8_t rate, uint32_t default_baud) case 111: return 111100; case 115: return 115200; } - Serial.println_P(PSTR("Invalid SERIAL3_BAUD")); + //Serial.println_P(PSTR("Invalid SERIAL3_BAUD")); return default_baud; } diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index 7b2884a9f7..7e8fed1d60 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -29,7 +29,7 @@ static int8_t test_sonar(uint8_t argc, const Menu::arg *argv); #ifdef OPTFLOW_ENABLED static int8_t test_optflow(uint8_t argc, const Menu::arg *argv); #endif -static int8_t test_xbee(uint8_t argc, const Menu::arg *argv); +//static int8_t test_xbee(uint8_t argc, const Menu::arg *argv); static int8_t test_eedump(uint8_t argc, const Menu::arg *argv); static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv); //static int8_t test_mission(uint8_t argc, const Menu::arg *argv); diff --git a/libraries/Desktop/Desktop.mk b/libraries/Desktop/Desktop.mk index e04a8e86f4..63ac2a7284 100644 --- a/libraries/Desktop/Desktop.mk +++ b/libraries/Desktop/Desktop.mk @@ -1,7 +1,7 @@ # # Copyright (c) 2010 Andrew Tridgell. All rights reserved. # based on Arduino.mk, Copyright (c) 2010 Michael Smith -# +# # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions # are met: @@ -10,7 +10,7 @@ # 2. Redistributions in binary form must reproduce the above copyright # notice, this list of conditions and the following disclaimer in the # documentation and/or other materials provided with the distribution. -# +# # THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE @@ -109,7 +109,7 @@ DEPFLAGS = -MD -MT $@ # XXX warning options TBD CXXOPTS = -fno-exceptions -D__AVR_ATmega2560__ -I$(SKETCHBOOK)/libraries/Desktop/include -DDESKTOP_BUILD=1 COPTS = -I$(SKETCHBOOK)/libraries/Desktop/include -DDESKTOP_BUILD=1 -ASOPTS = -assembler-with-cpp +ASOPTS = -assembler-with-cpp CXXFLAGS = -g $(DEFINES) $(OPTFLAGS) $(DEPFLAGS) $(CXXOPTS) CFLAGS = -g $(DEFINES) $(OPTFLAGS) $(DEPFLAGS) $(COPTS) @@ -171,12 +171,12 @@ else endif # these are library objects we don't want in the desktop build (maybe we'll add them later) -NODESKTOP := FastSerial/FastSerial.cpp AP_Compass/AP_Compass_HMC5843.cpp APM_BMP085/APM_BMP085.cpp AP_IMU/AP_IMU_Oilpan.cpp AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp +NODESKTOP := FastSerial/FastSerial.cpp AP_Compass/AP_Compass_HMC5843.cpp APM_BMP085/APM_BMP085.cpp AP_IMU/AP_IMU_Oilpan.cpp AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp RC_Channel/RC_Channel_aux.cpp # # Find sketchbook libraries referenced by the sketch. # -# Include paths for sketch libraries +# Include paths for sketch libraries # SKETCHLIBS := $(wildcard $(addprefix $(SKETCHBOOK)/libraries/,$(LIBTOKENS))) SKETCHLIBNAMES := $(notdir $(SKETCHLIBS)) @@ -196,10 +196,10 @@ ARDUINOLIBSRCDIRS := $(ARDUINOLIBS) $(addsuffix /utility,$(ARDUINOLIBS)) ARDUINOLIBSRCS := $(wildcard $(foreach suffix,$(SRCSUFFIXES),$(addsuffix /$(suffix),$(ARDUINOLIBSRCDIRS)))) ARDUINOLIBOBJS := $(addsuffix .o,$(basename $(subst $(ARDUINO),$(BUILDROOT),$(ARDUINOLIBSRCS)))) ARDUINOLIBINCLUDES := $(addprefix -I,$(ARDUINOLIBS)) -ARDUINOLIBINCLUDES := $(ARDUINOLIBINCLUDES) +ARDUINOLIBINCLUDES := $(ARDUINOLIBINCLUDES) # Library object files -LIBOBJS := $(SKETCHLIBOBJS) +LIBOBJS := $(SKETCHLIBOBJS) ################################################################################ # Built products @@ -212,7 +212,7 @@ SKETCHELF = $(BUILDROOT)/$(SKETCH).elf SKETCHMAP = $(BUILDROOT)/$(SKETCH).map # The core library -CORELIB = +CORELIB = # All of the objects that may be built ALLOBJS = $(SKETCHOBJS) $(LIBOBJS) $(CORELIBOBJS) @@ -338,7 +338,7 @@ $(CORELIB): $(CORELIBOBJS) # This process strives to be as faithful to the Arduino implementation as # possible. Conceptually, the process is as follows: # -# * All of the .pde files are concatenated, starting with the file named +# * All of the .pde files are concatenated, starting with the file named # for the sketch and followed by the others in alphabetical order. # * An insertion point is created in the concatenated file at # the first statement that isn't a preprocessor directive or comment. diff --git a/libraries/Desktop/Makefile.desktop b/libraries/Desktop/Makefile.desktop index e8b390726b..8c82eb3fb9 100644 --- a/libraries/Desktop/Makefile.desktop +++ b/libraries/Desktop/Makefile.desktop @@ -3,7 +3,7 @@ DESKTOP=$(PWD)/../libraries/Desktop include ../libraries/Desktop/Desktop.mk default: - make -f $(DESKTOP)/Makefile.desktop + make -f $(DESKTOP)/Makefile.desktop nologging: make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DLOGGING_ENABLED=DISABLED" @@ -19,3 +19,6 @@ hilnocli: heli: make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DFRAME_CONFIG=HELI_FRAME" + +helihil: + make -f $(DESKTOP)/Makefile.desktop EXTRAFLAGS="-DFRAME_CONFIG=HELI_FRAME -DHIL_MODE=HIL_MODE_ATTITUDE -DCLI_ENABLED=DISABLED -DLOGGING_ENABLED=DISABLED" \ No newline at end of file