diff --git a/ArduCopterMega/APM_Config.h b/ArduCopterMega/APM_Config.h index 625cda8cfc..03bf89b01e 100644 --- a/ArduCopterMega/APM_Config.h +++ b/ArduCopterMega/APM_Config.h @@ -3,14 +3,12 @@ // GPS is auto-selected //#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD - -//#define HIL_MODE HIL_MODE_ATTITUDE - -//#define LOITER_TEST 1 +//#define GCS_PROTOCOL GCS_PROTOCOL_NONE +//#define HIL_MODE HIL_MODE_ATTITUDE //#define BROKEN_SLIDER 0 // 1 = yes (use Yaw to enter CLI mode) -//#define FRAME_CONFIG QUAD_FRAME +#define FRAME_CONFIG QUAD_FRAME /* options: QUAD_FRAME @@ -21,7 +19,7 @@ HELI_FRAME */ -//#define FRAME_ORIENTATION X_FRAME +#define FRAME_ORIENTATION X_FRAME /* PLUS_FRAME X_FRAME @@ -29,7 +27,7 @@ */ -//#define CHANNEL_6_TUNING CH6_NONE +#define CHANNEL_6_TUNING CH6_NONE /* CH6_NONE CH6_STABILIZE_KP @@ -45,9 +43,14 @@ CH6_TOP_BOTTOM_RATIO CH6_PMAX CH6_RELAY + CH6_TRAVERSE_SPEED */ -// experimental!! -// Yaw is controled by targeting home. you will not have Yaw override. -// flying too close to home may induce spins. -//#define SIMPLE_LOOK_AT_HOME 0 +// See the config.h and defines.h files for how to set this up! +// +// lets use SIMPLE mode for Roll and Pitch during Alt Hold +#define ALT_HOLD_RP ROLL_PITCH_SIMPLE + +// lets use Manual throttle during Loiter +//#define LOITER_THR THROTTLE_MANUAL +# define RTL_YAW YAW_HOLD \ No newline at end of file diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 8eff2f9243..1ed6fd295a 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -58,7 +58,7 @@ And much more so PLEASE PM me on DIYDRONES to add your contribution to the List #include // ArduPilot Mega Vector/Matrix math Library #include // ArduPilot Mega IMU Library #include // ArduPilot Mega DCM Library -#include // PID library +#include // PI library #include // RC Channel Library #include // Range finder library #include // Optical Flow library @@ -261,6 +261,13 @@ static const char* flight_mode_strings[] = { //Vector3f accels_rot; //float accel_gain = 20; +// temp +int y_actual_speed; +int y_rate_error; + +// calc the +int x_actual_speed; +int x_rate_error; // Radio // ----- @@ -289,8 +296,6 @@ static boolean motor_auto_armed; // if true, //int max_stabilize_dampener; // //int max_yaw_dampener; // static boolean rate_yaw_flag; // used to transition yaw control from Rate control to Yaw hold -static byte yaw_debug; -static bool did_clear_yaw_control; static Vector3f omega; // LED output @@ -314,6 +319,8 @@ static const float radius_of_earth = 6378100; // meters static const float gravity = 9.81; // meters/ sec^2 static long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate static long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target + +static bool xtrack_enabled = false; static long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target static int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate static float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1? @@ -336,6 +343,7 @@ static float sin_pitch_y, sin_yaw_y, sin_roll_y; static bool simple_bearing_is_set = false; static long initial_simple_bearing; // used for Simple mode + // Acro #if CH7_OPTION == DO_FLIP static bool do_flip = false; @@ -349,11 +357,10 @@ static int airspeed; // m/s * 100 // --------------- static long bearing_error; // deg * 100 : 0 to 36000 static long altitude_error; // meters * 100 we are off in altitude -static float crosstrack_error; // meters we are off trackline +static long old_altitude; static long distance_error; // distance to the WP static long yaw_error; // how off are we pointed static long long_error, lat_error; // temp for debugging -static int loiter_error_max; // Battery Sensors // --------------- @@ -367,16 +374,11 @@ static float current_amps; static float current_total; static bool low_batt = false; -// Airspeed Sensors -// ---------------- - // Barometer Sensor variables // -------------------------- static long abs_pressure; static long ground_pressure; static int ground_temperature; -static int32_t baro_filter[BARO_FILTER_SIZE]; -static byte baro_filter_index; // Altitude Sensor variables // ---------------------- @@ -384,18 +386,20 @@ static int sonar_alt; static int baro_alt; static int baro_alt_offset; static byte altitude_sensor = BARO; // used to know which sensor is active, BARO or SONAR - +static int altitude_rate; // flight mode specific // -------------------- +static byte yaw_mode; +static byte roll_pitch_mode; +static byte throttle_mode; + static boolean takeoff_complete; // Flag for using take-off controls static boolean land_complete; -//static int takeoff_altitude; static int landing_distance; // meters; static long old_alt; // used for managing altitude rates static int velocity_land; static byte yaw_tracking = MAV_ROI_WPNEXT; // no tracking, point at next wp, or at a target -//static int throttle_slew; // used to smooth throttle tranistions // Loiter management // ----------------- @@ -408,11 +412,12 @@ static unsigned long loiter_time_max; // millis : how long to stay in LOITER static long nav_roll; // deg * 100 : target roll angle static long nav_pitch; // deg * 100 : target pitch angle static long nav_yaw; // deg * 100 : target yaw angle +static long auto_yaw; // deg * 100 : target yaw angle static long nav_lat; // for error calcs static long nav_lon; // for error calcs static int nav_throttle; // 0-1000 for throttle control -static long throttle_integrator; // used to control when we calculate nav_throttle +static long throttle_integrator; // used to integrate throttle output to predict battery life static bool invalid_throttle; // used to control when we calculate nav_throttle static bool set_throttle_cruise_flag = false; // used to track the throttle crouse value @@ -478,24 +483,23 @@ static float G_Dt = 0.02; // Integration time for the gyros (DCM algorithm // Performance monitoring // ---------------------- -static long perf_mon_timer; -static float imu_health; // Metric based on accel gain deweighting -static int G_Dt_max; // Max main loop cycle time in milliseconds -static int gps_fix_count; -static byte gcs_messages_sent; - +static long perf_mon_timer; +static float imu_health; // Metric based on accel gain deweighting +static int G_Dt_max; // Max main loop cycle time in milliseconds +static int gps_fix_count; +static byte gcs_messages_sent; // GCS // --- -static char GCS_buffer[53]; -static char display_PID = -1; // Flag used by DebugTerminal to indicate that the next PID calculation with this index should be displayed +static char GCS_buffer[53]; +static char display_PID = -1; // Flag used by DebugTerminal to indicate that the next PID calculation with this index should be displayed // System Timers // -------------- static unsigned long fast_loopTimer; // Time in miliseconds of main control loop static unsigned long fast_loopTimeStamp; // Time Stamp when fast loop was complete -static uint8_t delta_ms_fast_loop; // Delta Time in miliseconds -static int mainLoop_count; +static uint8_t delta_ms_fast_loop; // Delta Time in miliseconds +static int mainLoop_count; static unsigned long medium_loopTimer; // Time in miliseconds of navigation control loop static byte medium_loopCounter; // Counters for branching from main control loop to slower loops @@ -505,8 +509,9 @@ static unsigned long fiftyhz_loopTimer; static uint8_t delta_ms_fiftyhz; static byte slow_loopCounter; -static int superslow_loopCounter; -static byte flight_timer; // for limiting the execution of flight mode thingys +static int superslow_loopCounter; +static byte alt_timer; // for limiting the execution of flight mode thingys +static byte simple_timer; // for limiting the execution of flight mode thingys static unsigned long dTnav; // Delta Time in milliseconds for navigation computations @@ -514,9 +519,9 @@ static unsigned long nav_loopTimer; // used to track the elapsed ime for GPS static unsigned long elapsedTime; // for doing custom events static float load; // % MCU cycles used -static byte counter_one_herz; -static bool GPS_enabled = false; -static byte loop_step; +static byte counter_one_herz; +static bool GPS_enabled = false; +static byte loop_step; //////////////////////////////////////////////////////////////////////////////// // Top-level logic @@ -610,7 +615,9 @@ static void fast_loop() // custom code/exceptions for flight modes // --------------------------------------- - update_current_flight_mode(); + update_yaw_mode(); + update_roll_pitch_mode(); + update_throttle_mode(); // write out the servo PWM values // ------------------------------ @@ -694,13 +701,13 @@ static void medium_loop() // Read altitude from sensors // -------------------------- - update_alt(); + update_altitude(); // altitude smoothing // ------------------ //calc_altitude_smoothing_error(); - calc_altitude_error(); + altitude_error = get_altitude_error(); // invalidate the throttle hold value // ---------------------------------- @@ -1091,8 +1098,42 @@ void update_location(void) } #endif +void update_yaw_mode(void) +{ + switch(yaw_mode){ + case YAW_ACRO: + g.rc_4.servo_out = get_rate_yaw(g.rc_4.control_in); + return; + break; -void update_current_flight_mode(void) + case YAW_HOLD: + // calcualte new nav_yaw offset + nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, g.rc_3.control_in); + //Serial.printf("n:%d %ld, ",g.rc_4.control_in, nav_yaw); + break; + + case YAW_LOOK_AT_HOME: + // copter will always point at home + if(home_is_set){ + nav_yaw = point_at_home_yaw(); + } else { + nav_yaw = 0; + } + break; + + case YAW_AUTO: + nav_yaw += constrain(wrap_180(auto_yaw - nav_yaw), -20, 20); + nav_yaw = wrap_360(nav_yaw); + break; + } + + // Yaw control + g.rc_4.servo_out = get_stabilize_yaw(nav_yaw); + + //Serial.printf("4: %d\n",g.rc_4.servo_out); +} + +void update_roll_pitch_mode(void) { #if CH7_OPTION == DO_FLIP if (do_flip){ @@ -1101,221 +1142,87 @@ void update_current_flight_mode(void) } #endif - if(control_mode == AUTO){ + int control_roll, control_pitch; - // this is a hack to prevent run up of the throttle I term for alt hold - if(command_must_ID == MAV_CMD_NAV_TAKEOFF){ - invalid_throttle = (g.rc_3.control_in != 0); - // make invalid_throttle false if we are waiting to take off. - } - switch(command_must_ID){ - default: - // mix in user control with Nav control - g.rc_1.servo_out = g.rc_1.control_mix(nav_roll); - g.rc_2.servo_out = g.rc_2.control_mix(nav_pitch); - - // Roll control - g.rc_1.servo_out = get_stabilize_roll(g.rc_1.servo_out); - - // Pitch control - g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.servo_out); - - // Throttle control - if(invalid_throttle){ - auto_throttle(); - } - - // Yaw control - g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, .5); - break; - } - - }else{ - - switch(control_mode){ - case ACRO: - // Roll control - g.rc_1.servo_out = get_rate_roll(g.rc_1.control_in); - - // Pitch control - g.rc_2.servo_out = get_rate_pitch(g.rc_2.control_in); - - // Throttle control - g.rc_3.servo_out = get_throttle(g.rc_3.control_in); - - // Yaw control - g.rc_4.servo_out = get_rate_yaw(g.rc_4.control_in); - - break; - - case STABILIZE: - // calcualte new nav_yaw offset - nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, g.rc_3.control_in); - - // Roll control - g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in); - - // Pitch control - g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in); - - // Throttle control - g.rc_3.servo_out = get_throttle(g.rc_3.control_in); - - // Yaw control - g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, 1.0); - - //Serial.printf("%u\t%d\n", nav_yaw, g.rc_4.servo_out); - break; - - case SIMPLE: - flight_timer++; - - if(flight_timer > 6){ - flight_timer = 0; - - // make sure this is always 0 - simple_WP.lat = 0; - simple_WP.lng = 0; - - next_WP.lng = (float)g.rc_1.control_in * .9; // X: 4500 * .7 = 2250 = 25 meteres - next_WP.lat = -(float)g.rc_2.control_in * .9; // Y: 4500 * .7 = 2250 = 25 meteres - - // calc a new bearing - nav_bearing = get_bearing(&simple_WP, &next_WP) + initial_simple_bearing; - nav_bearing = wrap_360(nav_bearing); - wp_distance = get_distance(&simple_WP, &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_simple_nav(); - calc_nav_output(); - } - - /* - #if SIMPLE_LOOK_AT_HOME == 0 - // This is typical yaw behavior - - // are we at rest? reset nav_yaw - if(g.rc_3.control_in == 0){ - clear_yaw_control(); - }else{ - // Yaw control - output_manual_yaw(); - } - #else - // This is experimental, - // copter will always point at home - if(home_is_set) - point_at_home_yaw(); - - // Output Pitch, Roll, Yaw and Throttle - // ------------------------------------ - auto_yaw(); - #endif - */ - - // calcualte new nav_yaw offset - nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, g.rc_3.control_in); - - // Roll control - g.rc_1.servo_out = get_stabilize_roll(nav_roll); - - // Pitch control - g.rc_2.servo_out = get_stabilize_pitch(nav_pitch); - - // Throttle control - g.rc_3.servo_out = get_throttle(g.rc_3.control_in); - - // Yaw control - g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, 1.0); - //Serial.printf("%d \t %d\n", g.rc_3.servo_out, throttle_slew); + switch(roll_pitch_mode){ + case ROLL_PITCH_ACRO: + // Roll control + g.rc_1.servo_out = get_rate_roll(g.rc_1.control_in); + // Pitch control + g.rc_2.servo_out = get_rate_pitch(g.rc_2.control_in); + return; break; - case ALT_HOLD: - // allow interactive changing of atitude - adjust_altitude(); + case ROLL_PITCH_STABLE: + control_roll = g.rc_1.control_in; + control_pitch = g.rc_2.control_in; + break; - // calcualte new nav_yaw offset - nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, 1); // send 1 instead of throttle to get nav control with low throttle + case ROLL_PITCH_SIMPLE: + simple_timer++; + if(simple_timer > 5){ + simple_timer = 0; - // Roll control - g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in); + int delta = wrap_360(dcm.yaw_sensor - initial_simple_bearing)/100; - // Pitch control - g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in); + // pre-calc rotation (stored in real degrees) + // roll + float cos_x = sin(radians(90 - delta)); + // pitch + float sin_y = cos(radians(90 - delta)); - // Throttle control - if(invalid_throttle){ - auto_throttle(); - } + // Rotate input by the initial bearing + // roll + nav_roll = g.rc_1.control_in * cos_x + g.rc_2.control_in * sin_y; + // pitch + nav_pitch = -(g.rc_1.control_in * sin_y - g.rc_2.control_in * cos_x); + } + control_roll = nav_roll; + control_pitch = nav_pitch; + break; - // Yaw control - g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, 1.0); - break; + case ROLL_PITCH_AUTO: + // mix in user control with Nav control + control_roll = g.rc_1.control_mix(nav_roll); + control_pitch = g.rc_2.control_mix(nav_pitch); + break; + } - case CIRCLE: - case GUIDED: - case RTL: - // mix in user control with Nav control - g.rc_1.servo_out = g.rc_1.control_mix(nav_roll); - g.rc_2.servo_out = g.rc_2.control_mix(nav_pitch); + // Roll control + g.rc_1.servo_out = get_stabilize_roll(control_roll); - // Roll control - g.rc_1.servo_out = get_stabilize_roll(g.rc_1.servo_out); + // Pitch control + g.rc_2.servo_out = get_stabilize_pitch(control_pitch); +} - // Pitch control - g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.servo_out); - // Throttle control - if(invalid_throttle){ - auto_throttle(); - } +void update_throttle_mode(void) +{ + switch(throttle_mode){ + case THROTTLE_MANUAL: + g.rc_3.servo_out = get_throttle(g.rc_3.control_in); + break; - // Yaw control - g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, 0.25); - break; + case THROTTLE_HOLD: + // allow interactive changing of atitude + adjust_altitude(); - case LOITER: - // calcualte new nav_yaw offset - nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, 1); // send 1 instead of throttle to get nav control with low throttle + // fall through - // allow interactive changing of atitude - adjust_altitude(); + case THROTTLE_AUTO: + if(invalid_throttle){ + // get the AP throttle + nav_throttle = get_nav_throttle(altitude_error, 150); //150 = target speed of 1.5m/s - // mix in user control with Nav control - g.rc_1.servo_out = g.rc_1.control_mix(nav_roll); - g.rc_2.servo_out = g.rc_2.control_mix(nav_pitch); + // apply throttle control + g.rc_3.servo_out = get_throttle(nav_throttle); - // Roll control - g.rc_1.servo_out = get_stabilize_roll(g.rc_1.servo_out); - - // Pitch control - g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.servo_out); - - // Throttle control - if(invalid_throttle){ - auto_throttle(); - } - - // Yaw control - g.rc_4.servo_out = get_stabilize_yaw(nav_yaw, 1.0); - break; - - default: - //Serial.print("$"); - break; - } + // clear the new data flag + invalid_throttle = false; + } + break; } } @@ -1331,7 +1238,7 @@ static void update_navigation() // note: wp_control is handled by commands_logic // calculates desired Yaw - update_nav_yaw(); + update_auto_yaw(); // calculates the desired Roll and Pitch update_nav_wp(); @@ -1339,40 +1246,26 @@ static void update_navigation() case GUIDED: case RTL: - if(wp_distance > 5){ + if(wp_distance > 4){ // calculates desired Yaw // XXX this is an experiment #if FRAME_CONFIG == HELI_FRAME - update_nav_yaw(); + update_auto_yaw(); #endif + + wp_control = WP_MODE; + }else{ + set_mode(LOITER); + xtrack_enabled = false; } - #if LOITER_TEST == 0 - // calc a rate dampened pitch to the target - calc_rate_nav(g.waypoint_speed_max.get()); - - // rotate that pitch to the copter frame of reference - calc_nav_output(); - - #else - - // rate based test - // calc error to target - calc_loiter_nav2(); - - // use error as a rate towards the target - calc_rate_nav2(long_error/2, lat_error/2); - - // rotate pitch and roll to the copter frame of reference - calc_loiter_output(); - #endif + // calculates the desired Roll and Pitch + update_nav_wp(); break; // switch passthrough to LOITER case LOITER: - // are we Traversing or Loitering? - //wp_control = (wp_distance < 20) ? LOITER_MODE : WP_MODE; wp_control = LOITER_MODE; // calculates the desired Roll and Pitch @@ -1381,20 +1274,43 @@ static void update_navigation() case CIRCLE: yaw_tracking = MAV_ROI_WPNEXT; + // calculates desired Yaw - update_nav_yaw(); + update_auto_yaw(); - // hack to elmininate crosstrack effect - crosstrack_bearing = target_bearing; + // calc the lat and long error to the target + calc_location_error(); - // get a new nav_bearing - update_loiter(); + // use error as the desired rate towards the target + // nav_lon, nav_lat is calculated + calc_nav_rate(long_error, lat_error, 200, 0); // move at 3m/s, minimum 2m/s - // calc a rate dampened pitch to the target - calc_rate_nav(400); + // rotate nav_lat and nav_long by nav_bearing so we circle the target + { + // rotate the desired direction based on the distance to + // create a vector field + int angle = get_loiter_angle(); - // rotate that pitch to the copter frame of reference - calc_nav_output(); + // pre-calc rotation (stored in real degrees) + // roll + float cos_x = cos(radians(90 - angle)); + // pitch + float sin_y = sin(radians(90 - angle)); + + // Rotate input by the initial bearing + // roll + long temp_roll = nav_lon * sin_y - nav_lat * cos_x; + // pitch + long temp_pitch = nav_lon * cos_x + nav_lat * sin_y; + + // we used temp values to not change the equations + // in mid-rotation + nav_lon = temp_roll; + nav_lat = temp_pitch; + } + + // rotate pitch and roll to the copter frame of reference + calc_nav_pitch_roll(); break; } @@ -1430,12 +1346,19 @@ static void update_trig(void){ cos_roll_x = temp.c.z / cos_pitch_x; sin_roll_y = temp.c.y / cos_pitch_x; + //flat: + // 0 ° = cp: 1.00, sp: 0.00, cr: 1.00, sr: 0.00, cy: 0.00, sy: 1.00, + // 90° = cp: 1.00, sp: 0.00, cr: 1.00, sr: 0.00, cy: 1.00, sy: 0.00, + // 180 = cp: 1.00, sp: 0.10, cr: 1.00, sr: -0.01, cy: 0.00, sy: -1.00, + // 270 = cp: 1.00, sp: 0.10, cr: 1.00, sr: -0.01, cy: -1.00, sy: 0.00, + + //Vector3f accel_filt = imu.get_accel_filtered(); //accels_rot = dcm.get_dcm_matrix() * imu.get_accel_filtered(); } // updated at 10hz -static void update_alt() +static void update_altitude() { altitude_sensor = BARO; @@ -1452,6 +1375,14 @@ static void update_alt() baro_alt = read_barometer(); if(baro_alt < 1000){ + + #if SONAR_TILT_CORRECTION == 1 + // correct alt for angle of the sonar + float temp = cos_pitch_x * cos_roll_x; + temp = max(temp, 0.707); + sonar_alt = (float)sonar_alt * temp; + #endif + scale = (sonar_alt - 400) / 200; scale = constrain(scale, 0, 1); current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt; @@ -1465,26 +1396,26 @@ static void update_alt() current_loc.alt = baro_alt + home.alt; } + altitude_rate = (current_loc.alt - old_altitude) * 10; // 10 hz timer + old_altitude = current_loc.alt; #endif } static void adjust_altitude() { - flight_timer++; - if(flight_timer >= 2){ - flight_timer = 0; + alt_timer++; + if(alt_timer >= 2){ + alt_timer = 0; if(g.rc_3.control_in <= 200){ - next_WP.alt -= 3; // 1 meter per second - next_WP.alt = max(next_WP.alt, (current_loc.alt - 900)); // don't go more than 4 meters below current location - + next_WP.alt -= 1; // 1 meter per second + next_WP.alt = max(next_WP.alt, (current_loc.alt - 400)); // don't go less than 4 meters below current location + next_WP.alt = max(next_WP.alt, 100); // don't go less than 1 meter }else if (g.rc_3.control_in > 700){ - next_WP.alt += 4; // 1 meter per second - //next_WP.alt = min((current_loc.alt + 400), next_WP.alt); // don't go more than 4 meters below current location - next_WP.alt = min(next_WP.alt, (current_loc.alt + 600)); // don't go more than 4 meters below current location + next_WP.alt += 1; // 1 meter per second + next_WP.alt = min(next_WP.alt, (current_loc.alt + 400)); // don't go more than 4 meters below current location } - next_WP.alt = max(next_WP.alt, 100); // don't go more than 4 meters below current location } } @@ -1492,47 +1423,51 @@ static void tuning(){ //Outer Loop : Attitude #if CHANNEL_6_TUNING == CH6_STABILIZE_KP - g.pid_stabilize_roll.kP((float)g.rc_6.control_in / 1000.0); - g.pid_stabilize_pitch.kP((float)g.rc_6.control_in / 1000.0); + g.pi_stabilize_roll.kP((float)g.rc_6.control_in / 1000.0); + g.pi_stabilize_pitch.kP((float)g.rc_6.control_in / 1000.0); #elif CHANNEL_6_TUNING == CH6_STABILIZE_KI - g.pid_stabilize_roll.kI((float)g.rc_6.control_in / 1000.0); - g.pid_stabilize_pitch.kI((float)g.rc_6.control_in / 1000.0); + g.pi_stabilize_roll.kI((float)g.rc_6.control_in / 1000.0); + g.pi_stabilize_pitch.kI((float)g.rc_6.control_in / 1000.0); #elif CHANNEL_6_TUNING == CH6_YAW_KP - g.pid_stabilize_yaw.kP((float)g.rc_6.control_in / 1000.0); // range from 0.0 ~ 5.0 + g.pi_stabilize_yaw.kP((float)g.rc_6.control_in / 1000.0); // range from 0.0 ~ 5.0 #elif CHANNEL_6_TUNING == CH6_YAW_KI - g.pid_stabilize_yaw.kI((float)g.rc_6.control_in / 1000.0); + g.pi_stabilize_yaw.kI((float)g.rc_6.control_in / 1000.0); //Inner Loop : Rate #elif CHANNEL_6_TUNING == CH6_RATE_KP - g.pid_rate_roll.kP((float)g.rc_6.control_in / 1000.0); - g.pid_rate_pitch.kP((float)g.rc_6.control_in / 1000.0); + g.pi_rate_roll.kP((float)g.rc_6.control_in / 1000.0); + g.pi_rate_pitch.kP((float)g.rc_6.control_in / 1000.0); #elif CHANNEL_6_TUNING == CH6_RATE_KI - g.pid_rate_roll.kI((float)g.rc_6.control_in / 1000.0); - g.pid_rate_pitch.kI((float)g.rc_6.control_in / 1000.0); + g.pi_rate_roll.kI((float)g.rc_6.control_in / 1000.0); + g.pi_rate_pitch.kI((float)g.rc_6.control_in / 1000.0); #elif CHANNEL_6_TUNING == CH6_YAW_RATE_KP - g.pid_rate_yaw.kP((float)g.rc_6.control_in / 1000.0); + g.pi_rate_yaw.kP((float)g.rc_6.control_in / 1000.0); #elif CHANNEL_6_TUNING == CH6_YAW_RATE_KI - g.pid_rate_yaw.kI((float)g.rc_6.control_in / 1000.0); + g.pi_rate_yaw.kI((float)g.rc_6.control_in / 1000.0); //Altitude Hold #elif CHANNEL_6_TUNING == CH6_THROTTLE_KP - g.pid_throttle.kP((float)g.rc_6.control_in / 1000.0); // 0 to 1 + g.pi_throttle.kP((float)g.rc_6.control_in / 1000.0); // 0 to 1 #elif CHANNEL_6_TUNING == CH6_THROTTLE_KD - g.pid_throttle.kD((float)g.rc_6.control_in / 1000.0); // 0 to 1 + g.pi_throttle.kD((float)g.rc_6.control_in / 1000.0); // 0 to 1 //Extras #elif CHANNEL_6_TUNING == CH6_TOP_BOTTOM_RATIO g.top_bottom_ratio = (float)g.rc_6.control_in / 1000.0; + #elif CHANNEL_6_TUNING == CH6_TRAVERSE_SPEED + g.waypoint_speed_max = (float)g.rc_6.control_in / 1000.0; + + #elif CHANNEL_6_TUNING == CH6_PMAX g.pitch_max.set(g.rc_6.control_in * 2); // 0 to 2000 @@ -1542,8 +1477,6 @@ static void tuning(){ if(g.rc_6.control_in >= 400) relay_off(); #endif - - } static void update_nav_wp() @@ -1551,74 +1484,46 @@ static void update_nav_wp() // XXX Guided mode!!! if(wp_control == LOITER_MODE){ - #if LOITER_TEST == 0 // calc a pitch to the target - calc_loiter_nav(); + calc_location_error(); + + // use error as the desired rate towards the target + calc_nav_rate(long_error, lat_error, g.waypoint_speed_max, 0); // rotate pitch and roll to the copter frame of reference - calc_loiter_output(); - - #else - // calc error to target - calc_loiter_nav2(); - - // use error as a rate towards the target - calc_rate_nav2(long_error/2, lat_error/2); - - // rotate pitch and roll to the copter frame of reference - calc_loiter_output(); - #endif + calc_nav_pitch_roll(); } else { - // how far are we from the ideal trajectory? - // this pushes us back on course - update_crosstrack(); + // for long journey's reset the wind resopnse + // it assumes we are standing still. + g.pi_loiter_lat.reset_I(); + g.pi_loiter_lat.reset_I(); - // calc a rate dampened pitch to the target - calc_rate_nav(g.waypoint_speed_max.get()); + // calc the lat and long error to the target + calc_location_error(); - // rotate that pitch to the copter frame of reference - calc_nav_output(); + // use error as the desired rate towards the target + calc_nav_rate(long_error, lat_error, g.waypoint_speed_max, 100); + + // rotate pitch and roll to the copter frame of reference + calc_nav_pitch_roll(); } } -static void update_nav_yaw() +static void update_auto_yaw() { // this tracks a location so the copter is always pointing towards it. if(yaw_tracking == MAV_ROI_LOCATION){ - nav_yaw = get_bearing(¤t_loc, &target_WP); + auto_yaw = get_bearing(¤t_loc, &target_WP); }else if(yaw_tracking == MAV_ROI_WPNEXT){ - nav_yaw = target_bearing; + auto_yaw = target_bearing; } } -static void point_at_home_yaw() +static long point_at_home_yaw() { - nav_yaw = get_bearing(¤t_loc, &home); -} - -static void auto_throttle() -{ - // get the AP throttle - nav_throttle = get_nav_throttle(altitude_error); - - // apply throttle control - //g.rc_3.servo_out = get_throttle(nav_throttle - throttle_slew); - g.rc_3.servo_out = get_throttle(nav_throttle); - - //if(motor_armed){ - // remember throttle offset - //throttle_slew = g.rc_3.servo_out - g.rc_3.control_in; - //}else{ - // don't allow - //throttle_slew = 0; - //} - - // clear the new data flag - invalid_throttle = false; - - //Serial.printf("wp:%d, \te:%d \tt%d \t%d\n", (int)next_WP.alt, (int)altitude_error, nav_throttle, g.rc_3.servo_out); + return get_bearing(¤t_loc, &home); } diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index caf9b6c0f1..1d1d56fa3f 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -13,19 +13,19 @@ get_stabilize_roll(long target_angle) error = constrain(error, -2500, 2500); // desired Rate: - rate = g.pid_stabilize_roll.get_pi((float)error, delta_ms_fast_loop, 1.0); + rate = g.pi_stabilize_roll.get_pi(error, delta_ms_fast_loop); //Serial.printf("%d\t%d\t%d ", (int)target_angle, (int)error, (int)rate); #if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters // Rate P: error = rate - (long)(degrees(omega.x) * 100.0); - rate = g.pid_rate_roll.get_pi((float)error, delta_ms_fast_loop, 1.0); + rate = g.pi_rate_roll.get_pi(error, delta_ms_fast_loop); //Serial.printf("%d\t%d\n", (int)error, (int)rate); #endif // output control: - return (int)constrain(rate, -2500, 2500); - + return (int)constrain(rate, -2500, 2500); + } static int @@ -40,13 +40,13 @@ get_stabilize_pitch(long target_angle) error = constrain(error, -2500, 2500); // desired Rate: - rate = g.pid_stabilize_pitch.get_pi((float)error, delta_ms_fast_loop, 1.0); + rate = g.pi_stabilize_pitch.get_pi(error, delta_ms_fast_loop); //Serial.printf("%d\t%d\t%d ", (int)target_angle, (int)error, (int)rate); #if FRAME_CONFIG != HELI_FRAME // cannot use rate control for helicopters // Rate P: error = rate - (long)(degrees(omega.y) * 100.0); - rate = g.pid_rate_pitch.get_pi((float)error, delta_ms_fast_loop, 1.0); + rate = g.pi_rate_pitch.get_pi(error, delta_ms_fast_loop); //Serial.printf("%d\t%d\n", (int)error, (int)rate); #endif @@ -54,32 +54,31 @@ get_stabilize_pitch(long target_angle) return (int)constrain(rate, -2500, 2500); } + +#define YAW_ERROR_MAX 2000 static int -get_stabilize_yaw(long target_angle, float scaler) +get_stabilize_yaw(long target_angle) { long error; long rate; - error = wrap_180(target_angle - dcm.yaw_sensor); - + yaw_error = wrap_180(target_angle - dcm.yaw_sensor); // limit the error we're feeding to the PID - error = constrain(error, -4500, 4500); - - // desired Rate: - rate = g.pid_stabilize_yaw.get_pi((float)error, delta_ms_fast_loop, scaler); + yaw_error = constrain(yaw_error, -YAW_ERROR_MAX, YAW_ERROR_MAX); + rate = g.pi_stabilize_yaw.get_pi(yaw_error, delta_ms_fast_loop); //Serial.printf("%u\t%d\t%d\t", (int)target_angle, (int)error, (int)rate); #if FRAME_CONFIG == HELI_FRAME // cannot use rate control for helicopters if( ! g.heli_ext_gyro_enabled ) { // Rate P: error = rate - (long)(degrees(omega.z) * 100.0); - rate = g.pid_rate_yaw.get_pi((float)error, delta_ms_fast_loop, 1.0); + rate = g.pi_rate_yaw.get_pi(error, delta_ms_fast_loop); } #else // Rate P: - error = rate - (long)(degrees(omega.z) * 100.0); - rate = g.pid_rate_yaw.get_pi((float)error, delta_ms_fast_loop, 1.0); + error = rate - (long)(degrees(omega.z) * 100.0); + rate = g.pi_rate_yaw.get_pi(error, delta_ms_fast_loop); //Serial.printf("%d\t%d\n", (int)error, (int)rate); #endif @@ -87,6 +86,26 @@ get_stabilize_yaw(long target_angle, float scaler) return (int)constrain(rate, -2500, 2500); } +#define ALT_ERROR_MAX 300 +static int +get_nav_throttle(long z_error, int target_speed) +{ + int rate_error; + int throttle; + float scaler = (float)target_speed/(float)ALT_ERROR_MAX; + + // limit error to prevent I term run up + z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX); + target_speed = z_error * scaler; + + rate_error = target_speed - altitude_rate; + rate_error = constrain(rate_error, -90, 90); + + throttle = g.pi_throttle.get_pi(rate_error, delta_ms_medium_loop); + return g.throttle_cruise + rate_error; +} + + static int get_rate_roll(long target_rate) { @@ -94,7 +113,7 @@ get_rate_roll(long target_rate) target_rate = constrain(target_rate, -2500, 2500); error = (target_rate * 4.5) - (long)(degrees(omega.x) * 100.0); - target_rate = g.pid_rate_roll.get_pi((float)error, delta_ms_fast_loop, 1.0); + target_rate = g.pi_rate_roll.get_pi(error, delta_ms_fast_loop); // output control: return (int)constrain(target_rate, -2500, 2500); @@ -107,7 +126,7 @@ get_rate_pitch(long target_rate) target_rate = constrain(target_rate, -2500, 2500); error = (target_rate * 4.5) - (long)(degrees(omega.y) * 100.0); - target_rate = g.pid_rate_pitch.get_pi((float)error, delta_ms_fast_loop, 1.0); + target_rate = g.pi_rate_pitch.get_pi(error, delta_ms_fast_loop); // output control: return (int)constrain(target_rate, -2500, 2500); @@ -119,7 +138,7 @@ get_rate_yaw(long target_rate) long error; error = (target_rate * 4.5) - (long)(degrees(omega.z) * 100.0); - target_rate = g.pid_rate_yaw.get_pi((float)error, delta_ms_fast_loop, 1.0); + target_rate = g.pi_rate_yaw.get_pi(error, delta_ms_fast_loop); // output control: return (int)constrain(target_rate, -2500, 2500); @@ -128,15 +147,16 @@ get_rate_yaw(long target_rate) // Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc. // Keeps outdated data out of our calculations -static void -reset_nav_I(void) +static void reset_nav_I(void) { - g.pid_nav_lat.reset_I(); - g.pid_nav_lon.reset_I(); - g.pid_nav_wp.reset_I(); - g.pid_crosstrack.reset_I(); - g.pid_throttle.reset_I(); - // I removed these, they don't seem to be needed. + g.pi_loiter_lat.reset_I(); + g.pi_loiter_lat.reset_I(); + + g.pi_nav_lat.reset_I(); + g.pi_nav_lon.reset_I(); + + g.pi_crosstrack.reset_I(); + g.pi_throttle.reset_I(); } @@ -146,8 +166,7 @@ throttle control // user input: // ----------- -static int -get_throttle(int throttle_input) +static int get_throttle(int throttle_input) { throttle_input = (float)throttle_input * angle_boost(); //throttle_input = max(throttle_slew, throttle_input); @@ -177,6 +196,7 @@ get_nav_yaw_offset(int yaw_input, int reset) } } } + /* static int alt_hold_velocity() { @@ -185,7 +205,8 @@ static int alt_hold_velocity() error = min(error, 200); error = 1 - (error/ 200.0); return (accels_rot.z + 9.81) * accel_gain * error; -}*/ +} +*/ static float angle_boost() { diff --git a/ArduCopterMega/GCS_Mavlink.pde b/ArduCopterMega/GCS_Mavlink.pde index 95898197f4..261b591986 100644 --- a/ArduCopterMega/GCS_Mavlink.pde +++ b/ArduCopterMega/GCS_Mavlink.pde @@ -259,7 +259,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_action_t packet; mavlink_msg_action_decode(msg, &packet); if (mavlink_check_target(packet.target,packet.target_component)) break; - + if (in_mavlink_delay) { // don't execute action commands while in sensor // initialisation @@ -334,13 +334,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_ACTION_CALIBRATE_MAG: case MAV_ACTION_CALIBRATE_PRESSURE: break; - + case MAV_ACTION_CALIBRATE_ACC: imu.init_accel(mavlink_delay); result=1; break; - - + + //case MAV_ACTION_REBOOT: // this is a rough interpretation //startup_IMU_ground(); //result=1; @@ -1161,14 +1161,14 @@ static void mavlink_delay(unsigned long t) { unsigned long tstart; static unsigned long last_1hz, last_3hz, last_10hz, last_50hz; - + if (in_mavlink_delay) { // this should never happen, but let's not tempt fate by // letting the stack grow too much delay(t); return; } - + in_mavlink_delay = true; tstart = millis(); @@ -1200,6 +1200,6 @@ static void mavlink_delay(unsigned long t) } delay(1); } while (millis() - tstart < t); - + in_mavlink_delay = false; } diff --git a/ArduCopterMega/GCS_Standard.pde b/ArduCopterMega/GCS_Standard.pde index 2e75d1debc..b52a1b1d27 100644 --- a/ArduCopterMega/GCS_Standard.pde +++ b/ArduCopterMega/GCS_Standard.pde @@ -16,7 +16,7 @@ static void acknowledge(byte id, byte check1, byte check2) { byte mess_ck_a = 0; byte mess_ck_b = 0; int ck; - + SendSer("4D"); // This is the message preamble mess_buffer[0] = 0x03; ck = 3; @@ -26,13 +26,13 @@ static void acknowledge(byte id, byte check1, byte check2) { mess_buffer[3] = id; mess_buffer[4] = check1; mess_buffer[5] = check2; - + for (int i = 0; i < ck + 3; i++) SendSer (mess_buffer[i]); - + for (int i = 0; i < ck + 3; i++) { mess_ck_a += mess_buffer[i]; // Calculates checksums - mess_ck_b += mess_ck_a; + mess_ck_b += mess_ck_a; } SendSer(mess_ck_a); SendSer(mess_ck_b); @@ -41,7 +41,7 @@ static void acknowledge(byte id, byte check1, byte check2) { static void send_message(byte id) { send_message(id, 0l); } - + static void send_message(byte id, long param) { byte mess_buffer[54]; byte mess_ck_a = 0; @@ -49,9 +49,9 @@ static void send_message(byte id, long param) { int tempint; int ck; long templong; - + SendSer("4D"); // This is the message preamble - + switch(id) { case MSG_HEARTBEAT: // ** System Status message mess_buffer[0] = 0x07; @@ -67,7 +67,7 @@ static void send_message(byte id, long param) { mess_buffer[8] = tempint & 0xff; mess_buffer[9] = (tempint >> 8) & 0xff; break; - + case MSG_ATTITUDE: // ** Attitude message mess_buffer[0] = 0x06; ck = 6; @@ -81,7 +81,7 @@ static void send_message(byte id, long param) { mess_buffer[7] = tempint & 0xff; mess_buffer[8] = (tempint >> 8) & 0xff; break; - + case MSG_LOCATION: // ** Location / GPS message mess_buffer[0] = 0x12; ck = 18; @@ -90,32 +90,32 @@ static void send_message(byte id, long param) { mess_buffer[4] = (templong >> 8) & 0xff; mess_buffer[5] = (templong >> 16) & 0xff; mess_buffer[6] = (templong >> 24) & 0xff; - + templong = current_loc.lng; // Longitude *10 * *7 in 4 bytes mess_buffer[7] = templong & 0xff; mess_buffer[8] = (templong >> 8) & 0xff; mess_buffer[9] = (templong >> 16) & 0xff; mess_buffer[10] = (templong >> 24) & 0xff; - + tempint = g_gps->altitude / 100; // Altitude MSL in meters * 10 in 2 bytes mess_buffer[11] = tempint & 0xff; mess_buffer[12] = (tempint >> 8) & 0xff; - + tempint = g_gps->ground_speed; // Speed in M / S * 100 in 2 bytes mess_buffer[13] = tempint & 0xff; mess_buffer[14] = (tempint >> 8) & 0xff; - + tempint = dcm.yaw_sensor; // Ground Course in degreees * 100 in 2 bytes mess_buffer[15] = tempint & 0xff; mess_buffer[16] = (tempint >> 8) & 0xff; - + templong = g_gps->time; // Time of Week (milliseconds) in 4 bytes mess_buffer[17] = templong & 0xff; mess_buffer[18] = (templong >> 8) & 0xff; mess_buffer[19] = (templong >> 16) & 0xff; mess_buffer[20] = (templong >> 24) & 0xff; break; - + case MSG_PRESSURE: // ** Pressure message mess_buffer[0] = 0x04; ck = 4; @@ -126,14 +126,14 @@ static void send_message(byte id, long param) { mess_buffer[5] = tempint & 0xff; mess_buffer[6] = (tempint >> 8) & 0xff; break; - + // case 0xMSG_STATUS_TEXT: // ** Status Text message // mess_buffer[0]=sizeof(status_message[0])+1; // ck=mess_buffer[0]; // mess_buffer[2] = param&0xff; // for (int i=3;i> 8) & 0xff; break; - + case MSG_VALUE: // ** Requested Value message mess_buffer[0] = 0x06; ck = 6; @@ -179,7 +179,7 @@ static void send_message(byte id, long param) { case 0x21: templong = nav_bearing; break; case 0x22: templong = bearing_error; break; case 0x23: templong = crosstrack_bearing; break; - case 0x24: templong = crosstrack_error; break; + case 0x24: templong = crosstrack_correction; break; case 0x25: templong = altitude_error; break; case 0x26: templong = wp_radius; break; case 0x27: templong = loiter_radius; break; @@ -192,7 +192,7 @@ static void send_message(byte id, long param) { mess_buffer[7] = (templong >> 16) & 0xff; mess_buffer[8] = (templong >> 24) & 0xff; break; - + case MSG_COMMAND: // Command list item message mess_buffer[0] = 0x10; ck = 16; @@ -223,33 +223,33 @@ static void send_message(byte id, long param) { case MSG_TRIMS: // Radio Trims message //mess_buffer[0] = 0x10; //ck = 16; - //for(int i = 0; i < 8; i++) { + //for(int i = 0; i < 8; i++) { // tempint = radio_trim[i]; // trim values // mess_buffer[3 + 2 * i] = tempint & 0xff; // mess_buffer[4 + 2 * i] = (tempint >> 8) & 0xff; //} break; - + case MSG_MINS: // Radio Mins message /*mess_buffer[0] = 0x10; ck = 16; - for(int i = 0; i < 8; i++) { + for(int i = 0; i < 8; i++) { tempint = radio_min[i]; // min values mess_buffer[3 + 2 * i] = tempint & 0xff; mess_buffer[4 + 2 * i] = (tempint >> 8) & 0xff; }*/ break; - + case MSG_MAXS: // Radio Maxs message /*mess_buffer[0] = 0x10; ck = 16; - for(int i = 0; i < 8; i++) { + for(int i = 0; i < 8; i++) { tempint = radio_max[i]; // max values mess_buffer[3 + 2 * i] = tempint & 0xff; mess_buffer[4 + 2 * i] = (tempint >> 8) & 0xff; }*/ break; - + case MSG_PID: // PID Gains message /* mess_buffer[0] = 0x0f; @@ -276,16 +276,16 @@ static void send_message(byte id, long param) { */ break; } - + //mess_buffer[0] = length; // Message length mess_buffer[1] = id; // Message ID - mess_buffer[2] = 0x01; // Message version + mess_buffer[2] = 0x01; // Message version for (int i = 0; i < ck + 3; i++) SendSer (mess_buffer[i]); - + for (int i = 0; i < ck + 3; i++) { mess_ck_a += mess_buffer[i]; // Calculates checksums - mess_ck_b += mess_ck_a; + mess_ck_b += mess_ck_a; } SendSer(mess_ck_a); @@ -296,32 +296,32 @@ static void send_message(byte id, long param) { static void send_message(byte severity, const char *str) // This is the instance of send_message for message MSG_STATUS_TEXT { - if(severity >= DEBUG_LEVEL){ + if(severity >= DEBUG_LEVEL){ byte length = strlen(str) + 1; - + byte mess_buffer[54]; byte mess_ck_a = 0; byte mess_ck_b = 0; int ck; - + SendSer("4D"); // This is the message preamble if(length > 50) length = 50; mess_buffer[0] = length; ck = length; mess_buffer[1] = 0x05; // Message ID mess_buffer[2] = 0x01; // Message version - + mess_buffer[3] = severity; - + for (int i = 3; i < ck + 2; i++) - mess_buffer[i] = str[i - 3]; // places the text into mess_buffer at locations 3+ - + mess_buffer[i] = str[i - 3]; // places the text into mess_buffer at locations 3+ + for (int i = 0; i < ck + 3; i++) SendSer(mess_buffer[i]); - + for (int i = 0; i < ck + 3; i++) { mess_ck_a += mess_buffer[i]; // Calculates checksums - mess_ck_b += mess_ck_a; + mess_ck_b += mess_ck_a; } SendSer(mess_ck_a); SendSer(mess_ck_b); diff --git a/ArduCopterMega/Log.pde b/ArduCopterMega/Log.pde index b3f03a09bc..7987c03312 100644 --- a/ArduCopterMega/Log.pde +++ b/ArduCopterMega/Log.pde @@ -104,13 +104,9 @@ dump_log(uint8_t argc, const Menu::arg *argv) // check that the requested log number can be read dump_log = argv[1].i; - if(dump_log == 99){ - Log_Read(1, 4096); - } - - if (/*(argc != 2) || */ (dump_log < 1) || (dump_log > last_log_num)) { + if (/*(argc != 2) || */ (dump_log < 1)) { Serial.printf_P(PSTR("bad log # %d\n"), dump_log); - Log_Read(1, 4095); + Log_Read(0, 4095); erase_logs(NULL, NULL); return(-1); } @@ -139,6 +135,14 @@ erase_logs(uint8_t argc, const Menu::arg *argv) for(int j = 1; j < 4096; j++) DataFlash.PageErase(j); + clear_header(); + + Serial.printf_P(PSTR("\nLog erased.\n")); + return (0); +} + +static void clear_header() +{ DataFlash.StartWrite(1); DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE2); @@ -146,8 +150,6 @@ erase_logs(uint8_t argc, const Menu::arg *argv) DataFlash.WriteByte(0); DataFlash.WriteByte(END_BYTE); DataFlash.FinishWrite(); - Serial.printf_P(PSTR("\nLog erased.\n")); - return (0); } static int8_t @@ -537,11 +539,19 @@ static void Log_Write_Nav_Tuning() DataFlash.WriteInt((int)(target_bearing/100)); // 3 DataFlash.WriteInt((int)(nav_bearing/100)); // 4 + DataFlash.WriteInt(y_actual_speed); // 4 + DataFlash.WriteInt(y_rate_error); // 4 + DataFlash.WriteInt(x_actual_speed); // 4 + DataFlash.WriteInt(x_rate_error); // 4 + DataFlash.WriteInt((int)long_error); // 5 DataFlash.WriteInt((int)lat_error); // 6 DataFlash.WriteInt((int)nav_lon); // 7 DataFlash.WriteInt((int)nav_lat); // 8 + DataFlash.WriteInt((int)g.pi_nav_lon.get_integrator()); // 7 + DataFlash.WriteInt((int)g.pi_nav_lat.get_integrator()); // 8 + DataFlash.WriteByte(END_BYTE); } @@ -550,7 +560,9 @@ static void Log_Read_Nav_Tuning() { // 1 2 3 4 Serial.printf_P(PSTR( "NTUN, %d, %d, %d, %d, " - "%d, %d, %d, %d\n"), + "%d, %d, %d, %d, " + "%d, %d, %d, %d, " + "%d, %d\n"), DataFlash.ReadInt(), //distance DataFlash.ReadByte(), //bitmask @@ -559,6 +571,14 @@ static void Log_Read_Nav_Tuning() DataFlash.ReadInt(), DataFlash.ReadInt(), + DataFlash.ReadInt(), + DataFlash.ReadInt(), + + DataFlash.ReadInt(), + DataFlash.ReadInt(), + DataFlash.ReadInt(), + DataFlash.ReadInt(), + DataFlash.ReadInt(), DataFlash.ReadInt()); //nav bearing } @@ -576,11 +596,9 @@ static void Log_Write_Control_Tuning() DataFlash.WriteInt((int)(g.rc_4.servo_out/100)); // yaw - DataFlash.WriteByte(yaw_debug); DataFlash.WriteInt((int)(dcm.yaw_sensor/100)); DataFlash.WriteInt((int)(nav_yaw/100)); DataFlash.WriteInt((int)yaw_error/100); - DataFlash.WriteInt((int)(omega.z * 100)); // Alt hold DataFlash.WriteInt(g.rc_3.servo_out); @@ -589,7 +607,7 @@ static void Log_Write_Control_Tuning() DataFlash.WriteInt((int)next_WP.alt); // DataFlash.WriteInt((int)altitude_error); // - DataFlash.WriteInt((int)g.pid_throttle.get_integrator()); + DataFlash.WriteInt((int)g.pi_throttle.get_integrator()); DataFlash.WriteByte(END_BYTE); } @@ -598,9 +616,11 @@ static void Log_Write_Control_Tuning() // Read an control tuning packet static void Log_Read_Control_Tuning() { - Serial.printf_P(PSTR( "CTUN, %d, %d, " - "%d, %d, %d, %d, %1.4f, " - "%d, %d, %d, %d, %d, %d\n"), + Serial.printf_P(PSTR( "CTUN, " + "%d, %d, " + "%d, %d, %ld, " + "%d, %d, %ld, " + "%d, %d, %ld\n"), // Control DataFlash.ReadInt(), @@ -611,7 +631,8 @@ static void Log_Read_Control_Tuning() DataFlash.ReadInt(), DataFlash.ReadInt(), DataFlash.ReadInt(), - (float)DataFlash.ReadInt(),// Gyro Rate + //(float)DataFlash.ReadInt(),// Gyro Rate + DataFlash.ReadLong(), // Alt Hold DataFlash.ReadInt(), diff --git a/ArduCopterMega/Mavlink_Common.h b/ArduCopterMega/Mavlink_Common.h index 4c4ceca291..9ef47a15da 100644 --- a/ArduCopterMega/Mavlink_Common.h +++ b/ArduCopterMega/Mavlink_Common.h @@ -147,7 +147,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ wp_distance, altitude_error / 1.0e2, 0, - crosstrack_error); + 0); //} break; } diff --git a/ArduCopterMega/Parameters.h b/ArduCopterMega/Parameters.h index 9f739e7535..4ec2b2a3ca 100644 --- a/ArduCopterMega/Parameters.h +++ b/ArduCopterMega/Parameters.h @@ -17,7 +17,7 @@ public: // The increment will prevent old parameters from being used incorrectly // by newer code. // - static const uint16_t k_format_version = 106; + static const uint16_t k_format_version = 107; // The parameter software_type is set up solely for ground station use // and identifies the software type (eg ArduPilotMega versus ArduCopterMega) @@ -151,23 +151,20 @@ public: k_param_waypoint_speed_max, // - // 240: PID Controllers + // 240: PI/D Controllers // - // Heading-to-roll PID: - // heading error from commnd to roll command deviation from trim - // (bank to turn strategy) - // - k_param_pid_rate_roll = 240, - k_param_pid_rate_pitch, - k_param_pid_rate_yaw, - k_param_pid_stabilize_roll, - k_param_pid_stabilize_pitch, - k_param_pid_stabilize_yaw, - k_param_pid_nav_lat, - k_param_pid_nav_lon, - k_param_pid_nav_wp, - k_param_pid_throttle, - k_param_pid_crosstrack, + k_param_pi_rate_roll = 240, + k_param_pi_rate_pitch, + k_param_pi_rate_yaw, + k_param_pi_stabilize_roll, + k_param_pi_stabilize_pitch, + k_param_pi_stabilize_yaw, + k_param_pi_loiter_lat, + k_param_pi_loiter_lon, + k_param_pi_nav_lat, + k_param_pi_nav_lon, + k_param_pi_throttle, + k_param_pi_crosstrack, // 254,255: reserved @@ -248,7 +245,7 @@ public: AP_Int16 heli_ext_gyro_gain; // radio output 1000~2000 (value output on CH_7) #endif - // RC channels + // RC channels RC_Channel rc_1; RC_Channel rc_2; RC_Channel rc_3; @@ -260,18 +257,23 @@ public: RC_Channel rc_camera_pitch; RC_Channel rc_camera_roll; - // PID controllers - PID pid_rate_roll; - PID pid_rate_pitch; - PID pid_rate_yaw; - PID pid_stabilize_roll; - PID pid_stabilize_pitch; - PID pid_stabilize_yaw; - PID pid_nav_lat; - PID pid_nav_lon; - PID pid_nav_wp; - PID pid_throttle; - PID pid_crosstrack; + // PI/D controllers + APM_PI pi_rate_roll; + APM_PI pi_rate_pitch; + APM_PI pi_rate_yaw; + + APM_PI pi_stabilize_roll; + APM_PI pi_stabilize_pitch; + APM_PI pi_stabilize_yaw; + + APM_PI pi_loiter_lat; + APM_PI pi_loiter_lon; + + APM_PI pi_nav_lat; + APM_PI pi_nav_lon; + + APM_PI pi_throttle; + APM_PI pi_crosstrack; uint8_t junk; @@ -355,23 +357,24 @@ public: rc_camera_pitch (k_param_rc_9, PSTR("RC_CP_")), rc_camera_roll (k_param_rc_10, PSTR("RC_CR_")), - // PID controller group key name initial P initial I initial D initial imax + // PI controller group key name initial P initial I initial imax //-------------------------------------------------------------------------------------------------------------------------------------------------------------------- - pid_rate_roll (k_param_pid_rate_roll, PSTR("RATE_RLL_"), RATE_ROLL_P, RATE_ROLL_I, 0, RATE_ROLL_IMAX * 100), - pid_rate_pitch (k_param_pid_rate_pitch, PSTR("RATE_PIT_"), RATE_PITCH_P, RATE_PITCH_I, 0, RATE_PITCH_IMAX * 100), - pid_rate_yaw (k_param_pid_rate_yaw, PSTR("RATE_YAW_"), RATE_YAW_P, RATE_YAW_I, 0, RATE_YAW_IMAX * 100), + pi_rate_roll (k_param_pi_rate_roll, PSTR("RATE_RLL_"), RATE_ROLL_P, RATE_ROLL_I, RATE_ROLL_IMAX * 100), + pi_rate_pitch (k_param_pi_rate_pitch, PSTR("RATE_PIT_"), RATE_PITCH_P, RATE_PITCH_I, RATE_PITCH_IMAX * 100), + pi_rate_yaw (k_param_pi_rate_yaw, PSTR("RATE_YAW_"), RATE_YAW_P, RATE_YAW_I, RATE_YAW_IMAX * 100), - pid_stabilize_roll (k_param_pid_stabilize_roll, PSTR("STB_RLL_"), STABILIZE_ROLL_P, STABILIZE_ROLL_I, 0, STABILIZE_ROLL_IMAX * 100), - pid_stabilize_pitch (k_param_pid_stabilize_pitch, PSTR("STB_PIT_"), STABILIZE_PITCH_P, STABILIZE_PITCH_I, 0, STABILIZE_PITCH_IMAX * 100), - pid_stabilize_yaw (k_param_pid_stabilize_yaw, PSTR("STB_YAW_"), STABILIZE_YAW_P, STABILIZE_YAW_I, 0, STABILIZE_YAW_IMAX * 100), + pi_stabilize_roll (k_param_pi_stabilize_roll, PSTR("STB_RLL_"), STABILIZE_ROLL_P, STABILIZE_ROLL_I, STABILIZE_ROLL_IMAX * 100), + pi_stabilize_pitch (k_param_pi_stabilize_pitch, PSTR("STB_PIT_"), STABILIZE_PITCH_P, STABILIZE_PITCH_I, STABILIZE_PITCH_IMAX * 100), + pi_stabilize_yaw (k_param_pi_stabilize_yaw, PSTR("STB_YAW_"), STABILIZE_YAW_P, STABILIZE_YAW_I, STABILIZE_YAW_IMAX * 100), - pid_nav_lat (k_param_pid_nav_lat, PSTR("NAV_LAT_"), NAV_LOITER_P, NAV_LOITER_I, NAV_LOITER_D, NAV_LOITER_IMAX * 100), - pid_nav_lon (k_param_pid_nav_lon, PSTR("NAV_LON_"), NAV_LOITER_P, NAV_LOITER_I, NAV_LOITER_D, NAV_LOITER_IMAX * 100), - pid_nav_wp (k_param_pid_nav_wp, PSTR("NAV_WP_"), NAV_WP_P, NAV_WP_I, NAV_WP_D, NAV_WP_IMAX * 100), + pi_loiter_lat (k_param_pi_loiter_lat, PSTR("LOITER_LAT_"),LOITER_P, LOITER_I, LOITER_IMAX * 100), + pi_loiter_lon (k_param_pi_loiter_lon, PSTR("LOITER_LON_"),LOITER_P, LOITER_I, LOITER_IMAX * 100), - pid_throttle (k_param_pid_throttle, PSTR("THR_BAR_"), THROTTLE_P, THROTTLE_I, THROTTLE_D, THROTTLE_IMAX), - pid_crosstrack (k_param_pid_crosstrack, PSTR("XTRACK_"), XTRACK_P, XTRACK_I, XTRACK_D, XTRACK_IMAX), + pi_nav_lat (k_param_pi_nav_lat, PSTR("NAV_LAT_"), NAV_P, NAV_I, NAV_IMAX * 100), + pi_nav_lon (k_param_pi_nav_lon, PSTR("NAV_LON_"), NAV_P, NAV_I, NAV_IMAX * 100), + pi_throttle (k_param_pi_throttle, PSTR("THR_BAR_"), THROTTLE_P, THROTTLE_I, THROTTLE_IMAX), + pi_crosstrack (k_param_pi_crosstrack, PSTR("XTRACK_"), XTRACK_P, XTRACK_I, XTRACK_IMAX), junk(0) // XXX just so that we can add things without worrying about the trailing comma { diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde index 1871103fb2..53d93b4bf3 100644 --- a/ArduCopterMega/commands.pde +++ b/ArduCopterMega/commands.pde @@ -233,6 +233,7 @@ static void init_home() // Save prev loc this makes the calcs look better before commands are loaded prev_WP = home; + next_WP = home; } diff --git a/ArduCopterMega/commands_logic.pde b/ArduCopterMega/commands_logic.pde index fc744ae578..f2fdf565de 100644 --- a/ArduCopterMega/commands_logic.pde +++ b/ArduCopterMega/commands_logic.pde @@ -5,9 +5,6 @@ /********************************************************************************/ static void handle_process_must() { - // clear nav_lat, this is how we pitch towards the target based on speed - nav_lat = 0; - switch(next_command.id){ case MAV_CMD_NAV_TAKEOFF: @@ -203,6 +200,7 @@ static void do_RTL(void) temp.alt = read_alt_to_hold(); //so we know where we are navigating from + // -------------------------------------- next_WP = current_loc; // Loads WP from Memory @@ -210,6 +208,7 @@ static void do_RTL(void) set_next_WP(&temp); // output control mode to the ground station + // ----------------------------------------- gcs.send_message(MSG_HEARTBEAT); } diff --git a/ArduCopterMega/commands_process.pde b/ArduCopterMega/commands_process.pde index 7ae4cc9183..a6ac1f245c 100644 --- a/ArduCopterMega/commands_process.pde +++ b/ArduCopterMega/commands_process.pde @@ -67,9 +67,6 @@ static void verify_commands(void) if(verify_must()){ //Serial.printf("verified must cmd %d\n" , command_must_index); command_must_index = NO_COMMAND; - // reset rate controlled nav - g.pid_nav_wp.reset_I(); - }else{ //Serial.printf("verified must false %d\n" , command_must_index); } diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index dd4b67d186..c496fc5818 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -300,34 +300,120 @@ +////////////////////////////////////////////////////////////////////////////// +// Attitude Control +// + +// SIMPLE Mode +#ifndef SIMPLE_YAW +# define SIMPLE_YAW YAW_HOLD +#endif + +#ifndef SIMPLE_RP +# define SIMPLE_RP ROLL_PITCH_SIMPLE +#endif + +#ifndef SIMPLE_THR +# define SIMPLE_THR THROTTLE_MANUAL +#endif + +// Alt Hold Mode +#ifndef ALT_HOLD_YAW +# define ALT_HOLD_YAW YAW_HOLD +#endif + +#ifndef ALT_HOLD_RP +# define ALT_HOLD_RP ROLL_PITCH_STABLE +#endif + +#ifndef ALT_HOLD_THR +# define ALT_HOLD_THR THROTTLE_HOLD +#endif + +// AUTO Mode +#ifndef AUTO_YAW +# define AUTO_YAW YAW_AUTO +#endif + +#ifndef AUTO_RP +# define AUTO_RP ROLL_PITCH_AUTO +#endif + +#ifndef AUTO_THR +# define AUTO_THR THROTTLE_AUTO +#endif + +// CIRCLE Mode +#ifndef CIRCLE_YAW +# define CIRCLE_YAW YAW_HOLD +#endif + +#ifndef CIRCLE_RP +# define CIRCLE_RP ROLL_PITCH_AUTO +#endif + +#ifndef CIRCLE_THR +# define CIRCLE_THR THROTTLE_HOLD +#endif + +// LOITER Mode +#ifndef LOITER_YAW +# define LOITER_YAW YAW_HOLD +#endif + +#ifndef LOITER_RP +# define LOITER_RP ROLL_PITCH_AUTO +#endif + +#ifndef LOITER_THR +# define LOITER_THR THROTTLE_HOLD +#endif + + +// RTL Mode +#ifndef RTL_YAW +# define RTL_YAW YAW_AUTO +#endif + +#ifndef RTL_RP +# define RTL_RP ROLL_PITCH_AUTO +#endif + +#ifndef RTL_THR +# define RTL_THR THROTTLE_AUTO +#endif + + + + ////////////////////////////////////////////////////////////////////////////// // Attitude Control // #ifndef STABILIZE_ROLL_P -# define STABILIZE_ROLL_P 4.0 +# define STABILIZE_ROLL_P 3.6 #endif #ifndef STABILIZE_ROLL_I -# define STABILIZE_ROLL_I 0.025 +# define STABILIZE_ROLL_I 0.02 #endif #ifndef STABILIZE_ROLL_IMAX -# define STABILIZE_ROLL_IMAX 10 // degrees +# define STABILIZE_ROLL_IMAX 1.5 // degrees #endif #ifndef STABILIZE_PITCH_P -# define STABILIZE_PITCH_P 4.0 +# define STABILIZE_PITCH_P 3.6 #endif #ifndef STABILIZE_PITCH_I -# define STABILIZE_PITCH_I 0.025 +# define STABILIZE_PITCH_I 0.02 #endif #ifndef STABILIZE_PITCH_IMAX -# define STABILIZE_PITCH_IMAX 10 // degrees +# define STABILIZE_PITCH_IMAX 1.5 // degrees #endif ////////////////////////////////////////////////////////////////////////////// // Rate Control // #ifndef RATE_ROLL_P -# define RATE_ROLL_P .12 +# define RATE_ROLL_P .13 #endif #ifndef RATE_ROLL_I # define RATE_ROLL_I 0.0 @@ -337,7 +423,7 @@ #endif #ifndef RATE_PITCH_P -# define RATE_PITCH_P 0.12 +# define RATE_PITCH_P 0.13 #endif #ifndef RATE_PITCH_I # define RATE_PITCH_I 0.0 @@ -350,17 +436,17 @@ // YAW Control // #ifndef STABILIZE_YAW_P -# define STABILIZE_YAW_P 4.5 // increase for more aggressive Yaw Hold, decrease if it's bouncy +# define STABILIZE_YAW_P 7 // increase for more aggressive Yaw Hold, decrease if it's bouncy #endif #ifndef STABILIZE_YAW_I # define STABILIZE_YAW_I 0.01 // set to .0001 to try and get over user's steady state error caused by poor balance #endif #ifndef STABILIZE_YAW_IMAX -# define STABILIZE_YAW_IMAX 15 // degrees * 100 +# define STABILIZE_YAW_IMAX 8 // degrees * 100 #endif #ifndef RATE_YAW_P -# define RATE_YAW_P .15 // used to control response in turning +# define RATE_YAW_P .13 // used to control response in turning #endif #ifndef RATE_YAW_I # define RATE_YAW_I 0.0 @@ -382,53 +468,61 @@ ////////////////////////////////////////////////////////////////////////////// // Navigation control gains // +#ifndef LOITER_P +# define LOITER_P .4 // +#endif +#ifndef LOITER_I +# define LOITER_I 0.01 // +#endif +#ifndef LOITER_IMAX +# define LOITER_IMAX 8 // degrees° +#endif + +#ifndef NAV_P +# define NAV_P 2.4 // for 4.5 ms error = 13.5 pitch +#endif +#ifndef NAV_I +# define NAV_I 0.03 // this +#endif +#ifndef NAV_IMAX +# define NAV_IMAX 8 // degrees +#endif + +/* #ifndef NAV_LOITER_P -# define NAV_LOITER_P 2.4 //1.4 // +# define NAV_LOITER_P .4 //1.4 // #endif #ifndef NAV_LOITER_I -# define NAV_LOITER_I 0.01 // +# define NAV_LOITER_I 0.05 // #endif #ifndef NAV_LOITER_D -# define NAV_LOITER_D 1.0 //1.4 // +# define NAV_LOITER_D 2 // #endif #ifndef NAV_LOITER_IMAX -# define NAV_LOITER_IMAX 12 // degrees° -#endif - - - -#ifndef NAV_WP_P -# define NAV_WP_P 2.2 // for 4.5 ms error = 13.5 pitch -#endif -#ifndef NAV_WP_I -# define NAV_WP_I 0.06 // this -#endif -#ifndef NAV_WP_D -# define NAV_WP_D .5 // -#endif -#ifndef NAV_WP_IMAX -# define NAV_WP_IMAX 20 // degrees +# define NAV_LOITER_IMAX 8 // degrees° #endif +*/ #ifndef WAYPOINT_SPEED_MAX -# define WAYPOINT_SPEED_MAX 600 // for 6m/s error = 13mph +# define WAYPOINT_SPEED_MAX 300 // for 6m/s error = 13mph #endif + ////////////////////////////////////////////////////////////////////////////// // Throttle control gains // #ifndef THROTTLE_P -# define THROTTLE_P 0.35 // trying a lower val +# define THROTTLE_P 0.4 // trying a lower val #endif #ifndef THROTTLE_I -# define THROTTLE_I 0.01 //with 4m error, 12.5s windup -#endif -#ifndef THROTTLE_D -# define THROTTLE_D 0.4 // upped with filter +# define THROTTLE_I 0.10 //with 4m error, 12.5s windup #endif +//#ifndef THROTTLE_D +//# define THROTTLE_D 0.6 // upped with filter +//#endif #ifndef THROTTLE_IMAX -# define THROTTLE_IMAX 30 +# define THROTTLE_IMAX 40 #endif diff --git a/ArduCopterMega/control_modes.pde b/ArduCopterMega/control_modes.pde index 329ae5519a..fdbb906767 100644 --- a/ArduCopterMega/control_modes.pde +++ b/ArduCopterMega/control_modes.pde @@ -2,19 +2,20 @@ static void read_control_switch() { + static bool switch_debouncer = false; byte switchPosition = readSwitch(); - //motor_armed = (switchPosition < 5); if (oldSwitchPosition != switchPosition){ + if(switch_debouncer){ + // remember the prev location for GS + prev_WP = current_loc; + oldSwitchPosition = switchPosition; + switch_debouncer = false; - set_mode(flight_modes[switchPosition]); - - oldSwitchPosition = switchPosition; - prev_WP = current_loc; - - // reset navigation integrators - // ------------------------- - //reset_I(); + set_mode(flight_modes[switchPosition]); + }else{ + switch_debouncer = true; + } } } @@ -33,13 +34,6 @@ static void reset_control_switch() { oldSwitchPosition = -1; read_control_switch(); - //SendDebug("MSG: reset_control_switch "); - //SendDebugln(oldSwitchPosition , DEC); -} - -static void update_servo_switches() -{ - } static boolean trim_flag; @@ -101,8 +95,8 @@ static void auto_trim() static void trim_accel() { - g.pid_stabilize_roll.reset_I(); - g.pid_stabilize_pitch.reset_I(); + g.pi_stabilize_roll.reset_I(); + g.pi_stabilize_pitch.reset_I(); if(g.rc_1.control_in > 0){ imu.ay(imu.ay() + 1); diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h index 51a8f27853..6b485b146a 100644 --- a/ArduCopterMega/defines.h +++ b/ArduCopterMega/defines.h @@ -4,6 +4,24 @@ #define ENABLED 1 #define DISABLED 0 + +// Flight modes +// ------------ +#define YAW_HOLD 0 +#define YAW_ACRO 1 +#define YAW_AUTO 2 +#define YAW_LOOK_AT_HOME 3 + +#define ROLL_PITCH_STABLE 0 +#define ROLL_PITCH_ACRO 1 +#define ROLL_PITCH_SIMPLE 2 +#define ROLL_PITCH_AUTO 3 + +#define THROTTLE_MANUAL 0 +#define THROTTLE_HOLD 1 +#define THROTTLE_AUTO 2 + + // active altitude sensor // ---------------------- #define SONAR 0 @@ -147,7 +165,7 @@ #define CH6_TOP_BOTTOM_RATIO 11 #define CH6_PMAX 12 #define CH6_RELAY 13 - +#define CH6_TRAVERSE_SPEED 14 // nav byte mask // ------------- @@ -306,7 +324,7 @@ #define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO #define CURRENT_AMPS(x) ((x*(INPUT_VOLTAGE/1024.0))-CURR_AMPS_OFFSET)*CURR_AMP_PER_VOLT -#define BARO_FILTER_SIZE 6 +//#define BARO_FILTER_SIZE 8 /* ************************************************************** */ /* Expansion PIN's that people can use for various things. */ @@ -352,7 +370,7 @@ // sonar -#define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters +//#define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters // Hardware Parameters #define SLIDE_SWITCH_PIN 40 diff --git a/ArduCopterMega/leds.pde b/ArduCopterMega/leds.pde index eb35d4f751..546dfeeb88 100644 --- a/ArduCopterMega/leds.pde +++ b/ArduCopterMega/leds.pde @@ -89,6 +89,7 @@ static void clear_leds() digitalWrite(A_LED_PIN, LOW); digitalWrite(B_LED_PIN, LOW); digitalWrite(C_LED_PIN, LOW); + motor_light = false; led_mode = NORMAL_LEDS; } diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde index 48ac4ba0d5..c2f6ffc022 100644 --- a/ArduCopterMega/motors.pde +++ b/ArduCopterMega/motors.pde @@ -31,9 +31,17 @@ static void arm_motors() motor_armed = true; arming_counter = ARM_DELAY; - // Clear throttle slew + // Tune down DCM // ------------------- - //throttle_slew = 0; + #if HIL_MODE != HIL_MODE_ATTITUDE + dcm.kp_roll_pitch(0.030000); + dcm.ki_roll_pitch(0.000006); + #endif + + // tune down compass + // ----------------- + dcm.kp_yaw(0.08); + dcm.ki_yaw(0); // Remember Orientation // -------------------- @@ -49,15 +57,17 @@ static void arm_motors() startup_ground(); } + #if HIL_MODE != HIL_MODE_ATTITUDE + // read Baro pressure at ground - + // this resets Baro for more accuracy + //----------------------------------- + init_barometer(); + #endif + // temp hack motor_light = true; digitalWrite(A_LED_PIN, HIGH); - // tune down compass - // ----------------- - dcm.kp_yaw(0.08); - dcm.ki_yaw(0); - arming_counter++; } else{ arming_counter++; @@ -73,13 +83,22 @@ static void arm_motors() arming_counter = 0; }else if (arming_counter == DISARM_DELAY){ -#if HIL_MODE != HIL_MODE_DISABLED + #if HIL_MODE != HIL_MODE_DISABLED hil.send_text_P(SEVERITY_HIGH, PSTR("DISARMING MOTORS")); -#endif + #endif + motor_armed = false; arming_counter = DISARM_DELAY; compass.save_offsets(); + + // Tune down DCM + // ------------------- + #if HIL_MODE != HIL_MODE_ATTITUDE + dcm.kp_roll_pitch(0.12); // higher for quads + dcm.ki_roll_pitch(0.00000319); // 1/4 of the normal rate for 200 hz loop + #endif + // tune up compass // ----------------- dcm.kp_yaw(0.8); @@ -117,92 +136,3 @@ set_servos_4() output_motors_disarmed(); } } - -/***************************************** - * Set the flight control servos based on the current calculated values - *****************************************/ - - - - - - //if (num++ > 25){ - // num = 0; - - //Serial.print("kP: "); - //Serial.println(g.pid_stabilize_roll.kP(),3); - //*/ - - - /* - Serial.printf("yaw: %d, lat_e: %ld, lng_e: %ld, \tnlat: %ld, nlng: %ld,\tnrll: %ld, nptc: %ld, \tcx: %.2f, sy: %.2f, \ttber: %ld, \tnber: %ld\n", - (int)(dcm.yaw_sensor / 100), - lat_error, - long_error, - nav_lat, - nav_lon, - nav_roll, - nav_pitch, - cos_yaw_x, - sin_yaw_y, - target_bearing, - nav_bearing); - //*/ - - /* - - gcs_simple.write_byte(control_mode); - //gcs_simple.write_int(motor_out[CH_1]); - //gcs_simple.write_int(motor_out[CH_2]); - //gcs_simple.write_int(motor_out[CH_3]); - //gcs_simple.write_int(motor_out[CH_4]); - - gcs_simple.write_int(g.rc_3.servo_out); - - gcs_simple.write_int((int)(dcm.yaw_sensor / 100)); - - gcs_simple.write_int((int)nav_lat); - gcs_simple.write_int((int)nav_lon); - gcs_simple.write_int((int)nav_roll); - gcs_simple.write_int((int)nav_pitch); - - //gcs_simple.write_int((int)(cos_yaw_x * 100)); - //gcs_simple.write_int((int)(sin_yaw_y * 100)); - - gcs_simple.write_long(current_loc.lat); //28 - gcs_simple.write_long(current_loc.lng); //32 - gcs_simple.write_int((int)current_loc.alt); //34 - - gcs_simple.write_long(next_WP.lat); - gcs_simple.write_long(next_WP.lng); - gcs_simple.write_int((int)next_WP.alt); //44 - - gcs_simple.write_int((int)(target_bearing / 100)); - gcs_simple.write_int((int)(nav_bearing / 100)); - gcs_simple.write_int((int)(nav_yaw / 100)); - - if(altitude_sensor == BARO){ - gcs_simple.write_int((int)g.pid_baro_throttle.get_integrator()); - }else{ - gcs_simple.write_int((int)g.pid_sonar_throttle.get_integrator()); - } - - gcs_simple.write_int(g.throttle_cruise); - gcs_simple.write_int(g.throttle_cruise); - - //24 - gcs_simple.flush(10); // Message ID - - //*/ - //Serial.printf("\n tb %d\n", (int)(target_bearing / 100)); - //Serial.printf("\n nb %d\n", (int)(nav_bearing / 100)); - //Serial.printf("\n dcm %d\n", (int)(dcm.yaw_sensor / 100)); - - /*Serial.printf("a %ld, e %ld, i %d, t %d, b %4.2f\n", - current_loc.alt, - altitude_error, - (int)g.pid_baro_throttle.get_integrator(), - nav_throttle, - angle_boost()); - */ - //} diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde index 61c7e6ab31..ba26ec6f56 100644 --- a/ArduCopterMega/navigation.pde +++ b/ArduCopterMega/navigation.pde @@ -31,9 +31,14 @@ static void navigate() // -------------------------------------------- target_bearing = get_bearing(¤t_loc, &next_WP); - // nav_bearing will includes xtrac correction - // ------------------------------------------ - nav_bearing = target_bearing; + // nav_bearing will include xtrac correction + // ----------------------------------------- + xtrack_enabled = false; + if(xtrack_enabled){ + nav_bearing = wrap_360(target_bearing + get_crosstrack_correction()); + }else{ + nav_bearing = target_bearing; + } } static bool check_missed_wp() @@ -43,28 +48,10 @@ static bool check_missed_wp() return (abs(temp) > 10000); //we pased the waypoint by 10 ° } -static int -get_nav_throttle(long error) -{ - int throttle; - - // limit error to prevent I term run up - error = constrain(error, -600,600); - - throttle = g.pid_throttle.get_pid(error, delta_ms_medium_loop, 1.0); - throttle = g.throttle_cruise + constrain(throttle, -80, 80); - - // failed experiment - //int tem = alt_hold_velocity(); - //throttle -= tem; - - return throttle; -} - // ------------------------------ // long_error, lat_error -static void calc_loiter_nav2() +static void calc_location_error() { /* Becuase we are using lat and lon to do our distance errors here's a quick chart: @@ -80,180 +67,77 @@ static void calc_loiter_nav2() long_error = (float)(next_WP.lng - current_loc.lng) * scaleLongDown; // 500 - 0 = 500 roll EAST // Y PITCH - lat_error = current_loc.lat - next_WP.lat; // 0 - 500 = -500 pitch NORTH - - // constrain input, not output to let I term ramp up and do it's job again wind - long_error = constrain(long_error, -loiter_error_max, loiter_error_max); // +- 20m max error - lat_error = constrain(lat_error, -loiter_error_max, loiter_error_max); // +- 20m max error + lat_error = next_WP.lat - current_loc.lat; // 0 - 500 = -500 pitch NORTH } -// sets nav_lon, nav_lat -static void calc_rate_nav2(int target_x_speed, int target_y_speed) +#define NAV_ERR_MAX 400 +static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed) { - // find the rates: - // calc the cos of the error to tell how fast we are moving towards the target in cm - int y_speed = (float)g_gps->ground_speed * cos(radians((float)g_gps->ground_course/100.0)); - int y_error = constrain(target_y_speed - y_speed, -1000, 1000); + // moved to globals for logging + //int x_actual_speed, y_actual_speed; + //int x_rate_error, y_rate_error; + x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX); + y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX); - // calc the sin of the error to tell how fast we are moving laterally to the target in cm - int x_speed = (float)g_gps->ground_speed * sin(radians((float)g_gps->ground_course/100.0)); - int x_error = constrain(target_x_speed - x_speed, -1000, 1000); + float scaler = (float)max_speed/(float)NAV_ERR_MAX; + g.pi_loiter_lat.kP(scaler); + g.pi_loiter_lon.kP(scaler); - // how fast should we be going? - nav_lat += g.pid_nav_lat.get_pid(y_error, dTnav, 1.0); - nav_lat >>= 1; // divide by two for smooting + int x_target_speed = g.pi_loiter_lon.get_pi(x_error, dTnav); + int y_target_speed = g.pi_loiter_lat.get_pi(y_error, dTnav); - nav_lon += g.pid_nav_lon.get_pid(x_error, dTnav, 1.0); - nav_lon >>= 1; // divide by two for smooting + //Serial.printf("scaler: %1.3f, y_target_speed %d",scaler,y_target_speed); - //Serial.printf("dTnav: %ld, gs: %d, err: %d, int: %d, pitch: %ld", dTnav, targetspeed, error, (int)g.pid_nav_wp.get_integrator(), (long)nav_lat); - - // limit our output - nav_lat = constrain(nav_lat, -3500, 3500); // +- max error - nav_lon = constrain(nav_lon, -3500, 3500); // +- max error -} - - -// ------------------------------ - -//nav_lon, nav_lat -static void calc_loiter_nav() -{ - /* - Becuase we are using lat and lon to do our distance errors here's a quick chart: - 100 = 1m - 1000 = 11m = 36 feet - 1800 = 19.80m = 60 feet - 3000 = 33m - 10000 = 111m - pitch_max = 22° (2200) - */ - - // X ROLL - long_error = (float)(next_WP.lng - current_loc.lng) * scaleLongDown; // 500 - 0 = 500 roll EAST - - // Y PITCH - lat_error = current_loc.lat - next_WP.lat; // 0 - 500 = -500 pitch NORTH - - // constrain input, not output to let I term ramp up and do it's job again wind - long_error = constrain(long_error, -loiter_error_max, loiter_error_max); // +- 20m max error - lat_error = constrain(lat_error, -loiter_error_max, loiter_error_max); // +- 20m max error - - nav_lon = g.pid_nav_lon.get_pid(long_error, dTnav, 1.0); // X 700 * 2.5 = 1750, - nav_lat = g.pid_nav_lat.get_pid(lat_error, dTnav, 1.0); // Y invert lat (for pitch) -} - -//nav_lat -static void calc_simple_nav() -{ - // no dampening here in SIMPLE mode - nav_lat = constrain((wp_distance * 100), -4500, 4500); // +- 20m max error - // Scale response by kP - //nav_lat *= g.pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36° -} - -// sets nav_lon, nav_lat -static void calc_rate_nav(int speed) -{ - // which direction are we moving? - long heading_error = nav_bearing - g_gps->ground_course; - heading_error = wrap_180(heading_error); - - // calc the cos of the error to tell how fast we are moving towards the target in cm - int targetspeed = (float)g_gps->ground_speed * cos(radians((float)heading_error/100)); - - // calc the sin of the error to tell how fast we are moving laterally to the target in cm - int lateralspeed = (float)g_gps->ground_speed * sin(radians((float)heading_error/100)); - //targetspeed = max(targetspeed, 0); - - // Reduce speed on RTL - if(control_mode == RTL){ - int tmp = min(wp_distance, 80) * 50; - waypoint_speed = min(tmp, speed); - //waypoint_speed = max(waypoint_speed, 50); + if(x_target_speed > 0){ + x_target_speed = max(x_target_speed, min_speed); }else{ - int tmp = min(wp_distance, 200) * 90; - waypoint_speed = min(tmp, speed); - waypoint_speed = max(waypoint_speed, 50); - //waypoint_speed = g.waypoint_speed_max.get(); + x_target_speed = min(x_target_speed, -min_speed); } - int error = constrain(waypoint_speed - targetspeed, -1000, 1000); + if(y_target_speed > 0){ + y_target_speed = max(y_target_speed, min_speed); + }else{ + y_target_speed = min(y_target_speed, -min_speed); + } - nav_lat += g.pid_nav_wp.get_pid(error, dTnav, 1.0); - nav_lat >>= 1; // divide by two for smooting + // find the rates: + // calc the cos of the error to tell how fast we are moving towards the target in cm + y_actual_speed = (float)g_gps->ground_speed * cos(radians((float)g_gps->ground_course/100.0)); + y_rate_error = y_target_speed - y_actual_speed; // 413 + nav_lat = constrain(g.pi_nav_lat.get_pi(y_rate_error, dTnav), -3500, 3500); - nav_lon += lateralspeed * 2; // 2 is our fake PID gain - nav_lon >>= 1; // divide by two for smooting + //Serial.printf("yr: %d, nav_lat: %d, int:%d \n",y_rate_error, nav_lat, g.pi_nav_lat.get_integrator()); - //Serial.printf("dTnav: %ld, gs: %d, err: %d, int: %d, pitch: %ld", dTnav, targetspeed, error, (int)g.pid_nav_wp.get_integrator(), (long)nav_lat); - - // limit our output - nav_lat = constrain(nav_lat, -3500, 3500); // +- max error -} - - -// output pitch and roll -// ------------------------------ - -// nav_roll, nav_pitch -static void calc_loiter_output() -{ - // rotate the vector - nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * -cos_yaw_x; - // BAD - //NORTH -1000 * 1 - 1000 * 0 = -1000 // roll left - //WEST -1000 * 0 - 1000 * -1 = 1000 // roll right - Backwards - //EAST -1000 * 0 - 1000 * 1 = -1000 // roll left - Backwards - //SOUTH -1000 * -1 - 1000 * 0 = 1000 // roll right - - // GOOD - //NORTH -1000 * 1 - 1000 * 0 = -1000 // roll left - //WEST -1000 * 0 - 1000 * 1 = -1000 // roll right - //EAST -1000 * 0 - 1000 * -1 = 1000 // roll left - //SOUTH -1000 * -1 - 1000 * 0 = 1000 // roll right - - nav_pitch = ((float)nav_lon * -cos_yaw_x + (float)nav_lat * sin_yaw_y); - // BAD - //NORTH -1000 * 0 + 1000 * 1 = 1000 // pitch back - //WEST -1000 * -1 + 1000 * 0 = 1000 // pitch back - Backwards - //EAST -1000 * 1 + 1000 * 0 = -1000 // pitch forward - Backwards - //SOUTH -1000 * 0 + 1000 * -1 = -1000 // pitch forward - - // GOOD - //NORTH -1000 * 0 + 1000 * 1 = 1000 // pitch back - //WEST -1000 * 1 + 1000 * 0 = -1000 // pitch forward - //EAST -1000 * -1 + 1000 * 0 = 1000 // pitch back - //SOUTH -1000 * 0 + 1000 * -1 = -1000 // pitch forward + // calc the sin of the error to tell how fast we are moving laterally to the target in cm + x_actual_speed = (float)g_gps->ground_speed * sin(radians((float)g_gps->ground_course/100.0)); + x_rate_error = x_target_speed - x_actual_speed; + nav_lon = constrain(g.pi_nav_lon.get_pi(x_rate_error, dTnav), -3500, 3500); } // nav_roll, nav_pitch -static void calc_nav_output() +static void calc_nav_pitch_roll() { - // get the sin and cos of the bearing error - rotated 90° - float sin_nav_y = sin(radians((float)(9000 - bearing_error) / 100)); - float cos_nav_x = cos(radians((float)(bearing_error - 9000) / 100)); - // rotate the vector - //nav_roll = (float)nav_lat * cos_nav_x; - //nav_pitch = -(float)nav_lat * sin_nav_y; - nav_roll = (float)nav_lon * sin_nav_y - (float)nav_lat * -cos_nav_x; - nav_pitch = (float)nav_lon * cos_nav_x - (float)nav_lat * sin_nav_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; } // ------------------------------ static void calc_bearing_error() { - // 83 99 Yaw = -16 bearing_error = nav_bearing - dcm.yaw_sensor; bearing_error = wrap_180(bearing_error); } -static void calc_altitude_error() +static long get_altitude_error() { - altitude_error = next_WP.alt - current_loc.alt; + return next_WP.alt - current_loc.alt; } +/* static void calc_altitude_smoothing_error() { // limit climb rates - we draw a straight line between first location and edge of waypoint_radius @@ -268,26 +152,24 @@ static void calc_altitude_smoothing_error() altitude_error = target_altitude - current_loc.alt; } +*/ -static void update_loiter() +static int get_loiter_angle() { float power; + int angle; if(wp_distance <= g.loiter_radius){ power = float(wp_distance) / float(g.loiter_radius); power = constrain(power, 0.5, 1); - nav_bearing += (int)(9000.0 * (2.0 + power)); + angle = 90.0 * (2.0 + power); }else if(wp_distance < (g.loiter_radius + LOITER_RANGE)){ power = -((float)(wp_distance - g.loiter_radius - LOITER_RANGE) / LOITER_RANGE); power = constrain(power, 0.5, 1); //power = constrain(power, 0, 1); - nav_bearing -= power * 9000; - - }else{ - update_crosstrack(); - loiter_time = millis(); // keep start time for loiter updating till we get within LOITER_RANGE of orbit - + angle = power * 90; } - nav_bearing = wrap_360(nav_bearing); + + return angle; } @@ -305,25 +187,27 @@ static long wrap_180(long error) return error; } -static void update_crosstrack(void) +static long get_crosstrack_correction(void) { // Crosstrack Error // ---------------- if (cross_track_test() < 9000) { // If we are too far off or too close we don't do track following + // Meters we are off track line - crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / (float)100)) * (float)wp_distance; + float error = sin(radians((target_bearing - crosstrack_bearing) / (float)100)) * (float)wp_distance; // take meters * 100 to get adjustment to nav_bearing - long xtrack = g.pid_crosstrack.get_pid(crosstrack_error, dTnav, 1.0) * 100; - nav_bearing += constrain(xtrack, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get()); - nav_bearing = wrap_360(nav_bearing); + long _crosstrack_correction = g.pi_crosstrack.get_pi(error, dTnav) * 100; + + // constrain answer to 30° to avoid overshoot + return constrain(_crosstrack_correction, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get()); } } + static long cross_track_test() { - long temp = target_bearing - crosstrack_bearing; - temp = wrap_180(temp); + long temp = wrap_180(target_bearing - crosstrack_bearing); return abs(temp); } diff --git a/ArduCopterMega/read_commands.pde b/ArduCopterMega/read_commands.pde index a315b62fa5..baad2778a2 100644 --- a/ArduCopterMega/read_commands.pde +++ b/ArduCopterMega/read_commands.pde @@ -79,26 +79,26 @@ static void parseCommand(char *buffer) ///* switch(cmd[0]){ case 'P': - g.pid_stabilize_roll.kP((float)value / 1000); - g.pid_stabilize_pitch.kP((float)value / 1000); - g.pid_stabilize_pitch.save_gains(); + 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.pid_stabilize_roll.kI((float)value / 1000); - g.pid_stabilize_pitch.kI((float)value / 1000); - g.pid_stabilize_pitch.save_gains(); + 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.pid_stabilize_roll.kD((float)value / 1000); - //g.pid_stabilize_pitch.kD((float)value / 1000); + //g.pi_stabilize_roll.kD((float)value / 1000); + //g.pi_stabilize_pitch.kD((float)value / 1000); break; case 'X': - g.pid_stabilize_roll.imax(value * 100); - g.pid_stabilize_pitch.imax(value * 100); - g.pid_stabilize_pitch.save_gains(); + g.pi_stabilize_roll.imax(value * 100); + g.pi_stabilize_pitch.imax(value * 100); + g.pi_stabilize_pitch.save_gains(); break; case 'R': diff --git a/ArduCopterMega/sensors.pde b/ArduCopterMega/sensors.pde index 5406134391..f816a20ed4 100644 --- a/ArduCopterMega/sensors.pde +++ b/ArduCopterMega/sensors.pde @@ -12,9 +12,19 @@ static void init_barometer(void) #endif ground_temperature = barometer.Temp; + int i; // We take some readings... - for(int i = 0; i < 20; i++){ + for(i = 0; i < 60; i++){ + delay(20); + //read_baro_filtered(); + // get new data from absolute pressure sensor + barometer.Read(); + + Serial.printf("init %ld, %d, -, %ld, %ld\n", barometer.RawTemp, barometer.Temp, barometer.RawPress, barometer.Press); + } + + for(i = 0; i < 20; i++){ delay(20); #if HIL_MODE == HIL_MODE_SENSORS @@ -22,8 +32,13 @@ static void init_barometer(void) #endif // Get initial data from absolute pressure sensor - ground_pressure = read_baro_filtered(); + //ground_pressure = read_baro_filtered(); + // get new data from absolute pressure sensor + + barometer.Read(); + ground_pressure = barometer.Press; ground_temperature = (ground_temperature * 9 + barometer.Temp) / 10; + Serial.printf("init %ld, %d, -, %ld, %ld, -, %d, %ld\n", barometer.RawTemp, barometer.Temp, barometer.RawPress, barometer.Press, ground_temperature, ground_pressure); } abs_pressure = ground_pressure; @@ -34,12 +49,14 @@ static void init_barometer(void) static long read_baro_filtered(void) { - long pressure = 0; // get new data from absolute pressure sensor barometer.Read(); + return barometer.Press; + /* + long pressure = 0; // add new data into our filter baro_filter[baro_filter_index] = barometer.Press; baro_filter_index++; @@ -58,6 +75,7 @@ static long read_baro_filtered(void) // average our sampels return pressure /= BARO_FILTER_SIZE; + //*/ } static long read_barometer(void) diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index a3e754d21b..9b618dcadb 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -824,75 +824,6 @@ static void report_radio() print_blanks(2); } -/* -static void report_gains() -{ - Serial.printf_P(PSTR("Gains\n")); - print_divider(); - - // Rate - Serial.printf_P(PSTR("Rate:\nroll:\n")); - print_PID(&g.pid_rate_roll); - Serial.printf_P(PSTR("pitch:\n")); - print_PID(&g.pid_rate_pitch); - Serial.printf_P(PSTR("yaw:\n")); - print_PID(&g.pid_rate_yaw); - - // Stabilize - Serial.printf_P(PSTR("\nStabilize:\nroll:\n")); - print_PID(&g.pid_stabilize_roll); - Serial.printf_P(PSTR("pitch:\n")); - print_PID(&g.pid_stabilize_pitch); - Serial.printf_P(PSTR("yaw:\n")); - print_PID(&g.pid_stabilize_yaw); - - //Serial.printf_P(PSTR("Stab D: %4.3f\n"), (float)g.stabilize_dampener); - //Serial.printf_P(PSTR("Yaw D: %4.3f\n\n"), (float)g.hold_yaw_dampener); - - // Nav - Serial.printf_P(PSTR("Nav:\nlat:\n")); - print_PID(&g.pid_nav_lat); - Serial.printf_P(PSTR("long:\n")); - print_PID(&g.pid_nav_lon); - Serial.printf_P(PSTR("throttle:\n")); - print_PID(&g.pid_throttle); - print_blanks(2); -} -*/ - -/*static void report_xtrack() -{ - Serial.printf_P(PSTR("XTrack\n")); - print_divider(); - // radio - Serial.printf_P(PSTR("XTRACK: %4.2f\n" - "XTRACK angle: %d\n" - "PITCH_MAX: %ld"), - (float)g.crosstrack_gain, - (int)g.crosstrack_entry_angle, - (long)g.pitch_max); - print_blanks(2); -} -*/ - -/*static void report_throttle() -{ - Serial.printf_P(PSTR("Throttle\n")); - print_divider(); - - Serial.printf_P(PSTR("min: %d\n" - "max: %d\n" - "cruise: %d\n" - "failsafe_enabled: %d\n" - "failsafe_value: %d"), - (int)g.throttle_min, - (int)g.throttle_max, - (int)g.throttle_cruise, - (int)g.throttle_fs_enabled, - (int)g.throttle_fs_value); - print_blanks(2); -}*/ - static void report_imu() { Serial.printf_P(PSTR("IMU\n")); @@ -991,12 +922,11 @@ static void report_gyro() /***************************************************************************/ /*static void -print_PID(PID * pid) +print_PID(PI * pid) { - Serial.printf_P(PSTR("P: %4.2f, I:%4.2f, D:%4.2f, IMAX:%ld\n"), + Serial.printf_P(PSTR("P: %4.2f, I:%4.2f, IMAX:%ld\n"), pid->kP(), pid->kI(), - pid->kD(), (long)pid->imax()); } */ diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index 82e8ef9c92..8071583692 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -41,7 +41,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = { }; // Create the top-level menu object. -MENU(main_menu, "AC 2.0.39b Beta", main_menu_commands); +MENU(main_menu, "AC 2.0.40 Beta", main_menu_commands); #endif // CLI_ENABLED @@ -256,7 +256,6 @@ static void init_ardupilot() GPS_enabled = false; - //* // Read in the GPS for (byte counter = 0; ; counter++) { g_gps->update(); @@ -270,7 +269,6 @@ static void init_ardupilot() break; } } - //*/ // lengthen the idle timeout for gps Auto_detect // --------------------------------------------- @@ -280,10 +278,6 @@ static void init_ardupilot() // -------------------- report_gps(); - // used to limit the input of error for loiter - // ------------------------------------------- - loiter_error_max = (float)g.pitch_max.get() / (float)g.pid_nav_lat.kP(); - #if HIL_MODE != HIL_MODE_ATTITUDE // read Baro pressure at ground //----------------------------- @@ -301,7 +295,7 @@ static void init_ardupilot() //delay(100); startup_ground(); - //Serial.printf_P(PSTR("\nloiter: %d\n"), loiter_error_max); + //Serial.printf_P(PSTR("\nloiter: %d\n"), location_error_max); Log_Write_Startup(); SendDebug("\nReady to FLY "); @@ -321,24 +315,28 @@ static void startup_ground(void) report_imu(); #endif - #if HIL_MODE != HIL_MODE_ATTITUDE - // read Baro pressure at ground - - // this resets Baro for more accuracy - //----------------------------------- - init_barometer(); - #endif - - // setup DCM for copters: -#if HIL_MODE != HIL_MODE_ATTITUDE - dcm.kp_roll_pitch(0.12); // higher for quads - dcm.ki_roll_pitch(0.00000319); // 1/4 of the normal rate for 200 hz loop -#endif - // reset the leds // --------------------------- clear_leds(); } +/* +#define YAW_HOLD 0 +#define YAW_ACRO 1 +#define YAW_AUTO 2 +#define YAW_LOOK_AT_HOME 3 + +#define ROLL_PITCH_STABLE 0 +#define ROLL_PITCH_ACRO 1 +#define ROLL_PITCH_SIMPLE 2 +#define ROLL_PITCH_AUTO 3 + +#define THROTTLE_MANUAL 0 +#define THROTTLE_HOLD 1 +#define THROTTLE_AUTO 2 + +*/ + static void set_mode(byte mode) { if(control_mode == mode){ @@ -346,13 +344,6 @@ static void set_mode(byte mode) return; } - // XXX - Serial.printf_P(PSTR("\nRAM: %lu\n"), freeRAM()); - - // reset the Nav_WP I term - g.pid_nav_wp.reset_I(); - - old_control_mode = control_mode; control_mode = mode; @@ -365,46 +356,94 @@ static void set_mode(byte mode) motor_auto_armed = false; } - //send_text_P(SEVERITY_LOW,PSTR("control mode")); - //Serial.printf("set mode: %d\n",control_mode); Serial.println(flight_mode_strings[control_mode]); + // report the GPS and Motor arming status led_mode = NORMAL_LEDS; + // most modes do not calculate crosstrack correction + xtrack_enabled = false; + switch(control_mode) { case ACRO: + yaw_mode = YAW_ACRO; + roll_pitch_mode = ROLL_PITCH_ACRO; + throttle_mode = THROTTLE_MANUAL; + reset_nav_I(); + break; + + case STABILIZE: + yaw_mode = YAW_HOLD; + roll_pitch_mode = ROLL_PITCH_STABLE; + throttle_mode = THROTTLE_MANUAL; reset_nav_I(); break; case SIMPLE: - case STABILIZE: - do_loiter_at_location(); + yaw_mode = SIMPLE_YAW; + roll_pitch_mode = SIMPLE_RP; + throttle_mode = SIMPLE_THR; reset_nav_I(); break; case ALT_HOLD: + yaw_mode = ALT_HOLD_YAW; + roll_pitch_mode = ALT_HOLD_RP; + throttle_mode = ALT_HOLD_THR; + init_throttle_cruise(); - do_loiter_at_location(); break; case AUTO: + reset_nav_I(); + yaw_mode = AUTO_YAW; + roll_pitch_mode = AUTO_RP; + throttle_mode = AUTO_THR; + init_throttle_cruise(); + + // loads the commands from where we left off init_auto(); + + // do crosstrack correction + xtrack_enabled = true; break; case CIRCLE: - case LOITER: + yaw_mode = CIRCLE_YAW; + roll_pitch_mode = CIRCLE_RP; + throttle_mode = CIRCLE_THR; + init_throttle_cruise(); - do_loiter_at_location(); + next_WP = current_loc; + break; + + case LOITER: + yaw_mode = LOITER_YAW; + roll_pitch_mode = LOITER_RP; + throttle_mode = LOITER_THR; + + init_throttle_cruise(); + next_WP = current_loc; break; case GUIDED: + yaw_mode = YAW_AUTO; + roll_pitch_mode = ROLL_PITCH_AUTO; + throttle_mode = THROTTLE_AUTO; + + xtrack_enabled = true; init_throttle_cruise(); - set_next_WP(&guided_WP); + next_WP = current_loc; break; case RTL: + yaw_mode = RTL_YAW; + roll_pitch_mode = RTL_RP; + throttle_mode = RTL_THR; + + xtrack_enabled = true; init_throttle_cruise(); do_RTL(); break; @@ -497,7 +536,7 @@ init_throttle_cruise() { // are we moving from manual throttle to auto_throttle? if((old_control_mode <= SIMPLE) && (g.rc_3.control_in > 150)){ - g.pid_throttle.reset_I(); + g.pi_throttle.reset_I(); g.throttle_cruise.set_and_save(g.rc_3.control_in); } } diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index 8abc79df82..3baaf3cc08 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -210,68 +210,6 @@ test_radio(uint8_t argc, const Menu::arg *argv) } } -/* -static int8_t -test_wp_nav(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - dTnav = 200; - current_loc.lat = 32.9513090 * t7; - current_loc.lng = -117.2381700 * t7; - - do_loiter_at_location(); - - wp_control = LOITER_MODE; - - //dTnav: 0, gs: 305, err: 145, int: 0, pitch: 28508160 gps_GC: 0, gps_GS: 305 - while(1){ - read_radio(); - delay(dTnav); - - current_loc.lng = (-117.2381700 * t7) + g.rc_1.control_in / 2; - current_loc.lat = (32.9513090 * t7) + g.rc_2.control_in / 2; - - navigate(); - update_nav_wp(); - - Serial.printf("Lon_e: %ld, nLon, %ld, Lat_e %ld, nLat %ld\n", long_error, nav_lon, lat_error, nav_lat); - - if(Serial.available() > 0){ - return (0); - } - } -} - -//*/ - -/* -{ - print_hit_enter(); - delay(1000); - - g.rc_6.set_range(0,900); - g.rc_4.set_angle(9000); - dTnav = 200; - - //dTnav: 0, gs: 305, err: 145, int: 0, pitch: 28508160 gps_GC: 0, gps_GS: 305 - - while(1){ - delay(20); - read_radio(); - target_bearing = 0; - g_gps->ground_course = g.rc_4.control_in; - g_gps->ground_speed = g.rc_6.control_in; - calc_rate_nav(); - Serial.printf(" gps_GC: %ld, gps_GS: %d\n", g_gps->ground_course, g_gps->ground_speed); - - if(Serial.available() > 0){ - return (0); - } - } -} -*/ - static int8_t test_failsafe(uint8_t argc, const Menu::arg *argv) { @@ -340,7 +278,7 @@ test_stabilize(uint8_t argc, const Menu::arg *argv) init_rc_in(); control_mode = STABILIZE; - Serial.printf_P(PSTR("g.pid_stabilize_roll.kP: %4.4f\n"), g.pid_stabilize_roll.kP()); + Serial.printf_P(PSTR("g.pi_stabilize_roll.kP: %4.4f\n"), g.pi_stabilize_roll.kP()); Serial.printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener); motor_auto_armed = false; @@ -509,7 +447,7 @@ test_imu(uint8_t argc, const Menu::arg *argv) accels.x, accels.y, accels.z, gyros.x, gyros.y, gyros.z); */ - /* + ///* Serial.printf_P(PSTR("cp: %1.2f, sp: %1.2f, cr: %1.2f, sr: %1.2f, cy: %1.2f, sy: %1.2f,\n"), cos_pitch_x, sin_pitch_y, @@ -736,6 +674,9 @@ test_tuning(uint8_t argc, const Menu::arg *argv) #elif CHANNEL_6_TUNING == CH6_THROTTLE_KD Serial.printf_P(PSTR("baro kD: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); + #elif CHANNEL_6_TUNING == CH6_TRAVERSE_SPEED + Serial.printf_P(PSTR("traverse: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0)); + //Extras #elif CHANNEL_6_TUNING == CH6_TOP_BOTTOM_RATIO @@ -874,6 +815,7 @@ test_baro(uint8_t argc, const Menu::arg *argv) delay(100); baro_alt = read_barometer(); Serial.printf_P(PSTR("Baro: %dcm\n"), baro_alt); + //Serial.printf_P(PSTR("Baro, %d, %ld, %ld, %ld, %ld\n"), baro_alt, barometer.RawTemp, barometer.RawTemp2, barometer.RawPress, barometer.RawPress2); if(Serial.available() > 0){ return (0); }