From 9003b9549985476f7b0b8bc42167b602829a54ad Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 21 Jan 2012 22:04:35 -0800 Subject: [PATCH 01/20] Converted to cm for distance --- ArduCopter/ArduCopter.pde | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index e17b9e6481..0d755c6a18 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -323,6 +323,9 @@ static int16_t y_actual_speed; static int16_t x_rate_error; static int16_t y_rate_error; +//static int16_t my_max_speed; // used for debugging logs +//static int16_t target_x_rate; + //////////////////////////////////////////////////////////////////////////////// // Radio //////////////////////////////////////////////////////////////////////////////// @@ -360,8 +363,6 @@ static bool rc_override_active = false; // Status flag that tracks whether we are under GCS control static uint32_t rc_override_fs_timer = 0; - - //////////////////////////////////////////////////////////////////////////////// // Heli //////////////////////////////////////////////////////////////////////////////// @@ -604,9 +605,9 @@ static int16_t landing_boost; //////////////////////////////////////////////////////////////////////////////// // The location of the copter in relation to home, updated every GPS read static int32_t home_to_copter_bearing; -// distance between plane and home in meters (not cm!!!) +// distance between plane and home in cm static int32_t home_distance; -// distance between plane and next_WP in meters (not cm!!!) +// distance between plane and next_WP in cm static int32_t wp_distance; //////////////////////////////////////////////////////////////////////////////// @@ -1532,7 +1533,7 @@ void update_throttle_mode(void) takeoff_complete = false; // reset baro data if we are near home - if(home_distance < 4 || GPS_enabled == false){ + if(home_distance < 400 || GPS_enabled == false){ // 4m from home // causes Baro to do a quick recalibration // XXX commented until further testing // reset_baro(); @@ -1602,7 +1603,7 @@ void update_throttle_mode(void) // get the AP throttle, if landing boost > 0 we are actually Landing // This allows us to grab just the compensation. if(landing_boost > 0) - nav_throttle = get_nav_throttle(-100); + nav_throttle = get_nav_throttle(-200); else nav_throttle = get_nav_throttle(altitude_error); @@ -1743,7 +1744,7 @@ static void update_navigation() // are we in SIMPLE mode? if(do_simple && g.super_simple){ // get distance to home - if(home_distance > 10){ // 10m from home + if(home_distance > 1000){ // 10m from home // we reset the angular offset to be a vector from home to the quad initial_simple_bearing = home_to_copter_bearing; //Serial.printf("ISB: %d\n", initial_simple_bearing); From 5cc19bbe7cacc454ba338fb6bb355471bc3f5c93 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 21 Jan 2012 22:04:54 -0800 Subject: [PATCH 02/20] slightly less filtering for less latency --- ArduCopter/Attitude.pde | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index e32ea0cd9d..16c6cd59fe 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -165,7 +165,8 @@ get_nav_throttle(int32_t z_error) //output -= constrain(rate_d, -25, 25); // light filter of output - output = (old_output * 3 + output) / 4; + //output = (old_output * 3 + output) / 4; + output = (old_output + output) / 2; // save our output old_output = output; From f3cc1121aa7158ebf2b1d39dee82c7bb1c557458 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 21 Jan 2012 22:05:48 -0800 Subject: [PATCH 03/20] better default for speed governer --- ArduCopter/commands.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index a5c3dd5f98..7f5b8e05c9 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -178,7 +178,7 @@ static void set_next_WP(struct Location *wp) // reset speed governer // -------------------- - waypoint_speed_gov = 0; + waypoint_speed_gov = WAYPOINT_SPEED_MIN; } From 59d1f225d5f17fb7f7d7de27e79723b1bf58f089 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 21 Jan 2012 22:06:35 -0800 Subject: [PATCH 04/20] convert command into CM from M Making Landing boost be one at minimum to trigger better navthrottle output --- ArduCopter/commands_logic.pde | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 74ce2488de..d8bd2bbd36 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -365,7 +365,7 @@ static bool verify_land() //landing_boost = min(landing_boost, 15); float tmp = (1.75 * icount * icount) - (7.2 * icount); landing_boost = tmp; - landing_boost = constrain(landing_boost, 0, 200); + landing_boost = constrain(landing_boost, 1, 200); icount += 0.4; //Serial.printf("lb:%d, %1.4f, ic:%1.4f\n",landing_boost, tmp, icount); @@ -512,7 +512,7 @@ static void do_change_alt() static void do_within_distance() { - condition_value = command_cond_queue.lat; + condition_value = command_cond_queue.lat * 100; } static void do_yaw() From 7edd16e5fe129d69ac91b61708bb140506aee64c Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 21 Jan 2012 22:09:13 -0800 Subject: [PATCH 05/20] Added Loiter_D to replace Nav_P for better separation of loiter and navigation tuning. Upped Nav_P defaults to 3 based on windy flight testing Added minimum WP speed define of 1m/s Upped loiter_I for better wind performance - was not seeing any overshoot in logs Made Nav_I default of 0, since we are not using it in the code. --- ArduCopter/config.h | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 698cdd6d75..b309f5d6fa 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -622,20 +622,23 @@ # define LOITER_P 1.5 // was .25 in previous #endif #ifndef LOITER_I -# define LOITER_I 0.04 // Wind control +# define LOITER_I 0.09 // Wind control #endif #ifndef LOITER_IMAX # define LOITER_IMAX 30 // degrees° #endif +#ifndef LOITER_D +# define LOITER_D 2.2 // rate control +#endif ////////////////////////////////////////////////////////////////////////////// // WP Navigation control gains // #ifndef NAV_P -# define NAV_P 2.2 // 3 was causing rapid oscillations in Loiter +# define NAV_P 3.0 // 3 was causing rapid oscillations in Loiter #endif #ifndef NAV_I -# define NAV_I 0.15 // used in traverals +# define NAV_I 0 // #endif #ifndef NAV_IMAX # define NAV_IMAX 30 // degrees @@ -645,6 +648,10 @@ # define WAYPOINT_SPEED_MAX 600 // for 6m/s error = 13mph #endif +#ifndef WAYPOINT_SPEED_MIN +# define WAYPOINT_SPEED_MIN 100 // for 6m/s error = 13mph +#endif + ////////////////////////////////////////////////////////////////////////////// // Throttle control gains From 2fa24e05575866ab7db4c29470ccba9248c64d77 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 21 Jan 2012 22:09:39 -0800 Subject: [PATCH 06/20] added conversions of CM to M --- ArduCopter/GCS_Mavlink copy.txt | 2 +- ArduCopter/GCS_Mavlink.pde | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ArduCopter/GCS_Mavlink copy.txt b/ArduCopter/GCS_Mavlink copy.txt index a042cc80c0..0199b455be 100644 --- a/ArduCopter/GCS_Mavlink copy.txt +++ b/ArduCopter/GCS_Mavlink copy.txt @@ -270,7 +270,7 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) nav_pitch / 1.0e2, target_bearing / 1.0e2, dcm.yaw_sensor / 1.0e2, // was target_bearing - wp_distance, + wp_distance / 1.0e2, altitude_error / 1.0e2, 0, crosstrack_error); diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index b7c2e64854..a9e1140962 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -118,7 +118,7 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) nav_pitch / 1.0e2, nav_bearing / 1.0e2, target_bearing / 1.0e2, - wp_distance, + wp_distance / 1.0e2, altitude_error / 1.0e2, 0, crosstrack_error); // was 0 @@ -1518,13 +1518,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // set dcm hil sensor dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, packet.pitchspeed,packet.yawspeed); - + // rad/sec Vector3f gyros; gyros.x = (float)packet.rollspeed; gyros.y = (float)packet.pitchspeed; gyros.z = (float)packet.yawspeed; - + imu.set_gyro(gyros); //imu.set_accel(accels); From f638a4b81dbcea5d1d63989b55044c5252b89640 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 21 Jan 2012 22:10:28 -0800 Subject: [PATCH 07/20] recording nav_bearing rather than target bearing recording x and y speed rather than Iterms for nav since they are always 0 now --- ArduCopter/Log.pde | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 57b2136ff0..090e2b3501 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -528,16 +528,28 @@ static void Log_Write_Nav_Tuning() DataFlash.WriteByte(LOG_NAV_TUNING_MSG); DataFlash.WriteInt(wp_distance); // 1 - DataFlash.WriteInt(target_bearing/100); // 2 + DataFlash.WriteInt(nav_bearing/100); // 2 DataFlash.WriteInt(long_error); // 3 DataFlash.WriteInt(lat_error); // 4 DataFlash.WriteInt(nav_lon); // 5 DataFlash.WriteInt(nav_lat); // 6 - DataFlash.WriteInt(g.pi_nav_lon.get_integrator()); // 7 - DataFlash.WriteInt(g.pi_nav_lat.get_integrator()); // 8 + DataFlash.WriteInt(x_actual_speed); // 7 + DataFlash.WriteInt(y_actual_speed); // 8 DataFlash.WriteInt(g.pi_loiter_lon.get_integrator()); // 9 DataFlash.WriteInt(g.pi_loiter_lat.get_integrator()); // 10 + /*DataFlash.WriteInt(wp_distance); // 1 + DataFlash.WriteInt(nav_bearing/100); // 2 + DataFlash.WriteInt(my_max_speed); // 3 + DataFlash.WriteInt(long_error); // 4 + DataFlash.WriteInt(x_actual_speed); // 5 + DataFlash.WriteInt(target_x_rate); // 6 + DataFlash.WriteInt(x_rate_error); // 7 + DataFlash.WriteInt(nav_lon_p); // 8 + DataFlash.WriteInt(g.pi_loiter_lon.get_integrator()); // 9 + DataFlash.WriteInt(nav_lon); // 10 + */ + DataFlash.WriteByte(END_BYTE); } From fffa1af3b35600c4baafc2d62fb69c616f2a638d Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 21 Jan 2012 22:12:57 -0800 Subject: [PATCH 08/20] Fixed speed governor which was letting speed get to 0. added Loiter_d to replace Nav_P for rate control wp_distance calc now returns CM --- ArduCopter/navigation.pde | 32 +++++++++++++++----------------- 1 file changed, 15 insertions(+), 17 deletions(-) diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index c6ca18db00..02eabe763d 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -167,7 +167,8 @@ static void calc_loiter(int x_error, int y_error) int16_t x_target_speed = g.pi_loiter_lon.get_p(x_error); int16_t x_iterm = g.pi_loiter_lon.get_i(x_error, dTnav); x_rate_error = x_target_speed - x_actual_speed; - nav_lon_p = g.pi_nav_lon.get_p(x_rate_error); + nav_lon_p = x_rate_error * g.loiter_d; + nav_lon_p = constrain(nav_lon_p, -1200, 1200); nav_lon = nav_lon_p + x_iterm; nav_lon = constrain(nav_lon, -2500, 2500); @@ -177,7 +178,8 @@ static void calc_loiter(int x_error, int y_error) int16_t y_target_speed = g.pi_loiter_lat.get_p(y_error); int16_t y_iterm = g.pi_loiter_lat.get_i(y_error, dTnav); y_rate_error = y_target_speed - y_actual_speed; - nav_lat_p = g.pi_nav_lat.get_p(y_rate_error); + nav_lat_p = y_rate_error * g.loiter_d; + nav_lat_p = constrain(nav_lat_p, -1200, 1200); nav_lat = nav_lat_p + y_iterm; nav_lat = constrain(nav_lat, -2500, 2500); @@ -270,21 +272,20 @@ static int16_t calc_desired_speed(int16_t max_speed) */ // max_speed is default 600 or 6m/s - // (wp_distance * 50) = 1/2 of the distance converted to speed + // (wp_distance * .5) = 1/2 of the distance converted to speed // wp_distance is always in m/s and not cm/s - I know it's stupid that way // for example 4m from target = 200cm/s speed // we choose the lowest speed based on disance - max_speed = min(max_speed, (wp_distance * 100)); + max_speed = min(max_speed, wp_distance); + + // go at least 100cm/s + max_speed = max(max_speed, WAYPOINT_SPEED_MIN); // limit the ramp up of the speed // waypoint_speed_gov is reset to 0 at each new WP command - if(waypoint_speed_gov < max_speed){ + if(max_speed > waypoint_speed_gov){ waypoint_speed_gov += (int)(100.0 * dTnav); // increase at .5/ms - - // go at least 100cm/s - max_speed = max(100, waypoint_speed_gov); - // limit with governer - max_speed = min(max_speed, waypoint_speed_gov); + max_speed = waypoint_speed_gov; } return max_speed; @@ -296,7 +297,7 @@ static void calc_nav_rate(int max_speed) update_crosstrack(); // nav_bearing includes crosstrack - float temp = (9000 - nav_bearing) * RADX100; + float temp = (9000l - nav_bearing) * RADX100; x_rate_error = (cos(temp) * max_speed) - x_actual_speed; // 413 x_rate_error = constrain(x_rate_error, -1000, 1000); @@ -398,7 +399,6 @@ static void set_new_altitude(int32_t _new_alt) alt_change_flag = REACHED_ALT; //Serial.printf("reached alt\n"); } - //Serial.printf("new alt: %d Org alt: %d\n", target_altitude, original_altitude); } @@ -490,16 +490,14 @@ static int32_t wrap_180(int32_t error) return current_loc.alt - home.alt; } */ + // distance is returned in meters static int32_t get_distance(struct Location *loc1, struct Location *loc2) { - //if(loc1->lat == 0 || loc1->lng == 0) - // return -1; - //if(loc2->lat == 0 || loc2->lng == 0) - // return -1; float dlat = (float)(loc2->lat - loc1->lat); float dlong = ((float)(loc2->lng - loc1->lng)) * scaleLongDown; - return sqrt(sq(dlat) + sq(dlong)) * .01113195; + dlong = sqrt(sq(dlat) + sq(dlong)) * 1.113195; + return dlong; } /* //static int32_t get_alt_distance(struct Location *loc1, struct Location *loc2) From aace39e65c4f40c3deb1cc3078d16ad1e4539d1e Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 21 Jan 2012 22:13:57 -0800 Subject: [PATCH 09/20] Converted distance gains to CM added loiter_d for rate control, equal to old Nav_P gain --- ArduCopter/Parameters.h | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 19d06a93b6..a94f36417f 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -107,7 +107,7 @@ public: k_param_optflow_enabled, k_param_low_voltage, k_param_ch7_option, - k_param_sonar_type, + k_param_sonar_type, k_param_super_simple, //155 // @@ -167,7 +167,7 @@ public: // // 235: PI/D Controllers // - k_param_stablize_d = 234, + k_param_stabilize_d = 234, k_param_pi_rate_roll = 235, k_param_pi_rate_pitch, k_param_pi_rate_yaw, @@ -184,7 +184,7 @@ public: k_param_pi_acro_pitch, k_param_pi_optflow_roll, k_param_pi_optflow_pitch, // 250 - + k_param_loiter_d, // 254,255: reserved }; @@ -286,6 +286,7 @@ public: AP_Float camera_pitch_gain; AP_Float camera_roll_gain; AP_Float stablize_d; + AP_Float loiter_d; // PI/D controllers APM_PI pi_rate_roll; @@ -341,10 +342,10 @@ public: command_total (0, k_param_command_total, PSTR("WP_TOTAL")), command_index (0, k_param_command_index, PSTR("WP_INDEX")), command_nav_index (0, k_param_command_nav_index, PSTR("WP_MUST_INDEX")), - waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")), + waypoint_radius (WP_RADIUS_DEFAULT * 100, 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 * 100, k_param_crosstrack_gain, PSTR("XTRK_GAIN_SC")), + crosstrack_gain (CROSSTRACK_GAIN, k_param_crosstrack_gain, PSTR("XTRK_GAIN_SC")), auto_land_timeout (AUTO_LAND_TIME * 1000, k_param_auto_land_timeout, PSTR("AUTO_LAND")), throttle_min (0, k_param_throttle_min, PSTR("THR_MIN")), @@ -409,7 +410,8 @@ public: camera_pitch_gain (CAM_PITCH_GAIN, k_param_camera_pitch_gain, PSTR("CAM_P_G")), camera_roll_gain (CAM_ROLL_GAIN, k_param_camera_roll_gain, PSTR("CAM_R_G")), - stablize_d (STABILIZE_D, k_param_stablize_d, PSTR("STAB_D")), + stablize_d (STABILIZE_D, k_param_stabilize_d, PSTR("STAB_D")), + loiter_d (LOITER_D, k_param_loiter_d, PSTR("LOITER_D")), // PI controller group key name initial P initial I initial imax //-------------------------------------------------------------------------------------------------------------------------------------------------------------------- From 60e3508f7c12a5719cca4cf82b85f840415be5eb Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 21 Jan 2012 22:14:08 -0800 Subject: [PATCH 10/20] converted to cm Signed-off-by: Jason Short --- ArduCopter/radio.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index b1c0eaa5b2..a907c04fb8 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -158,7 +158,7 @@ static void throttle_failsafe(uint16_t pwm) // Don't enter Failsafe if we are not armed // home distance is in meters // This is to prevent accidental RTL - if((motor_armed == true) && (home_distance > 10) && (current_loc.alt > 400)){ + if((motor_armed == true) && (home_distance > 1000) && (current_loc.alt > 400)){ SendDebug("MSG FS ON "); SendDebugln(pwm, DEC); set_failsafe(true); From 1d9783652ad8d5286fae3dae572dd90e5e160dfc Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 21 Jan 2012 22:15:23 -0800 Subject: [PATCH 11/20] added Beta 4 tag --- ArduCopter/ArduCopter.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 0d755c6a18..86bd6e060f 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.2 b3" +#define THISFIRMWARE "ArduCopter V2.2 b4" /* ArduCopter Version 2.2 Authors: Jason Short From dd1e08a7e0fcf427eafa3cc26d75c73539d41c5c Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 22 Jan 2012 09:34:43 -0800 Subject: [PATCH 12/20] Causes Aero_SIM to fail --- ArduCopter/GCS_Mavlink.pde | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index a9e1140962..44e74f21bf 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1519,14 +1519,15 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, packet.pitchspeed,packet.yawspeed); - // rad/sec + // rad/sec + /* Vector3f gyros; gyros.x = (float)packet.rollspeed; gyros.y = (float)packet.pitchspeed; gyros.z = (float)packet.yawspeed; imu.set_gyro(gyros); - + */ //imu.set_accel(accels); break; } From 276913a9508db8484f44767b355a2e92b16d3569 Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Tue, 17 Jan 2012 10:19:27 -0800 Subject: [PATCH 13/20] APM_RC enable_out and disable_out added to APM1 and APM2 * Outputs still enabled by default --- libraries/APM_RC/APM_RC.h | 3 +++ libraries/APM_RC/APM_RC_APM1.cpp | 34 ++++++++++++++++++++++++++++++++ libraries/APM_RC/APM_RC_APM1.h | 3 +++ libraries/APM_RC/APM_RC_APM2.cpp | 28 ++++++++++++++++++++++++++ libraries/APM_RC/APM_RC_APM2.h | 3 +++ 5 files changed, 71 insertions(+) diff --git a/libraries/APM_RC/APM_RC.h b/libraries/APM_RC/APM_RC.h index 7f72b1b183..d388dcb412 100644 --- a/libraries/APM_RC/APM_RC.h +++ b/libraries/APM_RC/APM_RC.h @@ -41,6 +41,9 @@ class APM_RC_Class virtual void clearOverride(void) = 0; virtual void Force_Out() = 0; virtual void SetFastOutputChannels( uint32_t channelmask ) = 0; + virtual void enable_out(uint8_t) = 0; + virtual void disable_out(uint8_t) = 0; + }; diff --git a/libraries/APM_RC/APM_RC_APM1.cpp b/libraries/APM_RC/APM_RC_APM1.cpp index 9f13274a40..2c7c04e6f6 100644 --- a/libraries/APM_RC/APM_RC_APM1.cpp +++ b/libraries/APM_RC/APM_RC_APM1.cpp @@ -153,6 +153,40 @@ void APM_RC_APM1::OutputCh(uint8_t ch, uint16_t pwm) } } +void APM_RC_APM1::enable_out(uint8_t ch) +{ + switch(ch){ + case 0: TCCR5A |= (1< Date: Tue, 17 Jan 2012 11:46:57 -0800 Subject: [PATCH 14/20] APM_RC: PWM outputs are disabled by default on init --- libraries/APM_RC/APM_RC_APM1.cpp | 8 ++++---- libraries/APM_RC/APM_RC_APM2.cpp | 9 +++------ 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/libraries/APM_RC/APM_RC_APM1.cpp b/libraries/APM_RC/APM_RC_APM1.cpp index 2c7c04e6f6..cd242cd312 100644 --- a/libraries/APM_RC/APM_RC_APM1.cpp +++ b/libraries/APM_RC/APM_RC_APM1.cpp @@ -84,7 +84,7 @@ void APM_RC_APM1::Init( Arduino_Mega_ISR_Registry * isr_reg ) pinMode(13,OUTPUT); //OUT3 (PB7/OC1C) //Remember the registers not declared here remains zero by default... - TCCR1A =((1< 0.5us tick - TCCR1A =((1< 50hz freq OCR1A = 0xFFFF; // Init OCR registers to nil output signal @@ -94,9 +93,8 @@ void APM_RC_APM2::Init( Arduino_Mega_ISR_Registry * isr_reg ) pinMode(6,OUTPUT); // OUT5 (PH3/OC4A) // WGM: 1 1 1 0. Clear Timer on Compare, TOP is ICR4. - // COM4A, 4B, 4C enabled, set to low level on match. // CS41: prescale by 8 => 0.5us tick - TCCR4A =((1< 0.5us tick - TCCR3A =((1< Date: Tue, 17 Jan 2012 11:47:27 -0800 Subject: [PATCH 15/20] ArduCopter: Add implementation of motors_output_enable to each motors_ frame type --- ArduCopter/motors_hexa.pde | 10 ++++++++++ ArduCopter/motors_octa.pde | 12 ++++++++++++ ArduCopter/motors_octa_quad.pde | 12 ++++++++++++ ArduCopter/motors_quad.pde | 8 ++++++++ ArduCopter/motors_tri.pde | 7 +++++++ ArduCopter/motors_y6.pde | 10 ++++++++++ 6 files changed, 59 insertions(+) diff --git a/ArduCopter/motors_hexa.pde b/ArduCopter/motors_hexa.pde index 16405b0036..0dcd19daa7 100644 --- a/ArduCopter/motors_hexa.pde +++ b/ArduCopter/motors_hexa.pde @@ -10,6 +10,16 @@ static void init_motors_out() #endif } +static void motors_output_enable() +{ + APM_RC.enable_out(MOT_1); + APM_RC.enable_out(MOT_2); + APM_RC.enable_out(MOT_3); + APM_RC.enable_out(MOT_4); + APM_RC.enable_out(MOT_5); + APM_RC.enable_out(MOT_6); +} + static void output_motors_armed() { int roll_out, pitch_out; diff --git a/ArduCopter/motors_octa.pde b/ArduCopter/motors_octa.pde index bafb4c71fd..87cc1cd043 100644 --- a/ArduCopter/motors_octa.pde +++ b/ArduCopter/motors_octa.pde @@ -10,6 +10,18 @@ static void init_motors_out() #endif } +static void motors_output_enable() +{ + APM_RC.enable_out(MOT_1); + APM_RC.enable_out(MOT_2); + APM_RC.enable_out(MOT_3); + APM_RC.enable_out(MOT_4); + APM_RC.enable_out(MOT_5); + APM_RC.enable_out(MOT_6); + APM_RC.enable_out(MOT_7); + APM_RC.enable_out(MOT_8); +} + static void output_motors_armed() { int roll_out, pitch_out; diff --git a/ArduCopter/motors_octa_quad.pde b/ArduCopter/motors_octa_quad.pde index 8025be1dac..cb331ec95d 100644 --- a/ArduCopter/motors_octa_quad.pde +++ b/ArduCopter/motors_octa_quad.pde @@ -10,6 +10,18 @@ static void init_motors_out() #endif } +static void motors_output_enable() +{ + APM_RC.enable_out(MOT_1); + APM_RC.enable_out(MOT_2); + APM_RC.enable_out(MOT_3); + APM_RC.enable_out(MOT_4); + APM_RC.enable_out(MOT_5); + APM_RC.enable_out(MOT_6); + APM_RC.enable_out(MOT_7); + APM_RC.enable_out(MOT_8); +} + static void output_motors_armed() { int roll_out, pitch_out; diff --git a/ArduCopter/motors_quad.pde b/ArduCopter/motors_quad.pde index 8b3e3720b0..54a9c4d6f4 100644 --- a/ArduCopter/motors_quad.pde +++ b/ArduCopter/motors_quad.pde @@ -9,6 +9,14 @@ static void init_motors_out() #endif } +static void motors_output_enable() +{ + APM_RC.enable_out(MOT_1); + APM_RC.enable_out(MOT_2); + APM_RC.enable_out(MOT_3); + APM_RC.enable_out(MOT_4); +} + static void output_motors_armed() { int roll_out, pitch_out; diff --git a/ArduCopter/motors_tri.pde b/ArduCopter/motors_tri.pde index 8c7420487b..dda4ba7638 100644 --- a/ArduCopter/motors_tri.pde +++ b/ArduCopter/motors_tri.pde @@ -8,6 +8,13 @@ static void init_motors_out() #endif } +static void motors_output_enable() +{ + APM_RC.enable_out(MOT_1); + APM_RC.enable_out(MOT_2); + APM_RC.enable_out(MOT_4); +} + static void output_motors_armed() { diff --git a/ArduCopter/motors_y6.pde b/ArduCopter/motors_y6.pde index c87dfc28dc..991ced7e49 100644 --- a/ArduCopter/motors_y6.pde +++ b/ArduCopter/motors_y6.pde @@ -12,6 +12,16 @@ static void init_motors_out() #endif } +static void motors_output_enable() +{ + APM_RC.enable_out(MOT_1); + APM_RC.enable_out(MOT_2); + APM_RC.enable_out(MOT_3); + APM_RC.enable_out(MOT_4); + APM_RC.enable_out(MOT_5); + APM_RC.enable_out(MOT_6); +} + static void output_motors_armed() { int out_min = g.rc_3.radio_min; From 98a90d0352dbe1e2afdc2b34d4dd6010314812c9 Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Tue, 17 Jan 2012 11:47:45 -0800 Subject: [PATCH 16/20] ArduCopter: Call motors_output_enable at correct spot during init --- ArduCopter/radio.pde | 1 + ArduCopter/setup.pde | 1 + 2 files changed, 2 insertions(+) diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index a907c04fb8..5f37ee6a0f 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -100,6 +100,7 @@ static void init_rc_out() void output_min() { + motors_output_enable(); #if FRAME_CONFIG == HELI_FRAME heli_move_servos_to_mid(); #else diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index 3b7ed57455..96e65b9c29 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -1096,6 +1096,7 @@ static void print_enabled(boolean b) static void init_esc() { + motors_output_enable(); while(1){ read_radio(); delay(100); From 89e11afc096c47ffe5a013981744f99e3f4478e4 Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Thu, 19 Jan 2012 20:20:30 -0800 Subject: [PATCH 17/20] ArduCopter: Call APM_RC.enable_out for camera channels in init_camera() --- ArduCopter/Camera.pde | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ArduCopter/Camera.pde b/ArduCopter/Camera.pde index 0937053f9b..047a7061ba 100644 --- a/ArduCopter/Camera.pde +++ b/ArduCopter/Camera.pde @@ -4,6 +4,8 @@ static void init_camera() { + APM_RC.enable_out(CH_CAM_PITCH); + APM_RC.enable_out(CH_CAM_ROLL); // ch 6 high(right) is down. g.rc_camera_pitch.set_angle(4500); g.rc_camera_roll.set_angle(4500); From dfece8e3ff10ca076abeec32b033745f29df3c75 Mon Sep 17 00:00:00 2001 From: Pat Hickey Date: Thu, 19 Jan 2012 20:20:50 -0800 Subject: [PATCH 18/20] ArduPlane: Call APM_RC.enable_out for all channels in init_rc_out. --- ArduPlane/radio.pde | 21 ++++++++++----------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index e0a8b870b9..ac4b802ef8 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -33,18 +33,17 @@ static void init_rc_in() static void init_rc_out() { - APM_RC.OutputCh(CH_1, g.channel_roll.radio_trim); // Initialization of servo outputs - APM_RC.OutputCh(CH_2, g.channel_pitch.radio_trim); - APM_RC.OutputCh(CH_3, g.channel_throttle.radio_min); - APM_RC.OutputCh(CH_4, g.channel_rudder.radio_trim); - - APM_RC.OutputCh(CH_5, g.rc_5.radio_trim); - APM_RC.OutputCh(CH_6, g.rc_6.radio_trim); - APM_RC.OutputCh(CH_7, g.rc_7.radio_trim); - APM_RC.OutputCh(CH_8, g.rc_8.radio_trim); - APM_RC.Init( &isr_registry ); // APM Radio initialization + APM_RC.enable_out(CH_1); + APM_RC.enable_out(CH_2); + APM_RC.enable_out(CH_3); + APM_RC.enable_out(CH_4); + APM_RC.enable_out(CH_5); + APM_RC.enable_out(CH_6); + APM_RC.enable_out(CH_7); + APM_RC.enable_out(CH_8); + APM_RC.OutputCh(CH_1, g.channel_roll.radio_trim); // Initialization of servo outputs APM_RC.OutputCh(CH_2, g.channel_pitch.radio_trim); APM_RC.OutputCh(CH_3, g.channel_throttle.radio_min); @@ -53,7 +52,7 @@ static void init_rc_out() APM_RC.OutputCh(CH_5, g.rc_5.radio_trim); APM_RC.OutputCh(CH_6, g.rc_6.radio_trim); APM_RC.OutputCh(CH_7, g.rc_7.radio_trim); - APM_RC.OutputCh(CH_8, g.rc_8.radio_trim); + APM_RC.OutputCh(CH_8, g.rc_8.radio_trim); } static void read_radio() From 6476ba22f21bce1c1b84e7783c82036d1f2d05d1 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 22 Jan 2012 12:26:35 -0800 Subject: [PATCH 19/20] test to switch loiter controls --- ArduCopter/config.h | 2 +- ArduCopter/navigation.pde | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index b309f5d6fa..83f268d87f 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -628,7 +628,7 @@ # define LOITER_IMAX 30 // degrees° #endif #ifndef LOITER_D -# define LOITER_D 2.2 // rate control +# define LOITER_D 3 // rate control #endif ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 02eabe763d..52ec1580db 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -143,13 +143,13 @@ static void calc_location_error(struct Location *next_loc) */ #define NAV_ERR_MAX 800 -static void calc_loiter1(int x_error, int y_error) +static void calc_loiter(int x_error, int y_error) { int16_t lon_PI = g.pi_loiter_lon.get_pi(x_error, dTnav); - int16_t lon_D = 3 * x_actual_speed ; // this controls the small bumps + int16_t lon_D = g.loiter_d * x_actual_speed ; // this controls the small bumps int16_t lat_PI = g.pi_loiter_lat.get_pi(y_error, dTnav); - int16_t lat_D = 3 * y_actual_speed ; + int16_t lat_D = g.loiter_d * y_actual_speed ; //limit of terms lon_D = constrain(lon_D,-500,500); @@ -160,7 +160,7 @@ static void calc_loiter1(int x_error, int y_error) } -static void calc_loiter(int x_error, int y_error) +static void calc_loiter1(int x_error, int y_error) { // East/West x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX); //800 From 9c1e257826c23682fa5fd85aafe9ff790327bf01 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 23 Jan 2012 08:46:39 +1100 Subject: [PATCH 20/20] fixed example in PID library --- libraries/PID/examples/pid/Makefile | 1 - libraries/PID/examples/pid/pid.pde | 14 ++++++++++++-- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/libraries/PID/examples/pid/Makefile b/libraries/PID/examples/pid/Makefile index 85b4d8856b..d1f40fd90f 100644 --- a/libraries/PID/examples/pid/Makefile +++ b/libraries/PID/examples/pid/Makefile @@ -1,2 +1 @@ -BOARD = mega include ../../../AP_Common/Arduino.mk diff --git a/libraries/PID/examples/pid/pid.pde b/libraries/PID/examples/pid/pid.pde index 148f9644b8..509ed17166 100644 --- a/libraries/PID/examples/pid/pid.pde +++ b/libraries/PID/examples/pid/pid.pde @@ -4,20 +4,30 @@ */ #include +#include #include #include // ArduPilot RC Library #include // ArduPilot Mega RC Library +#include long radio_in; long radio_trim; -PID pid(10, "TEST1_"); +Arduino_Mega_ISR_Registry isr_registry; + +#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 + APM_RC_APM2 APM_RC; +#else + APM_RC_APM1 APM_RC; +#endif + +PID pid(AP_Var::k_key_none, NULL); void setup() { Serial.begin(38400); Serial.println("ArduPilot Mega PID library test"); - APM_RC.Init(); // APM Radio initialization + APM_RC.Init(&isr_registry); // APM Radio initialization delay(1000); //rc.trim();