diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 9368f051ae..2f49c5c4c2 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduCopter V2.1.1r8 alpha" +#define THISFIRMWARE "ArduCopter V2.1.1r9 alpha" /* ArduCopter Version 2.0 Beta Authors: Jason Short @@ -80,6 +80,7 @@ http://code.google.com/p/ardupilot-mega/downloads/list // Configuration #include "defines.h" #include "config.h" +#include "config_channels.h" // Local modules #include "Parameters.h" @@ -278,7 +279,6 @@ ModeFilter sonar_mode_filter; //////////////////////////////////////////////////////////////////////////////// // Global variables //////////////////////////////////////////////////////////////////////////////// -static const char *comma = ","; static const char* flight_mode_strings[] = { "STABILIZE", @@ -327,7 +327,7 @@ static int16_t y_rate_error; //////////////////////////////////////////////////////////////////////////////// // This is the state of the flight control system // There are multiple states defined such as STABILIZE, ACRO, -static byte control_mode = STABILIZE; +static int8_t control_mode = STABILIZE; // This is the state of simple mode. // Set in the control_mode.pde file when the control switch is read static bool do_simple = false; @@ -443,8 +443,6 @@ static int32_t target_bearing; // This is the angle from the copter to the "next_WP" location // with the addition of Crosstrack error in degrees * 100 static int32_t nav_bearing; -// This is the angle from the copter to the "home" location in degrees * 100 -static int32_t home_bearing; // Status of the Waypoint tracking mode. Options include: // NO_NAV_MODE, WP_MODE, LOITER_MODE, CIRCLE_MODE static byte wp_control; @@ -572,7 +570,8 @@ static int32_t baro_alt; static int32_t old_baro_alt; // The climb_rate as reported by Baro in cm/s static int16_t baro_rate; - +// +static boolean reset_throttle_flag; //////////////////////////////////////////////////////////////////////////////// // flight modes @@ -681,12 +680,23 @@ static int16_t nav_throttle; // 0-1000 for throttle control static uint32_t throttle_integrator; // This is a future value for replacing the throttle_cruise setup procedure. It's an average of throttle control // that is generated when the climb rate is within a certain threshold -static float throttle_avg = THROTTLE_CRUISE; +//static float throttle_avg = THROTTLE_CRUISE; // This is a flag used to trigger the updating of nav_throttle at 10hz static bool invalid_throttle; // Used to track the altitude offset for climbrate control -static int32_t target_altitude; +//static int32_t target_altitude; +//////////////////////////////////////////////////////////////////////////////// +// Climb rate control +//////////////////////////////////////////////////////////////////////////////// +// Time when we intiated command in millis - used for controlling decent rate +// The orginal altitude used to base our new altitude during decent +static int32_t original_altitude; +// Used to track the altitude offset for climbrate control +static int32_t target_altitude; +static uint32_t alt_change_timer; +static int8_t alt_change_flag; +static uint32_t alt_change; //////////////////////////////////////////////////////////////////////////////// // Navigation Yaw control @@ -737,18 +747,8 @@ static int16_t event_undo_value; //////////////////////////////////////////////////////////////////////////////// // Delay Mission Scripting Command //////////////////////////////////////////////////////////////////////////////// -static int32_t condition_value; // used in condition commands (eg delay, change alt, etc.) -static int32_t condition_start; - - -//////////////////////////////////////////////////////////////////////////////// -// Auto Landing -//////////////////////////////////////////////////////////////////////////////// -// Time when we intiated command in millis - used for controlling decent rate -static int32_t land_start; -// The orginal altitude used to base our new altitude during decent -static int32_t original_alt; - +static int32_t condition_value; // used in condition commands (eg delay, change alt, etc.) +static uint32_t condition_start; //////////////////////////////////////////////////////////////////////////////// @@ -794,6 +794,8 @@ static float dTnav; static int16_t superslow_loopCounter; // RTL Autoland Timer static uint32_t auto_land_timer; +// disarms the copter while in Acro or Stabilzie mode after 30 seconds of no flight +static uint8_t auto_disarming_counter; // Tracks if GPS is enabled based on statup routine @@ -802,7 +804,7 @@ static bool GPS_enabled = false; // Set true if we have new PWM data to act on from the Radio static bool new_radio_frame; // Used to auto exit the in-flight leveler -static int16_t auto_level_counter; +static int16_t auto_level_counter; // Reference to the AP relay object - APM1 only AP_Relay relay; @@ -865,6 +867,7 @@ void loop() counter_one_herz++; + // trgger our 1 hz loop if(counter_one_herz >= 50){ super_slow_loop(); counter_one_herz = 0; @@ -1129,7 +1132,7 @@ static void fifty_hz_loop() #if FRAME_CONFIG == TRI_FRAME // servo Yaw g.rc_4.calc_pwm(); - APM_RC.OutputCh(CH_7, g.rc_4.radio_out); + APM_RC.OutputCh(CH_TRI_YAW, g.rc_4.radio_out); #endif } @@ -1193,16 +1196,28 @@ static void slow_loop() default: slow_loopCounter = 0; break; - } } +#define AUTO_ARMING_DELAY 60 // 1Hz loop static void super_slow_loop() { if (g.log_bitmask & MASK_LOG_CUR) Log_Write_Current(); + // this function disarms the copter if it has been sitting on the ground for any moment of time greater than 30s + // but only of the control mode is manual + if((control_mode <= ACRO) && (g.rc_3.control_in == 0)){ + auto_disarming_counter++; + if(auto_disarming_counter == AUTO_ARMING_DELAY){ + init_disarm_motors(); + }else if (auto_disarming_counter > AUTO_ARMING_DELAY){ + auto_disarming_counter = AUTO_ARMING_DELAY + 1; + } + }else{ + auto_disarming_counter = 0; + } gcs_send_message(MSG_HEARTBEAT); gcs_data_stream_send(1,3); // agmatthews - USERHOOKS @@ -1558,18 +1573,27 @@ void update_throttle_mode(void) throttle_out = g.throttle_cruise + angle_boost + manual_boost; #endif - // reset next_WP.alt and don't go below 1 meter - next_WP.alt = max(current_loc.alt, 100); + //force a reset of the altitude change + clear_new_altitude(); /* - Serial.printf("tar_alt: %d, actual_alt: %d \talt_err: %d, \t manb: %d\n", + int16_t iterm = g.pi_alt_hold.get_integrator(); + + Serial.printf("tar_alt: %d, actual_alt: %d \talt_err: %d, \t manb: %d, iterm %d\n", next_WP.alt, current_loc.alt, altitude_error, - manual_boost); + manual_boost, + iterm); //*/ + reset_throttle_flag = true; }else{ + if(reset_throttle_flag) { + set_new_altitude(max(current_loc.alt, 100)); + reset_throttle_flag = false; + } + // 10hz, don't run up i term if(invalid_throttle && motor_auto_armed == true){ @@ -1582,14 +1606,13 @@ void update_throttle_mode(void) // clear the new data flag invalid_throttle = false; /* - Serial.printf("tar_alt: %d, actual_alt: %d \talt_err: %d, \tnav_thr: %d, \talt Int: %d, \trate_int %d \n", + Serial.printf("tar_alt: %d, actual_alt: %d \talt_err: %d, \tnav_thr: %d, \talt Int: %d\n", next_WP.alt, current_loc.alt, altitude_error, nav_throttle, - (int16_t)g.pi_alt_hold.get_integrator(), - (int16_t) g.pi_throttle.get_integrator()); - */ + (int16_t)g.pi_alt_hold.get_integrator()); + //*/ } #if FRAME_CONFIG == HELI_FRAME @@ -1637,23 +1660,26 @@ static void update_navigation() break; case RTL: + // We have reached Home if((wp_distance <= g.waypoint_radius) || check_missed_wp()){ // if this value > 0, we are set to trigger auto_land after 30 seconds set_mode(LOITER); auto_land_timer = millis(); + break; + } - }else if(current_loc.alt < (next_WP.alt - 300)){ - // don't navigate if we are below our target alt - wp_control = LOITER_MODE; + // We wait until we've reached out new altitude before coming home + // Arg doesn't work, it + //if(alt_change_flag != REACHED_ALT){ + // wp_control = NO_NAV_MODE; + //}else{ + wp_control = WP_MODE; - }else{ // calculates desired Yaw #if FRAME_CONFIG == HELI_FRAME update_auto_yaw(); #endif - - wp_control = WP_MODE; - } + //} // calculates the desired Roll and Pitch update_nav_wp(); @@ -1689,12 +1715,9 @@ static void update_navigation() case LAND: wp_control = LOITER_MODE; - if(verify_land()) { // JLN fix for auto land in RTL - set_mode(STABILIZE); - } else { - // calculates the desired Roll and Pitch - update_nav_wp(); - } + // verify land will override WP_control if we are below + // a certain altitude + verify_land(); // calculates the desired Roll and Pitch update_nav_wp(); @@ -1827,7 +1850,8 @@ static void update_altitude() scale = (sonar_alt - 400) / 200; scale = constrain(scale, 0, 1); - current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt; + // solve for a blended altitude + current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt; // solve for a blended climb_rate climb_rate = ((float)sonar_rate * (1.0 - scale)) + (float)baro_rate * scale; @@ -1840,7 +1864,6 @@ static void update_altitude() } }else{ - // NO Sonar case current_loc.alt = baro_alt + home.alt; climb_rate = baro_rate; @@ -1848,25 +1871,14 @@ static void update_altitude() // manage bad data climb_rate = constrain(climb_rate, -300, 300); + + // update the target altitude + next_WP.alt = get_new_altitude(); } static void adjust_altitude() { - /* - // old vert control - if(g.rc_3.control_in <= 200){ - next_WP.alt -= 1; // 1 meter per second - next_WP.alt = max(next_WP.alt, (current_loc.alt - 500)); // don't go less than 4 meters below current location - next_WP.alt = max(next_WP.alt, 100); // don't go less than 1 meter - //manual_boost = (g.rc_3.control_in == 0) ? -20 : 0; - - }else if (g.rc_3.control_in > 700){ - next_WP.alt += 1; // 1 meter per second - next_WP.alt = min(next_WP.alt, (current_loc.alt + 500)); // don't go more than 4 meters below current location - //manual_boost = (g.rc_3.control_in == 800) ? 20 : 0; - }*/ - if(g.rc_3.control_in <= 180){ // we remove 0 to 100 PWM from hover manual_boost = g.rc_3.control_in - 180; diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 8c075f3305..322375293b 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -170,9 +170,6 @@ get_nav_throttle(int32_t z_error) int16_t rate_error; int16_t output; - // XXX HACK, need a better way not to ramp this i term in large altitude changes. - float dt = (abs(z_error) < 400) ? .1 : 0.0; - // limit error to prevent I term run up z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX); @@ -180,7 +177,7 @@ get_nav_throttle(int32_t z_error) rate_error = g.pi_alt_hold.get_p(z_error); //_p = .85 // experiment to pipe iterm directly into the output - int16_t iterm = g.pi_alt_hold.get_i(z_error, dt); + int16_t iterm = g.pi_alt_hold.get_i(z_error, .1); // calculate rate error rate_error = rate_error - climb_rate; @@ -228,23 +225,37 @@ get_rate_yaw(int32_t target_rate) // Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc. // Keeps outdated data out of our calculations -static void reset_hold_I(void) -{ - g.pi_loiter_lat.reset_I(); - g.pi_loiter_lon.reset_I(); -} +//static void reset_hold_I(void) +//{ +// g.pi_loiter_lat.reset_I(); +// g.pi_loiter_lon.reset_I(); +//} // Keeps old data out of our calculation / logs static void reset_nav(void) { + // forces us to update nav throttle invalid_throttle = true; nav_throttle = 0; + + // always start Circle mode at same angle circle_angle = 0; + + // We must be heading to a new WP, so XTrack must be 0 crosstrack_error = 0; + + // Will be set by new command target_bearing = 0; + + // Will be set by new command wp_distance = 0; + + // Will be set by new command, used by loiter long_error = 0; lat_error = 0; + + // Will be set by new command, used by loiter + next_WP.alt = 0; } static void reset_rate_I() diff --git a/ArduCopter/Camera.pde b/ArduCopter/Camera.pde index e1d079f08e..0937053f9b 100644 --- a/ArduCopter/Camera.pde +++ b/ArduCopter/Camera.pde @@ -51,8 +51,8 @@ camera_stabilization() g.rc_camera_pitch.calc_pwm(); g.rc_camera_roll.calc_pwm(); - APM_RC.OutputCh(CH_5, g.rc_camera_pitch.radio_out); - APM_RC.OutputCh(CH_6, g.rc_camera_roll.radio_out); + APM_RC.OutputCh(CH_CAM_PITCH, g.rc_camera_pitch.radio_out); + APM_RC.OutputCh(CH_CAM_ROLL , g.rc_camera_roll.radio_out); //Serial.printf("c:%d\n", g.rc_camera_pitch.radio_out); } diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 61197d4186..e0d2acab55 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -308,7 +308,7 @@ static void Log_Read_Raw() for (int y = 0; y < 6; y++) { logvar = (float)DataFlash.ReadLong() / t7; Serial.print(logvar); - Serial.print(comma); + Serial.print(","); } Serial.println(" "); } diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index e5b6fe7a3c..cb3767332b 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -147,9 +147,13 @@ static void set_next_WP(struct Location *wp) // --------------------- next_WP = *wp; - // used to control and limit the rate of climb - not used right now! - // ----------------------------------------------------------------- - target_altitude = current_loc.alt; + // used to control and limit the rate of climb + // ------------------------------------------- + // We don't set next WP below 1m + next_WP.alt = max(next_WP.alt, 100); + + // Save new altitude so we can track it for climb_rate + set_new_altitude(next_WP.alt); // this is used to offset the shrinking longitude as we go towards the poles float rads = (fabs((float)next_WP.lat)/t7) * 0.0174532925; @@ -162,6 +166,9 @@ static void set_next_WP(struct Location *wp) target_bearing = get_bearing(&prev_WP, &next_WP); nav_bearing = target_bearing; + // calc the location error: + calc_location_error(&next_WP); + // to check if we have missed the WP // --------------------------------- original_target_bearing = target_bearing; diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index a480c2210c..4896d0a5b4 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -199,7 +199,7 @@ static void do_RTL(void) //so we know where we are navigating from // -------------------------------------- - next_WP = current_loc; + next_WP = current_loc; // Loads WP from Memory // -------------------- @@ -265,19 +265,13 @@ static void do_land() { wp_control = LOITER_MODE; - //Serial.println("dlnd "); - - // not really used right now, might be good for debugging land_complete = false; - // used to limit decent rate - land_start = millis(); - - // used to limit decent rate - original_alt = current_loc.alt; - // hold at our current location set_next_WP(¤t_loc); + + // Set a new target altitude + set_new_altitude(0); } static void do_loiter_unlimited() @@ -339,55 +333,44 @@ static bool verify_takeoff() return false; } // are we above our target altitude? - return (current_loc.alt > next_WP.alt); + //return (current_loc.alt > next_WP.alt); + return (current_loc.alt > target_altitude); } static bool verify_land() { + static int32_t old_alt = 0; static int16_t velocity_land = -1; - // land at .62 meter per second - next_WP.alt = original_alt - ((millis() - land_start) / 32); // condition_value = our initial - - if (old_alt == 0) + // landing detector + if ((current_loc.alt - home.alt) > 300){ old_alt = current_loc.alt; - - if (velocity_land == -1) velocity_land = 2000; - - - if ((current_loc.alt - home.alt) < 300){ + }else{ // a LP filter used to tell if we have landed // will drive to 0 if we are on the ground - maybe, the baro is noisy - velocity_land = ((velocity_land * 7) + (old_alt - current_loc.alt)) / 8; + int16_t delta = (old_alt - current_loc.alt) << 3; + velocity_land = ((velocity_land * 7) + delta ) / 8; } + //Serial.printf("velocity_land %d \n", velocity_land); + // remenber altitude for climb_rate old_alt = current_loc.alt; if ((current_loc.alt - home.alt) < 200){ // don't bank to hold position wp_control = NO_NAV_MODE; - - // Update by JLN for a safe AUTO landing - manual_boost = -10; - g.throttle_cruise += g.pi_alt_hold.get_integrator(); - g.pi_alt_hold.reset_I(); - g.pi_throttle.reset_I(); } - if((current_loc.alt - home.alt) < 100 && velocity_land <= 100){ + if((current_loc.alt - home.alt) < 100 && velocity_land <= 50){ land_complete = true; - // reset manual_boost hack - manual_boost = 0; // reset old_alt old_alt = 0; - return false; + return true; } - //Serial.printf("N, %d\n", velocity_land); - //Serial.printf("N_alt, %ld\n", next_WP.alt); return false; } @@ -396,7 +379,9 @@ static bool verify_nav_wp() // Altitude checking if(next_WP.options & MASK_OPTIONS_RELATIVE_ALT){ // we desire a certain minimum altitude - if (current_loc.alt > next_WP.alt){ + //if (current_loc.alt > next_WP.alt){ + if (current_loc.alt > target_altitude){ + // we have reached that altitude wp_verify_byte |= NAV_ALTITUDE; } @@ -509,7 +494,7 @@ static void do_change_alt() { Location temp = next_WP; condition_start = current_loc.alt; - condition_value = command_cond_queue.alt; + //condition_value = command_cond_queue.alt; temp.alt = command_cond_queue.alt; set_next_WP(&temp); } @@ -582,9 +567,9 @@ static void do_yaw() static bool verify_wait_delay() { //Serial.print("vwd"); - if ((unsigned)(millis() - condition_start) > condition_value){ + if ((unsigned)(millis() - condition_start) > (unsigned)condition_value){ //Serial.println("y"); - condition_value = 0; + condition_value = 0; return true; } //Serial.println("n"); @@ -594,16 +579,14 @@ static bool verify_wait_delay() static bool verify_change_alt() { //Serial.printf("change_alt, ca:%d, na:%d\n", (int)current_loc.alt, (int)next_WP.alt); - if (condition_start < next_WP.alt){ + if ((int32_t)condition_start < next_WP.alt){ // we are going higer if(current_loc.alt > next_WP.alt){ - condition_value = 0; return true; } }else{ // we are going lower if(current_loc.alt < next_WP.alt){ - condition_value = 0; return true; } } diff --git a/ArduCopter/commands_process.pde b/ArduCopter/commands_process.pde index 76ad7cde6b..f2cc90d52f 100644 --- a/ArduCopter/commands_process.pde +++ b/ArduCopter/commands_process.pde @@ -39,7 +39,7 @@ static void update_commands() //uint8_t tmp = g.command_index.get(); //Serial.printf("command_index %u \n", tmp); - if (g.command_total <= 1 || g.command_index == 255) + if (g.command_total <= 1 || g.command_index > 127) return; if(command_nav_queue.id == NO_COMMAND){ @@ -62,8 +62,14 @@ static void update_commands() g.command_index = command_nav_index = 255; // if we are on the ground, enter stabilize, else Land if (land_complete == true){ - set_mode(STABILIZE); - // disarm motors + // So what state does this leave us in? + // We are still in the same mode as what landed us, + // so maybe we try to continue to descend just in case we are still in the air + // This will also drive down the Iterm to -300 + set_new_altitude(-10000); + + // We can't disarm the motors easily. We could very well be wrong + // //init_disarm_motors(); } else { set_mode(LAND); diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 02b3548c79..60cbb35f7b 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -1,5 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // +#ifndef __ARDUCOPTER_CONFIG_H__ +#define __ARDUCOPTER_CONFIG_H__ ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// // @@ -855,3 +857,7 @@ #ifndef MAVLINK_TELEMETRY_PORT_DELAY # define MAVLINK_TELEMETRY_PORT_DELAY 2000 #endif + + + +#endif // __ARDUCOPTER_CONFIG_H__ diff --git a/ArduCopter/config_channels.h b/ArduCopter/config_channels.h new file mode 100644 index 0000000000..5429b273de --- /dev/null +++ b/ArduCopter/config_channels.h @@ -0,0 +1,73 @@ + +#ifndef __ARDUCOPTER_CONFIG_MOTORS_H__ +#define __ARDUCOPTER_CONFIG_MOTORS_H__ + +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// +// +// WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING +// +// DO NOT EDIT this file to adjust your configuration. Create your own +// APM_Config.h and use APM_Config.h.example as a reference. +// +// WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING WARNING +/// +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// +// + +#include "config.h" + +// +// +// Output CH mapping for ArduCopter motor channels +// +// +#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 +# define MOT_1 CH_1 +# define MOT_2 CH_2 +# define MOT_3 CH_3 +# define MOT_4 CH_4 +# define MOT_5 CH_5 +# define MOT_6 CH_6 +# define MOT_7 CH_7 +# define MOT_8 CH_8 +#elif CONFIG_APM_HARDWARE == APM_HARDWARE_APM1 +# define MOT_1 CH_1 +# define MOT_2 CH_2 +# define MOT_3 CH_3 +# define MOT_4 CH_4 +# define MOT_5 CH_7 +# define MOT_6 CH_8 +# define MOT_7 CH_10 +# define MOT_8 CH_11 +#endif + +// +// +// Output CH mapping for TRI_FRAME yaw servo +// +// +#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 +/* TODO find out correct channel for APM2 TRI_YAW */ +# define CH_TRI_YAW (-1) +#elif CONFIG_APM_HARDWARE == APM_HARDWARE_APM1 +# define CH_TRI_YAW CH_7 +#endif + +// +// +// Output CH mapping for Aux channels +// +// +#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 +/* Camera Pitch and Camera Roll: Not yet defined for APM2 + * They will likely be dependent on the frame config */ +# define CH_CAM_PITCH (-1) +# define CH_CAM_ROLL (-1) +#elif CONFIG_APM_HARDWARE == APM_HARDWARE_APM1 +# define CH_CAM_PITCH CH_5 +# define CH_CAM_ROLL CH_6 +#endif + +#endif // __ARDUCOPTER_CONFIG_MOTORS_H__ diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index efa8fe9f70..ff8f628bf9 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -106,6 +106,10 @@ #define HIL_MODE_ATTITUDE 1 #define HIL_MODE_SENSORS 2 +#define ASCENDING 1 +#define DESCENDING -1 +#define REACHED_ALT 0 + // Auto Pilot modes // ---------------- #define STABILIZE 0 // hold level position diff --git a/ArduCopter/events.pde b/ArduCopter/events.pde index 1908aff2fb..fbb55875a6 100644 --- a/ArduCopter/events.pde +++ b/ArduCopter/events.pde @@ -11,21 +11,22 @@ static void failsafe_on_event() { case AUTO: if (g.throttle_fs_action == 1) { + // do_rtl sets the altitude to the current altitude by default set_mode(RTL); - // send up up 10m - next_WP.alt += 1000; + // We add an additional 10m to the current altitude + //next_WP.alt += 1000; + set_new_altitude(target_altitude + 1000); } // 2 = Stay in AUTO and ignore failsafe default: if(home_is_set == true){ - // home distance is in meters - // This is to prevent accidental RTL - if ((home_distance > 15) && (current_loc.alt > 400)){ - set_mode(RTL); - // send up up 10m - next_WP.alt += 1000; - } + // same as above ^ + // do_rtl sets the altitude to the current altitude by default + set_mode(RTL); + // We add an additional 10m to the current altitude + //next_WP.alt += 1000; + set_new_altitude(target_altitude + 1000); }else{ // We have no GPS so we must land set_mode(LAND); diff --git a/ArduCopter/heli.pde b/ArduCopter/heli.pde index 2f39533114..5b1a707b94 100644 --- a/ArduCopter/heli.pde +++ b/ArduCopter/heli.pde @@ -234,7 +234,7 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o static void init_motors_out() { #if INSTANT_PWM == 0 - APM_RC.SetFastOutputChannels( MSK_CH_1 | MSK_CH_2 | MSK_CH_3 | MSK_CH_4 ); + APM_RC.SetFastOutputChannels( _BV(CH_1) | _BV(CH_2) | _BV(CH_3) | _BV(CH_4) ); #endif } diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 7a427e6903..7c05c0b400 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -142,3 +142,18 @@ set_servos_4() output_motors_disarmed(); } } + +int ch_of_mot( int mot ) { + switch (mot) { + case 1: return MOT_1; + case 2: return MOT_2; + case 3: return MOT_3; + case 4: return MOT_4; + case 5: return MOT_5; + case 6: return MOT_6; + case 7: return MOT_7; + case 8: return MOT_8; + } + return (-1); +} + diff --git a/ArduCopter/motors_hexa.pde b/ArduCopter/motors_hexa.pde index aed18d958d..16405b0036 100644 --- a/ArduCopter/motors_hexa.pde +++ b/ArduCopter/motors_hexa.pde @@ -5,8 +5,8 @@ static void init_motors_out() { #if INSTANT_PWM == 0 - APM_RC.SetFastOutputChannels( MSK_CH_1 | MSK_CH_2 | MSK_CH_3 | MSK_CH_4 - | MSK_CH_7 | MSK_CH_8 ); + APM_RC.SetFastOutputChannels( _BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) | _BV(MOT_4) + | _BV(MOT_5) | _BV(MOT_6) ); #endif } @@ -32,90 +32,88 @@ static void output_motors_armed() pitch_out = (float)g.rc_2.pwm_out * .866; //left side - motor_out[CH_2] = g.rc_3.radio_out + g.rc_1.pwm_out; // CCW Middle - motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW Front - motor_out[CH_8] = g.rc_3.radio_out + roll_out - pitch_out; // CW Back + motor_out[MOT_2] = g.rc_3.radio_out + g.rc_1.pwm_out; // CCW Middle + motor_out[MOT_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW Front + motor_out[MOT_6] = g.rc_3.radio_out + roll_out - pitch_out; // CW Back //right side - motor_out[CH_1] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW Middle - motor_out[CH_7] = g.rc_3.radio_out - roll_out + pitch_out; // CCW Front - motor_out[CH_4] = g.rc_3.radio_out - roll_out - pitch_out; // CCW Back + motor_out[MOT_1] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW Middle + motor_out[MOT_5] = g.rc_3.radio_out - roll_out + pitch_out; // CCW Front + motor_out[MOT_4] = g.rc_3.radio_out - roll_out - pitch_out; // CCW Back }else{ roll_out = (float)g.rc_1.pwm_out * .866; pitch_out = g.rc_2.pwm_out / 2; //Front side - motor_out[CH_1] = g.rc_3.radio_out + g.rc_2.pwm_out; // CW FRONT - motor_out[CH_7] = g.rc_3.radio_out + roll_out + pitch_out; // CCW FRONT LEFT - motor_out[CH_4] = g.rc_3.radio_out - roll_out + pitch_out; // CCW FRONT RIGHT + motor_out[MOT_1] = g.rc_3.radio_out + g.rc_2.pwm_out; // CW FRONT + motor_out[MOT_5] = g.rc_3.radio_out + roll_out + pitch_out; // CCW FRONT LEFT + motor_out[MOT_4] = g.rc_3.radio_out - roll_out + pitch_out; // CCW FRONT RIGHT //Back side - motor_out[CH_2] = g.rc_3.radio_out - g.rc_2.pwm_out; // CCW BACK - motor_out[CH_3] = g.rc_3.radio_out + roll_out - pitch_out; // CW, BACK LEFT - motor_out[CH_8] = g.rc_3.radio_out - roll_out - pitch_out; // CW BACK RIGHT + motor_out[MOT_2] = g.rc_3.radio_out - g.rc_2.pwm_out; // CCW BACK + motor_out[MOT_3] = g.rc_3.radio_out + roll_out - pitch_out; // CW, BACK LEFT + motor_out[MOT_6] = g.rc_3.radio_out - roll_out - pitch_out; // CW BACK RIGHT } // Yaw - motor_out[CH_2] += g.rc_4.pwm_out; // CCW - motor_out[CH_7] += g.rc_4.pwm_out; // CCW - motor_out[CH_4] += g.rc_4.pwm_out; // CCW + motor_out[MOT_2] += g.rc_4.pwm_out; // CCW + motor_out[MOT_5] += g.rc_4.pwm_out; // CCW + motor_out[MOT_4] += g.rc_4.pwm_out; // CCW - motor_out[CH_3] -= g.rc_4.pwm_out; // CW - motor_out[CH_1] -= g.rc_4.pwm_out; // CW - motor_out[CH_8] -= g.rc_4.pwm_out; // CW + motor_out[MOT_3] -= g.rc_4.pwm_out; // CW + motor_out[MOT_1] -= g.rc_4.pwm_out; // CW + motor_out[MOT_6] -= g.rc_4.pwm_out; // CW // Tridge's stability patch - for (int i = CH_1; i<=CH_8; i++) { - if(i == CH_5 || i == CH_6) - continue; - if (motor_out[i] > out_max) { - // note that i^1 is the opposite motor - motor_out[i^1] -= motor_out[i] - out_max; - motor_out[i] = out_max; + for (int m = 0; m <= 6; m++) { + int c = ch_of_mot(m); + int c_opp = ch_of_mot(m^1); // m^1 is the opposite motor. c_opp is channel of opposite motor. + if (motor_out[c] > out_max) { + motor_out[c_opp] -= motor_out[c] - out_max; + motor_out[c] = out_max; } } // limit output so motors don't stop - motor_out[CH_1] = max(motor_out[CH_1], out_min); - motor_out[CH_2] = max(motor_out[CH_2], out_min); - motor_out[CH_3] = max(motor_out[CH_3], out_min); - motor_out[CH_4] = max(motor_out[CH_4], out_min); - motor_out[CH_7] = max(motor_out[CH_7], out_min); - motor_out[CH_8] = max(motor_out[CH_8], out_min); + motor_out[MOT_1] = max(motor_out[MOT_1], out_min); + motor_out[MOT_2] = max(motor_out[MOT_2], out_min); + motor_out[MOT_3] = max(motor_out[MOT_3], out_min); + motor_out[MOT_4] = max(motor_out[MOT_4], out_min); + motor_out[MOT_5] = max(motor_out[MOT_5], out_min); + motor_out[MOT_6] = max(motor_out[MOT_6], out_min); #if CUT_MOTORS == ENABLED // if we are not sending a throttle output, we cut the motors if(g.rc_3.servo_out == 0){ - motor_out[CH_1] = g.rc_3.radio_min; - motor_out[CH_2] = g.rc_3.radio_min; - motor_out[CH_3] = g.rc_3.radio_min; - motor_out[CH_4] = g.rc_3.radio_min; - motor_out[CH_7] = g.rc_3.radio_min; - motor_out[CH_8] = g.rc_3.radio_min; + motor_out[MOT_1] = g.rc_3.radio_min; + motor_out[MOT_2] = g.rc_3.radio_min; + motor_out[MOT_3] = g.rc_3.radio_min; + motor_out[MOT_4] = g.rc_3.radio_min; + motor_out[MOT_5] = g.rc_3.radio_min; + motor_out[MOT_6] = g.rc_3.radio_min; } #endif // this filter slows the acceleration of motors vs the deceleration // Idea by Denny Rowland to help with his Yaw issue - for(int8_t i = CH_1; i <= CH_8; i++ ) { - if(i == CH_5 || i == CH_6) - continue; - if(motor_filtered[i] < motor_out[i]){ - motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2; + for(int8_t m = 0; m <= 6; m++ ) { + int c = ch_of_mot(m); + if(motor_filtered[c] < motor_out[c]){ + motor_filtered[c] = (motor_out[c] + motor_filtered[c]) / 2; }else{ // don't filter - motor_filtered[i] = motor_out[i]; + motor_filtered[c] = motor_out[c]; } } - APM_RC.OutputCh(CH_1, motor_filtered[CH_1]); - APM_RC.OutputCh(CH_2, motor_filtered[CH_2]); - APM_RC.OutputCh(CH_3, motor_filtered[CH_3]); - APM_RC.OutputCh(CH_4, motor_filtered[CH_4]); - APM_RC.OutputCh(CH_7, motor_filtered[CH_7]); - APM_RC.OutputCh(CH_8, motor_filtered[CH_8]); + APM_RC.OutputCh(MOT_1, motor_filtered[MOT_1]); + APM_RC.OutputCh(MOT_2, motor_filtered[MOT_2]); + APM_RC.OutputCh(MOT_3, motor_filtered[MOT_3]); + APM_RC.OutputCh(MOT_4, motor_filtered[MOT_4]); + APM_RC.OutputCh(MOT_5, motor_filtered[MOT_5]); + APM_RC.OutputCh(MOT_6, motor_filtered[MOT_6]); #if INSTANT_PWM == 1 // InstantPWM @@ -140,43 +138,43 @@ static void output_motors_disarmed() } // Send commands to motors - APM_RC.OutputCh(CH_1, g.rc_3.radio_min); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min); - APM_RC.OutputCh(CH_7, g.rc_3.radio_min); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_1, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_2, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_3, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_4, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_5, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_6, g.rc_3.radio_min); } static void output_motor_test() { - motor_out[CH_1] = g.rc_3.radio_min; - motor_out[CH_2] = g.rc_3.radio_min; - motor_out[CH_3] = g.rc_3.radio_min; - motor_out[CH_4] = g.rc_3.radio_min; - motor_out[CH_7] = g.rc_3.radio_min; - motor_out[CH_8] = g.rc_3.radio_min; + motor_out[MOT_1] = g.rc_3.radio_min; + motor_out[MOT_2] = g.rc_3.radio_min; + motor_out[MOT_3] = g.rc_3.radio_min; + motor_out[MOT_4] = g.rc_3.radio_min; + motor_out[MOT_5] = g.rc_3.radio_min; + motor_out[MOT_6] = g.rc_3.radio_min; if(g.frame_orientation == X_FRAME){ // 31 // 24 if(g.rc_1.control_in > 3000){ // right - motor_out[CH_1] += 100; + motor_out[MOT_1] += 100; } if(g.rc_1.control_in < -3000){ // left - motor_out[CH_2] += 100; + motor_out[MOT_2] += 100; } if(g.rc_2.control_in > 3000){ // back - motor_out[CH_8] += 100; - motor_out[CH_4] += 100; + motor_out[MOT_6] += 100; + motor_out[MOT_4] += 100; } if(g.rc_2.control_in < -3000){ // front - motor_out[CH_7] += 100; - motor_out[CH_3] += 100; + motor_out[MOT_5] += 100; + motor_out[MOT_3] += 100; } }else{ @@ -184,56 +182,56 @@ static void output_motor_test() // 2 1 // 4 if(g.rc_1.control_in > 3000){ // right - motor_out[CH_4] += 100; - motor_out[CH_8] += 100; + motor_out[MOT_4] += 100; + motor_out[MOT_6] += 100; } if(g.rc_1.control_in < -3000){ // left - motor_out[CH_7] += 100; - motor_out[CH_3] += 100; + motor_out[MOT_5] += 100; + motor_out[MOT_3] += 100; } if(g.rc_2.control_in > 3000){ // back - motor_out[CH_2] += 100; + motor_out[MOT_2] += 100; } if(g.rc_2.control_in < -3000){ // front - motor_out[CH_1] += 100; + motor_out[MOT_1] += 100; } } - APM_RC.OutputCh(CH_1, motor_out[CH_1]); - APM_RC.OutputCh(CH_2, motor_out[CH_2]); - APM_RC.OutputCh(CH_3, motor_out[CH_3]); - APM_RC.OutputCh(CH_4, motor_out[CH_4]); - APM_RC.OutputCh(CH_7, motor_out[CH_7]); - APM_RC.OutputCh(CH_8, motor_out[CH_8]); + APM_RC.OutputCh(MOT_1, motor_out[MOT_1]); + APM_RC.OutputCh(MOT_2, motor_out[MOT_2]); + APM_RC.OutputCh(MOT_3, motor_out[MOT_3]); + APM_RC.OutputCh(MOT_4, motor_out[MOT_4]); + APM_RC.OutputCh(MOT_5, motor_out[MOT_5]); + APM_RC.OutputCh(MOT_6, motor_out[MOT_6]); } /* - APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100); + APM_RC.OutputCh(MOT_2, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100); delay(1000); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min); - APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100); + APM_RC.OutputCh(MOT_3, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100); delay(1000); - APM_RC.OutputCh(CH_7, g.rc_3.radio_min); - APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100); + APM_RC.OutputCh(MOT_5, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100); delay(1000); - APM_RC.OutputCh(CH_1, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100); + APM_RC.OutputCh(MOT_1, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100); delay(1000); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100); + APM_RC.OutputCh(MOT_4, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100); delay(1000); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100); + APM_RC.OutputCh(MOT_6, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100); delay(1000); } */ diff --git a/ArduCopter/motors_octa.pde b/ArduCopter/motors_octa.pde index c7705b43a5..5b8be50425 100644 --- a/ArduCopter/motors_octa.pde +++ b/ArduCopter/motors_octa.pde @@ -5,8 +5,8 @@ static void init_motors_out() { #if INSTANT_PWM == 0 - APM_RC.SetFastOutputChannels( MSK_CH_1 | MSK_CH_2 | MSK_CH_3 | MSK_CH_4 - | MSK_CH_7 | MSK_CH_8 | MSK_CH_10 | MSK_CH_11 ); + APM_RC.SetFastOutputChannels( _BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) | _BV(MOT_4) + | _BV(MOT_5) | _BV(MOT_6) | _BV(MOT_7) | _BV(MOT_8) ); #endif } @@ -32,40 +32,40 @@ static void output_motors_armed() pitch_out = (float)g.rc_2.pwm_out * 0.4; //Front side - motor_out[CH_1] = g.rc_3.radio_out + g.rc_2.pwm_out - roll_out; // CW FRONT RIGHT - motor_out[CH_7] = g.rc_3.radio_out + g.rc_2.pwm_out + roll_out; // CCW FRONT LEFT + motor_out[MOT_1] = g.rc_3.radio_out + g.rc_2.pwm_out - roll_out; // CW FRONT RIGHT + motor_out[MOT_5] = g.rc_3.radio_out + g.rc_2.pwm_out + roll_out; // CCW FRONT LEFT //Back side - motor_out[CH_2] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out; // CW BACK LEFT - motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out; // CCW BACK RIGHT + motor_out[MOT_2] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out; // CW BACK LEFT + motor_out[MOT_4] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out; // CCW BACK RIGHT //Left side - motor_out[CH_10] = g.rc_3.radio_out + g.rc_1.pwm_out + pitch_out; // CW LEFT FRONT - motor_out[CH_8] = g.rc_3.radio_out + g.rc_1.pwm_out - pitch_out; // CCW LEFT BACK + motor_out[MOT_7] = g.rc_3.radio_out + g.rc_1.pwm_out + pitch_out; // CW LEFT FRONT + motor_out[MOT_6] = g.rc_3.radio_out + g.rc_1.pwm_out - pitch_out; // CCW LEFT BACK //Right side - motor_out[CH_11] = g.rc_3.radio_out - g.rc_1.pwm_out - pitch_out; // CW RIGHT BACK - motor_out[CH_3] = g.rc_3.radio_out - g.rc_1.pwm_out + pitch_out; // CCW RIGHT FRONT + motor_out[MOT_8] = g.rc_3.radio_out - g.rc_1.pwm_out - pitch_out; // CW RIGHT BACK + motor_out[MOT_3] = g.rc_3.radio_out - g.rc_1.pwm_out + pitch_out; // CCW RIGHT FRONT }else if(g.frame_orientation == PLUS_FRAME){ roll_out = (float)g.rc_1.pwm_out * 0.71; pitch_out = (float)g.rc_2.pwm_out * 0.71; //Front side - motor_out[CH_1] = g.rc_3.radio_out + g.rc_2.pwm_out; // CW FRONT - motor_out[CH_3] = g.rc_3.radio_out - roll_out + pitch_out; // CCW FRONT RIGHT - motor_out[CH_7] = g.rc_3.radio_out + roll_out + pitch_out; // CCW FRONT LEFT + motor_out[MOT_1] = g.rc_3.radio_out + g.rc_2.pwm_out; // CW FRONT + motor_out[MOT_3] = g.rc_3.radio_out - roll_out + pitch_out; // CCW FRONT RIGHT + motor_out[MOT_5] = g.rc_3.radio_out + roll_out + pitch_out; // CCW FRONT LEFT //Left side - motor_out[CH_10] = g.rc_3.radio_out + g.rc_1.pwm_out; // CW LEFT + motor_out[MOT_7] = g.rc_3.radio_out + g.rc_1.pwm_out; // CW LEFT //Right side - motor_out[CH_11] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW RIGHT + motor_out[MOT_8] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW RIGHT //Back side - motor_out[CH_2] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW BACK - motor_out[CH_4] = g.rc_3.radio_out - roll_out - pitch_out; // CCW BACK RIGHT - motor_out[CH_8] = g.rc_3.radio_out + roll_out - pitch_out; // CCW BACK LEFT + motor_out[MOT_2] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW BACK + motor_out[MOT_4] = g.rc_3.radio_out - roll_out - pitch_out; // CCW BACK RIGHT + motor_out[MOT_6] = g.rc_3.radio_out + roll_out - pitch_out; // CCW BACK LEFT }else if(g.frame_orientation == V_FRAME){ @@ -83,92 +83,91 @@ static void output_motors_armed() pitch_out4 = (float)g.rc_2.pwm_out * 0.98; //Front side - motor_out[CH_10] = g.rc_3.radio_out + g.rc_2.pwm_out - roll_out; // CW FRONT RIGHT - motor_out[CH_7] = g.rc_3.radio_out + g.rc_2.pwm_out + roll_out; // CCW FRONT LEFT + motor_out[MOT_7] = g.rc_3.radio_out + g.rc_2.pwm_out - roll_out; // CW FRONT RIGHT + motor_out[MOT_5] = g.rc_3.radio_out + g.rc_2.pwm_out + roll_out; // CCW FRONT LEFT //Left side - motor_out[CH_1] = g.rc_3.radio_out + g.rc_1.pwm_out + pitch_out2; // CW LEFT FRONT - motor_out[CH_3] = g.rc_3.radio_out + g.rc_1.pwm_out - pitch_out3; // CCW LEFT BACK + motor_out[MOT_1] = g.rc_3.radio_out + g.rc_1.pwm_out + pitch_out2; // CW LEFT FRONT + motor_out[MOT_3] = g.rc_3.radio_out + g.rc_1.pwm_out - pitch_out3; // CCW LEFT BACK //Right side - motor_out[CH_2] = g.rc_3.radio_out - g.rc_1.pwm_out - pitch_out3; // CW RIGHT BACK - motor_out[CH_8] = g.rc_3.radio_out - g.rc_1.pwm_out + pitch_out2; // CCW RIGHT FRONT + motor_out[MOT_2] = g.rc_3.radio_out - g.rc_1.pwm_out - pitch_out3; // CW RIGHT BACK + motor_out[MOT_6] = g.rc_3.radio_out - g.rc_1.pwm_out + pitch_out2; // CCW RIGHT FRONT //Back side - motor_out[CH_11] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out4; // CW BACK LEFT - motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out4; // CCW BACK RIGHT + motor_out[MOT_8] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out4; // CW BACK LEFT + motor_out[MOT_4] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out4; // CCW BACK RIGHT } // Yaw - motor_out[CH_3] += g.rc_4.pwm_out; // CCW - motor_out[CH_4] += g.rc_4.pwm_out; // CCW - motor_out[CH_7] += g.rc_4.pwm_out; // CCW - motor_out[CH_8] += g.rc_4.pwm_out; // CCW + motor_out[MOT_3] += g.rc_4.pwm_out; // CCW + motor_out[MOT_4] += g.rc_4.pwm_out; // CCW + motor_out[MOT_5] += g.rc_4.pwm_out; // CCW + motor_out[MOT_6] += g.rc_4.pwm_out; // CCW - motor_out[CH_1] -= g.rc_4.pwm_out; // CW - motor_out[CH_2] -= g.rc_4.pwm_out; // CW - motor_out[CH_10] -= g.rc_4.pwm_out; // CW - motor_out[CH_11] -= g.rc_4.pwm_out; // CW + motor_out[MOT_1] -= g.rc_4.pwm_out; // CW + motor_out[MOT_2] -= g.rc_4.pwm_out; // CW + motor_out[MOT_7] -= g.rc_4.pwm_out; // CW + motor_out[MOT_8] -= g.rc_4.pwm_out; // CW // TODO add stability patch - motor_out[CH_1] = min(motor_out[CH_1], out_max); - motor_out[CH_2] = min(motor_out[CH_2], out_max); - motor_out[CH_3] = min(motor_out[CH_3], out_max); - motor_out[CH_4] = min(motor_out[CH_4], out_max); - motor_out[CH_7] = min(motor_out[CH_7], out_max); - motor_out[CH_8] = min(motor_out[CH_8], out_max); - motor_out[CH_10] = min(motor_out[CH_10], out_max); - motor_out[CH_11] = min(motor_out[CH_11], out_max); + motor_out[MOT_1] = min(motor_out[MOT_1], out_max); + motor_out[MOT_2] = min(motor_out[MOT_2], out_max); + motor_out[MOT_3] = min(motor_out[MOT_3], out_max); + motor_out[MOT_4] = min(motor_out[MOT_4], out_max); + motor_out[MOT_5] = min(motor_out[MOT_5], out_max); + motor_out[MOT_6] = min(motor_out[MOT_6], out_max); + motor_out[MOT_7] = min(motor_out[MOT_7], out_max); + motor_out[MOT_8] = min(motor_out[MOT_8], out_max); // limit output so motors don't stop - motor_out[CH_1] = max(motor_out[CH_1], out_min); - motor_out[CH_2] = max(motor_out[CH_2], out_min); - motor_out[CH_3] = max(motor_out[CH_3], out_min); - motor_out[CH_4] = max(motor_out[CH_4], out_min); - motor_out[CH_7] = max(motor_out[CH_7], out_min); - motor_out[CH_8] = max(motor_out[CH_8], out_min); - motor_out[CH_10] = max(motor_out[CH_10], out_min); - motor_out[CH_11] = max(motor_out[CH_11], out_min); + motor_out[MOT_1] = max(motor_out[MOT_1], out_min); + motor_out[MOT_2] = max(motor_out[MOT_2], out_min); + motor_out[MOT_3] = max(motor_out[MOT_3], out_min); + motor_out[MOT_4] = max(motor_out[MOT_4], out_min); + motor_out[MOT_5] = max(motor_out[MOT_5], out_min); + motor_out[MOT_6] = max(motor_out[MOT_6], out_min); + motor_out[MOT_7] = max(motor_out[MOT_7], out_min); + motor_out[MOT_8] = max(motor_out[MOT_8], out_min); #if CUT_MOTORS == ENABLED // if we are not sending a throttle output, we cut the motors if(g.rc_3.servo_out == 0){ - motor_out[CH_1] = g.rc_3.radio_min; - motor_out[CH_2] = g.rc_3.radio_min; - motor_out[CH_3] = g.rc_3.radio_min; - motor_out[CH_4] = g.rc_3.radio_min; - motor_out[CH_7] = g.rc_3.radio_min; - motor_out[CH_8] = g.rc_3.radio_min; - motor_out[CH_10] = g.rc_3.radio_min; - motor_out[CH_11] = g.rc_3.radio_min; + motor_out[MOT_1] = g.rc_3.radio_min; + motor_out[MOT_2] = g.rc_3.radio_min; + motor_out[MOT_3] = g.rc_3.radio_min; + motor_out[MOT_4] = g.rc_3.radio_min; + motor_out[MOT_5] = g.rc_3.radio_min; + motor_out[MOT_6] = g.rc_3.radio_min; + motor_out[MOT_7] = g.rc_3.radio_min; + motor_out[MOT_8] = g.rc_3.radio_min; } #endif // this filter slows the acceleration of motors vs the deceleration // Idea by Denny Rowland to help with his Yaw issue - for(int8_t i = CH_1; i <= CH_11; i++ ) { - if(i == CH_5 || i == CH_6 || i == CH_9) - continue; - if(motor_filtered[i] < motor_out[i]){ - motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2; + for(int8_t m = 0; m <= 8; m++ ) { + int c = ch_of_mot(m); + if(motor_filtered[c] < motor_out[c]){ + motor_filtered[c] = (motor_out[c] + motor_filtered[c]) / 2; }else{ // don't filter - motor_filtered[i] = motor_out[i]; + motor_filtered[c] = motor_out[c]; } } - APM_RC.OutputCh(CH_1, motor_filtered[CH_1]); - APM_RC.OutputCh(CH_2, motor_filtered[CH_2]); - APM_RC.OutputCh(CH_3, motor_filtered[CH_3]); - APM_RC.OutputCh(CH_4, motor_filtered[CH_4]); - APM_RC.OutputCh(CH_7, motor_filtered[CH_7]); - APM_RC.OutputCh(CH_8, motor_filtered[CH_8]); - APM_RC.OutputCh(CH_10, motor_filtered[CH_10]); - APM_RC.OutputCh(CH_11, motor_filtered[CH_11]); + APM_RC.OutputCh(MOT_1, motor_filtered[MOT_1]); + APM_RC.OutputCh(MOT_2, motor_filtered[MOT_2]); + APM_RC.OutputCh(MOT_3, motor_filtered[MOT_3]); + APM_RC.OutputCh(MOT_4, motor_filtered[MOT_4]); + APM_RC.OutputCh(MOT_5, motor_filtered[MOT_5]); + APM_RC.OutputCh(MOT_6, motor_filtered[MOT_6]); + APM_RC.OutputCh(MOT_7, motor_filtered[MOT_7]); + APM_RC.OutputCh(MOT_8, motor_filtered[MOT_8]); #if INSTANT_PWM == 1 // InstantPWM @@ -192,86 +191,86 @@ static void output_motors_disarmed() } // Send commands to motors - APM_RC.OutputCh(CH_1, g.rc_3.radio_min); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_7, g.rc_3.radio_min); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min); - APM_RC.OutputCh(CH_11, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_1, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_2, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_5, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_6, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_8, g.rc_3.radio_min); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min); - APM_RC.OutputCh(CH_10, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_3, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_4, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_7, g.rc_3.radio_min); } static void output_motor_test() { if( g.frame_orientation == X_FRAME || g.frame_orientation == PLUS_FRAME ) { - APM_RC.OutputCh(CH_7, g.rc_3.radio_min); - APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100); + APM_RC.OutputCh(MOT_5, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100); delay(1000); - APM_RC.OutputCh(CH_1, g.rc_3.radio_min); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100); + APM_RC.OutputCh(MOT_1, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100); delay(1000); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min); - APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 100); + APM_RC.OutputCh(MOT_3, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_8, g.rc_3.radio_min + 100); delay(1000); - APM_RC.OutputCh(CH_11, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100); + APM_RC.OutputCh(MOT_8, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100); delay(1000); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100); + APM_RC.OutputCh(MOT_4, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100); delay(1000); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100); + APM_RC.OutputCh(MOT_2, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100); delay(1000); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min); - APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 100); + APM_RC.OutputCh(MOT_6, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_7, g.rc_3.radio_min + 100); delay(1000); - APM_RC.OutputCh(CH_10, g.rc_3.radio_min); - APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100); + APM_RC.OutputCh(MOT_7, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100); delay(1000); } if( g.frame_orientation == V_FRAME ) { - APM_RC.OutputCh(CH_7, g.rc_3.radio_min); - APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 100); + APM_RC.OutputCh(MOT_5, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_7, g.rc_3.radio_min + 100); delay(1000); - APM_RC.OutputCh(CH_10, g.rc_3.radio_min); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100); + APM_RC.OutputCh(MOT_7, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100); delay(1000); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100); + APM_RC.OutputCh(MOT_6, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100); delay(1000); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100); + APM_RC.OutputCh(MOT_2, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100); delay(1000); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min); - APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 100); + APM_RC.OutputCh(MOT_4, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_8, g.rc_3.radio_min + 100); delay(1000); - APM_RC.OutputCh(CH_11, g.rc_3.radio_min); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100); + APM_RC.OutputCh(MOT_8, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100); delay(1000); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min); - APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100); + APM_RC.OutputCh(MOT_3, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100); delay(1000); - APM_RC.OutputCh(CH_1, g.rc_3.radio_min); - APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100); + APM_RC.OutputCh(MOT_1, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100); delay(1000); } } diff --git a/ArduCopter/motors_octa_quad.pde b/ArduCopter/motors_octa_quad.pde index 732dcdf64d..a8c4331ee0 100644 --- a/ArduCopter/motors_octa_quad.pde +++ b/ArduCopter/motors_octa_quad.pde @@ -5,8 +5,8 @@ static void init_motors_out() { #if INSTANT_PWM == 0 - APM_RC.SetFastOutputChannels( MSK_CH_1 | MSK_CH_2 | MSK_CH_3 | MSK_CH_4 - | MSK_CH_7 | MSK_CH_8 | MSK_CH_10 | MSK_CH_11 ); + APM_RC.SetFastOutputChannels( _BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) | _BV(MOT_4) + | _BV(MOT_5) | _BV(MOT_6) | _BV(MOT_7) | _BV(MOT_8) ); #endif } @@ -32,20 +32,20 @@ static void output_motors_armed() pitch_out = (float)g.rc_2.pwm_out * .707; // Front Left - motor_out[CH_7] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out + pitch_out); // CCW TOP - motor_out[CH_8] = g.rc_3.radio_out + roll_out + pitch_out; // CW + motor_out[MOT_5] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out + pitch_out); // CCW TOP + motor_out[MOT_6] = g.rc_3.radio_out + roll_out + pitch_out; // CW // Front Right - motor_out[CH_10] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out + pitch_out); // CCW TOP - motor_out[CH_11] = g.rc_3.radio_out - roll_out + pitch_out; // CW + motor_out[MOT_7] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out + pitch_out); // CCW TOP + motor_out[MOT_8] = g.rc_3.radio_out - roll_out + pitch_out; // CW // Back Left - motor_out[CH_3] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out - pitch_out); // CCW TOP - motor_out[CH_4] = g.rc_3.radio_out + roll_out - pitch_out; // CW + motor_out[MOT_3] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out - pitch_out); // CCW TOP + motor_out[MOT_4] = g.rc_3.radio_out + roll_out - pitch_out; // CW // Back Right - motor_out[CH_1] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out - pitch_out); // CCW TOP - motor_out[CH_2] = g.rc_3.radio_out - roll_out - pitch_out; // CW + motor_out[MOT_1] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out - pitch_out); // CCW TOP + motor_out[MOT_2] = g.rc_3.radio_out - roll_out - pitch_out; // CW @@ -54,73 +54,72 @@ static void output_motors_armed() pitch_out = g.rc_2.pwm_out; // Left - motor_out[CH_7] = (g.rc_3.radio_out * g.top_bottom_ratio) - roll_out; // CCW TOP - motor_out[CH_8] = g.rc_3.radio_out - roll_out; // CW + motor_out[MOT_5] = (g.rc_3.radio_out * g.top_bottom_ratio) - roll_out; // CCW TOP + motor_out[MOT_6] = g.rc_3.radio_out - roll_out; // CW // Right - motor_out[CH_1] = (g.rc_3.radio_out * g.top_bottom_ratio) + roll_out; // CCW TOP - motor_out[CH_2] = g.rc_3.radio_out + roll_out; // CW + motor_out[MOT_1] = (g.rc_3.radio_out * g.top_bottom_ratio) + roll_out; // CCW TOP + motor_out[MOT_2] = g.rc_3.radio_out + roll_out; // CW // Front - motor_out[CH_10] = (g.rc_3.radio_out * g.top_bottom_ratio) + pitch_out; // CCW TOP - motor_out[CH_11] = g.rc_3.radio_out + pitch_out; // CW + motor_out[MOT_7] = (g.rc_3.radio_out * g.top_bottom_ratio) + pitch_out; // CCW TOP + motor_out[MOT_8] = g.rc_3.radio_out + pitch_out; // CW // Back - motor_out[CH_3] = (g.rc_3.radio_out * g.top_bottom_ratio) - pitch_out; // CCW TOP - motor_out[CH_4] = g.rc_3.radio_out - pitch_out; // CW + motor_out[MOT_3] = (g.rc_3.radio_out * g.top_bottom_ratio) - pitch_out; // CCW TOP + motor_out[MOT_4] = g.rc_3.radio_out - pitch_out; // CW } // Yaw - motor_out[CH_1] += g.rc_4.pwm_out; // CCW - motor_out[CH_3] += g.rc_4.pwm_out; // CCW - motor_out[CH_7] += g.rc_4.pwm_out; // CCW - motor_out[CH_10] += g.rc_4.pwm_out; // CCW + motor_out[MOT_1] += g.rc_4.pwm_out; // CCW + motor_out[MOT_3] += g.rc_4.pwm_out; // CCW + motor_out[MOT_5] += g.rc_4.pwm_out; // CCW + motor_out[MOT_7] += g.rc_4.pwm_out; // CCW - motor_out[CH_2] -= g.rc_4.pwm_out; // CW - motor_out[CH_4] -= g.rc_4.pwm_out; // CW - motor_out[CH_8] -= g.rc_4.pwm_out; // CW - motor_out[CH_11] -= g.rc_4.pwm_out; // CW + motor_out[MOT_2] -= g.rc_4.pwm_out; // CW + motor_out[MOT_4] -= g.rc_4.pwm_out; // CW + motor_out[MOT_6] -= g.rc_4.pwm_out; // CW + motor_out[MOT_8] -= g.rc_4.pwm_out; // CW // TODO add stability patch - motor_out[CH_1] = min(motor_out[CH_1], out_max); - motor_out[CH_2] = min(motor_out[CH_2], out_max); - motor_out[CH_3] = min(motor_out[CH_3], out_max); - motor_out[CH_4] = min(motor_out[CH_4], out_max); - motor_out[CH_7] = min(motor_out[CH_7], out_max); - motor_out[CH_8] = min(motor_out[CH_8], out_max); - motor_out[CH_10] = min(motor_out[CH_10], out_max); - motor_out[CH_11] = min(motor_out[CH_11], out_max); + motor_out[MOT_1] = min(motor_out[MOT_1], out_max); + motor_out[MOT_2] = min(motor_out[MOT_2], out_max); + motor_out[MOT_3] = min(motor_out[MOT_3], out_max); + motor_out[MOT_4] = min(motor_out[MOT_4], out_max); + motor_out[MOT_5] = min(motor_out[MOT_5], out_max); + motor_out[MOT_6] = min(motor_out[MOT_6], out_max); + motor_out[MOT_7] = min(motor_out[MOT_7], out_max); + motor_out[MOT_8] = min(motor_out[MOT_8], out_max); // limit output so motors don't stop - motor_out[CH_1] = max(motor_out[CH_1], out_min); - motor_out[CH_2] = max(motor_out[CH_2], out_min); - motor_out[CH_3] = max(motor_out[CH_3], out_min); - motor_out[CH_4] = max(motor_out[CH_4], out_min); - motor_out[CH_7] = max(motor_out[CH_7], out_min); - motor_out[CH_8] = max(motor_out[CH_8], out_min); - motor_out[CH_10] = max(motor_out[CH_10], out_min); - motor_out[CH_11] = max(motor_out[CH_11], out_min); + motor_out[MOT_1] = max(motor_out[MOT_1], out_min); + motor_out[MOT_2] = max(motor_out[MOT_2], out_min); + motor_out[MOT_3] = max(motor_out[MOT_3], out_min); + motor_out[MOT_4] = max(motor_out[MOT_4], out_min); + motor_out[MOT_5] = max(motor_out[MOT_5], out_min); + motor_out[MOT_6] = max(motor_out[MOT_6], out_min); + motor_out[MOT_7] = max(motor_out[MOT_7], out_min); + motor_out[MOT_8] = max(motor_out[MOT_8], out_min); #if CUT_MOTORS == ENABLED // if we are not sending a throttle output, we cut the motors if(g.rc_3.servo_out == 0){ - motor_out[CH_1] = g.rc_3.radio_min; - motor_out[CH_2] = g.rc_3.radio_min; - motor_out[CH_3] = g.rc_3.radio_min; - motor_out[CH_4] = g.rc_3.radio_min; - motor_out[CH_7] = g.rc_3.radio_min; - motor_out[CH_8] = g.rc_3.radio_min; - motor_out[CH_10] = g.rc_3.radio_min; - motor_out[CH_11] = g.rc_3.radio_min; + motor_out[MOT_1] = g.rc_3.radio_min; + motor_out[MOT_2] = g.rc_3.radio_min; + motor_out[MOT_3] = g.rc_3.radio_min; + motor_out[MOT_4] = g.rc_3.radio_min; + motor_out[MOT_5] = g.rc_3.radio_min; + motor_out[MOT_6] = g.rc_3.radio_min; + motor_out[MOT_7] = g.rc_3.radio_min; + motor_out[MOT_8] = g.rc_3.radio_min; } #endif // this filter slows the acceleration of motors vs the deceleration // Idea by Denny Rowland to help with his Yaw issue - for(int8_t i = CH_1; i <= CH_11; i++ ) { - if(i == CH_5 || i == CH_6 || i == CH_9) - continue; + for(int8_t m = 0; m <= 8; m++ ) { + int i = ch_of_mot(m); if(motor_filtered[i] < motor_out[i]){ motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2; }else{ @@ -129,14 +128,14 @@ static void output_motors_armed() } } - APM_RC.OutputCh(CH_1, motor_filtered[CH_1]); - APM_RC.OutputCh(CH_2, motor_filtered[CH_2]); - APM_RC.OutputCh(CH_3, motor_filtered[CH_3]); - APM_RC.OutputCh(CH_4, motor_filtered[CH_4]); - APM_RC.OutputCh(CH_7, motor_filtered[CH_7]); - APM_RC.OutputCh(CH_8, motor_filtered[CH_8]); - APM_RC.OutputCh(CH_10, motor_filtered[CH_10]); - APM_RC.OutputCh(CH_11, motor_filtered[CH_11]); + APM_RC.OutputCh(MOT_1, motor_filtered[MOT_1]); + APM_RC.OutputCh(MOT_2, motor_filtered[MOT_2]); + APM_RC.OutputCh(MOT_3, motor_filtered[MOT_3]); + APM_RC.OutputCh(MOT_4, motor_filtered[MOT_4]); + APM_RC.OutputCh(MOT_5, motor_filtered[MOT_5]); + APM_RC.OutputCh(MOT_6, motor_filtered[MOT_6]); + APM_RC.OutputCh(MOT_7, motor_filtered[MOT_7]); + APM_RC.OutputCh(MOT_8, motor_filtered[MOT_8]); #if INSTANT_PWM == 1 // InstantPWM @@ -160,48 +159,48 @@ static void output_motors_disarmed() } // Send commands to motors - APM_RC.OutputCh(CH_1, g.rc_3.radio_min); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min); - APM_RC.OutputCh(CH_7, g.rc_3.radio_min); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min); - APM_RC.OutputCh(CH_10, g.rc_3.radio_min); - APM_RC.OutputCh(CH_11, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_1, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_2, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_3, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_4, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_5, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_6, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_7, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_8, g.rc_3.radio_min); } static void output_motor_test() { - APM_RC.OutputCh(CH_8, g.rc_3.radio_min); - APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 100); + APM_RC.OutputCh(MOT_6, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_7, g.rc_3.radio_min + 100); delay(1000); - APM_RC.OutputCh(CH_10, g.rc_3.radio_min); - APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 100); + APM_RC.OutputCh(MOT_7, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_8, g.rc_3.radio_min + 100); delay(1000); - APM_RC.OutputCh(CH_11, g.rc_3.radio_min); - APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100); + APM_RC.OutputCh(MOT_8, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100); delay(1000); - APM_RC.OutputCh(CH_1, g.rc_3.radio_min); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100); + APM_RC.OutputCh(MOT_1, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100); delay(1000); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100); + APM_RC.OutputCh(MOT_2, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100); delay(1000); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100); + APM_RC.OutputCh(MOT_3, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100); delay(1000); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min); - APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100); + APM_RC.OutputCh(MOT_4, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100); delay(1000); - APM_RC.OutputCh(CH_7, g.rc_3.radio_min); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100); + APM_RC.OutputCh(MOT_5, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100); delay(1000); } diff --git a/ArduCopter/motors_quad.pde b/ArduCopter/motors_quad.pde index 5bcf2b740b..e613cc6aa7 100644 --- a/ArduCopter/motors_quad.pde +++ b/ArduCopter/motors_quad.pde @@ -5,7 +5,7 @@ static void init_motors_out() { #if INSTANT_PWM == 0 - APM_RC.SetFastOutputChannels( MSK_CH_1 | MSK_CH_2 | MSK_CH_3 | MSK_CH_4 ); + APM_RC.SetFastOutputChannels( _BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) | _BV(MOT_4) ); #endif } @@ -33,12 +33,12 @@ static void output_motors_armed() pitch_out = g.rc_2.pwm_out * .707; // left - motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // FRONT - motor_out[CH_2] = g.rc_3.radio_out + roll_out - pitch_out; // BACK + motor_out[MOT_3] = g.rc_3.radio_out + roll_out + pitch_out; // FRONT + motor_out[MOT_2] = g.rc_3.radio_out + roll_out - pitch_out; // BACK // right - motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out; // FRONT - motor_out[CH_4] = g.rc_3.radio_out - roll_out - pitch_out; // BACK + motor_out[MOT_1] = g.rc_3.radio_out - roll_out + pitch_out; // FRONT + motor_out[MOT_4] = g.rc_3.radio_out - roll_out - pitch_out; // BACK }else{ @@ -46,20 +46,20 @@ static void output_motors_armed() pitch_out = g.rc_2.pwm_out; // right motor - motor_out[CH_1] = g.rc_3.radio_out - roll_out; + motor_out[MOT_1] = g.rc_3.radio_out - roll_out; // left motor - motor_out[CH_2] = g.rc_3.radio_out + roll_out; + motor_out[MOT_2] = g.rc_3.radio_out + roll_out; // front motor - motor_out[CH_3] = g.rc_3.radio_out + pitch_out; + motor_out[MOT_3] = g.rc_3.radio_out + pitch_out; // back motor - motor_out[CH_4] = g.rc_3.radio_out - pitch_out; + motor_out[MOT_4] = g.rc_3.radio_out - pitch_out; } // Yaw input - motor_out[CH_1] += g.rc_4.pwm_out; // CCW - motor_out[CH_2] += g.rc_4.pwm_out; // CCW - motor_out[CH_3] -= g.rc_4.pwm_out; // CW - motor_out[CH_4] -= g.rc_4.pwm_out; // CW + motor_out[MOT_1] += g.rc_4.pwm_out; // CCW + motor_out[MOT_2] += g.rc_4.pwm_out; // CCW + motor_out[MOT_3] -= g.rc_4.pwm_out; // CW + motor_out[MOT_4] -= g.rc_4.pwm_out; // CW /* We need to clip motor output at out_max. When cipping a motors * output we also need to compensate for the instability by @@ -67,7 +67,7 @@ static void output_motors_armed() * ensures that we retain control when one or more of the motors * is at its maximum output */ - for (int i=CH_1; i<=CH_4; i++) { + for (int i=MOT_1; i<=MOT_4; i++) { if (motor_out[i] > out_max) { // note that i^1 is the opposite motor motor_out[i^1] -= motor_out[i] - out_max; @@ -76,24 +76,24 @@ static void output_motors_armed() } // limit output so motors don't stop - motor_out[CH_1] = max(motor_out[CH_1], out_min); - motor_out[CH_2] = max(motor_out[CH_2], out_min); - motor_out[CH_3] = max(motor_out[CH_3], out_min); - motor_out[CH_4] = max(motor_out[CH_4], out_min); + motor_out[MOT_1] = max(motor_out[MOT_1], out_min); + motor_out[MOT_2] = max(motor_out[MOT_2], out_min); + motor_out[MOT_3] = max(motor_out[MOT_3], out_min); + motor_out[MOT_4] = max(motor_out[MOT_4], out_min); #if CUT_MOTORS == ENABLED // if we are not sending a throttle output, we cut the motors if(g.rc_3.servo_out == 0){ - motor_out[CH_1] = g.rc_3.radio_min; - motor_out[CH_2] = g.rc_3.radio_min; - motor_out[CH_3] = g.rc_3.radio_min; - motor_out[CH_4] = g.rc_3.radio_min; + motor_out[MOT_1] = g.rc_3.radio_min; + motor_out[MOT_2] = g.rc_3.radio_min; + motor_out[MOT_3] = g.rc_3.radio_min; + motor_out[MOT_4] = g.rc_3.radio_min; } #endif // this filter slows the acceleration of motors vs the deceleration // Idea by Denny Rowland to help with his Yaw issue - for(int8_t i=CH_1; i <= CH_4; i++ ) { + for(int8_t i=MOT_1; i <= MOT_4; i++ ) { if(motor_filtered[i] < motor_out[i]){ motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2; }else{ @@ -102,10 +102,10 @@ static void output_motors_armed() } } - APM_RC.OutputCh(CH_1, motor_filtered[CH_1]); - APM_RC.OutputCh(CH_2, motor_filtered[CH_2]); - APM_RC.OutputCh(CH_3, motor_filtered[CH_3]); - APM_RC.OutputCh(CH_4, motor_filtered[CH_4]); + APM_RC.OutputCh(MOT_1, motor_filtered[MOT_1]); + APM_RC.OutputCh(MOT_2, motor_filtered[MOT_2]); + APM_RC.OutputCh(MOT_3, motor_filtered[MOT_3]); + APM_RC.OutputCh(MOT_4, motor_filtered[MOT_4]); #if INSTANT_PWM == 1 // InstantPWM @@ -130,52 +130,52 @@ static void output_motors_disarmed() } // Send commands to motors - APM_RC.OutputCh(CH_1, g.rc_3.radio_min); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_1, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_2, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_3, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_4, g.rc_3.radio_min); } /* //static void debug_motors() { Serial.printf("1:%d\t2:%d\t3:%d\t4:%d\n", - motor_out[CH_1], - motor_out[CH_2], - motor_out[CH_3], - motor_out[CH_4]); + motor_out[MOT_1], + motor_out[MOT_2], + motor_out[MOT_3], + motor_out[MOT_4]); } //*/ static void output_motor_test() { - motor_out[CH_1] = g.rc_3.radio_min; - motor_out[CH_2] = g.rc_3.radio_min; - motor_out[CH_3] = g.rc_3.radio_min; - motor_out[CH_4] = g.rc_3.radio_min; + motor_out[MOT_1] = g.rc_3.radio_min; + motor_out[MOT_2] = g.rc_3.radio_min; + motor_out[MOT_3] = g.rc_3.radio_min; + motor_out[MOT_4] = g.rc_3.radio_min; if(g.frame_orientation == X_FRAME){ // 31 // 24 if(g.rc_1.control_in > 3000){ - motor_out[CH_1] += 100; - motor_out[CH_4] += 100; + motor_out[MOT_1] += 100; + motor_out[MOT_4] += 100; } if(g.rc_1.control_in < -3000){ - motor_out[CH_2] += 100; - motor_out[CH_3] += 100; + motor_out[MOT_2] += 100; + motor_out[MOT_3] += 100; } if(g.rc_2.control_in > 3000){ - motor_out[CH_2] += 100; - motor_out[CH_4] += 100; + motor_out[MOT_2] += 100; + motor_out[MOT_4] += 100; } if(g.rc_2.control_in < -3000){ - motor_out[CH_1] += 100; - motor_out[CH_3] += 100; + motor_out[MOT_1] += 100; + motor_out[MOT_3] += 100; } }else{ @@ -183,22 +183,22 @@ static void output_motor_test() // 2 1 // 4 if(g.rc_1.control_in > 3000) - motor_out[CH_1] += 100; + motor_out[MOT_1] += 100; if(g.rc_1.control_in < -3000) - motor_out[CH_2] += 100; + motor_out[MOT_2] += 100; if(g.rc_2.control_in > 3000) - motor_out[CH_4] += 100; + motor_out[MOT_4] += 100; if(g.rc_2.control_in < -3000) - motor_out[CH_3] += 100; + motor_out[MOT_3] += 100; } - APM_RC.OutputCh(CH_1, motor_out[CH_1]); - APM_RC.OutputCh(CH_2, motor_out[CH_2]); - APM_RC.OutputCh(CH_3, motor_out[CH_3]); - APM_RC.OutputCh(CH_4, motor_out[CH_4]); + APM_RC.OutputCh(MOT_1, motor_out[MOT_1]); + APM_RC.OutputCh(MOT_2, motor_out[MOT_2]); + APM_RC.OutputCh(MOT_3, motor_out[MOT_3]); + APM_RC.OutputCh(MOT_4, motor_out[MOT_4]); } #endif diff --git a/ArduCopter/motors_tri.pde b/ArduCopter/motors_tri.pde index 849eb1ab12..4c956965e9 100644 --- a/ArduCopter/motors_tri.pde +++ b/ArduCopter/motors_tri.pde @@ -4,7 +4,7 @@ static void init_motors_out() { #if INSTANT_PWM == 0 - APM_RC.SetFastOutputChannels( MSK_CH_1 | MSK_CH_2 | MSK_CH_4 ); + APM_RC.SetFastOutputChannels( _BV(MOT_1) | _BV(MOT_2) | _BV(MOT_4) ); #endif } @@ -28,50 +28,50 @@ static void output_motors_armed() int pitch_out = g.rc_2.pwm_out / 2; //left front - motor_out[CH_2] = g.rc_3.radio_out + roll_out + pitch_out; + motor_out[MOT_2] = g.rc_3.radio_out + roll_out + pitch_out; //right front - motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out; + motor_out[MOT_1] = g.rc_3.radio_out - roll_out + pitch_out; // rear - motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out; + motor_out[MOT_4] = g.rc_3.radio_out - g.rc_2.pwm_out; - //motor_out[CH_4] += (float)(abs(g.rc_4.control_in)) * .013; + //motor_out[MOT_4] += (float)(abs(g.rc_4.control_in)) * .013; // Tridge's stability patch - if (motor_out[CH_1] > out_max) { - motor_out[CH_2] -= (motor_out[CH_1] - out_max) >> 1; - motor_out[CH_4] -= (motor_out[CH_1] - out_max) >> 1; - motor_out[CH_1] = out_max; + if (motor_out[MOT_1] > out_max) { + motor_out[MOT_2] -= (motor_out[MOT_1] - out_max) >> 1; + motor_out[MOT_4] -= (motor_out[MOT_1] - out_max) >> 1; + motor_out[MOT_1] = out_max; } - if (motor_out[CH_2] > out_max) { - motor_out[CH_1] -= (motor_out[CH_2] - out_max) >> 1; - motor_out[CH_4] -= (motor_out[CH_2] - out_max) >> 1; - motor_out[CH_2] = out_max; + if (motor_out[MOT_2] > out_max) { + motor_out[MOT_1] -= (motor_out[MOT_2] - out_max) >> 1; + motor_out[MOT_4] -= (motor_out[MOT_2] - out_max) >> 1; + motor_out[MOT_2] = out_max; } - if (motor_out[CH_4] > out_max) { - motor_out[CH_1] -= (motor_out[CH_4] - out_max) >> 1; - motor_out[CH_2] -= (motor_out[CH_4] - out_max) >> 1; - motor_out[CH_4] = out_max; + if (motor_out[MOT_4] > out_max) { + motor_out[MOT_1] -= (motor_out[MOT_4] - out_max) >> 1; + motor_out[MOT_2] -= (motor_out[MOT_4] - out_max) >> 1; + motor_out[MOT_4] = out_max; } // limit output so motors don't stop - motor_out[CH_1] = max(motor_out[CH_1], out_min); - motor_out[CH_2] = max(motor_out[CH_2], out_min); - motor_out[CH_4] = max(motor_out[CH_4], out_min); + motor_out[MOT_1] = max(motor_out[MOT_1], out_min); + motor_out[MOT_2] = max(motor_out[MOT_2], out_min); + motor_out[MOT_4] = max(motor_out[MOT_4], out_min); #if CUT_MOTORS == ENABLED // if we are not sending a throttle output, we cut the motors if(g.rc_3.servo_out == 0){ - motor_out[CH_1] = g.rc_3.radio_min; - motor_out[CH_2] = g.rc_3.radio_min; - motor_out[CH_4] = g.rc_3.radio_min; + motor_out[MOT_1] = g.rc_3.radio_min; + motor_out[MOT_2] = g.rc_3.radio_min; + motor_out[MOT_4] = g.rc_3.radio_min; } #endif - APM_RC.OutputCh(CH_1, motor_out[CH_1]); - APM_RC.OutputCh(CH_2, motor_out[CH_2]); - APM_RC.OutputCh(CH_4, motor_out[CH_4]); + APM_RC.OutputCh(MOT_1, motor_out[MOT_1]); + APM_RC.OutputCh(MOT_2, motor_out[MOT_2]); + APM_RC.OutputCh(MOT_4, motor_out[MOT_4]); #if INSTANT_PWM == 1 // InstantPWM @@ -94,33 +94,33 @@ static void output_motors_disarmed() } // Send commands to motors - APM_RC.OutputCh(CH_1, g.rc_3.radio_min); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_1, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_2, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_4, g.rc_3.radio_min); } static void output_motor_test() { - motor_out[CH_1] = g.rc_3.radio_min; - motor_out[CH_2] = g.rc_3.radio_min; - motor_out[CH_4] = g.rc_3.radio_min; + motor_out[MOT_1] = g.rc_3.radio_min; + motor_out[MOT_2] = g.rc_3.radio_min; + motor_out[MOT_4] = g.rc_3.radio_min; if(g.rc_1.control_in > 3000){ // right - motor_out[CH_1] += 100; + motor_out[MOT_1] += 100; } if(g.rc_1.control_in < -3000){ // left - motor_out[CH_2] += 100; + motor_out[MOT_2] += 100; } if(g.rc_2.control_in > 3000){ // back - motor_out[CH_4] += 100; + motor_out[MOT_4] += 100; } - APM_RC.OutputCh(CH_1, motor_out[CH_1]); - APM_RC.OutputCh(CH_2, motor_out[CH_2]); - APM_RC.OutputCh(CH_4, motor_out[CH_4]); + APM_RC.OutputCh(MOT_1, motor_out[MOT_1]); + APM_RC.OutputCh(MOT_2, motor_out[MOT_2]); + APM_RC.OutputCh(MOT_4, motor_out[MOT_4]); } #endif diff --git a/ArduCopter/motors_y6.pde b/ArduCopter/motors_y6.pde index fdd0f74417..98094e0eaf 100644 --- a/ArduCopter/motors_y6.pde +++ b/ArduCopter/motors_y6.pde @@ -7,8 +7,8 @@ static void init_motors_out() { #if INSTANT_PWM == 0 - APM_RC.SetFastOutputChannels( MSK_CH_1 | MSK_CH_2 | MSK_CH_3 | MSK_CH_4 - | MSK_CH_7 | MSK_CH_8 ); + APM_RC.SetFastOutputChannels( _BV(MOT_1) | _BV(MOT_2) | _BV(MOT_3) | _BV(MOT_4) + | _BV(MOT_5) | _BV(MOT_6) ); #endif } @@ -30,24 +30,24 @@ static void output_motors_armed() // Multi-Wii Mix //left - motor_out[CH_2] = (g.rc_3.radio_out * g.top_bottom_ratio) + g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // LEFT TOP - CW - motor_out[CH_3] = g.rc_3.radio_out + g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // BOTTOM_LEFT - CCW + motor_out[MOT_2] = (g.rc_3.radio_out * g.top_bottom_ratio) + g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // LEFT TOP - CW + motor_out[MOT_3] = g.rc_3.radio_out + g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // BOTTOM_LEFT - CCW //right - motor_out[CH_7] = (g.rc_3.radio_out * g.top_bottom_ratio) - g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // RIGHT TOP - CW - motor_out[CH_1] = g.rc_3.radio_out - g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // BOTTOM_RIGHT - CCW + motor_out[MOT_5] = (g.rc_3.radio_out * g.top_bottom_ratio) - g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // RIGHT TOP - CW + motor_out[MOT_1] = g.rc_3.radio_out - g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // BOTTOM_RIGHT - CCW //back - motor_out[CH_8] = (g.rc_3.radio_out * g.top_bottom_ratio) - (g.rc_2.pwm_out * 4 / 3); // REAR TOP - CCW - motor_out[CH_4] = g.rc_3.radio_out - (g.rc_2.pwm_out * 4 / 3); // BOTTOM_REAR - CW + motor_out[MOT_6] = (g.rc_3.radio_out * g.top_bottom_ratio) - (g.rc_2.pwm_out * 4 / 3); // REAR TOP - CCW + motor_out[MOT_4] = g.rc_3.radio_out - (g.rc_2.pwm_out * 4 / 3); // BOTTOM_REAR - CW //left - motor_out[CH_2] -= YAW_DIRECTION * g.rc_4.pwm_out; // LEFT TOP - CW - motor_out[CH_3] += YAW_DIRECTION * g.rc_4.pwm_out; // LEFT BOTTOM - CCW + motor_out[MOT_2] -= YAW_DIRECTION * g.rc_4.pwm_out; // LEFT TOP - CW + motor_out[MOT_3] += YAW_DIRECTION * g.rc_4.pwm_out; // LEFT BOTTOM - CCW //right - motor_out[CH_7] -= YAW_DIRECTION * g.rc_4.pwm_out; // RIGHT TOP - CW - motor_out[CH_1] += YAW_DIRECTION * g.rc_4.pwm_out; // RIGHT BOTTOM - CCW + motor_out[MOT_5] -= YAW_DIRECTION * g.rc_4.pwm_out; // RIGHT TOP - CW + motor_out[MOT_1] += YAW_DIRECTION * g.rc_4.pwm_out; // RIGHT BOTTOM - CCW //back - motor_out[CH_8] += YAW_DIRECTION * g.rc_4.pwm_out; // REAR TOP - CCW - motor_out[CH_4] -= YAW_DIRECTION * g.rc_4.pwm_out; // REAR BOTTOM - CW + motor_out[MOT_6] += YAW_DIRECTION * g.rc_4.pwm_out; // REAR TOP - CCW + motor_out[MOT_4] -= YAW_DIRECTION * g.rc_4.pwm_out; // REAR BOTTOM - CW /* @@ -55,62 +55,61 @@ static void output_motors_armed() int pitch_out = g.rc_2.pwm_out / 2; //left - motor_out[CH_2] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out + pitch_out); // CCW TOP - motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW + motor_out[MOT_2] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out + pitch_out); // CCW TOP + motor_out[MOT_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW //right - motor_out[CH_7] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out + pitch_out); // CCW TOP - motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out; // CW + motor_out[MOT_5] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out + pitch_out); // CCW TOP + motor_out[MOT_1] = g.rc_3.radio_out - roll_out + pitch_out; // CW //back - motor_out[CH_8] = ((g.rc_3.radio_out * g.top_bottom_ratio) - g.rc_2.pwm_out); // CCW TOP - motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW + motor_out[MOT_6] = ((g.rc_3.radio_out * g.top_bottom_ratio) - g.rc_2.pwm_out); // CCW TOP + motor_out[MOT_4] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW // Yaw //top - motor_out[CH_2] += g.rc_4.pwm_out; // CCW - motor_out[CH_7] += g.rc_4.pwm_out; // CCW - motor_out[CH_8] += g.rc_4.pwm_out; // CCW + motor_out[MOT_2] += g.rc_4.pwm_out; // CCW + motor_out[MOT_5] += g.rc_4.pwm_out; // CCW + motor_out[MOT_6] += g.rc_4.pwm_out; // CCW //bottom - motor_out[CH_3] -= g.rc_4.pwm_out; // CW - motor_out[CH_1] -= g.rc_4.pwm_out; // CW - motor_out[CH_4] -= g.rc_4.pwm_out; // CW + motor_out[MOT_3] -= g.rc_4.pwm_out; // CW + motor_out[MOT_1] -= g.rc_4.pwm_out; // CW + motor_out[MOT_4] -= g.rc_4.pwm_out; // CW */ // TODO: add stability patch - motor_out[CH_1] = min(motor_out[CH_1], out_max); - motor_out[CH_2] = min(motor_out[CH_2], out_max); - motor_out[CH_3] = min(motor_out[CH_3], out_max); - motor_out[CH_4] = min(motor_out[CH_4], out_max); - motor_out[CH_7] = min(motor_out[CH_7], out_max); - motor_out[CH_8] = min(motor_out[CH_8], out_max); + motor_out[MOT_1] = min(motor_out[MOT_1], out_max); + motor_out[MOT_2] = min(motor_out[MOT_2], out_max); + motor_out[MOT_3] = min(motor_out[MOT_3], out_max); + motor_out[MOT_4] = min(motor_out[MOT_4], out_max); + motor_out[MOT_5] = min(motor_out[MOT_5], out_max); + motor_out[MOT_6] = min(motor_out[MOT_6], out_max); // limit output so motors don't stop - motor_out[CH_1] = max(motor_out[CH_1], out_min); - motor_out[CH_2] = max(motor_out[CH_2], out_min); - motor_out[CH_3] = max(motor_out[CH_3], out_min); - motor_out[CH_4] = max(motor_out[CH_4], out_min); - motor_out[CH_7] = max(motor_out[CH_7], out_min); - motor_out[CH_8] = max(motor_out[CH_8], out_min); + motor_out[MOT_1] = max(motor_out[MOT_1], out_min); + motor_out[MOT_2] = max(motor_out[MOT_2], out_min); + motor_out[MOT_3] = max(motor_out[MOT_3], out_min); + motor_out[MOT_4] = max(motor_out[MOT_4], out_min); + motor_out[MOT_5] = max(motor_out[MOT_5], out_min); + motor_out[MOT_6] = max(motor_out[MOT_6], out_min); #if CUT_MOTORS == ENABLED // if we are not sending a throttle output, we cut the motors if(g.rc_3.servo_out == 0){ - motor_out[CH_1] = g.rc_3.radio_min; - motor_out[CH_2] = g.rc_3.radio_min; - motor_out[CH_3] = g.rc_3.radio_min; - motor_out[CH_4] = g.rc_3.radio_min; - motor_out[CH_7] = g.rc_3.radio_min; - motor_out[CH_8] = g.rc_3.radio_min; + motor_out[MOT_1] = g.rc_3.radio_min; + motor_out[MOT_2] = g.rc_3.radio_min; + motor_out[MOT_3] = g.rc_3.radio_min; + motor_out[MOT_4] = g.rc_3.radio_min; + motor_out[MOT_5] = g.rc_3.radio_min; + motor_out[MOT_6] = g.rc_3.radio_min; } #endif // this filter slows the acceleration of motors vs the deceleration // Idea by Denny Rowland to help with his Yaw issue - for(int8_t i = CH_1; i <= CH_8; i++ ) { - if(i == CH_5 || i == CH_6) - continue; + for(int8_t m = 0; m <= 6; m++ ) { + int i = ch_of_mot(m); if(motor_filtered[i] < motor_out[i]){ motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2; }else{ @@ -119,12 +118,12 @@ static void output_motors_armed() } } - APM_RC.OutputCh(CH_1, motor_filtered[CH_1]); - APM_RC.OutputCh(CH_2, motor_filtered[CH_2]); - APM_RC.OutputCh(CH_3, motor_filtered[CH_3]); - APM_RC.OutputCh(CH_4, motor_filtered[CH_4]); - APM_RC.OutputCh(CH_7, motor_filtered[CH_7]); - APM_RC.OutputCh(CH_8, motor_filtered[CH_8]); + APM_RC.OutputCh(MOT_1, motor_filtered[MOT_1]); + APM_RC.OutputCh(MOT_2, motor_filtered[MOT_2]); + APM_RC.OutputCh(MOT_3, motor_filtered[MOT_3]); + APM_RC.OutputCh(MOT_4, motor_filtered[MOT_4]); + APM_RC.OutputCh(MOT_5, motor_filtered[MOT_5]); + APM_RC.OutputCh(MOT_6, motor_filtered[MOT_6]); #if INSTANT_PWM == 1 // InstantPWM @@ -148,45 +147,45 @@ static void output_motors_disarmed() } // Send commands to motors - APM_RC.OutputCh(CH_1, g.rc_3.radio_min); - APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min); - APM_RC.OutputCh(CH_7, g.rc_3.radio_min); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_1, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_2, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_3, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_4, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_5, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_6, g.rc_3.radio_min); } static void output_motor_test() { - motor_out[CH_1] = g.rc_3.radio_min; - motor_out[CH_2] = g.rc_3.radio_min; - motor_out[CH_3] = g.rc_3.radio_min; - motor_out[CH_4] = g.rc_3.radio_min; - motor_out[CH_7] = g.rc_3.radio_min; - motor_out[CH_8] = g.rc_3.radio_min; + motor_out[MOT_1] = g.rc_3.radio_min; + motor_out[MOT_2] = g.rc_3.radio_min; + motor_out[MOT_3] = g.rc_3.radio_min; + motor_out[MOT_4] = g.rc_3.radio_min; + motor_out[MOT_5] = g.rc_3.radio_min; + motor_out[MOT_6] = g.rc_3.radio_min; if(g.rc_1.control_in > 3000){ // right - motor_out[CH_1] += 100; - motor_out[CH_7] += 100; + motor_out[MOT_1] += 100; + motor_out[MOT_5] += 100; } if(g.rc_1.control_in < -3000){ // left - motor_out[CH_2] += 100; - motor_out[CH_3] += 100; + motor_out[MOT_2] += 100; + motor_out[MOT_3] += 100; } if(g.rc_2.control_in > 3000){ // back - motor_out[CH_8] += 100; - motor_out[CH_4] += 100; + motor_out[MOT_6] += 100; + motor_out[MOT_4] += 100; } - APM_RC.OutputCh(CH_1, motor_out[CH_1]); - APM_RC.OutputCh(CH_2, motor_out[CH_2]); - APM_RC.OutputCh(CH_3, motor_out[CH_4]); - APM_RC.OutputCh(CH_4, motor_out[CH_4]); - APM_RC.OutputCh(CH_7, motor_out[CH_7]); - APM_RC.OutputCh(CH_8, motor_out[CH_8]); + APM_RC.OutputCh(MOT_1, motor_out[MOT_1]); + APM_RC.OutputCh(MOT_2, motor_out[MOT_2]); + APM_RC.OutputCh(MOT_3, motor_out[MOT_4]); + APM_RC.OutputCh(MOT_4, motor_out[MOT_4]); + APM_RC.OutputCh(MOT_5, motor_out[MOT_5]); + APM_RC.OutputCh(MOT_6, motor_out[MOT_6]); } #endif diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index a5ba2e3790..66521347b6 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -367,12 +367,125 @@ static void update_crosstrack(void) } } - static int32_t get_altitude_error() { + // Next_WP alt is our target alt + // It changes based on climb rate + // until it reaches the target_altitude return next_WP.alt - current_loc.alt; } +static void clear_new_altitude() +{ + alt_change_flag = REACHED_ALT; +} + +static void set_new_altitude(int32_t _new_alt) +{ + // just to be clear + next_WP.alt = current_loc.alt; + + // for calculating the delta time + alt_change_timer = millis(); + + // save the target altitude + target_altitude = _new_alt; + + // reset our altitude integrator + alt_change = 0; + + // save the original altitude + original_altitude = current_loc.alt; + + // to decide if we have reached the target altitude + if(target_altitude > original_altitude){ + // we are below, going up + alt_change_flag = ASCENDING; + Serial.printf("go up\n"); + }else if(target_altitude < original_altitude){ + // we are above, going down + alt_change_flag = DESCENDING; + Serial.printf("go down\n"); + }else{ + // No Change + alt_change_flag = REACHED_ALT; + Serial.printf("reached alt\n"); + } + + //Serial.printf("new alt: %d Org alt: %d\n", target_altitude, original_altitude); +} + +static int32_t get_new_altitude() +{ + // returns a new next_WP.alt + + if(alt_change_flag == ASCENDING){ + // we are below, going up + if(current_loc.alt >= target_altitude){ + alt_change_flag = REACHED_ALT; + } + + // we shouldn't command past our target + if(next_WP.alt >= target_altitude){ + return target_altitude; + } + }else if (alt_change_flag == DESCENDING){ + // we are above, going down + if(current_loc.alt <= target_altitude) + alt_change_flag = REACHED_ALT; + + // we shouldn't command past our target + if(next_WP.alt <= target_altitude){ + return target_altitude; + } + } + + // if we have reached our target altitude, return the target alt + if(alt_change_flag == REACHED_ALT){ + return target_altitude; + } + + int32_t diff = abs(next_WP.alt - target_altitude); + int8_t _scale = 4; + + if (next_WP.alt < target_altitude){ + // we are below the target alt + + if(diff < 200){ + // we are going up + _scale = 5; + } else { + _scale = 4; + } + + }else { + // we are above the target + // stay at 16 for speed + //_scale = 16; + + if(diff < 400){ + // we are going down + _scale = 5; + + }else if(diff < 100){ + _scale = 6; + } + } + + int32_t change = (millis() - alt_change_timer) >> _scale; + + if(alt_change_flag == ASCENDING){ + alt_change += change; + }else{ + alt_change -= change; + } + // for generating delta time + alt_change_timer = millis(); + + return original_altitude + alt_change; +} + + static int32_t wrap_360(int32_t error) { if (error > 36000) error -= 36000; diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index f319757e1f..b1c0eaa5b2 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -2,7 +2,7 @@ //Function that will read the radio data, limit servos and trigger a failsafe // ---------------------------------------------------------------------------- -static byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling +static int8_t failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling static void default_dead_zones() { @@ -55,8 +55,8 @@ static void init_rc_out() init_motors_out(); // this is the camera pitch5 and roll6 - APM_RC.OutputCh(CH_5, 1500); - APM_RC.OutputCh(CH_6, 1500); + APM_RC.OutputCh(CH_CAM_PITCH, 1500); + APM_RC.OutputCh(CH_CAM_ROLL, 1500); for(byte i = 0; i < 5; i++){ delay(20); @@ -103,18 +103,18 @@ void output_min() #if FRAME_CONFIG == HELI_FRAME heli_move_servos_to_mid(); #else - APM_RC.OutputCh(CH_1, g.rc_3.radio_min); // Initialization of servo outputs - APM_RC.OutputCh(CH_2, g.rc_3.radio_min); - APM_RC.OutputCh(CH_3, g.rc_3.radio_min); - APM_RC.OutputCh(CH_4, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_1, g.rc_3.radio_min); // Initialization of servo outputs + APM_RC.OutputCh(MOT_2, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_3, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_4, g.rc_3.radio_min); #endif - APM_RC.OutputCh(CH_7, g.rc_3.radio_min); - APM_RC.OutputCh(CH_8, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_5, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_6, g.rc_3.radio_min); #if FRAME_CONFIG == OCTA_FRAME - APM_RC.OutputCh(CH_10, g.rc_3.radio_min); - APM_RC.OutputCh(CH_11, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_7, g.rc_3.radio_min); + APM_RC.OutputCh(MOT_8, g.rc_3.radio_min); #endif } @@ -153,12 +153,16 @@ static void throttle_failsafe(uint16_t pwm) // throttle has dropped below the mark failsafeCounter++; if (failsafeCounter == 9){ - SendDebug("MSG FS ON "); - SendDebugln(pwm, DEC); + // }else if(failsafeCounter == 10) { // Don't enter Failsafe if we are not armed - if(motor_armed == true) + // home distance is in meters + // This is to prevent accidental RTL + if((motor_armed == true) && (home_distance > 10) && (current_loc.alt > 400)){ + SendDebug("MSG FS ON "); + SendDebugln(pwm, DEC); set_failsafe(true); + } }else if (failsafeCounter > 10){ failsafeCounter = 11; } diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index 5579e9ee83..0d714d62b2 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -271,7 +271,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) { byte _switchPosition = 0; byte _oldSwitchPosition = 0; - byte mode = 0; + int8_t mode = 0; Serial.printf_P(PSTR("\nMode switch to edit, aileron: select modes, rudder: Simple on/off\n")); print_hit_enter(); @@ -1102,16 +1102,16 @@ init_esc() read_radio(); delay(100); dancing_light(); - APM_RC.OutputCh(CH_1, g.rc_3.radio_in); - APM_RC.OutputCh(CH_2, g.rc_3.radio_in); - APM_RC.OutputCh(CH_3, g.rc_3.radio_in); - APM_RC.OutputCh(CH_4, g.rc_3.radio_in); - APM_RC.OutputCh(CH_7, g.rc_3.radio_in); - APM_RC.OutputCh(CH_8, g.rc_3.radio_in); + APM_RC.OutputCh(MOT_1, g.rc_3.radio_in); + APM_RC.OutputCh(MOT_2, g.rc_3.radio_in); + APM_RC.OutputCh(MOT_3, g.rc_3.radio_in); + APM_RC.OutputCh(MOT_4, g.rc_3.radio_in); + APM_RC.OutputCh(MOT_5, g.rc_3.radio_in); + APM_RC.OutputCh(MOT_6, g.rc_3.radio_in); #if FRAME_CONFIG == OCTA_FRAME - APM_RC.OutputCh(CH_10, g.rc_3.radio_in); - APM_RC.OutputCh(CH_11, g.rc_3.radio_in); + APM_RC.OutputCh(MOT_7, g.rc_3.radio_in); + APM_RC.OutputCh(MOT_8, g.rc_3.radio_in); #endif } diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 6c610d7137..b518400297 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -321,7 +321,7 @@ static void init_ardupilot() #endif // initialise sonar - #if HIL_MODE != HIL_MODE_ATTITUDE && CONFIG_SONAR == ENABLED + #if CONFIG_SONAR == ENABLED init_sonar(); #endif @@ -490,9 +490,7 @@ static void set_mode(byte mode) roll_pitch_mode = ALT_HOLD_RP; throttle_mode = ALT_HOLD_THR; - next_WP = current_loc; - // 1m is the alt hold limit - next_WP.alt = max(next_WP.alt, 100); + set_next_WP(¤t_loc); break; case AUTO: @@ -508,9 +506,7 @@ static void set_mode(byte mode) yaw_mode = CIRCLE_YAW; roll_pitch_mode = CIRCLE_RP; throttle_mode = CIRCLE_THR; - next_WP = current_loc; - - // reset the desired circle angle + set_next_WP(¤t_loc); circle_angle = 0; break; @@ -518,14 +514,14 @@ static void set_mode(byte mode) yaw_mode = LOITER_YAW; roll_pitch_mode = LOITER_RP; throttle_mode = LOITER_THR; - next_WP = current_loc; + set_next_WP(¤t_loc); break; case POSITION: yaw_mode = YAW_HOLD; roll_pitch_mode = ROLL_PITCH_AUTO; throttle_mode = THROTTLE_MANUAL; - next_WP = current_loc; + set_next_WP(¤t_loc); break; case GUIDED: @@ -579,11 +575,12 @@ static void set_mode(byte mode) // removes the navigation from roll and pitch commands, but leaves the wind compensation reset_nav(); - // removes the navigation from roll and pitch commands, but leaves the wind compensation - if(GPS_enabled) + #if WIND_COMP_STAB == 1 + if(GPS_enabled){ wp_control = NO_NAV_MODE; update_nav_wp(); - + } + #endif } Log_Write_Mode(control_mode); diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index d3d391b614..de155c4975 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -167,7 +167,7 @@ test_eedump(uint8_t argc, const Menu::arg *argv) g.rc_4.control_in, g.rc_4.radio_out); - APM_RC.OutputCh(CH_7, g.rc_4.radio_out); + APM_RC.OutputCh(CH_TRI_YAW, g.rc_4.radio_out); if(Serial.available() > 0){ return (0); @@ -765,10 +765,10 @@ test_current(uint8_t argc, const Menu::arg *argv) current_amps, current_total); - APM_RC.OutputCh(CH_1, g.rc_3.radio_in); - APM_RC.OutputCh(CH_2, g.rc_3.radio_in); - APM_RC.OutputCh(CH_3, g.rc_3.radio_in); - APM_RC.OutputCh(CH_4, g.rc_3.radio_in); + APM_RC.OutputCh(MOT_1, g.rc_3.radio_in); + APM_RC.OutputCh(MOT_2, g.rc_3.radio_in); + APM_RC.OutputCh(MOT_3, g.rc_3.radio_in); + APM_RC.OutputCh(MOT_4, g.rc_3.radio_in); if(Serial.available() > 0){ return (0); diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 71ac9a8b79..21f0b705c5 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -168,7 +168,7 @@ def fly_RTL(mavproxy, mav, side=60): print("# Enter RTL") mavproxy.send('switch 3\n') tstart = time.time() - while time.time() < tstart + 120: + while time.time() < tstart + 200: m = mav.recv_match(type='VFR_HUD', blocking=True) pos = current_location(mav) #delta = get_distance(start, pos) @@ -208,7 +208,7 @@ def fly_simple(mavproxy, mav, side=60, timeout=120): '''fly Simple, flying N then E''' mavproxy.send('switch 6\n') wait_mode(mav, 'STABILIZE') - mavproxy.send('rc 3 1450\n') + mavproxy.send('rc 3 1440\n') tstart = time.time() failed = False @@ -246,7 +246,7 @@ def land(mavproxy, mav, timeout=60): mavproxy.send('switch 2\n') wait_mode(mav, 'LAND') print("Entered Landing Mode") - ret = wait_altitude(mav, -5, 5) + ret = wait_altitude(mav, -5, 1) print("LANDING: ok= %s" % ret) return ret @@ -437,8 +437,8 @@ def fly_ArduCopter(viewerip=None): failed = True # Loiter for 30 seconds - print("# Loiter for 30 seconds") - if not loiter(mavproxy, mav, 30): + print("# Loiter for 45 seconds") + if not loiter(mavproxy, mav, 45): print("loiter failed") failed = True diff --git a/Tools/autotest/common.py b/Tools/autotest/common.py index 15c926f733..6f91574928 100644 --- a/Tools/autotest/common.py +++ b/Tools/autotest/common.py @@ -195,7 +195,9 @@ def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, time seq = m.seq m = mav.recv_match(type='NAV_CONTROLLER_OUTPUT', blocking=True) wp_dist = m.wp_dist - print("test: WP %u (wp_dist=%u), current_wp: %u, wpnum_end: %u" % (seq, wp_dist, current_wp, wpnum_end)) + m = mav.recv_match(type='VFR_HUD', blocking=True) + + print("test: WP %u (wp_dist=%u Alt=%d), current_wp: %u, wpnum_end: %u" % (seq, wp_dist, m.alt, current_wp, wpnum_end)) if seq == current_wp+1 or (seq > current_wp+1 and allow_skip): print("test: Starting new waypoint %u" % seq) tstart = time.time() diff --git a/Tools/autotest/pysim/sim_multicopter.py b/Tools/autotest/pysim/sim_multicopter.py index 184713f1f5..a3cb744a05 100755 --- a/Tools/autotest/pysim/sim_multicopter.py +++ b/Tools/autotest/pysim/sim_multicopter.py @@ -168,10 +168,10 @@ while True: frame_count += 1 t = time.time() if t - lastt > 1.0: - print("%.2f fps zspeed=%.2f zaccel=%.2f h=%.1f a=%.1f yaw=%.1f yawrate=%.1f" % ( - frame_count/(t-lastt), - a.velocity.z, a.accel.z, a.position.z, a.altitude, - a.yaw, a.yaw_rate)) + #print("%.2f fps zspeed=%.2f zaccel=%.2f h=%.1f a=%.1f yaw=%.1f yawrate=%.1f" % ( + # frame_count/(t-lastt), + # a.velocity.z, a.accel.z, a.position.z, a.altitude, + # a.yaw, a.yaw_rate)) lastt = t frame_count = 0 frame_end = time.time() diff --git a/libraries/APM_RC/APM_RC.h b/libraries/APM_RC/APM_RC.h index e9a00e2c85..7f72b1b183 100644 --- a/libraries/APM_RC/APM_RC.h +++ b/libraries/APM_RC/APM_RC.h @@ -21,20 +21,6 @@ #define CH_10 9 #define CH_11 10 -#define MSK_CH_1 (1 << CH_1) -#define MSK_CH_2 (1 << CH_2) -#define MSK_CH_3 (1 << CH_3) -#define MSK_CH_4 (1 << CH_4) -#define MSK_CH_5 (1 << CH_5) -#define MSK_CH_6 (1 << CH_6) -#define MSK_CH_7 (1 << CH_7) -#define MSK_CH_8 (1 << CH_8) -#define MSK_CH_9 (1 << CH_9) -#define MSK_CH_10 (1 << CH_10) -#define MSK_CH_11 (1 << CH_11) - - - #define NUM_CHANNELS 8 diff --git a/libraries/APM_RC/APM_RC_APM1.cpp b/libraries/APM_RC/APM_RC_APM1.cpp index b2aa1464f1..1a480832f4 100644 --- a/libraries/APM_RC/APM_RC_APM1.cpp +++ b/libraries/APM_RC/APM_RC_APM1.cpp @@ -216,16 +216,16 @@ void APM_RC_APM1::Force_Out6_Out7(void) void APM_RC_APM1::SetFastOutputChannels(uint32_t chmask) { - if ((chmask & ( MSK_CH_1 | MSK_CH_2 | MSK_CH_9)) != 0) + if ((chmask & ( _BV(CH_1) | _BV(CH_2) | _BV(CH_9))) != 0) _set_speed_ch1_ch2_ch9(OUTPUT_SPEED_400HZ); - if ((chmask & ( MSK_CH_3 | MSK_CH_4 | MSK_CH_10 )) != 0) + if ((chmask & ( _BV(CH_3) | _BV(CH_4) | _BV(CH_10))) != 0) _set_speed_ch3_ch4_ch10(OUTPUT_SPEED_400HZ); - if ((chmask & ( MSK_CH_5 | MSK_CH_6 )) != 0) + if ((chmask & ( _BV(CH_5) | _BV(CH_6))) != 0) _set_speed_ch5_ch6(OUTPUT_SPEED_400HZ); - if ((chmask & ( MSK_CH_7 | MSK_CH_8 | MSK_CH_11 )) != 0) + if ((chmask & ( _BV(CH_7) | _BV(CH_8) | _BV(CH_11))) != 0) _set_speed_ch7_ch8_ch11(OUTPUT_SPEED_400HZ); } diff --git a/libraries/APM_RC/APM_RC_APM2.cpp b/libraries/APM_RC/APM_RC_APM2.cpp index 191febb1f3..ee7b55f50f 100644 --- a/libraries/APM_RC/APM_RC_APM2.cpp +++ b/libraries/APM_RC/APM_RC_APM2.cpp @@ -197,13 +197,13 @@ void APM_RC_APM2::Force_Out6_Out7(void) { } void APM_RC_APM2::SetFastOutputChannels(uint32_t chmask) { - if ((chmask & ( MSK_CH_1 | MSK_CH_2 )) != 0) + if ((chmask & ( _BV(CH_1) | _BV(CH_2))) != 0) _set_speed_ch1_ch2(OUTPUT_SPEED_400HZ); - if ((chmask & ( MSK_CH_3 | MSK_CH_4 | MSK_CH_5 )) != 0) + if ((chmask & ( _BV(CH_3) | _BV(CH_4) | _BV(CH_5))) != 0) _set_speed_ch3_ch4_ch5(OUTPUT_SPEED_400HZ); - if ((chmask & ( MSK_CH_6 | MSK_CH_7 | MSK_CH_8 )) != 0) + if ((chmask & ( _BV(CH_6) | _BV(CH_7) | _BV(CH_8))) != 0) _set_speed_ch6_ch7_ch8(OUTPUT_SPEED_400HZ); } diff --git a/libraries/AP_ADC/AP_ADC_HIL.h b/libraries/AP_ADC/AP_ADC_HIL.h index dee48c586c..f7bcde465c 100644 --- a/libraries/AP_ADC/AP_ADC_HIL.h +++ b/libraries/AP_ADC/AP_ADC_HIL.h @@ -79,7 +79,7 @@ class AP_ADC_HIL : public AP_ADC // @param index the axis for the accelerometer(0-x,1-y,2-z) inline void setAccel(uint8_t index, int16_t val) { int16_t temp = val * accelScale[index] / 1000 + accelBias[index]; - adcValue[sensors[index+3]] = (sensors[index+3] < 0) ? -temp : temp; + adcValue[sensors[index+3]] = (sensorSign[index+3] < 0) ? -temp : temp; } /// diff --git a/libraries/AP_Common/menu.cpp b/libraries/AP_Common/menu.cpp index 7d30565c62..3c29258169 100644 --- a/libraries/AP_Common/menu.cpp +++ b/libraries/AP_Common/menu.cpp @@ -30,7 +30,8 @@ Menu::Menu(const prog_char *prompt, const Menu::command *commands, uint8_t entri void Menu::run(void) { - uint8_t len, i, ret; + int8_t ret; + uint8_t len, i; uint8_t argc; int c; char *s; @@ -123,12 +124,12 @@ Menu::run(void) return; } } - + if (cmd_found==false) { Serial.println("Invalid command, type 'help'"); } - + } } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp index 492fc9a2c3..3e8b6019f1 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp @@ -1,65 +1,65 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*- -/* - AP_RangeFinder_MaxsonarXL.cpp - Arduino Library for Sharpe GP2Y0A02YK0F - infrared proximity sensor - Code by Jose Julio and Randy Mackay. DIYDrones.com - - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. - - Sparkfun URL: http://www.sparkfun.com/products/9491 - datasheet: http://www.sparkfun.com/datasheets/Sensors/Proximity/XL-EZ0-Datasheet.pdf - - Sensor should be connected to one of the analog ports - - Variables: - int raw_value : raw value from the sensor - int distance : distance in cm - int max_distance : maximum measurable distance (in cm) - int min_distance : minimum measurable distance (in cm) - - Methods: - read() : read value from analog port and returns the distance (in cm) - -*/ - -// AVR LibC Includes -#include "WConstants.h" -#include "AP_RangeFinder_MaxsonarXL.h" - -// Constructor ////////////////////////////////////////////////////////////// - -AP_RangeFinder_MaxsonarXL::AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source, - ModeFilter *filter) : - RangeFinder(source, filter), _scaler(AP_RANGEFINDER_MAXSONARXL_SCALER) -{ - max_distance = AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE; - min_distance = AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE; -} - -// Public Methods ////////////////////////////////////////////////////////////// -float AP_RangeFinder_MaxsonarXL::calculate_scaler(int sonar_type, float adc_refence_voltage) -{ - float type_scaler = 1.0; - switch(sonar_type) { - case AP_RANGEFINDER_MAXSONARXL: - type_scaler = AP_RANGEFINDER_MAXSONARXL_SCALER; - min_distance = AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE; - max_distance = AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE; - break; - case AP_RANGEFINDER_MAXSONARLV: - type_scaler = AP_RANGEFINDER_MAXSONARLV_SCALER; - min_distance = AP_RANGEFINDER_MAXSONARLV_MIN_DISTANCE; - max_distance = AP_RANGEFINDER_MAXSONARLV_MAX_DISTANCE; - break; - case AP_RANGEFINDER_MAXSONARXLL: - type_scaler = AP_RANGEFINDER_MAXSONARXLL_SCALER; - min_distance = AP_RANGEFINDER_MAXSONARXLL_MIN_DISTANCE; - max_distance = AP_RANGEFINDER_MAXSONARXLL_MAX_DISTANCE; - break; - } - _scaler = type_scaler * adc_refence_voltage / 5.0; - return _scaler; +// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*- +/* + AP_RangeFinder_MaxsonarXL.cpp - Arduino Library for Sharpe GP2Y0A02YK0F + infrared proximity sensor + Code by Jose Julio and Randy Mackay. DIYDrones.com + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + Sparkfun URL: http://www.sparkfun.com/products/9491 + datasheet: http://www.sparkfun.com/datasheets/Sensors/Proximity/XL-EZ0-Datasheet.pdf + + Sensor should be connected to one of the analog ports + + Variables: + int raw_value : raw value from the sensor + int distance : distance in cm + int max_distance : maximum measurable distance (in cm) + int min_distance : minimum measurable distance (in cm) + + Methods: + read() : read value from analog port and returns the distance (in cm) + +*/ + +// AVR LibC Includes +#include "WConstants.h" +#include "AP_RangeFinder_MaxsonarXL.h" + +// Constructor ////////////////////////////////////////////////////////////// + +AP_RangeFinder_MaxsonarXL::AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source, ModeFilter *filter): + RangeFinder(source, filter), + _scaler(AP_RANGEFINDER_MAXSONARXL_SCALER) + { + max_distance = AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE; + min_distance = AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE; + } + +// Public Methods ////////////////////////////////////////////////////////////// +float AP_RangeFinder_MaxsonarXL::calculate_scaler(int sonar_type, float adc_refence_voltage) +{ + float type_scaler = 1.0; + switch(sonar_type) { + case AP_RANGEFINDER_MAXSONARXL: + type_scaler = AP_RANGEFINDER_MAXSONARXL_SCALER; + min_distance = AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE; + max_distance = AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE; + break; + case AP_RANGEFINDER_MAXSONARLV: + type_scaler = AP_RANGEFINDER_MAXSONARLV_SCALER; + min_distance = AP_RANGEFINDER_MAXSONARLV_MIN_DISTANCE; + max_distance = AP_RANGEFINDER_MAXSONARLV_MAX_DISTANCE; + break; + case AP_RANGEFINDER_MAXSONARXLL: + type_scaler = AP_RANGEFINDER_MAXSONARXLL_SCALER; + min_distance = AP_RANGEFINDER_MAXSONARXLL_MIN_DISTANCE; + max_distance = AP_RANGEFINDER_MAXSONARXLL_MAX_DISTANCE; + break; + } + _scaler = type_scaler * adc_refence_voltage / 5.0; + return _scaler; } \ No newline at end of file diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h index a589e987a5..12780a6dfc 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h @@ -1,33 +1,34 @@ -#ifndef AP_RangeFinder_MaxsonarXL_H -#define AP_RangeFinder_MaxsonarXL_H - -#include "RangeFinder.h" - -// XL-EZ0 (aka XL) -#define AP_RANGEFINDER_MAXSONARXL 0 -#define AP_RANGEFINDER_MAXSONARXL_SCALER 1.0 -#define AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE 20 -#define AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE 765 - -// LV-EZ0 (aka LV) -#define AP_RANGEFINDER_MAXSONARLV 1 -#define AP_RANGEFINDER_MAXSONARLV_SCALER (2.54/2.0) -#define AP_RANGEFINDER_MAXSONARLV_MIN_DISTANCE 15 -#define AP_RANGEFINDER_MAXSONARLV_MAX_DISTANCE 645 - -// XL-EZL0 (aka XLL) -#define AP_RANGEFINDER_MAXSONARXLL 2 -#define AP_RANGEFINDER_MAXSONARXLL_SCALER 2.0 -#define AP_RANGEFINDER_MAXSONARXLL_MIN_DISTANCE 20 -#define AP_RANGEFINDER_MAXSONARXLL_MAX_DISTANCE 1068 - -class AP_RangeFinder_MaxsonarXL : public RangeFinder -{ - public: - AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source, ModeFilter *filter); - int convert_raw_to_distance(int _raw_value) { return _raw_value * _scaler; } // read value from analog port and return distance in cm - float calculate_scaler(int sonar_type, float adc_refence_voltage); - private: - float _scaler; // used to account for different sonar types -}; -#endif +#ifndef AP_RangeFinder_MaxsonarXL_H +#define AP_RangeFinder_MaxsonarXL_H + +#include "RangeFinder.h" + +// XL-EZ0 (aka XL) +#define AP_RANGEFINDER_MAXSONARXL 0 +#define AP_RANGEFINDER_MAXSONARXL_SCALER 1.0 +#define AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE 20 +#define AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE 765 + +// LV-EZ0 (aka LV) +#define AP_RANGEFINDER_MAXSONARLV 1 +#define AP_RANGEFINDER_MAXSONARLV_SCALER (2.54/2.0) +#define AP_RANGEFINDER_MAXSONARLV_MIN_DISTANCE 15 +#define AP_RANGEFINDER_MAXSONARLV_MAX_DISTANCE 645 + +// XL-EZL0 (aka XLL) +#define AP_RANGEFINDER_MAXSONARXLL 2 +#define AP_RANGEFINDER_MAXSONARXLL_SCALER 2.0 +#define AP_RANGEFINDER_MAXSONARXLL_MIN_DISTANCE 20 +#define AP_RANGEFINDER_MAXSONARXLL_MAX_DISTANCE 1068 + +class AP_RangeFinder_MaxsonarXL : public RangeFinder +{ + public: + AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source, ModeFilter *filter); + int convert_raw_to_distance(int _raw_value) { return _raw_value * _scaler; } // read value from analog port and return distance in cm + float calculate_scaler(int sonar_type, float adc_refence_voltage); + + private: + float _scaler; // used to account for different sonar types +}; +#endif diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp index 61f4e7d2ea..bb8a20c896 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp @@ -1,42 +1,41 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*- -/* - AP_RangeFinder_SharpGP2Y.cpp - Arduino Library for Sharpe GP2Y0A02YK0F - infrared proximity sensor - Code by Jose Julio and Randy Mackay. DIYDrones.com - - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. - - Sensor should be conected to one of the analog ports - - Sparkfun URL: http://www.sparkfun.com/products/8958 - datasheet: http://www.sparkfun.com/datasheets/Sensors/Infrared/gp2y0a02yk_e.pdf - - Variables: - int raw_value : raw value from the sensor - int distance : distance in cm - int max_distance : maximum measurable distance (in cm) - int min_distance : minimum measurable distance (in cm) - - Methods: - read() : read value from analog port - -*/ - -// AVR LibC Includes -#include "WConstants.h" -#include "AP_RangeFinder_SharpGP2Y.h" - -// Constructor ////////////////////////////////////////////////////////////// - -AP_RangeFinder_SharpGP2Y::AP_RangeFinder_SharpGP2Y(AP_AnalogSource *source, - ModeFilter *filter) : +// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*- +/* + AP_RangeFinder_SharpGP2Y.cpp - Arduino Library for Sharpe GP2Y0A02YK0F + infrared proximity sensor + Code by Jose Julio and Randy Mackay. DIYDrones.com + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + Sensor should be conected to one of the analog ports + + Sparkfun URL: http://www.sparkfun.com/products/8958 + datasheet: http://www.sparkfun.com/datasheets/Sensors/Infrared/gp2y0a02yk_e.pdf + + Variables: + int raw_value : raw value from the sensor + int distance : distance in cm + int max_distance : maximum measurable distance (in cm) + int min_distance : minimum measurable distance (in cm) + + Methods: + read() : read value from analog port + +*/ + +// AVR LibC Includes +#include "WConstants.h" +#include "AP_RangeFinder_SharpGP2Y.h" + +// Constructor ////////////////////////////////////////////////////////////// + +AP_RangeFinder_SharpGP2Y::AP_RangeFinder_SharpGP2Y(AP_AnalogSource *source, ModeFilter *filter) : RangeFinder(source, filter) -{ - max_distance = AP_RANGEFINDER_SHARPEGP2Y_MAX_DISTANCE; - min_distance = AP_RANGEFINDER_SHARPEGP2Y_MIN_DISTANCE; -} - -// Public Methods ////////////////////////////////////////////////////////////// +{ + max_distance = AP_RANGEFINDER_SHARPEGP2Y_MAX_DISTANCE; + min_distance = AP_RANGEFINDER_SHARPEGP2Y_MIN_DISTANCE; +} + +// Public Methods ////////////////////////////////////////////////////////////// diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h b/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h index 8798857945..4f5a328979 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h @@ -1,16 +1,21 @@ -#ifndef AP_RangeFinder_SharpGP2Y_H -#define AP_RangeFinder_SharpGP2Y_H - -#include "RangeFinder.h" - -#define AP_RANGEFINDER_SHARPEGP2Y_MIN_DISTANCE 20 -#define AP_RANGEFINDER_SHARPEGP2Y_MAX_DISTANCE 150 - -class AP_RangeFinder_SharpGP2Y : public RangeFinder -{ - public: +#ifndef AP_RangeFinder_SharpGP2Y_H +#define AP_RangeFinder_SharpGP2Y_H + +#include "RangeFinder.h" + +#define AP_RANGEFINDER_SHARPEGP2Y_MIN_DISTANCE 20 +#define AP_RANGEFINDER_SHARPEGP2Y_MAX_DISTANCE 150 + +class AP_RangeFinder_SharpGP2Y : public RangeFinder +{ + public: AP_RangeFinder_SharpGP2Y(AP_AnalogSource *source, ModeFilter *filter); - int convert_raw_to_distance(int _raw_value) { if( _raw_value == 0 ) return max_distance; else return 14500/_raw_value; } // read value from analog port and return distance in cm - -}; -#endif + int convert_raw_to_distance(int _raw_value) { + if( _raw_value == 0 ) + return max_distance; + else + return 14500/_raw_value; + } // read value from analog port and return distance in cm + +}; +#endif diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp index 962148eb1c..582e072d60 100644 --- a/libraries/AP_RangeFinder/RangeFinder.cpp +++ b/libraries/AP_RangeFinder/RangeFinder.cpp @@ -1,46 +1,45 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*- -/* - AP_RangeFinder.cpp - Arduino Library for Sharpe GP2Y0A02YK0F - infrared proximity sensor - Code by Jose Julio and Randy Mackay. DIYDrones.com - - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. - - This has the basic functions that all RangeFinders need implemented -*/ - -// AVR LibC Includes -#include "WConstants.h" -#include "RangeFinder.h" - - - -// Public Methods ////////////////////////////////////////////////////////////// - -void RangeFinder::set_orientation(int x, int y, int z) -{ - orientation_x = x; - orientation_y = y; - orientation_z = z; -} - -// Read Sensor data - only the raw_value is filled in by this parent class -int RangeFinder::read() -{ - int temp_dist; - +// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*- +/* + AP_RangeFinder.cpp - Arduino Library for Sharpe GP2Y0A02YK0F + infrared proximity sensor + Code by Jose Julio and Randy Mackay. DIYDrones.com + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This has the basic functions that all RangeFinders need implemented +*/ + +// AVR LibC Includes +#include "WConstants.h" +#include "RangeFinder.h" + + + +// Public Methods ////////////////////////////////////////////////////////////// + +void RangeFinder::set_orientation(int x, int y, int z) +{ + orientation_x = x; + orientation_y = y; + orientation_z = z; +} + +// Read Sensor data - only the raw_value is filled in by this parent class +int RangeFinder::read() +{ + int temp_dist; + raw_value = _analog_source->read(); - - // convert analog value to distance in cm (using child implementation most likely) - temp_dist = convert_raw_to_distance(raw_value); - - // ensure distance is within min and max - temp_dist = constrain(temp_dist, min_distance, max_distance); - - distance = _mode_filter->get_filtered_with_sample(temp_dist); - return distance; -} - + // convert analog value to distance in cm (using child implementation most likely) + temp_dist = convert_raw_to_distance(raw_value); + + // ensure distance is within min and max + temp_dist = constrain(temp_dist, min_distance, max_distance); + + distance = _mode_filter->get_filtered_with_sample(temp_dist); + return distance; +} + diff --git a/libraries/AP_RangeFinder/RangeFinder.h b/libraries/AP_RangeFinder/RangeFinder.h index 64b38e7e89..1870cd5a94 100644 --- a/libraries/AP_RangeFinder/RangeFinder.h +++ b/libraries/AP_RangeFinder/RangeFinder.h @@ -1,44 +1,43 @@ -#ifndef RangeFinder_h -#define RangeFinder_h - -#include -#include +#ifndef RangeFinder_h +#define RangeFinder_h + +#include +#include #include "../AP_AnalogSource/AP_AnalogSource.h" -#include "../ModeFilter/ModeFilter.h" // ArduPilot Mega RC Library - -/* -#define AP_RANGEFINDER_ORIENTATION_FRONT 0, 10, 0 -#define AP_RANGEFINDER_ORIENTATION_RIGHT -10, 0, 0 -#define AP_RANGEFINDER_ORIENTATION_BACK 0,-10, 0 -#define AP_RANGEFINDER_ORIENTATION_LEFT 10, 0, 0 -#define AP_RANGEFINDER_ORIENTATION_UP 0, 0,-10 -#define AP_RANGEFINDER_ORIENTATION_DOWN 0, 0, 10 -#define AP_RANGEFINDER_ORIENTATION_FRONT_RIGHT -5, -5, 0 -#define AP_RANGEFINDER_ORIENTATION_BACK_RIGHT -5, -5, 0 -#define AP_RANGEFINDER_ORIENTATION_BACK_LEFT 5, -5, 0 -#define AP_RANGEFINDER_ORIENTATION_FRONT_LEFT 5, 5, 0 -*/ - -class RangeFinder -{ - protected: +#include "../ModeFilter/ModeFilter.h" // ArduPilot Mega RC Library + +/* +#define AP_RANGEFINDER_ORIENTATION_FRONT 0, 10, 0 +#define AP_RANGEFINDER_ORIENTATION_RIGHT -10, 0, 0 +#define AP_RANGEFINDER_ORIENTATION_BACK 0,-10, 0 +#define AP_RANGEFINDER_ORIENTATION_LEFT 10, 0, 0 +#define AP_RANGEFINDER_ORIENTATION_UP 0, 0,-10 +#define AP_RANGEFINDER_ORIENTATION_DOWN 0, 0, 10 +#define AP_RANGEFINDER_ORIENTATION_FRONT_RIGHT -5, -5, 0 +#define AP_RANGEFINDER_ORIENTATION_BACK_RIGHT -5, -5, 0 +#define AP_RANGEFINDER_ORIENTATION_BACK_LEFT 5, -5, 0 +#define AP_RANGEFINDER_ORIENTATION_FRONT_LEFT 5, 5, 0 +*/ + +class RangeFinder +{ + protected: RangeFinder(AP_AnalogSource * source, ModeFilter *filter) : _analog_source(source), - _mode_filter(filter) - {} - public: - - int raw_value; // raw value from the sensor - int distance; // distance in cm - int max_distance; // maximum measurable distance (in cm) - should be set in child's constructor - int min_distance; // minimum measurable distance (in cm) - should be set in child's constructor - int orientation_x, orientation_y, orientation_z; - - virtual void set_orientation(int x, int y, int z); - virtual int convert_raw_to_distance(int _raw_value) { return _raw_value; } // function that each child class should override to convert voltage to distance - virtual int read(); // read value from sensor and return distance in cm - + _mode_filter(filter) {} + public: + + int raw_value; // raw value from the sensor + int distance; // distance in cm + int max_distance; // maximum measurable distance (in cm) - should be set in child's constructor + int min_distance; // minimum measurable distance (in cm) - should be set in child's constructor + int orientation_x, orientation_y, orientation_z; + + virtual void set_orientation(int x, int y, int z); + virtual int convert_raw_to_distance(int _raw_value) { return _raw_value; } // function that each child class should override to convert voltage to distance + virtual int read(); // read value from sensor and return distance in cm + AP_AnalogSource *_analog_source; - ModeFilter *_mode_filter; -}; -#endif + ModeFilter *_mode_filter; +}; +#endif diff --git a/libraries/AP_RangeFinder/keywords.txt b/libraries/AP_RangeFinder/keywords.txt index be2e98d447..b8abff90b0 100644 --- a/libraries/AP_RangeFinder/keywords.txt +++ b/libraries/AP_RangeFinder/keywords.txt @@ -1,14 +1,14 @@ -RangeFinder KEYWORD1 -AP_RangeFinder KEYWORD1 -AP_RangeFinder_SharpGP2Y KEYWORD1 -AP_RangeFinder_MaxsonarXL KEYWORD1 -read KEYWORD2 -set_orientation KEYWORD2 -convert_raw_to_distance KEYWORD2 -raw_value KEYWORD2 -distance KEYWORD2 -max_distance KEYWORD2 -min_distance KEYWORD2 -orientation_x KEYWORD2 -orientation_y KEYWORD2 -orientation_z KEYWORD2 +RangeFinder KEYWORD1 +AP_RangeFinder KEYWORD1 +AP_RangeFinder_SharpGP2Y KEYWORD1 +AP_RangeFinder_MaxsonarXL KEYWORD1 +read KEYWORD2 +set_orientation KEYWORD2 +convert_raw_to_distance KEYWORD2 +raw_value KEYWORD2 +distance KEYWORD2 +max_distance KEYWORD2 +min_distance KEYWORD2 +orientation_x KEYWORD2 +orientation_y KEYWORD2 +orientation_z KEYWORD2 \ No newline at end of file diff --git a/libraries/Desktop/support/sitl.cpp b/libraries/Desktop/support/sitl.cpp index a085662a2b..8e4d35c904 100644 --- a/libraries/Desktop/support/sitl.cpp +++ b/libraries/Desktop/support/sitl.cpp @@ -135,7 +135,7 @@ static void sitl_fdm_input(void) count++; if (millis() - last_report > 1000) { - printf("SIM %u FPS\n", count); + //printf("SIM %u FPS\n", count); count = 0; last_report = millis(); } diff --git a/libraries/Desktop/support/sitl_adc.cpp b/libraries/Desktop/support/sitl_adc.cpp index 7b241a9a21..8ea7de885d 100644 --- a/libraries/Desktop/support/sitl_adc.cpp +++ b/libraries/Desktop/support/sitl_adc.cpp @@ -111,11 +111,12 @@ void sitl_update_adc(float roll, float pitch, float yaw, (fabs(roll - dcm.roll_sensor/100.0) > 5.0 || fabs(pitch - dcm.pitch_sensor/100.0) > 5.0))) { last_report = tnow; - printf("roll=%5.1f / %5.1f pitch=%5.1f / %5.1f rr=%5.2f / %5.2f pr=%5.2f / %5.2f\n", + /*printf("roll=%5.1f / %5.1f pitch=%5.1f / %5.1f rr=%5.2f / %5.2f pr=%5.2f / %5.2f\n", roll, dcm.roll_sensor/100.0, pitch, dcm.pitch_sensor/100.0, rollRate, ToDeg(omega.x), pitchRate, ToDeg(omega.y)); + */ } }