From 6fd7c1dcdad77c5cdc302c774b1ef53301329154 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Wed, 26 Oct 2011 09:46:16 -0700 Subject: [PATCH 01/13] AP_PI AP_Var change using the If statement rather than Max to avoid potential AP_Var issues. I don't know if this is a real prob or not. Just being careful. --- libraries/APM_PI/APM_PI.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/libraries/APM_PI/APM_PI.cpp b/libraries/APM_PI/APM_PI.cpp index 6fd710a32a..5b18fc4ed7 100644 --- a/libraries/APM_PI/APM_PI.cpp +++ b/libraries/APM_PI/APM_PI.cpp @@ -11,8 +11,12 @@ long APM_PI::get_pi(int32_t error, float dt) { _integrator += ((float)error * _ki) * dt; - _integrator = min(_integrator, (float)_imax); - _integrator = max(_integrator, (float)-_imax); + + if (_integrator < -_imax) { + _integrator = -_imax; + } else if (_integrator > _imax) { + _integrator = _imax; + } return (float)error * _kp + _integrator; } From 76bf784f8c6447dee3892388420bf0a79b75d277 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Thu, 27 Oct 2011 11:34:00 -0700 Subject: [PATCH 02/13] RC_Channel Dead zone Added APVar dead_zone to the RC_Channel library so you could edit it in the Mission planner Made CH filtering off by default. --- ArduCopter/control_modes.pde | 4 ++++ ArduCopter/radio.pde | 12 +++++++----- libraries/RC_Channel/RC_Channel.cpp | 2 +- libraries/RC_Channel/RC_Channel.h | 8 +++++--- 4 files changed, 17 insertions(+), 9 deletions(-) diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index 438bec2d6b..3d604f612d 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -139,8 +139,12 @@ static void auto_trim() led_mode = NORMAL_LEDS; clear_leds(); imu.save(); + //Serial.println("Done"); auto_level_counter = 0; + + // set TC + init_throttle_cruise(); } } } diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index faca5889c5..649ef66774 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -4,6 +4,13 @@ // ---------------------------------------------------------------------------- static byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling +static void default_dead_zones() +{ + g.rc_1.set_dead_zone(60); + g.rc_2.set_dead_zone(60); + g.rc_3.set_dead_zone(60); + g.rc_4.set_dead_zone(200); +} static void init_rc_in() { @@ -29,14 +36,9 @@ static void init_rc_in() g.rc_4.dead_zone = 300; */ - g.rc_1.set_dead_zone(60); - g.rc_2.set_dead_zone(60); - g.rc_3.set_dead_zone(60); - g.rc_4.set_dead_zone(200); //set auxiliary ranges g.rc_5.set_range(0,1000); - g.rc_5.set_filter(false); g.rc_6.set_range(0,1000); g.rc_7.set_range(0,1000); g.rc_8.set_range(0,1000); diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 11af18e10a..d290dd01cd 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -38,7 +38,7 @@ RC_Channel::set_angle(int angle) void RC_Channel::set_dead_zone(int dzone) { - _dead_zone = abs(dzone >>1); + _dead_zone.set_and_save(abs(dzone >>1)); } void diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 9ce43fd1c0..1bbe02fd58 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -26,9 +26,10 @@ class RC_Channel{ radio_trim(&_group, 1, 1500, name ? PSTR("TRIM") : 0), radio_max (&_group, 2, 1900, name ? PSTR("MAX") : 0), _high(1), - _filter(true), + _filter(false), _reverse (&_group, 3, 1, name ? PSTR("REV") : 0), - _dead_zone(0), + _dead_zone (&_group, 4, 0, name ? PSTR("DZ") : 0), + //_dead_zone(0), scale_output(1.0) {} @@ -97,7 +98,8 @@ class RC_Channel{ bool _filter; AP_Int8 _reverse; - int16_t _dead_zone; // used to keep noise down and create a dead zone. + AP_Int16 _dead_zone; + //int16_t _dead_zone; // used to keep noise down and create a dead zone. uint8_t _type; int16_t _high; int16_t _low; From d4e5281a4c2a111011cedbd5b72163aba0278e55 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Thu, 27 Oct 2011 11:35:02 -0700 Subject: [PATCH 03/13] Deafaut dead_zone call Sets the correct default values for the RC_Channel group for Quads. --- ArduCopter/system.pde | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 2b287016f5..8580808fcb 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -126,6 +126,9 @@ static void init_ardupilot() // save the new format version g.format_version.set_and_save(Parameters::k_format_version); + // save default radio values + default_dead_zones(); + Serial.printf_P(PSTR("Please Run Setup...\n")); while (true) { delay(1000); @@ -142,6 +145,8 @@ static void init_ardupilot() } }else{ + // save default radio values + //default_dead_zones(); // Load all auto-loaded EEPROM variables AP_Var::load_all(); From a076b455069495fa27500fbdc7a8345c6380f1d6 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Thu, 27 Oct 2011 11:52:00 -0700 Subject: [PATCH 04/13] Added Acro Params Allows user's to fly separate PI loops settings for Acro --- ArduCopter/Parameters.h | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index e24a43ed47..98d1a7325d 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -156,7 +156,7 @@ public: // // 240: PI/D Controllers // - k_param_pi_rate_roll = 240, + k_param_pi_rate_roll = 235, k_param_pi_rate_pitch, k_param_pi_rate_yaw, k_param_pi_stabilize_roll, @@ -169,6 +169,8 @@ public: k_param_pi_alt_hold, k_param_pi_throttle, k_param_pi_crosstrack, + k_param_pi_acro_roll, + k_param_pi_acro_pitch, // 254,255: reserved @@ -286,6 +288,9 @@ public: APM_PI pi_throttle; APM_PI pi_crosstrack; + APM_PI pi_acro_roll; + APM_PI pi_acro_pitch; + uint8_t junk; // Note: keep initializers here in the same order as they are declared above. @@ -396,6 +401,9 @@ public: pi_throttle (k_param_pi_throttle, PSTR("THR_RATE_"), THROTTLE_P, THROTTLE_I, THROTTLE_IMAX), pi_crosstrack (k_param_pi_crosstrack, PSTR("XTRACK_"), XTRACK_P, XTRACK_I, XTRACK_IMAX), + pi_acro_roll (k_param_pi_acro_roll, PSTR("ACRO_RLL_"), ACRO_ROLL_P, ACRO_ROLL_I, ACRO_ROLL_IMAX * 100), + pi_acro_pitch (k_param_pi_acro_pitch, PSTR("ACRO_PIT_"), ACRO_PITCH_P, ACRO_PITCH_I, ACRO_PITCH_IMAX * 100), + junk(0) // XXX just so that we can add things without worrying about the trailing comma { } From dd23883097f8ccbf94deee791cf0f44f2dcbe4c7 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Thu, 27 Oct 2011 11:54:46 -0700 Subject: [PATCH 05/13] Motor arming delays Speeding up delays for motor arming commands - Level and Inflight leveling. --- ArduCopter/motors.pde | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 3e1f5e2fea..36eff7d7af 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -2,8 +2,8 @@ #define ARM_DELAY 10 // one second #define DISARM_DELAY 10 // one second -#define LEVEL_DELAY 120 // twelve seconds -#define AUTO_LEVEL_DELAY 150 // twentyfive seconds +#define LEVEL_DELAY 70 // twelve seconds +#define AUTO_LEVEL_DELAY 90 // twentyfive seconds // called at 10hz From 0b4a3ccadb9d9800b7b0d4c3a7e7fb2743318f25 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Thu, 27 Oct 2011 11:55:13 -0700 Subject: [PATCH 06/13] Acro PI defaults --- ArduCopter/config.h | 25 ++++++++++++++++++++++++- 1 file changed, 24 insertions(+), 1 deletion(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 7d4f8a176d..5796d6a5d8 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -382,7 +382,30 @@ #endif ////////////////////////////////////////////////////////////////////////////// -// Rate Control +// Acro Rate Control +// +#ifndef ACRO_ROLL_P +# define ACRO_ROLL_P 0.145 +#endif +#ifndef ACRO_ROLL_I +# define ACRO_ROLL_I 0.0 +#endif +#ifndef ACRO_ROLL_IMAX +# define ACRO_ROLL_IMAX 15 // degrees +#endif + +#ifndef ACRO_PITCH_P +# define ACRO_PITCH_P 0.145 +#endif +#ifndef ACRO_PITCH_I +# define ACRO_PITCH_I 0 //0.18 +#endif +#ifndef ACRO_PITCH_IMAX +# define ACRO_PITCH_IMAX 15 // degrees +#endif + +////////////////////////////////////////////////////////////////////////////// +// Stabilize Rate Control // #ifndef RATE_ROLL_P # define RATE_ROLL_P 0.145 From c6c79662051af425c36e8385008bda05b3ee5624 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Thu, 27 Oct 2011 12:31:46 -0700 Subject: [PATCH 07/13] Attitude Made the Acro mode more NG like. Should be much more nimble! Tweaked the Accel hold with sim tests. not flight tested or enabled by default. added option to set scalar in angle boost --- ArduCopter/Attitude.pde | 73 ++++++++++++++++++++++++----------------- 1 file changed, 43 insertions(+), 30 deletions(-) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index ccbd382dde..e7b232b359 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -85,7 +85,7 @@ get_stabilize_yaw(long target_angle) #define ALT_ERROR_MAX 400 static int -get_nav_throttle(long z_error) +get_nav_throttle(long z_error) { // limit error to prevent I term run up z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX); @@ -98,28 +98,41 @@ get_nav_throttle(long z_error) return (int)g.pi_throttle.get_pi(rate_error, .1); } +#define ALT_ERROR_MAX2 300 +static int +get_nav_throttle2(long z_error) +{ + if (z_error > ALT_ERROR_MAX2){ + return g.pi_throttle.kP() * 80; + + }else if (z_error < -ALT_ERROR_MAX2){ + return g.pi_throttle.kP() * -60; + + } else{ + // limit error to prevent I term run up + z_error = constrain(z_error, -ALT_ERROR_MAX2, ALT_ERROR_MAX2); + int rate_error = g.pi_alt_hold.get_pi(z_error, .1); //_p = .85 + + rate_error = rate_error - altitude_rate; + + // limit the rate + rate_error = constrain(rate_error, -100, 120); + return (int)g.pi_throttle.get_pi(rate_error, .1) + alt_hold_velocity(); + } +} + static int get_rate_roll(long target_rate) { - long error; - target_rate = constrain(target_rate, -2500, 2500); - error = (target_rate * 4.5) - (long)(degrees(omega.x) * 100.0); - target_rate = g.pi_rate_roll.get_pi(error, G_Dt); - - // output control: - return (int)constrain(target_rate, -2500, 2500); + long error = (target_rate * 3.5) - (long)(degrees(omega.x) * 100.0); + return g.pi_acro_roll.get_pi(error, G_Dt); } static int get_rate_pitch(long target_rate) { - long error; - target_rate = constrain(target_rate, -2500, 2500); - error = (target_rate * 4.5) - (long)(degrees(omega.y) * 100.0); - target_rate = g.pi_rate_pitch.get_pi(error, G_Dt); - - // output control: - return (int)constrain(target_rate, -2500, 2500); + long error = (target_rate * 3.5) - (long)(degrees(omega.y) * 100.0); + return g.pi_acro_pitch.get_pi(error, G_Dt); } static int @@ -139,7 +152,7 @@ get_rate_yaw(long target_rate) static void reset_hold_I(void) { g.pi_loiter_lat.reset_I(); - g.pi_loiter_lat.reset_I(); + g.pi_loiter_lon.reset_I(); g.pi_crosstrack.reset_I(); } @@ -189,19 +202,21 @@ get_nav_yaw_offset(int yaw_input, int reset) static int alt_hold_velocity() { #if ACCEL_ALT_HOLD == 1 + Vector3f accel_filt; + float error; + // subtract filtered Accel - float error = abs(next_WP.alt - current_loc.alt); + error = abs(next_WP.alt - current_loc.alt) - 25; + error = min(error, 50.0); + error = max(error, 0.0); + error = 1 - (error/ 50.0); - error -= 100; - error = min(error, 200.0); - error = max(error, 0.0); - error = 1 - (error/ 200.0); - float sum = accels_rot_sum / (float)accels_rot_count; + accel_filt = imu.get_accel_filtered(); + accels_rot = dcm.get_dcm_matrix() * imu.get_accel_filtered(); + int output = (accels_rot.z + 9.81) * alt_hold_gain * error; // alt_hold_gain = 12 - accels_rot_sum = 0; - accels_rot_count = 0; - - int output = (sum + 9.81) * alt_hold_gain * error; + //Serial.printf("s: %1.4f, g:%1.4f, e:%1.4f, o:%d\n",sum, alt_hold_gain, error, output); + return constrain(output, -70, 70); // fast rise //s: -17.6241, g:0.0000, e:1.0000, o:0 @@ -209,17 +224,15 @@ static int alt_hold_velocity() //s: -19.3193, g:0.0000, e:1.0000, o:0 //s: -13.1310, g:47.8700, e:1.0000, o:-158 - //Serial.printf("s: %1.4f, g:%1.4f, e:%1.4f, o:%d\n",sum, alt_hold_gain, error, output); - return output; #else return 0; #endif } -static int get_angle_boost() +static int get_angle_boost(int value) { float temp = cos_pitch_x * cos_roll_x; temp = 1.0 - constrain(temp, .5, 1.0); - return (int)(temp * g.throttle_cruise); + return (int)(temp * value); } From c8c26d8193e479923cc0411c89671c76f009df0b Mon Sep 17 00:00:00 2001 From: Jason Short Date: Thu, 27 Oct 2011 12:33:19 -0700 Subject: [PATCH 08/13] added boost scalar arguments removed accel math from Trig function into Attitude.pde --- ArduCopter/ArduCopter.pde | 15 ++++----------- 1 file changed, 4 insertions(+), 11 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index fb017f079d..21a9572c65 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1022,8 +1022,10 @@ void update_throttle_mode(void) case THROTTLE_MANUAL: if (g.rc_3.control_in > 0){ - g.rc_3.servo_out = g.rc_3.control_in + get_angle_boost(); + g.rc_3.servo_out = g.rc_3.control_in + get_angle_boost(g.rc_3.control_in); }else{ + g.pi_stabilize_roll.reset_I(); + g.pi_stabilize_pitch.reset_I(); g.pi_rate_roll.reset_I(); g.pi_rate_pitch.reset_I(); g.rc_3.servo_out = 0; @@ -1050,8 +1052,7 @@ void update_throttle_mode(void) invalid_throttle = false; } - // apply throttle control at 200 hz - g.rc_3.servo_out = g.throttle_cruise + nav_throttle + get_angle_boost() + alt_hold_velocity(); + g.rc_3.servo_out = g.throttle_cruise + nav_throttle + get_angle_boost(g.throttle_cruise); break; } } @@ -1171,14 +1172,6 @@ static void update_trig(void){ // 90° = cos_yaw: 1.00, sin_yaw: 0.00, // 180 = cos_yaw: 0.00, sin_yaw: -1.00, // 270 = cos_yaw: -1.00, sin_yaw: 0.00, - - - #if ACCEL_ALT_HOLD == 1 - Vector3f accel_filt = imu.get_accel_filtered(); - accels_rot = dcm.get_dcm_matrix() * imu.get_accel_filtered(); - accels_rot_sum += accels_rot.z; - accels_rot_count++; - #endif } // updated at 10hz From d089059f585a8e08c4ade373d953b320af17c141 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Thu, 27 Oct 2011 12:33:44 -0700 Subject: [PATCH 09/13] Set default gain to 20 --- ArduCopter/APM_Config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 328afef9b3..0847662c09 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -40,7 +40,7 @@ */ #define ACCEL_ALT_HOLD 0 // disabled by default, work in progress -#define ACCEL_ALT_HOLD_GAIN 12.0 +#define ACCEL_ALT_HOLD_GAIN 20.0 // ACCEL_ALT_HOLD 1 to enable experimental alt_hold_mode // See the config.h and defines.h files for how to set this up! From 2458fe078f0cca3e3d3fc509f8a35b49b37f9ad5 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Thu, 27 Oct 2011 12:37:33 -0700 Subject: [PATCH 10/13] Loiter updates Added new option to do rate based Loiter with lat and long values, avoiding GPS heading latency issues. --- ArduCopter/navigation.pde | 42 +++++++++++++++++++++++++++++++++------ 1 file changed, 36 insertions(+), 6 deletions(-) diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 8c4f1ff6cb..1689d84d4c 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -92,19 +92,49 @@ static void calc_loiter(int x_error, int y_error) nav_lon = constrain(nav_lon, -3500, 3500); } +static void calc_loiter2(int x_error, int y_error) +{ + static int last_x_error = 0; + static int last_y_error = 0; + + x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX); + y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX); + + 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); + + // find the rates: + x_actual_speed = (float)(last_x_error - x_error)/dTnav; + y_actual_speed = (float)(last_y_error - y_error)/dTnav; + + // save speeds + last_x_error = x_error; + last_y_error = y_error; + + y_rate_error = y_target_speed - y_actual_speed; // 413 + y_rate_error = constrain(y_rate_error, -250, 250); // added a rate error limit to keep pitching down to a minimum + nav_lat = g.pi_nav_lat.get_pi(y_rate_error, dTnav); + nav_lat = constrain(nav_lat, -3500, 3500); + + x_rate_error = x_target_speed - x_actual_speed; + x_rate_error = constrain(x_rate_error, -250, 250); + nav_lon = g.pi_nav_lon.get_pi(x_rate_error, dTnav); + nav_lon = constrain(nav_lon, -3500, 3500); +} + // nav_roll, nav_pitch static void calc_loiter_pitch_roll() { - float temp = radians((float)(9000 - (dcm.yaw_sensor))/100.0); - float _cos_yaw_x = cos(temp); - float _sin_yaw_y = sin(temp); + //float temp = radians((float)(9000 - (dcm.yaw_sensor))/100.0); + //float _cos_yaw_x = cos(temp); + //float _sin_yaw_y = sin(temp); -// Serial.printf("ys %ld, cyx %1.4f, _cyx %1.4f\n", dcm.yaw_sensor, cos_yaw_x, _cos_yaw_x); + //Serial.printf("ys %ld, cx %1.4f, _cx %1.4f | sy %1.4f, _sy %1.4f\n", dcm.yaw_sensor, cos_yaw_x, _cos_yaw_x, sin_yaw_y, _sin_yaw_y); // rotate the vector - nav_roll = (float)nav_lon * _sin_yaw_y - (float)nav_lat * _cos_yaw_x; - nav_pitch = (float)nav_lon * _cos_yaw_x + (float)nav_lat * _sin_yaw_y; + nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x; + nav_pitch = (float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y; // flip pitch because forward is negative nav_pitch = -nav_pitch; From 04b5776601cf610889035861cc9c9186543c6a20 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Thu, 27 Oct 2011 12:42:28 -0700 Subject: [PATCH 11/13] 50 rev Work in progress, revved the EEPROM --- ArduCopter/ArduCopter.pde | 2 +- ArduCopter/Parameters.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 21a9572c65..b51e0d59b3 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduCopter V2.0.49 Beta" +#define THISFIRMWARE "ArduCopter V2.0.50 Beta" /* ArduCopter Version 2.0 Beta Authors: Jason Short diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 98d1a7325d..73c5f4cb81 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/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 = 111; + static const uint16_t k_format_version = 112; // The parameter software_type is set up solely for ground station use // and identifies the software type (eg ArduPilotMega versus ArduCopterMega) From 645b9c1d481703320ac649b56232f96a4c88bb5b Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Tue, 25 Oct 2011 19:27:23 -0600 Subject: [PATCH 12/13] Initial rewrite of command logic. Changes mission structure so that conditional and immediate commands are located between associated waypoints instead of after the second waypoint. --- ArduPlane/ArduPlane.pde | 26 ++--- ArduPlane/GCS_Mavlink.pde | 24 ++--- ArduPlane/Log.pde | 8 +- ArduPlane/Parameters.h | 12 +-- ArduPlane/commands.pde | 62 ++++++++---- ArduPlane/commands_logic.pde | 135 +++++++++++++------------- ArduPlane/commands_process.pde | 170 ++++++++++++++------------------- ArduPlane/defines.h | 3 +- ArduPlane/navigation.pde | 2 +- ArduPlane/test.pde | 6 +- 10 files changed, 224 insertions(+), 224 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index fbee3da2de..2ceb2ff77a 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -266,19 +266,20 @@ static long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of pla static float nav_gain_scaler = 1; // Gain scaling for headwind/tailwind TODO: why does this variable need to be initialized to 1? static long hold_course = -1; // deg * 100 dir of plane -static byte command_must_index; // current command memory location -static byte command_may_index; // current command memory location -static byte command_must_ID; // current command ID -static byte command_may_ID; // current command ID +static byte command_index; // current command memory location +static byte nav_command_index; // active nav command memory location +static byte non_nav_command_index; // active non-nav command memory location +static byte nav_command_ID = NO_COMMAND; // active nav command ID +static byte non_nav_command_ID = NO_COMMAND; // active non-nav command ID // Airspeed // -------- static int airspeed; // m/s * 100 -static int airspeed_nudge; // m/s * 100 : additional airspeed based on throttle stick position in top 1/2 of range -static float airspeed_error; // m/s * 100 -static float airspeed_fbwB; // m/s * 100 -static long energy_error; // energy state error (kinetic + potential) for altitude hold -static long airspeed_energy_error; // kinetic portion of energy error +static int airspeed_nudge; // m/s * 100 : additional airspeed based on throttle stick position in top 1/2 of range +static float airspeed_error; // m/s * 100 +static float airspeed_fbwB; // m/s * 100 +static long energy_error; // energy state error (kinetic + potential) for altitude hold +static long airspeed_energy_error; // kinetic portion of energy error // Location Errors // --------------- @@ -360,8 +361,9 @@ static struct Location home; // home location static struct Location prev_WP; // last waypoint static struct Location current_loc; // current location static struct Location next_WP; // next waypoint -static struct Location next_command; // command preloaded -static struct Location guided_WP; // guided mode waypoint +static struct Location guided_WP; // guided mode waypoint +static struct Location next_nav_command; // command preloaded +static struct Location next_nonnav_command; // command preloaded static long target_altitude; // used for altitude management between waypoints static long offset_altitude; // used for altitude management between waypoints static bool home_is_set; // Flag for if we have g_gps lock and have set the home location @@ -739,7 +741,7 @@ static void update_current_flight_mode(void) if(control_mode == AUTO){ crash_checker(); - switch(command_must_ID){ + switch(nav_command_ID){ case MAV_CMD_NAV_TAKEOFF: if (hold_course > -1) { calc_nav_roll(); diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 23a4d4b298..22bbd0dfe3 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -532,7 +532,7 @@ static void NOINLINE send_current_waypoint(mavlink_channel_t chan) { mavlink_msg_waypoint_current_send( chan, - g.waypoint_index); + g.command_index); } static void NOINLINE send_statustext(mavlink_channel_t chan) @@ -838,7 +838,7 @@ GCS_MAVLINK::update(void) } if (waypoint_receiving && - waypoint_request_i <= (unsigned)g.waypoint_total) { + waypoint_request_i <= (unsigned)g.command_total) { send_message(MSG_NEXT_WAYPOINT); } @@ -1278,7 +1278,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_msg_waypoint_count_send( chan,msg->sysid, msg->compid, - g.waypoint_total + 1); // + home + g.command_total + 1); // + home waypoint_timelast_send = millis(); waypoint_sending = true; @@ -1304,7 +1304,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; // send waypoint - tell_command = get_wp_with_index(packet.seq); + tell_command = get_cmd_with_index(packet.seq); // set frame of waypoint uint8_t frame; @@ -1320,7 +1320,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // time that the mav should loiter in milliseconds uint8_t current = 0; // 1 (true), 0 (false) - if (packet.seq == (uint16_t)g.waypoint_index) + if (packet.seq == (uint16_t)g.command_index) current = 1; uint8_t autocontinue = 1; // 1 (true), 0 (false) @@ -1435,8 +1435,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_msg_waypoint_clear_all_decode(msg, &packet); if (mavlink_check_target(packet.target_system, packet.target_component)) break; - // clear all waypoints - g.waypoint_total.set_and_save(0); + // clear all commands + g.command_total.set_and_save(0); // note that we don't send multiple acks, as otherwise a // GCS that is doing a clear followed by a set may see @@ -1455,7 +1455,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // set current command change_command(packet.seq); - mavlink_msg_waypoint_current_send(chan, g.waypoint_index); + mavlink_msg_waypoint_current_send(chan, g.command_index); break; } @@ -1470,7 +1470,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) if (packet.count > MAX_WAYPOINTS) { packet.count = MAX_WAYPOINTS; } - g.waypoint_total.set_and_save(packet.count - 1); + g.command_total.set_and_save(packet.count - 1); waypoint_timelast_receive = millis(); waypoint_receiving = true; @@ -1645,13 +1645,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) goto mission_failed; } - set_wp_with_index(tell_command, packet.seq); + set_cmd_with_index(tell_command, packet.seq); // update waypoint receiving state machine waypoint_timelast_receive = millis(); waypoint_request_i++; - if (waypoint_request_i > (uint16_t)g.waypoint_total){ + if (waypoint_request_i > (uint16_t)g.command_total){ mavlink_msg_waypoint_ack_send( chan, msg->sysid, @@ -1988,7 +1988,7 @@ void GCS_MAVLINK::queued_waypoint_send() { if (waypoint_receiving && - waypoint_request_i <= (unsigned)g.waypoint_total) { + waypoint_request_i <= (unsigned)g.command_total) { mavlink_msg_waypoint_request_send( chan, waypoint_dest_sysid, diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index 09bfdd7cf7..310c837cef 100644 --- a/ArduPlane/Log.pde +++ b/ArduPlane/Log.pde @@ -380,15 +380,15 @@ static void Log_Write_Startup(byte type) DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(LOG_STARTUP_MSG); DataFlash.WriteByte(type); - DataFlash.WriteByte(g.waypoint_total); + DataFlash.WriteByte(g.command_total); DataFlash.WriteByte(END_BYTE); // create a location struct to hold the temp Waypoints for printing - struct Location cmd = get_wp_with_index(0); + struct Location cmd = get_cmd_with_index(0); Log_Write_Cmd(0, &cmd); - for (int i = 1; i <= g.waypoint_total; i++){ - cmd = get_wp_with_index(i); + for (int i = 1; i <= g.command_total; i++){ + cmd = get_cmd_with_index(i); Log_Write_Cmd(i, &cmd); } } diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index e831686f48..330f19bd5b 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -162,8 +162,8 @@ public: // 220: Waypoint data // k_param_waypoint_mode = 220, - k_param_waypoint_total, - k_param_waypoint_index, + k_param_command_total, + k_param_command_index, k_param_waypoint_radius, k_param_loiter_radius, @@ -254,8 +254,8 @@ public: // Waypoints // AP_Int8 waypoint_mode; - AP_Int8 waypoint_total; - AP_Int8 waypoint_index; + AP_Int8 command_total; + AP_Int8 command_index; AP_Int8 waypoint_radius; AP_Int8 loiter_radius; @@ -371,8 +371,8 @@ public: airspeed_offset (0, k_param_airspeed_offset, PSTR("ARSPD_OFFSET")), /* XXX waypoint_mode missing here */ - waypoint_total (0, k_param_waypoint_total, PSTR("WP_TOTAL")), - waypoint_index (0, k_param_waypoint_index, PSTR("WP_INDEX")), + command_total (0, k_param_command_total, PSTR("CMD_TOTAL")), + command_index (0, k_param_command_index, PSTR("CMD_INDEX")), waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")), loiter_radius (LOITER_RADIUS_DEFAULT, k_param_loiter_radius, PSTR("WP_LOITER_RAD")), diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde index e9d5ebd0aa..7e032991c1 100644 --- a/ArduPlane/commands.pde +++ b/ArduPlane/commands.pde @@ -1,22 +1,42 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* Functions in this file: + void init_commands() + void update_auto() + void reload_commands_airstart() + struct Location get_cmd_with_index(int i) + void set_cmd_with_index(struct Location temp, int i) + void increment_cmd_index() + void decrement_cmd_index() + long read_alt_to_hold() + void set_next_WP(struct Location *wp) + void set_guided_WP(void) + void init_home() +************************************************************ +*/ + static void init_commands() { - //read_EEPROM_waypoint_info(); - g.waypoint_index.set_and_save(0); - command_must_index = 0; - command_may_index = 0; - next_command.id = CMD_BLANK; + g.command_index.set_and_save(0); + nav_command_ID = NO_COMMAND; + non_nav_command_ID = NO_COMMAND; + next_nav_command.id = CMD_BLANK; } static void update_auto() { - if (g.waypoint_index >= g.waypoint_total) { + if (g.command_index >= g.command_total) { handle_no_commands(); - if(g.waypoint_total == 0) { + if(g.command_total == 0) { next_WP.lat = home.lat + 1000; // so we don't have bad calcs next_WP.lng = home.lng + 1000; // so we don't have bad calcs } + } else { + g.command_index--; + nav_command_ID = NO_COMMAND; + non_nav_command_ID = NO_COMMAND; + next_nav_command.id = CMD_BLANK; + process_next_command(); } } @@ -24,20 +44,20 @@ static void update_auto() static void reload_commands_airstart() { init_commands(); - g.waypoint_index.load(); // XXX can we assume it's been loaded already by ::load_all? - decrement_WP_index(); + g.command_index.load(); // XXX can we assume it's been loaded already by ::load_all? + decrement_cmd_index(); } // Getters // ------- -static struct Location get_wp_with_index(int i) +static struct Location get_cmd_with_index(int i) { struct Location temp; long mem; // Find out proper location in memory by using the start_byte position + the index // -------------------------------------------------------------------------------- - if (i > g.waypoint_total) { + if (i > g.command_total) { temp.id = CMD_BLANK; }else{ // read WP position @@ -70,9 +90,9 @@ static struct Location get_wp_with_index(int i) // Setters // ------- -static void set_wp_with_index(struct Location temp, int i) +static void set_cmd_with_index(struct Location temp, int i) { - i = constrain(i, 0, g.waypoint_total.get()); + i = constrain(i, 0, g.command_total.get()); uint32_t mem = WP_START_BYTE + (i * WP_SIZE); // Set altitude options bitmask @@ -101,17 +121,17 @@ static void set_wp_with_index(struct Location temp, int i) eeprom_write_dword((uint32_t *) mem, temp.lng); } -static void increment_WP_index() +static void increment_cmd_index() { - if (g.waypoint_index <= g.waypoint_total) { - g.waypoint_index.set_and_save(g.waypoint_index + 1); + if (g.command_index <= g.command_total) { + g.command_index.set_and_save(g.command_index + 1); } } -static void decrement_WP_index() +static void decrement_cmd_index() { - if (g.waypoint_index > 0) { - g.waypoint_index.set_and_save(g.waypoint_index - 1); + if (g.command_index > 0) { + g.command_index.set_and_save(g.command_index - 1); } } @@ -225,9 +245,9 @@ void init_home() gcs_send_text_fmt(PSTR("gps alt: %lu"), (unsigned long)home.alt); - // Save Home to EEPROM + // Save Home to EEPROM - Command 0 // ------------------- - set_wp_with_index(home, 0); + set_cmd_with_index(home, 0); // Save prev loc // ------------- diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 8f73520e31..996fa11f44 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -4,13 +4,13 @@ // Command Event Handlers /********************************************************************************/ static void -handle_process_must() +handle_process_nav_cmd() { // reset navigation integrators // ------------------------- reset_I(); - switch(next_command.id){ + switch(next_nav_command.id){ case MAV_CMD_NAV_TAKEOFF: do_takeoff(); @@ -46,9 +46,9 @@ handle_process_must() } static void -handle_process_may() +handle_process_condition_command() { - switch(next_command.id){ + switch(next_nonnav_command.id){ case MAV_CMD_CONDITION_DELAY: do_wait_delay(); @@ -62,15 +62,14 @@ handle_process_may() do_change_alt(); break; - /* case MAV_CMD_NAV_LAND_OPTIONS: // TODO - Add the command or equiv to MAVLink (repair in verify_may() also) + /* case MAV_CMD_NAV_LAND_OPTIONS: // TODO - Add the command or equiv to MAVLink (repair in verify_condition() also) gcs_send_text_P(SEVERITY_LOW,PSTR("Landing options set")); // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 - landing_pitch = next_command.lng * 100; - g.airspeed_cruise = next_command.alt * 100; - g.throttle_cruise = next_command.lat; - landing_distance = next_command.p1; - //landing_roll = command.lng; + landing_pitch = next_nav_command.lng * 100; + g.airspeed_cruise = next_nav_command.alt * 100; + g.throttle_cruise = next_nav_command.lat; + landing_distance = next_nav_command.p1; SendDebug_P("MSG: throttle_cruise = "); SendDebugln(g.throttle_cruise,DEC); @@ -82,9 +81,9 @@ handle_process_may() } } -static void handle_process_now() +static void handle_process_do_command() { - switch(next_command.id){ + switch(next_nonnav_command.id){ case MAV_CMD_DO_JUMP: do_jump(); @@ -118,18 +117,18 @@ static void handle_process_now() static void handle_no_commands() { - next_command = home; - next_command.alt = read_alt_to_hold(); - next_command.id = MAV_CMD_NAV_LOITER_UNLIM; + next_nav_command = home; + next_nav_command.alt = read_alt_to_hold(); + next_nav_command.id = MAV_CMD_NAV_LOITER_UNLIM; } /********************************************************************************/ // Verify command Handlers /********************************************************************************/ -static bool verify_must() +static bool verify_nav_command() // Returns true if command complete { - switch(command_must_ID) { + switch(nav_command_ID) { case MAV_CMD_NAV_TAKEOFF: return verify_takeoff(); @@ -160,15 +159,15 @@ static bool verify_must() break; default: - gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_must: Invalid or no current Nav cmd")); + gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_nav: Invalid or no current Nav cmd")); return false; break; } } -static bool verify_may() +static bool verify_condition_command() // Returns true if command complete { - switch(command_may_ID) { + switch(non_nav_command_ID) { case NO_COMMAND: break; @@ -185,7 +184,7 @@ static bool verify_may() break; default: - gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_may: Invalid or no current Condition cmd")); + gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Invalid or no current Condition cmd")); break; } return false; @@ -212,12 +211,12 @@ static void do_RTL(void) static void do_takeoff() { - set_next_WP(&next_command); + set_next_WP(&next_nav_command); // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 - takeoff_pitch = (int)next_command.p1 * 100; + takeoff_pitch = (int)next_nav_command.p1 * 100; //Serial.printf_P(PSTR("TO pitch:")); Serial.println(takeoff_pitch); //Serial.printf_P(PSTR("home.alt:")); Serial.println(home.alt); - takeoff_altitude = next_command.alt; + takeoff_altitude = next_nav_command.alt; //Serial.printf_P(PSTR("takeoff_altitude:")); Serial.println(takeoff_altitude); next_WP.lat = home.lat + 1000; // so we don't have bad calcs next_WP.lng = home.lng + 1000; // so we don't have bad calcs @@ -227,30 +226,30 @@ static void do_takeoff() static void do_nav_wp() { - set_next_WP(&next_command); + set_next_WP(&next_nav_command); } static void do_land() { - set_next_WP(&next_command); + set_next_WP(&next_nav_command); } static void do_loiter_unlimited() { - set_next_WP(&next_command); + set_next_WP(&next_nav_command); } static void do_loiter_turns() { - set_next_WP(&next_command); - loiter_total = next_command.p1 * 360; + set_next_WP(&next_nav_command); + loiter_total = next_nav_command.p1 * 360; } static void do_loiter_time() { - set_next_WP(&next_command); + set_next_WP(&next_nav_command); loiter_time = millis(); - loiter_time_max = next_command.p1; // units are (seconds * 10) + loiter_time_max = next_nav_command.p1; // units are (seconds * 10) } /********************************************************************************/ @@ -287,7 +286,7 @@ static bool verify_takeoff() static bool verify_land() { - // we don't verify landing - we never go to a new Must command after Land + // we don't verify landing - we never go to a new Nav command after Land if (((wp_distance > 0) && (wp_distance <= (2*g_gps->ground_speed/100))) || (current_loc.alt <= next_WP.alt + 300)){ @@ -317,7 +316,7 @@ static bool verify_nav_wp() hold_course = -1; update_crosstrack(); if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) { - gcs_send_text_fmt(PSTR("Reached Waypoint #%i"),command_must_index); + gcs_send_text_fmt(PSTR("Reached Waypoint #%i"),nav_command_index); return true; } // add in a more complex case @@ -341,7 +340,7 @@ static bool verify_loiter_time() update_loiter(); calc_bearing_error(); if ((millis() - loiter_time) > (unsigned long)loiter_time_max * 10000l) { // scale loiter_time_max from (sec*10) to milliseconds - gcs_send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete")); + gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER time complete")); return true; } return false; @@ -353,7 +352,7 @@ static bool verify_loiter_turns() calc_bearing_error(); if(loiter_sum > loiter_total) { loiter_total = 0; - gcs_send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER orbits complete")); + gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER orbits complete")); // clear the command queue; return true; } @@ -377,13 +376,13 @@ static bool verify_RTL() static void do_wait_delay() { condition_start = millis(); - condition_value = next_command.lat * 1000; // convert to milliseconds + condition_value = next_nonnav_command.lat * 1000; // convert to milliseconds } static void do_change_alt() { - condition_rate = next_command.lat; - condition_value = next_command.alt; + condition_rate = next_nonnav_command.lat; + condition_value = next_nonnav_command.alt; target_altitude = current_loc.alt + (condition_rate / 10); // Divide by ten for 10Hz update next_WP.alt = condition_value; // For future nav calculations offset_altitude = 0; // For future nav calculations @@ -391,7 +390,7 @@ static void do_change_alt() static void do_within_distance() { - condition_value = next_command.lat; + condition_value = next_nonnav_command.lat; } /********************************************************************************/ @@ -438,53 +437,55 @@ static void do_loiter_at_location() static void do_jump() { struct Location temp; - if(next_command.lat > 0) { + if(next_nonnav_command.lat > 0) { - command_must_index = 0; - command_may_index = 0; - temp = get_wp_with_index(g.waypoint_index); - temp.lat = next_command.lat - 1; // Decrement repeat counter + nav_command_ID = NO_COMMAND; + non_nav_command_ID = NO_COMMAND; + temp = get_cmd_with_index(g.command_index); + temp.lat = next_nonnav_command.lat - 1; // Decrement repeat counter - set_wp_with_index(temp, g.waypoint_index); - g.waypoint_index.set_and_save(next_command.p1 - 1); - } else if (next_command.lat == -1) { - g.waypoint_index.set_and_save(next_command.p1 - 1); + set_cmd_with_index(temp, g.command_index); + g.command_index.set_and_save(next_nonnav_command.p1 - 1); + } else if (next_nonnav_command.lat == -1) { // A repeat count of -1 = repeat forever + nav_command_ID = NO_COMMAND; + non_nav_command_ID = NO_COMMAND; + g.command_index.set_and_save(next_nonnav_command.p1 - 1); } } static void do_change_speed() { // Note: we have no implementation for commanded ground speed, only air speed and throttle - if(next_command.alt > 0) - g.airspeed_cruise.set_and_save(next_command.alt * 100); + if(next_nonnav_command.alt > 0) + g.airspeed_cruise.set_and_save(next_nonnav_command.alt * 100); - if(next_command.lat > 0) - g.throttle_cruise.set_and_save(next_command.lat); + if(next_nonnav_command.lat > 0) + g.throttle_cruise.set_and_save(next_nonnav_command.lat); } static void do_set_home() { - if(next_command.p1 == 1 && GPS_enabled) { + if(next_nonnav_command.p1 == 1 && GPS_enabled) { init_home(); } else { home.id = MAV_CMD_NAV_WAYPOINT; - home.lng = next_command.lng; // Lon * 10**7 - home.lat = next_command.lat; // Lat * 10**7 - home.alt = max(next_command.alt, 0); + home.lng = next_nonnav_command.lng; // Lon * 10**7 + home.lat = next_nonnav_command.lat; // Lat * 10**7 + home.alt = max(next_nonnav_command.alt, 0); home_is_set = true; } } static void do_set_servo() { - APM_RC.OutputCh(next_command.p1 - 1, next_command.alt); + APM_RC.OutputCh(next_nonnav_command.p1 - 1, next_nonnav_command.alt); } static void do_set_relay() { - if (next_command.p1 == 1) { + if (next_nonnav_command.p1 == 1) { relay.on(); - } else if (next_command.p1 == 0) { + } else if (next_nonnav_command.p1 == 0) { relay.off(); }else{ relay.toggle(); @@ -493,16 +494,16 @@ static void do_set_relay() static void do_repeat_servo() { - event_id = next_command.p1 - 1; + event_id = next_nonnav_command.p1 - 1; - if(next_command.p1 >= CH_5 + 1 && next_command.p1 <= CH_8 + 1) { + if(next_nonnav_command.p1 >= CH_5 + 1 && next_nonnav_command.p1 <= CH_8 + 1) { event_timer = 0; - event_delay = next_command.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) - event_repeat = next_command.lat * 2; - event_value = next_command.alt; + event_delay = next_nonnav_command.lng * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) + event_repeat = next_nonnav_command.lat * 2; + event_value = next_nonnav_command.alt; - switch(next_command.p1) { + switch(next_nonnav_command.p1) { case CH_5: event_undo_value = g.rc_5.radio_trim; break; @@ -524,7 +525,7 @@ static void do_repeat_relay() { event_id = RELAY_TOGGLE; event_timer = 0; - event_delay = next_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) - event_repeat = next_command.alt * 2; + event_delay = next_nonnav_command.lat * 500.0; // /2 (half cycle time) * 1000 (convert to milliseconds) + event_repeat = next_nonnav_command.alt * 2; update_events(); } diff --git a/ArduPlane/commands_process.pde b/ArduPlane/commands_process.pde index 35d3a02e64..b1b010650f 100644 --- a/ArduPlane/commands_process.pde +++ b/ArduPlane/commands_process.pde @@ -4,14 +4,13 @@ //---------------------------------------- static void change_command(uint8_t cmd_index) { - struct Location temp = get_wp_with_index(cmd_index); + struct Location temp = get_cmd_with_index(cmd_index); if (temp.id > MAV_CMD_NAV_LAST ){ gcs_send_text_P(SEVERITY_LOW,PSTR("Bad Request - cannot change to non-Nav cmd")); } else { - command_must_index = NO_COMMAND; - next_command.id = NO_COMMAND; - g.waypoint_index.set_and_save(cmd_index - 1); - load_next_command_from_EEPROM(); + nav_command_ID = NO_COMMAND; + next_nav_command.id = NO_COMMAND; + g.command_index.set_and_save(cmd_index - 1); process_next_command(); } } @@ -21,136 +20,113 @@ static void change_command(uint8_t cmd_index) // -------------------- static void update_commands(void) { - // This function loads commands into three buffers - // when a new command is loaded, it is processed with process_XXX() - // ---------------------------------------------------------------- if(home_is_set == false){ return; // don't do commands } if(control_mode == AUTO){ - load_next_command_from_EEPROM(); process_next_command(); } // Other (eg GCS_Auto) modes may be implemented here } static void verify_commands(void) { - if(verify_must()){ - command_must_index = NO_COMMAND; + if(verify_nav_command()){ + nav_command_ID = NO_COMMAND; } - if(verify_may()){ - command_may_index = NO_COMMAND; - command_may_ID = NO_COMMAND; + if(verify_condition_command()){ + non_nav_command_ID = NO_COMMAND; } } -static void load_next_command_from_EEPROM() -{ - // fetch next command if the next command queue is empty - // ----------------------------------------------------- - if(next_command.id == NO_COMMAND){ - next_command = get_wp_with_index(g.waypoint_index + 1); - } - - // If the preload failed, return or just Loiter - // generate a dynamic command for RTL - // -------------------------------------------- - if(next_command.id == NO_COMMAND){ - // we are out of commands! - gcs_send_text_P(SEVERITY_LOW,PSTR("out of commands!")); - handle_no_commands(); - } -} static void process_next_command() { + // This function makes sure that we always have a current navigation command + // and loads conditional or immediate commands if applicable + + struct Location temp; + // these are Navigation/Must commands // --------------------------------- - if (command_must_index == 0){ // no current command loaded - if (next_command.id < MAV_CMD_NAV_LAST ){ - increment_WP_index(); - //save_command_index(); // TO DO - fix - to Recover from in air Restart - command_must_index = g.waypoint_index; - - //SendDebug_P("MSG new command_must_id "); - //SendDebug(next_command.id,DEC); - //SendDebug_P(" index:"); - //SendDebugln(command_must_index,DEC); - if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Cmd(g.waypoint_index, &next_command); - process_must(); + if (nav_command_ID == NO_COMMAND){ // no current navigation command loaded + + temp.id = MAV_CMD_NAV_LAST; + while(temp.id >= MAV_CMD_NAV_LAST && nav_command_index <= g.command_total) { + nav_command_index++; + temp = get_cmd_with_index(nav_command_index); + } + if(nav_command_index > g.command_total){ + // we are out of commands! + gcs_send_text_P(SEVERITY_LOW,PSTR("out of commands!")); + handle_no_commands(); + } else { + next_nav_command = temp; + nav_command_ID = next_nav_command.id; + non_nav_command_index = NO_COMMAND; // This will cause the next intervening non-nav command (if any) to be loaded + non_nav_command_ID = NO_COMMAND; + + if (g.log_bitmask & MASK_LOG_CMD) { + Log_Write_Cmd(g.command_index, &next_nav_command); + } + process_nav_cmd(); } } - // these are Condition/May commands - // ---------------------- - if (command_may_index == 0){ - if (next_command.id > MAV_CMD_NAV_LAST && next_command.id < MAV_CMD_CONDITION_LAST ){ - increment_WP_index(); // this command is from the command list in EEPROM - command_may_index = g.waypoint_index; - //SendDebug_P("MSG new command_may_id "); - //SendDebug(next_command.id,DEC); - //Serial.printf_P(PSTR("new command_may_index ")); - //Serial.println(command_may_index,DEC); - if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Cmd(g.waypoint_index, &next_command); - process_may(); + // these are Condition/May and Do/Now commands + // ------------------------------------------- + if (non_nav_command_index == NO_COMMAND) { // If the index is NO_COMMAND then we have just loaded a nav command + non_nav_command_index = nav_command_index + 1; + } else if (non_nav_command_ID == NO_COMMAND) { // If the ID is NO_COMMAND then we have just completed a non-nav command + non_nav_command_index++; + } + + if(nav_command_index < g.command_total && non_nav_command_ID == NO_COMMAND) { + temp = get_cmd_with_index(non_nav_command_index); + if(temp.id <= MAV_CMD_NAV_LAST) { // The next command is a nav command. No non-nav commands to do + g.command_index.set_and_save(nav_command_index); + non_nav_command_index = nav_command_index; + non_nav_command_ID = WAIT_COMMAND; + } else { // The next command is a non-nav command. Prepare to execute it. + g.command_index.set_and_save(non_nav_command_index); + next_nonnav_command = temp; + non_nav_command_ID = next_nonnav_command.id; + if (g.log_bitmask & MASK_LOG_CMD) { + Log_Write_Cmd(g.command_index, &next_nonnav_command); + } + + process_non_nav_command(); } - // these are Do/Now commands - // --------------------------- - if (next_command.id > MAV_CMD_CONDITION_LAST){ - increment_WP_index(); // this command is from the command list in EEPROM - //SendDebug_P("MSG new command_now_id "); - //SendDebug(next_command.id,DEC); - if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Cmd(g.waypoint_index, &next_command); - process_now(); - } } } /**************************************************/ // These functions implement the commands. /**************************************************/ -static void process_must() +static void process_nav_cmd() { - gcs_send_text_P(SEVERITY_LOW,PSTR("New cmd: ")); + gcs_send_text_P(SEVERITY_LOW,PSTR("New nav command loaded")); - // clear May indexes - command_may_index = NO_COMMAND; - command_may_ID = NO_COMMAND; + // clear non-nav command ID and index + non_nav_command_index = NO_COMMAND; // Redundant - remove? + non_nav_command_ID = NO_COMMAND; // Redundant - remove? - command_must_ID = next_command.id; - handle_process_must(); + handle_process_nav_cmd(); - // invalidate command so a new one is loaded - // ----------------------------------------- - next_command.id = NO_COMMAND; } -static void process_may() +static void process_non_nav_command() { - gcs_send_text_P(SEVERITY_LOW,PSTR("")); + gcs_send_text_P(SEVERITY_LOW,PSTR("new non-nav command loaded")); - command_may_ID = next_command.id; - handle_process_may(); - - // invalidate command so a new one is loaded - // ----------------------------------------- - next_command.id = NO_COMMAND; + if(non_nav_command_ID < MAV_CMD_CONDITION_LAST) { + handle_process_condition_command(); + } else { + handle_process_do_command(); + // flag command ID so a new one is loaded + // ----------------------------------------- + non_nav_command_ID = NO_COMMAND; + } } - -static void process_now() -{ - handle_process_now(); - - // invalidate command so a new one is loaded - // ----------------------------------------- - next_command.id = NO_COMMAND; - - gcs_send_text(SEVERITY_LOW, ""); -} - diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index b049d477bd..5117f44156 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -77,6 +77,7 @@ // Commands - Note that APM now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library #define CMD_BLANK 0 // there is no command stored in the mem location requested #define NO_COMMAND 0 +#define WAIT_COMMAND 999 // Command/Waypoint/Location Options Bitmask //-------------------- @@ -169,7 +170,7 @@ enum gcs_severity { // Events // ------ #define EVENT_WILL_REACH_WAYPOINT 1 -#define EVENT_SET_NEW_WAYPOINT_INDEX 2 +#define EVENT_SET_NEW_COMMAND_INDEX 2 #define EVENT_LOADED_WAYPOINT 3 #define EVENT_LOOP 4 diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index 3c67117767..2f1e803c7a 100644 --- a/ArduPlane/navigation.pde +++ b/ArduPlane/navigation.pde @@ -100,7 +100,7 @@ static void calc_altitude_error() }else{ target_altitude = constrain(target_altitude, prev_WP.alt, next_WP.alt); } - } else if (command_may_ID != MAV_CMD_CONDITION_CHANGE_ALT) { + } else if (non_nav_command_ID != MAV_CMD_CONDITION_CHANGE_ALT) { target_altitude = next_WP.alt; } diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde index 9c0adbd26d..b60876b509 100644 --- a/ArduPlane/test.pde +++ b/ArduPlane/test.pde @@ -306,12 +306,12 @@ test_wp(uint8_t argc, const Menu::arg *argv) Serial.printf_P(PSTR("Hold altitude of %dm\n"), (int)g.RTL_altitude/100); } - Serial.printf_P(PSTR("%d waypoints\n"), (int)g.waypoint_total); + Serial.printf_P(PSTR("%d waypoints\n"), (int)g.command_total); Serial.printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius); Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius); - for(byte i = 0; i <= g.waypoint_total; i++){ - struct Location temp = get_wp_with_index(i); + for(byte i = 0; i <= g.command_total; i++){ + struct Location temp = get_cmd_with_index(i); test_wp_print(&temp, i); } From cbf59e96bd320fe3b9778ed3c9ba0f5b67934cef Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Thu, 27 Oct 2011 13:43:19 -0600 Subject: [PATCH 13/13] Bug fixes for command logic re-write --- ArduPlane/commands.pde | 5 ++++- ArduPlane/commands_logic.pde | 13 +++++++++++++ ArduPlane/commands_process.pde | 21 ++++++++++++++++----- ArduPlane/defines.h | 2 +- 4 files changed, 34 insertions(+), 7 deletions(-) diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde index 7e032991c1..a9f7934e74 100644 --- a/ArduPlane/commands.pde +++ b/ArduPlane/commands.pde @@ -32,7 +32,10 @@ static void update_auto() next_WP.lng = home.lng + 1000; // so we don't have bad calcs } } else { - g.command_index--; + if(g.command_index != 0) { + g.command_index = nav_command_index; + nav_command_index--; + } nav_command_ID = NO_COMMAND; non_nav_command_ID = NO_COMMAND; next_nav_command.id = CMD_BLANK; diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 996fa11f44..11145912d8 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -10,6 +10,7 @@ handle_process_nav_cmd() // ------------------------- reset_I(); + gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nav_command.id); switch(next_nav_command.id){ case MAV_CMD_NAV_TAKEOFF: @@ -48,6 +49,7 @@ handle_process_nav_cmd() static void handle_process_condition_command() { + gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nonnav_command.id); switch(next_nonnav_command.id){ case MAV_CMD_CONDITION_DELAY: @@ -83,6 +85,7 @@ handle_process_condition_command() static void handle_process_do_command() { + gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nonnav_command.id); switch(next_nonnav_command.id){ case MAV_CMD_DO_JUMP: @@ -117,9 +120,14 @@ static void handle_process_do_command() static void handle_no_commands() { + gcs_send_text_fmt(PSTR("Returning to Home")); next_nav_command = home; next_nav_command.alt = read_alt_to_hold(); next_nav_command.id = MAV_CMD_NAV_LOITER_UNLIM; + nav_command_ID = MAV_CMD_NAV_LOITER_UNLIM; + non_nav_command_ID = WAIT_COMMAND; + handle_process_nav_cmd(); + } /********************************************************************************/ @@ -182,6 +190,11 @@ static bool verify_condition_command() // Returns true if command complete case MAV_CMD_CONDITION_CHANGE_ALT: return verify_change_alt(); break; + + case WAIT_COMMAND: + return 0; + break; + default: gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Invalid or no current Condition cmd")); diff --git a/ArduPlane/commands_process.pde b/ArduPlane/commands_process.pde index b1b010650f..5ef18b36b1 100644 --- a/ArduPlane/commands_process.pde +++ b/ArduPlane/commands_process.pde @@ -8,8 +8,11 @@ static void change_command(uint8_t cmd_index) if (temp.id > MAV_CMD_NAV_LAST ){ gcs_send_text_P(SEVERITY_LOW,PSTR("Bad Request - cannot change to non-Nav cmd")); } else { + gcs_send_text_fmt(PSTR("Received Request - jump to command #%i"),cmd_index); nav_command_ID = NO_COMMAND; next_nav_command.id = NO_COMMAND; + non_nav_command_ID = NO_COMMAND; + nav_command_index = cmd_index - 1; g.command_index.set_and_save(cmd_index - 1); process_next_command(); } @@ -47,16 +50,18 @@ static void process_next_command() // and loads conditional or immediate commands if applicable struct Location temp; + byte old_index; // these are Navigation/Must commands // --------------------------------- if (nav_command_ID == NO_COMMAND){ // no current navigation command loaded - + old_index = nav_command_index; temp.id = MAV_CMD_NAV_LAST; while(temp.id >= MAV_CMD_NAV_LAST && nav_command_index <= g.command_total) { nav_command_index++; temp = get_cmd_with_index(nav_command_index); } + gcs_send_text_fmt(PSTR("Nav command index updated to #%i"),nav_command_index); if(nav_command_index > g.command_total){ // we are out of commands! gcs_send_text_P(SEVERITY_LOW,PSTR("out of commands!")); @@ -77,21 +82,27 @@ static void process_next_command() // these are Condition/May and Do/Now commands // ------------------------------------------- if (non_nav_command_index == NO_COMMAND) { // If the index is NO_COMMAND then we have just loaded a nav command - non_nav_command_index = nav_command_index + 1; + non_nav_command_index = old_index + 1; + //gcs_send_text_fmt(PSTR("Non-Nav command index #%i"),non_nav_command_index); } else if (non_nav_command_ID == NO_COMMAND) { // If the ID is NO_COMMAND then we have just completed a non-nav command non_nav_command_index++; } - if(nav_command_index < g.command_total && non_nav_command_ID == NO_COMMAND) { + //gcs_send_text_fmt(PSTR("Nav command index #%i"),nav_command_index); + //gcs_send_text_fmt(PSTR("Non-Nav command index #%i"),non_nav_command_index); + //gcs_send_text_fmt(PSTR("Non-Nav command ID #%i"),non_nav_command_ID); + if(nav_command_index <= (int)g.command_total && non_nav_command_ID == NO_COMMAND) { temp = get_cmd_with_index(non_nav_command_index); if(temp.id <= MAV_CMD_NAV_LAST) { // The next command is a nav command. No non-nav commands to do g.command_index.set_and_save(nav_command_index); non_nav_command_index = nav_command_index; non_nav_command_ID = WAIT_COMMAND; + gcs_send_text_fmt(PSTR("Non-Nav command ID updated to #%i"),non_nav_command_ID); } else { // The next command is a non-nav command. Prepare to execute it. g.command_index.set_and_save(non_nav_command_index); next_nonnav_command = temp; non_nav_command_ID = next_nonnav_command.id; + gcs_send_text_fmt(PSTR("Non-Nav command ID updated to #%i"),non_nav_command_ID); if (g.log_bitmask & MASK_LOG_CMD) { Log_Write_Cmd(g.command_index, &next_nonnav_command); } @@ -107,7 +118,7 @@ static void process_next_command() /**************************************************/ static void process_nav_cmd() { - gcs_send_text_P(SEVERITY_LOW,PSTR("New nav command loaded")); + //gcs_send_text_P(SEVERITY_LOW,PSTR("New nav command loaded")); // clear non-nav command ID and index non_nav_command_index = NO_COMMAND; // Redundant - remove? @@ -119,7 +130,7 @@ static void process_nav_cmd() static void process_non_nav_command() { - gcs_send_text_P(SEVERITY_LOW,PSTR("new non-nav command loaded")); + //gcs_send_text_P(SEVERITY_LOW,PSTR("new non-nav command loaded")); if(non_nav_command_ID < MAV_CMD_CONDITION_LAST) { handle_process_condition_command(); diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 5117f44156..19a3dc5ba5 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -77,7 +77,7 @@ // Commands - Note that APM now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library #define CMD_BLANK 0 // there is no command stored in the mem location requested #define NO_COMMAND 0 -#define WAIT_COMMAND 999 +#define WAIT_COMMAND 255 // Command/Waypoint/Location Options Bitmask //--------------------