diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index e17b9e6481..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 @@ -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); 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; 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); 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..44e74f21bf 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,15 +1518,16 @@ 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 + + // 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_gyro(gyros); + */ //imu.set_accel(accels); break; } 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); } 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 //-------------------------------------------------------------------------------------------------------------------------------------------------------------------- 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; } 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() diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 698cdd6d75..83f268d87f 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 3 // 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 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; diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index c6ca18db00..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,14 +160,15 @@ 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 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) diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index b1c0eaa5b2..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 @@ -158,7 +159,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); 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); 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() 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..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< +#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();