diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index aa5adbeee9..2f6a5dc6cc 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -69,6 +69,3 @@ // enable this for the new 'APM2' hardware // #define CONFIG_APM_HARDWARE APM_HARDWARE_APM2 // #define APM2_BETA_HARDWARE // for developers who received an early beta board with the older BMP085 - -#define LOITER_METHOD 0 -// set to 1 to try an alternative Loiter control method diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 7efbfa4191..7b26e8d9bf 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduCopter V2.1.1r3 alpha" +#define THISFIRMWARE "ArduCopter V2.1.1r4 alpha" /* ArduCopter Version 2.0 Beta Authors: Jason Short @@ -366,6 +366,7 @@ static bool nav_ok; static const float radius_of_earth = 6378100; // meters static const float gravity = 9.81; // meters/ sec^2 static int32_t target_bearing; // deg * 100 : 0 to 360 location of the plane to the target +static int32_t nav_bearing; // deg * 100 : 0 to 360 location of the plane to the target static int32_t home_bearing; // used to track difference in angle static byte wp_control; // used to control - navgation or loiter @@ -1390,8 +1391,6 @@ static void update_navigation() // reset LOITER to current position next_WP = current_loc; - // clear Loiter Iterms - reset_nav(); }else{ // this is also set by GPS in update_nav wp_control = LOITER_MODE; @@ -1762,7 +1761,8 @@ static void update_nav_wp() // use error as the desired rate towards the target calc_nav_rate(g.waypoint_speed_max); // rotate pitch and roll to the copter frame of reference - calc_nav_pitch_roll(); + //calc_nav_pitch_roll(); + calc_loiter_pitch_roll(); }else if(wp_control == NO_NAV_MODE){ nav_roll = 0; @@ -1772,11 +1772,16 @@ static void update_nav_wp() static void update_auto_yaw() { + // If we Loiter, don't start Yawing, allow Yaw control + if(wp_control == LOITER_MODE) + return; + // this tracks a location so the copter is always pointing towards it. if(yaw_tracking == MAV_ROI_LOCATION){ auto_yaw = get_bearing(¤t_loc, &target_WP); }else if(yaw_tracking == MAV_ROI_WPNEXT){ + // Point towards next WP auto_yaw = target_bearing; } // MAV_ROI_NONE = basic Yaw hold diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index f694621898..2501554847 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -243,8 +243,8 @@ static void reset_nav(void) g.pi_nav_lon.reset_I(); // considering not reseting wind control - //g.pi_loiter_lat.reset_I(); - //g.pi_loiter_lon.reset_I(); + g.pi_loiter_lat.reset_I(); + g.pi_loiter_lon.reset_I(); circle_angle = 0; crosstrack_error = 0; diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 06397209f9..ad02ff3c3e 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -73,11 +73,11 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack } uint8_t status = MAV_STATE_ACTIVE; - + if (!motor_armed) { status = MAV_STATE_STANDBY; } - + uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000 mavlink_msg_sys_status_send( @@ -116,12 +116,12 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) chan, nav_roll / 1.0e2, nav_pitch / 1.0e2, + nav_bearing / 1.0e2, target_bearing / 1.0e2, - dcm.yaw_sensor / 1.0e2, // was target_bearing wp_distance, altitude_error / 1.0e2, - nav_lon, // was 0 - nav_lat); // was 0 + 0, + crosstrack_error); // was 0 } static void NOINLINE send_gps_raw(mavlink_channel_t chan) diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index abbb50d5a2..e5e68693ec 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -329,7 +329,7 @@ public: waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")), loiter_radius (LOITER_RADIUS, k_param_loiter_radius, PSTR("WP_LOITER_RAD")), waypoint_speed_max (WAYPOINT_SPEED_MAX, k_param_waypoint_speed_max, PSTR("WP_SPEED_MAX")), - crosstrack_gain (CROSSTRACK_GAIN, k_param_crosstrack_gain, PSTR("XTRK_GAIN_SC")), + crosstrack_gain (CROSSTRACK_GAIN * 100, k_param_crosstrack_gain, PSTR("XTRK_GAIN_SC")), throttle_min (0, k_param_throttle_min, PSTR("THR_MIN")), throttle_max (1000, k_param_throttle_max, PSTR("THR_MAX")), diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index 954cfa7c62..f65ded6797 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -8,6 +8,9 @@ static void init_commands() prev_nav_index = NO_COMMAND; command_cond_queue.id = NO_COMMAND; command_nav_queue.id = NO_COMMAND; + + // default Yaw tracking + yaw_tracking = MAV_ROI_WPNEXT; } // Getters @@ -176,6 +179,7 @@ static void set_next_WP(struct Location *wp) wp_totalDistance = get_distance(¤t_loc, &next_WP); wp_distance = wp_totalDistance; target_bearing = get_bearing(&prev_WP, &next_WP); + nav_bearing = target_bearing; // to check if we have missed the WP // --------------------------------- diff --git a/ArduCopter/commands_process.pde b/ArduCopter/commands_process.pde index eed5e236f0..0bff900e53 100644 --- a/ArduCopter/commands_process.pde +++ b/ArduCopter/commands_process.pde @@ -122,7 +122,7 @@ static void execute_nav_command(void) process_nav_command(); // clear navigation prameters - reset_nav(); + //reset_nav(); // clear May indexes to force loading of more commands // existing May commands are tossed. diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 27d4eb1716..6f15346fd4 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -508,7 +508,7 @@ #ifndef STABILIZE_D -# define STABILIZE_D .25 +# define STABILIZE_D .2 #endif // Jasons default values that are good for smaller payload motors. @@ -606,17 +606,17 @@ // Navigation control gains // #ifndef LOITER_P -# define LOITER_P .2 // .3 was too aggressive +# define LOITER_P .25 // #endif #ifndef LOITER_I -# define LOITER_I 0.05 // Wind control +# define LOITER_I 0.1 // Wind control #endif #ifndef LOITER_IMAX # define LOITER_IMAX 30 // degrees° #endif #ifndef NAV_P -# define NAV_P 1.5 // 3 was causing rapid oscillations in Loiter +# define NAV_P 2.2 // 3 was causing rapid oscillations in Loiter #endif #ifndef NAV_I # define NAV_I 0.15 // used in traverals @@ -649,7 +649,7 @@ // RATE control #ifndef THROTTLE_P -# define THROTTLE_P 0.4 // +# define THROTTLE_P 0.5 // #endif #ifndef THROTTLE_I # define THROTTLE_I 0.0 // @@ -663,7 +663,7 @@ // Crosstrack compensation // #ifndef CROSSTRACK_GAIN -# define CROSSTRACK_GAIN 40 +# define CROSSTRACK_GAIN 1 #endif diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 316458f7ba..4065aac72b 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -22,6 +22,10 @@ static byte navigate() target_bearing = get_bearing(¤t_loc, &next_WP); home_to_copter_bearing = get_bearing(&home, ¤t_loc); + // nav_bearing will includes xtrac correction + // ------------------------------------------ + nav_bearing = target_bearing; + return 1; } @@ -55,13 +59,16 @@ static void calc_XY_velocity(){ x_GPS_speed = (x_GPS_speed * 3 + x_diff) / 4; y_GPS_speed = (y_GPS_speed * 3 + y_diff) / 4; - if(g_gps->ground_speed > 120){ + // Above simply works better than GPS groundspeed + // which is proving to be problematic + + /*if(g_gps->ground_speed > 120){ // Derive X/Y speed from GPS // this is far more accurate when traveling about 1.5m/s float temp = g_gps->ground_course * RADX100; x_GPS_speed = sin(temp) * (float)g_gps->ground_speed; y_GPS_speed = cos(temp) * (float)g_gps->ground_speed; - } + }*/ last_longitude = g_gps->longitude; last_latutude = g_gps->latitude; @@ -110,7 +117,7 @@ static void calc_loiter(int x_error, int y_error) nav_lat_p = constrain(nav_lat_p, -3500, 3500); nav_lat = nav_lat_p + y_iterm; - ///* + /* int8_t ttt = 1.0/dTnav; int16_t t2 = g.pi_nav_lat.get_integrator(); @@ -119,6 +126,7 @@ static void calc_loiter(int x_error, int y_error) wp_distance, //1 y_error, //2 y_GPS_speed, //3 + y_actual_speed, //4 ; y_target_speed, //5 y_rate_error, //6 @@ -152,16 +160,24 @@ static void calc_loiter(int x_error, int y_error) static void estimate_velocity() { // we need to extimate velocity when below GPS threshold of 1.5m/s - if(g_gps->ground_speed < 150){ + //if(g_gps->ground_speed < 120){ // some smoothing to prevent bumpy rides x_actual_speed = (x_actual_speed * 15 + x_GPS_speed) / 16; y_actual_speed = (y_actual_speed * 15 + y_GPS_speed) / 16; - }else{ + // integration of nav_p angle + //x_actual_speed += (nav_lon_p >>2); + //y_actual_speed += (nav_lat_p >>2); + + // this is just what worked best in SIM + //x_actual_speed = (x_actual_speed * 2 + x_GPS_speed * 1) / 4; + //y_actual_speed = (y_actual_speed * 2 + y_GPS_speed * 1) / 4; + + //}else{ // less smoothing needed since the GPS already filters - x_actual_speed = (x_actual_speed * 3 + x_GPS_speed) / 4; - y_actual_speed = (y_actual_speed * 3 + y_GPS_speed) / 4; - } + // x_actual_speed = (x_actual_speed * 3 + x_GPS_speed) / 4; + // y_actual_speed = (y_actual_speed * 3 + y_GPS_speed) / 4; + //} } // this calculation rotates our World frame of reference to the copter's frame of reference @@ -206,32 +222,47 @@ static void calc_nav_rate(int max_speed) max_speed = min(max_speed, waypoint_speed_gov); } - float temp = (target_bearing - g_gps->ground_course) * RADX100; - // push us towards the original track update_crosstrack(); + // nav_bearing includes crosstrack + float temp = (9000 - nav_bearing) * RADX100; + // heading laterally, we want a zero speed here - x_actual_speed = -sin(temp) * (float)g_gps->ground_speed; - x_rate_error = crosstrack_error -x_actual_speed; - x_rate_error = constrain(x_rate_error, -1400, 1400); - nav_lon = constrain(g.pi_nav_lon.get_pi(x_rate_error, dTnav), -3500, 3500); - /*Serial.printf("max_sp %d,\tx_actual_sp %d,\tx_rate_err: %d, Xtrack %d, \tnav_lon: %d,\ty_actual_sp %d,\ty_rate_err: %d,\tnav_lat: %d,\n", - max_speed, - x_actual_speed, - x_rate_error, - crosstrack_error, - nav_lon, - y_actual_speed, - y_rate_error, - nav_lat); - //*/ + //x_actual_speed = -sin(temp) * (float)g_gps->ground_speed; + + x_rate_error = (cos(temp) * max_speed) - x_actual_speed; // 413 + x_rate_error = constrain(x_rate_error, -1400, 1400); + int16_t x_iterm = g.pi_loiter_lon.get_i(x_rate_error, dTnav); + + nav_lon_p = g.pi_nav_lon.get_p(x_rate_error); + nav_lon_p = constrain(nav_lon_p, -3500, 3500); + nav_lon = nav_lon_p + x_iterm; // heading towards target - y_actual_speed = cos(temp) * (float)g_gps->ground_speed; - y_rate_error = max_speed - y_actual_speed; // 413 - y_rate_error = constrain(y_rate_error, -1400, 1400); // added a rate error limit to keep pitching down to a minimum - nav_lat = constrain(g.pi_nav_lat.get_pi(y_rate_error, dTnav), -3500, 3500); + //y_actual_speed = cos(temp) * (float)g_gps->ground_speed; + + y_rate_error = (sin(temp) * max_speed) - y_actual_speed; // 413 + y_rate_error = constrain(y_rate_error, -1400, 1400); // added a rate error limit to keep pitching down to a minimum + int16_t y_iterm = g.pi_loiter_lat.get_i(y_rate_error, dTnav); + + nav_lat_p = g.pi_nav_lat.get_p(y_rate_error); + nav_lat_p = constrain(nav_lat_p, -3500, 3500); + nav_lat = nav_lat_p + y_iterm; + + /* + Serial.printf("max_sp %d,\t x_sp %d, y_sp %d,\t x_re: %d, y_re: %d, \tnav_lon: %d, nav_lat: %d, Xi:%d, Yi:%d, \t XE %d \n", + max_speed, + x_actual_speed, + y_actual_speed, + x_rate_error, + y_rate_error, + nav_lon, + nav_lat, + x_iterm, + y_iterm, + crosstrack_error); + //*/ // nav_lat and nav_lon will be rotated to the angle of the quad in calc_nav_pitch_roll() @@ -246,53 +277,21 @@ static void calc_nav_rate(int max_speed) nav_lat);*/ } + static void update_crosstrack(void) { // Crosstrack Error // ---------------- - if (cross_track_test() < 4000) { // If we are too far off or too close we don't do track following + if (abs(wrap_180(target_bearing - original_target_bearing)) < 4500) { // If we are too far off or too close we don't do track following float temp = (target_bearing - original_target_bearing) * RADX100; crosstrack_error = sin(temp) * (wp_distance * g.crosstrack_gain); // Meters we are off track line - crosstrack_error = constrain(crosstrack_error, -1200, 1200); + nav_bearing = target_bearing + constrain(crosstrack_error, -3000, 3000); + nav_bearing = wrap_360(nav_bearing); + }else{ + nav_bearing = target_bearing; } } -// used to generate the offset angle for testing crosstrack viability -static int32_t cross_track_test() -{ - int32_t temp; - temp = target_bearing - original_target_bearing; - temp = wrap_180(temp); - return abs(temp); -} - - -// this calculation is different than loiter above because we are in a different Frame of Reference. -// nav_lat is pointed towards the target, where as in Loiter, nav_lat is pointed north! -static void calc_nav_pitch_roll() -{ - int32_t angle = wrap_360(dcm.yaw_sensor - target_bearing); - float temp = (9000l - angle) * RADX100; - //t: 1.5465, t1: -10.9451, t2: 1.5359, t3: 1.5465 - float _cos_yaw_x = cos(temp); - float _sin_yaw_y = sin(temp); - - // rotate the vector - nav_roll = (float)nav_lon * _sin_yaw_y - (float)nav_lat * _cos_yaw_x; - nav_pitch = (float)nav_lon * _cos_yaw_x + (float)nav_lat * _sin_yaw_y; - - // flip pitch because forward is negative - nav_pitch = -nav_pitch; - - /*Serial.printf("Yaw %d, Tbear:%d, \tangle: %d, \t_cos_yaw_x:%1.4f, _sin_yaw_y:%1.4f, nav_roll:%d, nav_pitch:%d\n", - dcm.yaw_sensor, - target_bearing, - angle, - _cos_yaw_x, - _sin_yaw_y, - nav_roll, - nav_pitch);*/ -} static int32_t get_altitude_error() {