From 323f2bfa5999a4b20bf423288e23a01b3bab77bc Mon Sep 17 00:00:00 2001 From: jasonshort Date: Tue, 15 Mar 2011 05:54:48 +0000 Subject: [PATCH] Update to get SIMPLE mode running. Seems to work! git-svn-id: https://arducopter.googlecode.com/svn/trunk@1774 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/ArduCopterMega.pde | 137 ++++++++++++++++++------------ ArduCopterMega/Attitude.pde | 4 +- ArduCopterMega/config.h | 4 +- ArduCopterMega/motors.pde | 32 +++---- ArduCopterMega/navigation.pde | 33 ++++--- ArduCopterMega/system.pde | 23 +++-- 6 files changed, 138 insertions(+), 95 deletions(-) diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 5a827ed45f..76b2549929 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -407,17 +407,18 @@ byte slow_loopCounter; byte superslow_loopCounter; byte fbw_timer; // for limiting the execution of FBW input -unsigned long nav_loopTimer; // used to track the elapsed ime for GPS nav +//unsigned long nav_loopTimer; // used to track the elapsed ime for GPS nav unsigned long nav2_loopTimer; // used to track the elapsed ime for GPS nav -unsigned long dTnav; // Delta Time in milliseconds for navigation computations +//unsigned long dTnav; // Delta Time in milliseconds for navigation computations unsigned long dTnav2; // Delta Time in milliseconds for navigation computations unsigned long elapsedTime; // for doing custom events float load; // % MCU cycles used byte counter_one_herz; -byte GPS_failure_counter = 255; +byte GPS_failure_counter = 3; +bool GPS_disabled = false; //////////////////////////////////////////////////////////////////////////////// // Top-level logic @@ -498,7 +499,6 @@ void medium_loop() // ---------- read_radio(); // read the radio first - // reads all of the necessary trig functions for cameras, throttle, etc. update_trig(); @@ -506,13 +506,16 @@ void medium_loop() // ----------------------------------------- switch(medium_loopCounter) { - // This case deals with the GPS - //------------------------------- + // This case deals with the GPS and Compass + //----------------------------------------- case 0: medium_loopCounter++; if(GPS_failure_counter > 0){ update_GPS(); + + }else if(GPS_failure_counter == 0){ + GPS_disabled = true; } //readCommands(); @@ -529,32 +532,39 @@ void medium_loop() case 1: medium_loopCounter++; - // hack to stop navigation in Simple mode - if (control_mode == SIMPLE) - break; - - if(g_gps->new_data){ - g_gps->new_data = false; - dTnav = millis() - nav_loopTimer; - nav_loopTimer = millis(); - - // calculate the plane's desired bearing - // ------------------------------------- - navigate(); - } - // calc pitch and roll to target // ----------------------------- dTnav2 = millis() - nav2_loopTimer; nav2_loopTimer = millis(); + // hack to stop navigation in Simple mode + if (control_mode == SIMPLE) + break; + + if (control_mode == FBW) + break; + + // Auto control modes: + if(g_gps->new_data){ + g_gps->new_data = false; + + // we are not tracking I term on navigation, so this isn't needed + //dTnav = millis() - nav_loopTimer; + //nav_loopTimer = millis(); + + // calculate the copter's desired bearing and WP distance + // ------------------------------------------------------ + navigate(); + } + + // we call these regardless of GPS because of the rapid nature of the yaw sensor + // ----------------------------------------------------------------------------- if(wp_distance < 800){ // 8 meters calc_loiter_nav(); }else{ calc_waypoint_nav(); } - break; // command processing @@ -563,7 +573,7 @@ void medium_loop() medium_loopCounter++; // Read altitude from sensors - // ------------------ + // -------------------------- update_alt(); // perform next command @@ -581,41 +591,55 @@ void medium_loop() if (g.log_bitmask & MASK_LOG_ATTITUDE_MED && (g.log_bitmask & MASK_LOG_ATTITUDE_FAST == 0)) Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (int)dcm.yaw_sensor); -#if HIL_MODE != HIL_MODE_ATTITUDE + #if HIL_MODE != HIL_MODE_ATTITUDE if (g.log_bitmask & MASK_LOG_CTUN) Log_Write_Control_Tuning(); -#endif + #endif if (g.log_bitmask & MASK_LOG_NTUN) Log_Write_Nav_Tuning(); if (g.log_bitmask & MASK_LOG_GPS){ - if(home_is_set) - Log_Write_GPS(g_gps->time, current_loc.lat, current_loc.lng, g_gps->altitude, current_loc.alt, (long) g_gps->ground_speed, g_gps->ground_course, g_gps->fix, g_gps->num_sats); + if(home_is_set){ + Log_Write_GPS(g_gps->time, + current_loc.lat, + current_loc.lng, + g_gps->altitude, + current_loc.alt, + (long)g_gps->ground_speed, + g_gps->ground_course, + g_gps->fix, + g_gps->num_sats); + } } + gcs.send_message(MSG_ATTITUDE); // Sends attitude data break; // This case controls the slow loop //--------------------------------- case 4: + medium_loopCounter = 0; + if (g.current_enabled){ read_current(); } - // shall we trim the copter? - // ------------------------ + // Accel trims = hold > 2 seconds + // Throttle cruise = switch less than 1 second + // -------------------------------------------- read_trim_switch(); - // shall we check for engine start? - // -------------------------------- + // Check for engine arming + // ----------------------- arm_motors(); - medium_loopCounter = 0; slow_loop(); break; default: + // this is just a catch all + // ------------------------ medium_loopCounter = 0; break; } @@ -632,10 +656,10 @@ void medium_loop() if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (int)dcm.yaw_sensor); -#if HIL_MODE != HIL_MODE_ATTITUDE - if (g.log_bitmask & MASK_LOG_RAW) - Log_Write_Raw(); -#endif + #if HIL_MODE != HIL_MODE_ATTITUDE + if (g.log_bitmask & MASK_LOG_RAW) + Log_Write_Raw(); + #endif #if GCS_PROTOCOL == 6 // This is here for Benjamin Pelletier. Please do not remove without checking with me. Doug W readgcsinput(); @@ -688,6 +712,9 @@ void slow_loop() slow_loopCounter = 0; update_events(); + // blink if we are armed + update_motor_light(); + // XXX this should be a "GCS slow loop" interface #if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK gcs.data_stream_send(1,5); @@ -724,7 +751,7 @@ void update_GPS(void) update_GPS_light(); if (g_gps->new_data && g_gps->fix) { - GPS_failure_counter = 255; + GPS_failure_counter = 3; // XXX We should be sending GPS data off one of the regular loops so that we send // no-GPS-fix data too @@ -755,7 +782,7 @@ void update_GPS(void) Log_Write_Startup(TYPE_GROUNDSTART_MSG); // reset our nav loop timer - nav_loopTimer = millis(); + //nav_loopTimer = millis(); init_home(); // init altitude @@ -866,17 +893,28 @@ void update_current_flight_mode(void) fbw_timer++; // 25 hz if(fbw_timer > 4){ - Location temp = current_loc; - temp.lng += g.rc_1.control_in / 2; - temp.lat -= g.rc_2.control_in / 2; + fbw_timer = 0; + + current_loc.lat = 0; + current_loc.lng = 0; + + next_WP.lng = (float)g.rc_1.control_in *.4; // X: 4500 / 2 = 2250 = 25 meteres + next_WP.lat = -((float)g.rc_2.control_in *.4); // Y: 4500 / 2 = 2250 = 25 meteres // calc a new bearing - nav_bearing = get_bearing(¤t_loc, &temp) + initial_simple_bearing; + nav_bearing = get_bearing(¤t_loc, &next_WP) + initial_simple_bearing; nav_bearing = wrap_360(nav_bearing); - wp_distance = get_distance(¤t_loc, &temp); - + wp_distance = get_distance(¤t_loc, &next_WP); calc_bearing_error(); - + /* + Serial.printf("lat: %ld lon:%ld, bear:%ld, dist:%ld, init:%ld, err:%ld ", + next_WP.lat, + next_WP.lng, + nav_bearing, + wp_distance, + initial_simple_bearing, + bearing_error); + */ // get nav_pitch and nav_roll calc_waypoint_nav(); } @@ -905,20 +943,15 @@ void update_current_flight_mode(void) if(fbw_timer > 10){ fbw_timer = 0; - if(home_is_set == false){ - scaleLongDown = 1; - // we are not using GPS - // reset the location - // RTL won't function + if(GPS_disabled){ current_loc.lat = home.lat = 0; current_loc.lng = home.lng = 0; - // set dTnav manually - dTnav = 200; } next_WP.lng = home.lng + g.rc_1.control_in / 2; // X: 4500 / 2 = 2250 = 25 meteres - // forward is negative so we reverse it to get a positive North translation next_WP.lat = home.lat - g.rc_2.control_in / 2; // Y: 4500 / 2 = 2250 = 25 meteres + + calc_loiter_nav(); } // Output Pitch, Roll, Yaw and Throttle diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index 33bae47e78..400b2e3882 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -144,7 +144,7 @@ output_yaw_with_hold(boolean hold) long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377 rate = constrain(rate, -36000, 36000); // limit to something fun! - long error = ((long)g.rc_4.control_in * 6) - rate; // control is += 6000 * 6 = 36000 + long error = ((long)g.rc_4.control_in * 6) - rate; // control is += 6000 * 6 = 36000 // -error = CCW, +error = CW if(g.rc_4.control_in == 0){ @@ -231,7 +231,7 @@ void calc_nav_throttle() float scaler = 1.0; if(error < 0){ - scaler = (altitude_sensor == BARO) ? .5 : .8; + scaler = (altitude_sensor == BARO) ? .5 : .5; } if(altitude_sensor == BARO){ diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index bcda642387..0342f8052e 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -287,7 +287,7 @@ #define ACRO_RATE_PITCH_IMAX_CENTIDEGREE ACRO_RATE_PITCH_IMAX * 100 #ifndef ACRO_RATE_YAW_P -# define ACRO_RATE_YAW_P .5 +# define ACRO_RATE_YAW_P .1 #endif #ifndef ACRO_RATE_YAW_I # define ACRO_RATE_YAW_I 0.0 @@ -394,7 +394,7 @@ # define NAV_P 2.0 #endif #ifndef NAV_I -# define NAV_I 0.00 +# define NAV_I 0.1 #endif #ifndef NAV_D # define NAV_D 0.00 diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde index 95191decd4..9a00d94427 100644 --- a/ArduCopterMega/motors.pde +++ b/ArduCopterMega/motors.pde @@ -13,12 +13,14 @@ void arm_motors() if (arming_counter > ARM_DELAY) { motor_armed = true; } else{ + //Serial.printf("arm %d\n", arming_counter); arming_counter++; } }else if (g.rc_4.control_in < -2700) { if (arming_counter > DISARM_DELAY){ motor_armed = false; }else{ + //Serial.printf("arm %d\n", arming_counter); arming_counter++; } }else{ @@ -67,10 +69,10 @@ set_servos_4() motor_out[CH_3] = g.rc_3.radio_out + g.rc_2.pwm_out; motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out; - motor_out[CH_1] += g.rc_4.pwm_out; // CCW - motor_out[CH_2] += g.rc_4.pwm_out; // CCW - motor_out[CH_3] -= g.rc_4.pwm_out; // CW - motor_out[CH_4] -= g.rc_4.pwm_out; // CW + motor_out[CH_1] += g.rc_4.pwm_out; // CCW + motor_out[CH_2] += g.rc_4.pwm_out; // CCW + motor_out[CH_3] -= g.rc_4.pwm_out; // CW + motor_out[CH_4] -= g.rc_4.pwm_out; // CW }else if(g.frame_type == X_FRAME){ //Serial.println("X_FRAME"); @@ -123,11 +125,11 @@ set_servos_4() //left side motor_out[CH_2] = g.rc_3.radio_out + g.rc_1.pwm_out; // CCW motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW - motor_out[CH_8] = g.rc_3.radio_out + roll_out - pitch_out; // CW + motor_out[CH_8] = g.rc_3.radio_out + roll_out - pitch_out; // CW //right side motor_out[CH_1] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW - motor_out[CH_7] = g.rc_3.radio_out - roll_out + pitch_out; // CCW + motor_out[CH_7] = g.rc_3.radio_out - roll_out + pitch_out; // CCW motor_out[CH_4] = g.rc_3.radio_out - roll_out - pitch_out; // CCW motor_out[CH_7] += g.rc_4.pwm_out; // CCW @@ -136,9 +138,9 @@ set_servos_4() motor_out[CH_3] -= g.rc_4.pwm_out; // CW motor_out[CH_1] -= g.rc_4.pwm_out; // CW - motor_out[CH_8] -= g.rc_4.pwm_out; // CW + motor_out[CH_8] -= g.rc_4.pwm_out; // CW - }else{ + }else{ Serial.print("frame error"); @@ -157,6 +159,7 @@ set_servos_4() } num++; + if (num > 50){ num = 0; /* @@ -172,8 +175,6 @@ set_servos_4() } */ - - //Serial.print("!"); //debugging with Channel 6 @@ -196,7 +197,7 @@ set_servos_4() init_pids(); //*/ - /* + ///* write_int(motor_out[CH_1]); write_int(motor_out[CH_2]); write_int(motor_out[CH_3]); @@ -214,14 +215,13 @@ set_servos_4() write_int((int)nav_roll); write_int((int)nav_pitch); - //24 - write_long(current_loc.lat); //28 - write_long(current_loc.lng); //32 - write_int((int)current_loc.alt); //34 + write_long(current_loc.lat); // 28 + write_long(current_loc.lng); // 32 + write_int((int)current_loc.alt); // 34 write_long(next_WP.lat); write_long(next_WP.lng); - write_int((int)next_WP.alt); //44 + write_int((int)next_WP.alt); // 44 flush(10); //*/ diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde index e01d80d2b8..fb121422ab 100644 --- a/ArduCopterMega/navigation.pde +++ b/ArduCopterMega/navigation.pde @@ -70,14 +70,12 @@ void calc_loiter_nav() lat_error = constrain(lat_error, -DIST_ERROR_MAX, DIST_ERROR_MAX); // +- 20m max error // Convert distance into ROLL X - nav_lon = long_error * g.pid_nav_lon.kP(); // 1800 * 2 = 3600 or 36° - //nav_lon = g.pid_nav_lon.get_pid(long_error, dTnav2, 1.0); - //nav_lon = constrain(nav_lon, -DIST_ERROR_MAX, DIST_ERROR_MAX); // Limit max command + //nav_lon = long_error * g.pid_nav_lon.kP(); // 1800 * 2 = 3600 or 36° + nav_lon = g.pid_nav_lon.get_pid(long_error, dTnav2, 1.0); // PITCH Y - //nav_lat = g.pid_nav_lat.get_pid(lat_error, dTnav2, 1.0); - nav_lat = lat_error * g.pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36° - //nav_lat = constrain(nav_lat, -DIST_ERROR_MAX, DIST_ERROR_MAX); // Limit max command + //nav_lat = lat_error * g.pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36° + nav_lat = g.pid_nav_lat.get_pid(lat_error, dTnav2, 1.0); // rotate the vector nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x; @@ -91,23 +89,30 @@ void calc_loiter_nav() void calc_waypoint_nav() { - nav_lat = constrain(wp_distance, -DIST_ERROR_MAX, DIST_ERROR_MAX); // +- 20m max error + nav_lat = constrain((wp_distance * 100), -1800, 1800); // +- 20m max error + //nav_lat = max(wp_distance, -DIST_ERROR_MAX); + //nav_lat = min(wp_distance, DIST_ERROR_MAX); + + //Serial.printf("nav_lat %ld, ", nav_lat); + // Scale response by kP + nav_lat *= g.pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36° + //Serial.printf("%ld, ",nav_lat); // get the sin and cos of the bearing error - rotated 90° sin_nav_y = sin(radians((float)(9000 - bearing_error) / 100)); cos_nav_x = cos(radians((float)(bearing_error - 9000) / 100)); + //Serial.printf("X%2.4f, Y%2.4f ", cos_nav_x, sin_nav_y); // rotate the vector nav_roll = (float)nav_lat * cos_nav_x; nav_pitch = (float)nav_lat * sin_nav_y; - // Scale response by kP - nav_roll *= g.pid_nav_lon.kP(); // 1800 * 2 = 3600 or 36° - nav_pitch *= g.pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36° + //Serial.printf("R%ld, P%ld ", nav_roll, nav_pitch); long pmax = g.pitch_max.get(); nav_roll = constrain(nav_roll, -pmax, pmax); nav_pitch = constrain(nav_pitch, -pmax, pmax); + //Serial.printf("R%ld, P%ld \n", nav_roll, nav_pitch); } void calc_bearing_error() @@ -205,10 +210,10 @@ long get_altitude_above_home(void) long get_distance(struct Location *loc1, struct Location *loc2) { - if(loc1->lat == 0 || loc1->lng == 0) - return -1; - if(loc2->lat == 0 || loc2->lng == 0) - return -1; + //if(loc1->lat == 0 || loc1->lng == 0) + // return -1; + //if(loc2->lat == 0 || loc2->lng == 0) + // return -1; float dlat = (float)(loc2->lat - loc1->lat); float dlong = ((float)(loc2->lng - loc1->lng)) * scaleLongDown; return sqrt(sq(dlat) + sq(dlong)) * .01113195; diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index 7faf1c3db5..56350e7722 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -82,7 +82,7 @@ void init_ardupilot() // Serial3.begin(SERIAL3_BAUD, 128, 128); - Serial.printf_P(PSTR("\n\nInit ArduPilotMega (unstable development version)" + Serial.printf_P(PSTR("\n\nInit ArduCopterMega" "\n\nFree RAM: %lu\n"), freeRAM()); @@ -92,9 +92,11 @@ void init_ardupilot() if (!g.format_version.load() || g.format_version != Parameters::k_format_version) { - Serial.printf_P(PSTR("\n\nEEPROM format version %d not compatible with this firmware (requires %d)" + /*Serial.printf_P(PSTR("\n\nEEPROM format version %d not compatible with this firmware (requires %d)" "\n\nForcing complete parameter reset..."), - g.format_version.get(), Parameters::k_format_version); + g.format_version.get(), + Parameters::k_format_version); + */ // erase all parameters AP_Var::erase_all(); @@ -102,14 +104,14 @@ void init_ardupilot() // save the new format version g.format_version.set_and_save(Parameters::k_format_version); - Serial.println_P(PSTR("done.")); + //Serial.println_P(PSTR("done.")); }else{ unsigned long before = micros(); // Load all auto-loaded EEPROM variables AP_Var::load_all(); - Serial.printf_P(PSTR("load_all took %luus\n"), micros() - before); - Serial.printf_P(PSTR("using %u bytes of memory\n"), AP_Var::get_memory_use()); + // Serial.printf_P(PSTR("load_all took %luus\n"), micros() - before); + // Serial.printf_P(PSTR("using %u bytes of memory\n"), AP_Var::get_memory_use()); } #ifdef RADIO_OVERRIDE_DEFAULTS @@ -171,11 +173,10 @@ void init_ardupilot() Serial.printf_P(PSTR("\n" "Entering interactive setup mode...\n" "\n" - "If using the Arduino Serial Monitor, ensure Line Ending is set to Carriage Return.\n" "Type 'help' to list commands, 'exit' to leave a submenu.\n" "Visit the 'setup' menu for first-time configuration.\n")); for (;;) { - Serial.println_P(PSTR("\nMove the slide switch and reset to FLY.\n")); + //Serial.println_P(PSTR("\nMove the slide switch and reset to FLY.\n")); main_menu.run(); } } @@ -341,7 +342,8 @@ void update_GPS_light(void) { // GPS LED on if we have a fix or Blink GPS LED if we are receiving data // --------------------------------------------------------------------- - switch (g_gps->status()) { + switch (g_gps->status()){ + case(2): digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix. break; @@ -362,7 +364,10 @@ void update_GPS_light(void) digitalWrite(C_LED_PIN, LOW); break; } +} +void update_motor_light(void) +{ if(motor_armed == true){ motor_light = !motor_light;