Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of https://code.google.com/p/ardupilot-mega

This commit is contained in:
Chris Anderson 2012-01-11 00:14:31 -08:00
parent 87683aae93 f35411ac45
commit 73d6421373
41 changed files with 1138 additions and 918 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

132
ArduCopter/ArduCopter.pde
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- 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 ArduCopter Version 2.0 Beta
Authors: Jason Short Authors: Jason Short
@ -80,6 +80,7 @@ http://code.google.com/p/ardupilot-mega/downloads/list
// Configuration // Configuration
#include "defines.h" #include "defines.h"
#include "config.h" #include "config.h"
#include "config_channels.h"
// Local modules // Local modules
#include "Parameters.h" #include "Parameters.h"
@ -278,7 +279,6 @@ ModeFilter sonar_mode_filter;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Global variables // Global variables
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
static const char *comma = ",";
static const char* flight_mode_strings[] = { static const char* flight_mode_strings[] = {
"STABILIZE", "STABILIZE",
@ -327,7 +327,7 @@ static int16_t y_rate_error;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// This is the state of the flight control system // This is the state of the flight control system
// There are multiple states defined such as STABILIZE, ACRO, // 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. // This is the state of simple mode.
// Set in the control_mode.pde file when the control switch is read // Set in the control_mode.pde file when the control switch is read
static bool do_simple = false; 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 // This is the angle from the copter to the "next_WP" location
// with the addition of Crosstrack error in degrees * 100 // with the addition of Crosstrack error in degrees * 100
static int32_t nav_bearing; 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: // Status of the Waypoint tracking mode. Options include:
// NO_NAV_MODE, WP_MODE, LOITER_MODE, CIRCLE_MODE // NO_NAV_MODE, WP_MODE, LOITER_MODE, CIRCLE_MODE
static byte wp_control; static byte wp_control;
@ -572,7 +570,8 @@ static int32_t baro_alt;
static int32_t old_baro_alt; static int32_t old_baro_alt;
// The climb_rate as reported by Baro in cm/s // The climb_rate as reported by Baro in cm/s
static int16_t baro_rate; static int16_t baro_rate;
//
static boolean reset_throttle_flag;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// flight modes // flight modes
@ -681,12 +680,23 @@ static int16_t nav_throttle; // 0-1000 for throttle control
static uint32_t throttle_integrator; static uint32_t throttle_integrator;
// This is a future value for replacing the throttle_cruise setup procedure. It's an average of throttle control // 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 // 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 // This is a flag used to trigger the updating of nav_throttle at 10hz
static bool invalid_throttle; static bool invalid_throttle;
// Used to track the altitude offset for climbrate control // 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 // Navigation Yaw control
@ -737,18 +747,8 @@ static int16_t event_undo_value;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Delay Mission Scripting Command // Delay Mission Scripting Command
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
static int32_t condition_value; // used in condition commands (eg delay, change alt, etc.) static int32_t condition_value; // used in condition commands (eg delay, change alt, etc.)
static int32_t condition_start; static uint32_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;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
@ -794,6 +794,8 @@ static float dTnav;
static int16_t superslow_loopCounter; static int16_t superslow_loopCounter;
// RTL Autoland Timer // RTL Autoland Timer
static uint32_t auto_land_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 // 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 // Set true if we have new PWM data to act on from the Radio
static bool new_radio_frame; static bool new_radio_frame;
// Used to auto exit the in-flight leveler // 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 // Reference to the AP relay object - APM1 only
AP_Relay relay; AP_Relay relay;
@ -865,6 +867,7 @@ void loop()
counter_one_herz++; counter_one_herz++;
// trgger our 1 hz loop
if(counter_one_herz >= 50){ if(counter_one_herz >= 50){
super_slow_loop(); super_slow_loop();
counter_one_herz = 0; counter_one_herz = 0;
@ -1129,7 +1132,7 @@ static void fifty_hz_loop()
#if FRAME_CONFIG == TRI_FRAME #if FRAME_CONFIG == TRI_FRAME
// servo Yaw // servo Yaw
g.rc_4.calc_pwm(); 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 #endif
} }
@ -1193,16 +1196,28 @@ static void slow_loop()
default: default:
slow_loopCounter = 0; slow_loopCounter = 0;
break; break;
} }
} }
#define AUTO_ARMING_DELAY 60
// 1Hz loop // 1Hz loop
static void super_slow_loop() static void super_slow_loop()
{ {
if (g.log_bitmask & MASK_LOG_CUR) if (g.log_bitmask & MASK_LOG_CUR)
Log_Write_Current(); 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_send_message(MSG_HEARTBEAT);
gcs_data_stream_send(1,3); gcs_data_stream_send(1,3);
// agmatthews - USERHOOKS // agmatthews - USERHOOKS
@ -1558,18 +1573,27 @@ void update_throttle_mode(void)
throttle_out = g.throttle_cruise + angle_boost + manual_boost; throttle_out = g.throttle_cruise + angle_boost + manual_boost;
#endif #endif
// reset next_WP.alt and don't go below 1 meter //force a reset of the altitude change
next_WP.alt = max(current_loc.alt, 100); 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, next_WP.alt,
current_loc.alt, current_loc.alt,
altitude_error, altitude_error,
manual_boost); manual_boost,
iterm);
//*/ //*/
reset_throttle_flag = true;
}else{ }else{
if(reset_throttle_flag) {
set_new_altitude(max(current_loc.alt, 100));
reset_throttle_flag = false;
}
// 10hz, don't run up i term // 10hz, don't run up i term
if(invalid_throttle && motor_auto_armed == true){ if(invalid_throttle && motor_auto_armed == true){
@ -1582,14 +1606,13 @@ void update_throttle_mode(void)
// clear the new data flag // clear the new data flag
invalid_throttle = false; 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, next_WP.alt,
current_loc.alt, current_loc.alt,
altitude_error, altitude_error,
nav_throttle, nav_throttle,
(int16_t)g.pi_alt_hold.get_integrator(), (int16_t)g.pi_alt_hold.get_integrator());
(int16_t) g.pi_throttle.get_integrator()); //*/
*/
} }
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
@ -1637,23 +1660,26 @@ static void update_navigation()
break; break;
case RTL: case RTL:
// We have reached Home
if((wp_distance <= g.waypoint_radius) || check_missed_wp()){ if((wp_distance <= g.waypoint_radius) || check_missed_wp()){
// if this value > 0, we are set to trigger auto_land after 30 seconds // if this value > 0, we are set to trigger auto_land after 30 seconds
set_mode(LOITER); set_mode(LOITER);
auto_land_timer = millis(); auto_land_timer = millis();
break;
}
}else if(current_loc.alt < (next_WP.alt - 300)){ // We wait until we've reached out new altitude before coming home
// don't navigate if we are below our target alt // Arg doesn't work, it
wp_control = LOITER_MODE; //if(alt_change_flag != REACHED_ALT){
// wp_control = NO_NAV_MODE;
//}else{
wp_control = WP_MODE;
}else{
// calculates desired Yaw // calculates desired Yaw
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
update_auto_yaw(); update_auto_yaw();
#endif #endif
//}
wp_control = WP_MODE;
}
// calculates the desired Roll and Pitch // calculates the desired Roll and Pitch
update_nav_wp(); update_nav_wp();
@ -1689,12 +1715,9 @@ static void update_navigation()
case LAND: case LAND:
wp_control = LOITER_MODE; wp_control = LOITER_MODE;
if(verify_land()) { // JLN fix for auto land in RTL // verify land will override WP_control if we are below
set_mode(STABILIZE); // a certain altitude
} else { verify_land();
// calculates the desired Roll and Pitch
update_nav_wp();
}
// calculates the desired Roll and Pitch // calculates the desired Roll and Pitch
update_nav_wp(); update_nav_wp();
@ -1827,7 +1850,8 @@ static void update_altitude()
scale = (sonar_alt - 400) / 200; scale = (sonar_alt - 400) / 200;
scale = constrain(scale, 0, 1); 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 // solve for a blended climb_rate
climb_rate = ((float)sonar_rate * (1.0 - scale)) + (float)baro_rate * scale; climb_rate = ((float)sonar_rate * (1.0 - scale)) + (float)baro_rate * scale;
@ -1840,7 +1864,6 @@ static void update_altitude()
} }
}else{ }else{
// NO Sonar case // NO Sonar case
current_loc.alt = baro_alt + home.alt; current_loc.alt = baro_alt + home.alt;
climb_rate = baro_rate; climb_rate = baro_rate;
@ -1848,25 +1871,14 @@ static void update_altitude()
// manage bad data // manage bad data
climb_rate = constrain(climb_rate, -300, 300); climb_rate = constrain(climb_rate, -300, 300);
// update the target altitude
next_WP.alt = get_new_altitude();
} }
static void static void
adjust_altitude() 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){ if(g.rc_3.control_in <= 180){
// we remove 0 to 100 PWM from hover // we remove 0 to 100 PWM from hover
manual_boost = g.rc_3.control_in - 180; manual_boost = g.rc_3.control_in - 180;

29
ArduCopter/Attitude.pde
View File

@ -170,9 +170,6 @@ get_nav_throttle(int32_t z_error)
int16_t rate_error; int16_t rate_error;
int16_t output; 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 // limit error to prevent I term run up
z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX); 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 rate_error = g.pi_alt_hold.get_p(z_error); //_p = .85
// experiment to pipe iterm directly into the output // 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 // calculate rate error
rate_error = rate_error - climb_rate; 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. // Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
// Keeps outdated data out of our calculations // Keeps outdated data out of our calculations
static void reset_hold_I(void) //static void reset_hold_I(void)
{ //{
g.pi_loiter_lat.reset_I(); // g.pi_loiter_lat.reset_I();
g.pi_loiter_lon.reset_I(); // g.pi_loiter_lon.reset_I();
} //}
// Keeps old data out of our calculation / logs // Keeps old data out of our calculation / logs
static void reset_nav(void) static void reset_nav(void)
{ {
// forces us to update nav throttle
invalid_throttle = true; invalid_throttle = true;
nav_throttle = 0; nav_throttle = 0;
// always start Circle mode at same angle
circle_angle = 0; circle_angle = 0;
// We must be heading to a new WP, so XTrack must be 0
crosstrack_error = 0; crosstrack_error = 0;
// Will be set by new command
target_bearing = 0; target_bearing = 0;
// Will be set by new command
wp_distance = 0; wp_distance = 0;
// Will be set by new command, used by loiter
long_error = 0; long_error = 0;
lat_error = 0; lat_error = 0;
// Will be set by new command, used by loiter
next_WP.alt = 0;
} }
static void reset_rate_I() static void reset_rate_I()

4
ArduCopter/Camera.pde
View File

@ -51,8 +51,8 @@ camera_stabilization()
g.rc_camera_pitch.calc_pwm(); g.rc_camera_pitch.calc_pwm();
g.rc_camera_roll.calc_pwm(); g.rc_camera_roll.calc_pwm();
APM_RC.OutputCh(CH_5, g.rc_camera_pitch.radio_out); APM_RC.OutputCh(CH_CAM_PITCH, g.rc_camera_pitch.radio_out);
APM_RC.OutputCh(CH_6, g.rc_camera_roll.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); //Serial.printf("c:%d\n", g.rc_camera_pitch.radio_out);
} }

2
ArduCopter/Log.pde
View File

@ -308,7 +308,7 @@ static void Log_Read_Raw()
for (int y = 0; y < 6; y++) { for (int y = 0; y < 6; y++) {
logvar = (float)DataFlash.ReadLong() / t7; logvar = (float)DataFlash.ReadLong() / t7;
Serial.print(logvar); Serial.print(logvar);
Serial.print(comma); Serial.print(",");
} }
Serial.println(" "); Serial.println(" ");
} }

13
ArduCopter/commands.pde
View File

@ -147,9 +147,13 @@ static void set_next_WP(struct Location *wp)
// --------------------- // ---------------------
next_WP = *wp; next_WP = *wp;
// used to control and limit the rate of climb - not used right now! // used to control and limit the rate of climb
// ----------------------------------------------------------------- // -------------------------------------------
target_altitude = current_loc.alt; // 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 // this is used to offset the shrinking longitude as we go towards the poles
float rads = (fabs((float)next_WP.lat)/t7) * 0.0174532925; 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); target_bearing = get_bearing(&prev_WP, &next_WP);
nav_bearing = target_bearing; nav_bearing = target_bearing;
// calc the location error:
calc_location_error(&next_WP);
// to check if we have missed the WP // to check if we have missed the WP
// --------------------------------- // ---------------------------------
original_target_bearing = target_bearing; original_target_bearing = target_bearing;

63
ArduCopter/commands_logic.pde
View File

@ -199,7 +199,7 @@ static void do_RTL(void)
//so we know where we are navigating from //so we know where we are navigating from
// -------------------------------------- // --------------------------------------
next_WP = current_loc; next_WP = current_loc;
// Loads WP from Memory // Loads WP from Memory
// -------------------- // --------------------
@ -265,19 +265,13 @@ static void do_land()
{ {
wp_control = LOITER_MODE; wp_control = LOITER_MODE;
//Serial.println("dlnd ");
// not really used right now, might be good for debugging
land_complete = false; 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 // hold at our current location
set_next_WP(&current_loc); set_next_WP(&current_loc);
// Set a new target altitude
set_new_altitude(0);
} }
static void do_loiter_unlimited() static void do_loiter_unlimited()
@ -339,55 +333,44 @@ static bool verify_takeoff()
return false; return false;
} }
// are we above our target altitude? // 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 bool verify_land()
{ {
static int32_t old_alt = 0; static int32_t old_alt = 0;
static int16_t velocity_land = -1; static int16_t velocity_land = -1;
// land at .62 meter per second // landing detector
next_WP.alt = original_alt - ((millis() - land_start) / 32); // condition_value = our initial if ((current_loc.alt - home.alt) > 300){
if (old_alt == 0)
old_alt = current_loc.alt; old_alt = current_loc.alt;
if (velocity_land == -1)
velocity_land = 2000; velocity_land = 2000;
}else{
if ((current_loc.alt - home.alt) < 300){
// a LP filter used to tell if we have landed // 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 // 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 // remenber altitude for climb_rate
old_alt = current_loc.alt; old_alt = current_loc.alt;
if ((current_loc.alt - home.alt) < 200){ if ((current_loc.alt - home.alt) < 200){
// don't bank to hold position // don't bank to hold position
wp_control = NO_NAV_MODE; 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; land_complete = true;
// reset manual_boost hack
manual_boost = 0;
// reset old_alt // reset old_alt
old_alt = 0; 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; return false;
} }
@ -396,7 +379,9 @@ static bool verify_nav_wp()
// Altitude checking // Altitude checking
if(next_WP.options & MASK_OPTIONS_RELATIVE_ALT){ if(next_WP.options & MASK_OPTIONS_RELATIVE_ALT){
// we desire a certain minimum altitude // 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 // we have reached that altitude
wp_verify_byte |= NAV_ALTITUDE; wp_verify_byte |= NAV_ALTITUDE;
} }
@ -509,7 +494,7 @@ static void do_change_alt()
{ {
Location temp = next_WP; Location temp = next_WP;
condition_start = current_loc.alt; condition_start = current_loc.alt;
condition_value = command_cond_queue.alt; //condition_value = command_cond_queue.alt;
temp.alt = command_cond_queue.alt; temp.alt = command_cond_queue.alt;
set_next_WP(&temp); set_next_WP(&temp);
} }
@ -582,9 +567,9 @@ static void do_yaw()
static bool verify_wait_delay() static bool verify_wait_delay()
{ {
//Serial.print("vwd"); //Serial.print("vwd");
if ((unsigned)(millis() - condition_start) > condition_value){ if ((unsigned)(millis() - condition_start) > (unsigned)condition_value){
//Serial.println("y"); //Serial.println("y");
condition_value = 0; condition_value = 0;
return true; return true;
} }
//Serial.println("n"); //Serial.println("n");
@ -594,16 +579,14 @@ static bool verify_wait_delay()
static bool verify_change_alt() static bool verify_change_alt()
{ {
//Serial.printf("change_alt, ca:%d, na:%d\n", (int)current_loc.alt, (int)next_WP.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 // we are going higer
if(current_loc.alt > next_WP.alt){ if(current_loc.alt > next_WP.alt){
condition_value = 0;
return true; return true;
} }
}else{ }else{
// we are going lower // we are going lower
if(current_loc.alt < next_WP.alt){ if(current_loc.alt < next_WP.alt){
condition_value = 0;
return true; return true;
} }
} }

12
ArduCopter/commands_process.pde
View File

@ -39,7 +39,7 @@ static void update_commands()
//uint8_t tmp = g.command_index.get(); //uint8_t tmp = g.command_index.get();
//Serial.printf("command_index %u \n", tmp); //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; return;
if(command_nav_queue.id == NO_COMMAND){ if(command_nav_queue.id == NO_COMMAND){
@ -62,8 +62,14 @@ static void update_commands()
g.command_index = command_nav_index = 255; g.command_index = command_nav_index = 255;
// if we are on the ground, enter stabilize, else Land // if we are on the ground, enter stabilize, else Land
if (land_complete == true){ if (land_complete == true){
set_mode(STABILIZE); // So what state does this leave us in?
// disarm motors // 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(); //init_disarm_motors();
} else { } else {
set_mode(LAND); set_mode(LAND);

6
ArduCopter/config.h
View File

@ -1,5 +1,7 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // -*- 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 #ifndef MAVLINK_TELEMETRY_PORT_DELAY
# define MAVLINK_TELEMETRY_PORT_DELAY 2000 # define MAVLINK_TELEMETRY_PORT_DELAY 2000
#endif #endif
#endif // __ARDUCOPTER_CONFIG_H__

73
ArduCopter/config_channels.h Normal file
View File

@ -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__

4
ArduCopter/defines.h
View File

@ -106,6 +106,10 @@
#define HIL_MODE_ATTITUDE 1 #define HIL_MODE_ATTITUDE 1
#define HIL_MODE_SENSORS 2 #define HIL_MODE_SENSORS 2
#define ASCENDING 1
#define DESCENDING -1
#define REACHED_ALT 0
// Auto Pilot modes // Auto Pilot modes
// ---------------- // ----------------
#define STABILIZE 0 // hold level position #define STABILIZE 0 // hold level position

19
ArduCopter/events.pde
View File

@ -11,21 +11,22 @@ static void failsafe_on_event()
{ {
case AUTO: case AUTO:
if (g.throttle_fs_action == 1) { if (g.throttle_fs_action == 1) {
// do_rtl sets the altitude to the current altitude by default
set_mode(RTL); set_mode(RTL);
// send up up 10m // We add an additional 10m to the current altitude
next_WP.alt += 1000; //next_WP.alt += 1000;
set_new_altitude(target_altitude + 1000);
} }
// 2 = Stay in AUTO and ignore failsafe // 2 = Stay in AUTO and ignore failsafe
default: default:
if(home_is_set == true){ if(home_is_set == true){
// home distance is in meters // same as above ^
// This is to prevent accidental RTL // do_rtl sets the altitude to the current altitude by default
if ((home_distance > 15) && (current_loc.alt > 400)){ set_mode(RTL);
set_mode(RTL); // We add an additional 10m to the current altitude
// send up up 10m //next_WP.alt += 1000;
next_WP.alt += 1000; set_new_altitude(target_altitude + 1000);
}
}else{ }else{
// We have no GPS so we must land // We have no GPS so we must land
set_mode(LAND); set_mode(LAND);

2
ArduCopter/heli.pde
View File

@ -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() static void init_motors_out()
{ {
#if INSTANT_PWM == 0 #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 #endif
} }

15
ArduCopter/motors.pde
View File

@ -142,3 +142,18 @@ set_servos_4()
output_motors_disarmed(); 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);
}

184
ArduCopter/motors_hexa.pde
View File

@ -5,8 +5,8 @@
static void init_motors_out() static void init_motors_out()
{ {
#if INSTANT_PWM == 0 #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)
| MSK_CH_7 | MSK_CH_8 ); | _BV(MOT_5) | _BV(MOT_6) );
#endif #endif
} }
@ -32,90 +32,88 @@ static void output_motors_armed()
pitch_out = (float)g.rc_2.pwm_out * .866; pitch_out = (float)g.rc_2.pwm_out * .866;
//left side //left side
motor_out[CH_2] = g.rc_3.radio_out + g.rc_1.pwm_out; // CCW Middle motor_out[MOT_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[MOT_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_6] = g.rc_3.radio_out + roll_out - pitch_out; // CW Back
//right side //right side
motor_out[CH_1] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW Middle motor_out[MOT_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[MOT_5] = 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_4] = g.rc_3.radio_out - roll_out - pitch_out; // CCW Back
}else{ }else{
roll_out = (float)g.rc_1.pwm_out * .866; roll_out = (float)g.rc_1.pwm_out * .866;
pitch_out = g.rc_2.pwm_out / 2; pitch_out = g.rc_2.pwm_out / 2;
//Front side //Front side
motor_out[CH_1] = g.rc_3.radio_out + g.rc_2.pwm_out; // CW FRONT motor_out[MOT_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[MOT_5] = 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_4] = g.rc_3.radio_out - roll_out + pitch_out; // CCW FRONT RIGHT
//Back side //Back side
motor_out[CH_2] = g.rc_3.radio_out - g.rc_2.pwm_out; // CCW BACK motor_out[MOT_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[MOT_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_6] = g.rc_3.radio_out - roll_out - pitch_out; // CW BACK RIGHT
} }
// Yaw // Yaw
motor_out[CH_2] += g.rc_4.pwm_out; // CCW motor_out[MOT_2] += g.rc_4.pwm_out; // CCW
motor_out[CH_7] += g.rc_4.pwm_out; // CCW motor_out[MOT_5] += g.rc_4.pwm_out; // CCW
motor_out[CH_4] += 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[MOT_3] -= g.rc_4.pwm_out; // CW
motor_out[CH_1] -= g.rc_4.pwm_out; // CW motor_out[MOT_1] -= g.rc_4.pwm_out; // CW
motor_out[CH_8] -= g.rc_4.pwm_out; // CW motor_out[MOT_6] -= g.rc_4.pwm_out; // CW
// Tridge's stability patch // Tridge's stability patch
for (int i = CH_1; i<=CH_8; i++) { for (int m = 0; m <= 6; m++) {
if(i == CH_5 || i == CH_6) int c = ch_of_mot(m);
continue; int c_opp = ch_of_mot(m^1); // m^1 is the opposite motor. c_opp is channel of opposite motor.
if (motor_out[i] > out_max) { if (motor_out[c] > out_max) {
// note that i^1 is the opposite motor motor_out[c_opp] -= motor_out[c] - out_max;
motor_out[i^1] -= motor_out[i] - out_max; motor_out[c] = out_max;
motor_out[i] = out_max;
} }
} }
// limit output so motors don't stop // limit output so motors don't stop
motor_out[CH_1] = max(motor_out[CH_1], out_min); motor_out[MOT_1] = max(motor_out[MOT_1], out_min);
motor_out[CH_2] = max(motor_out[CH_2], out_min); motor_out[MOT_2] = max(motor_out[MOT_2], out_min);
motor_out[CH_3] = max(motor_out[CH_3], out_min); motor_out[MOT_3] = max(motor_out[MOT_3], out_min);
motor_out[CH_4] = max(motor_out[CH_4], out_min); motor_out[MOT_4] = max(motor_out[MOT_4], out_min);
motor_out[CH_7] = max(motor_out[CH_7], out_min); motor_out[MOT_5] = max(motor_out[MOT_5], out_min);
motor_out[CH_8] = max(motor_out[CH_8], out_min); motor_out[MOT_6] = max(motor_out[MOT_6], out_min);
#if CUT_MOTORS == ENABLED #if CUT_MOTORS == ENABLED
// if we are not sending a throttle output, we cut the motors // if we are not sending a throttle output, we cut the motors
if(g.rc_3.servo_out == 0){ if(g.rc_3.servo_out == 0){
motor_out[CH_1] = g.rc_3.radio_min; motor_out[MOT_1] = g.rc_3.radio_min;
motor_out[CH_2] = g.rc_3.radio_min; motor_out[MOT_2] = g.rc_3.radio_min;
motor_out[CH_3] = g.rc_3.radio_min; motor_out[MOT_3] = g.rc_3.radio_min;
motor_out[CH_4] = g.rc_3.radio_min; motor_out[MOT_4] = g.rc_3.radio_min;
motor_out[CH_7] = g.rc_3.radio_min; motor_out[MOT_5] = g.rc_3.radio_min;
motor_out[CH_8] = g.rc_3.radio_min; motor_out[MOT_6] = g.rc_3.radio_min;
} }
#endif #endif
// this filter slows the acceleration of motors vs the deceleration // this filter slows the acceleration of motors vs the deceleration
// Idea by Denny Rowland to help with his Yaw issue // Idea by Denny Rowland to help with his Yaw issue
for(int8_t i = CH_1; i <= CH_8; i++ ) { for(int8_t m = 0; m <= 6; m++ ) {
if(i == CH_5 || i == CH_6) int c = ch_of_mot(m);
continue; if(motor_filtered[c] < motor_out[c]){
if(motor_filtered[i] < motor_out[i]){ motor_filtered[c] = (motor_out[c] + motor_filtered[c]) / 2;
motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2;
}else{ }else{
// don't filter // 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(MOT_1, motor_filtered[MOT_1]);
APM_RC.OutputCh(CH_2, motor_filtered[CH_2]); APM_RC.OutputCh(MOT_2, motor_filtered[MOT_2]);
APM_RC.OutputCh(CH_3, motor_filtered[CH_3]); APM_RC.OutputCh(MOT_3, motor_filtered[MOT_3]);
APM_RC.OutputCh(CH_4, motor_filtered[CH_4]); APM_RC.OutputCh(MOT_4, motor_filtered[MOT_4]);
APM_RC.OutputCh(CH_7, motor_filtered[CH_7]); APM_RC.OutputCh(MOT_5, motor_filtered[MOT_5]);
APM_RC.OutputCh(CH_8, motor_filtered[CH_8]); APM_RC.OutputCh(MOT_6, motor_filtered[MOT_6]);
#if INSTANT_PWM == 1 #if INSTANT_PWM == 1
// InstantPWM // InstantPWM
@ -140,43 +138,43 @@ static void output_motors_disarmed()
} }
// Send commands to motors // Send commands to motors
APM_RC.OutputCh(CH_1, g.rc_3.radio_min); APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min); APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min); APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min); APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min); APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min); APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
} }
static void output_motor_test() static void output_motor_test()
{ {
motor_out[CH_1] = g.rc_3.radio_min; motor_out[MOT_1] = g.rc_3.radio_min;
motor_out[CH_2] = g.rc_3.radio_min; motor_out[MOT_2] = g.rc_3.radio_min;
motor_out[CH_3] = g.rc_3.radio_min; motor_out[MOT_3] = g.rc_3.radio_min;
motor_out[CH_4] = g.rc_3.radio_min; motor_out[MOT_4] = g.rc_3.radio_min;
motor_out[CH_7] = g.rc_3.radio_min; motor_out[MOT_5] = g.rc_3.radio_min;
motor_out[CH_8] = g.rc_3.radio_min; motor_out[MOT_6] = g.rc_3.radio_min;
if(g.frame_orientation == X_FRAME){ if(g.frame_orientation == X_FRAME){
// 31 // 31
// 24 // 24
if(g.rc_1.control_in > 3000){ // right 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 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 if(g.rc_2.control_in > 3000){ // back
motor_out[CH_8] += 100; motor_out[MOT_6] += 100;
motor_out[CH_4] += 100; motor_out[MOT_4] += 100;
} }
if(g.rc_2.control_in < -3000){ // front if(g.rc_2.control_in < -3000){ // front
motor_out[CH_7] += 100; motor_out[MOT_5] += 100;
motor_out[CH_3] += 100; motor_out[MOT_3] += 100;
} }
}else{ }else{
@ -184,56 +182,56 @@ static void output_motor_test()
// 2 1 // 2 1
// 4 // 4
if(g.rc_1.control_in > 3000){ // right if(g.rc_1.control_in > 3000){ // right
motor_out[CH_4] += 100; motor_out[MOT_4] += 100;
motor_out[CH_8] += 100; motor_out[MOT_6] += 100;
} }
if(g.rc_1.control_in < -3000){ // left if(g.rc_1.control_in < -3000){ // left
motor_out[CH_7] += 100; motor_out[MOT_5] += 100;
motor_out[CH_3] += 100; motor_out[MOT_3] += 100;
} }
if(g.rc_2.control_in > 3000){ // back 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 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(MOT_1, motor_out[MOT_1]);
APM_RC.OutputCh(CH_2, motor_out[CH_2]); APM_RC.OutputCh(MOT_2, motor_out[MOT_2]);
APM_RC.OutputCh(CH_3, motor_out[CH_3]); APM_RC.OutputCh(MOT_3, motor_out[MOT_3]);
APM_RC.OutputCh(CH_4, motor_out[CH_4]); APM_RC.OutputCh(MOT_4, motor_out[MOT_4]);
APM_RC.OutputCh(CH_7, motor_out[CH_7]); APM_RC.OutputCh(MOT_5, motor_out[MOT_5]);
APM_RC.OutputCh(CH_8, motor_out[CH_8]); APM_RC.OutputCh(MOT_6, motor_out[MOT_6]);
} }
/* /*
APM_RC.OutputCh(CH_2, g.rc_3.radio_min); APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100); APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100);
delay(1000); delay(1000);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min); APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100); APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100);
delay(1000); delay(1000);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min); APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100); APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
delay(1000); delay(1000);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min); APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100); APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100);
delay(1000); delay(1000);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min); APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100); APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100);
delay(1000); delay(1000);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min); APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100); APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100);
delay(1000); delay(1000);
} }
*/ */

223
ArduCopter/motors_octa.pde
View File

@ -5,8 +5,8 @@
static void init_motors_out() static void init_motors_out()
{ {
#if INSTANT_PWM == 0 #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)
| MSK_CH_7 | MSK_CH_8 | MSK_CH_10 | MSK_CH_11 ); | _BV(MOT_5) | _BV(MOT_6) | _BV(MOT_7) | _BV(MOT_8) );
#endif #endif
} }
@ -32,40 +32,40 @@ static void output_motors_armed()
pitch_out = (float)g.rc_2.pwm_out * 0.4; pitch_out = (float)g.rc_2.pwm_out * 0.4;
//Front side //Front side
motor_out[CH_1] = g.rc_3.radio_out + g.rc_2.pwm_out - roll_out; // CW FRONT RIGHT motor_out[MOT_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_5] = g.rc_3.radio_out + g.rc_2.pwm_out + roll_out; // CCW FRONT LEFT
//Back side //Back side
motor_out[CH_2] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out; // CW BACK LEFT motor_out[MOT_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_4] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out; // CCW BACK RIGHT
//Left side //Left side
motor_out[CH_10] = g.rc_3.radio_out + g.rc_1.pwm_out + pitch_out; // CW LEFT FRONT motor_out[MOT_7] = 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_6] = g.rc_3.radio_out + g.rc_1.pwm_out - pitch_out; // CCW LEFT BACK
//Right side //Right side
motor_out[CH_11] = g.rc_3.radio_out - g.rc_1.pwm_out - pitch_out; // CW RIGHT BACK motor_out[MOT_8] = 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_3] = g.rc_3.radio_out - g.rc_1.pwm_out + pitch_out; // CCW RIGHT FRONT
}else if(g.frame_orientation == PLUS_FRAME){ }else if(g.frame_orientation == PLUS_FRAME){
roll_out = (float)g.rc_1.pwm_out * 0.71; roll_out = (float)g.rc_1.pwm_out * 0.71;
pitch_out = (float)g.rc_2.pwm_out * 0.71; pitch_out = (float)g.rc_2.pwm_out * 0.71;
//Front side //Front side
motor_out[CH_1] = g.rc_3.radio_out + g.rc_2.pwm_out; // CW FRONT motor_out[MOT_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[MOT_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_5] = g.rc_3.radio_out + roll_out + pitch_out; // CCW FRONT LEFT
//Left side //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 //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 //Back side
motor_out[CH_2] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW BACK motor_out[MOT_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[MOT_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_6] = g.rc_3.radio_out + roll_out - pitch_out; // CCW BACK LEFT
}else if(g.frame_orientation == V_FRAME){ }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; pitch_out4 = (float)g.rc_2.pwm_out * 0.98;
//Front side //Front side
motor_out[CH_10] = g.rc_3.radio_out + g.rc_2.pwm_out - roll_out; // CW FRONT RIGHT motor_out[MOT_7] = 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_5] = g.rc_3.radio_out + g.rc_2.pwm_out + roll_out; // CCW FRONT LEFT
//Left side //Left side
motor_out[CH_1] = g.rc_3.radio_out + g.rc_1.pwm_out + pitch_out2; // CW LEFT FRONT motor_out[MOT_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_3] = g.rc_3.radio_out + g.rc_1.pwm_out - pitch_out3; // CCW LEFT BACK
//Right side //Right side
motor_out[CH_2] = g.rc_3.radio_out - g.rc_1.pwm_out - pitch_out3; // CW RIGHT BACK motor_out[MOT_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_6] = g.rc_3.radio_out - g.rc_1.pwm_out + pitch_out2; // CCW RIGHT FRONT
//Back side //Back side
motor_out[CH_11] = g.rc_3.radio_out - g.rc_2.pwm_out + roll_out4; // CW BACK LEFT motor_out[MOT_8] = 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_4] = g.rc_3.radio_out - g.rc_2.pwm_out - roll_out4; // CCW BACK RIGHT
} }
// Yaw // Yaw
motor_out[CH_3] += g.rc_4.pwm_out; // CCW motor_out[MOT_3] += g.rc_4.pwm_out; // CCW
motor_out[CH_4] += g.rc_4.pwm_out; // CCW motor_out[MOT_4] += g.rc_4.pwm_out; // CCW
motor_out[CH_7] += g.rc_4.pwm_out; // CCW motor_out[MOT_5] += g.rc_4.pwm_out; // CCW
motor_out[CH_8] += 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[MOT_1] -= g.rc_4.pwm_out; // CW
motor_out[CH_2] -= g.rc_4.pwm_out; // CW motor_out[MOT_2] -= g.rc_4.pwm_out; // CW
motor_out[CH_10] -= g.rc_4.pwm_out; // CW motor_out[MOT_7] -= g.rc_4.pwm_out; // CW
motor_out[CH_11] -= g.rc_4.pwm_out; // CW motor_out[MOT_8] -= g.rc_4.pwm_out; // CW
// TODO add stability patch // TODO add stability patch
motor_out[CH_1] = min(motor_out[CH_1], out_max); motor_out[MOT_1] = min(motor_out[MOT_1], out_max);
motor_out[CH_2] = min(motor_out[CH_2], out_max); motor_out[MOT_2] = min(motor_out[MOT_2], out_max);
motor_out[CH_3] = min(motor_out[CH_3], out_max); motor_out[MOT_3] = min(motor_out[MOT_3], out_max);
motor_out[CH_4] = min(motor_out[CH_4], out_max); motor_out[MOT_4] = min(motor_out[MOT_4], out_max);
motor_out[CH_7] = min(motor_out[CH_7], out_max); motor_out[MOT_5] = min(motor_out[MOT_5], out_max);
motor_out[CH_8] = min(motor_out[CH_8], out_max); motor_out[MOT_6] = min(motor_out[MOT_6], out_max);
motor_out[CH_10] = min(motor_out[CH_10], out_max); motor_out[MOT_7] = min(motor_out[MOT_7], out_max);
motor_out[CH_11] = min(motor_out[CH_11], out_max); motor_out[MOT_8] = min(motor_out[MOT_8], out_max);
// limit output so motors don't stop // limit output so motors don't stop
motor_out[CH_1] = max(motor_out[CH_1], out_min); motor_out[MOT_1] = max(motor_out[MOT_1], out_min);
motor_out[CH_2] = max(motor_out[CH_2], out_min); motor_out[MOT_2] = max(motor_out[MOT_2], out_min);
motor_out[CH_3] = max(motor_out[CH_3], out_min); motor_out[MOT_3] = max(motor_out[MOT_3], out_min);
motor_out[CH_4] = max(motor_out[CH_4], out_min); motor_out[MOT_4] = max(motor_out[MOT_4], out_min);
motor_out[CH_7] = max(motor_out[CH_7], out_min); motor_out[MOT_5] = max(motor_out[MOT_5], out_min);
motor_out[CH_8] = max(motor_out[CH_8], out_min); motor_out[MOT_6] = max(motor_out[MOT_6], out_min);
motor_out[CH_10] = max(motor_out[CH_10], out_min); motor_out[MOT_7] = max(motor_out[MOT_7], out_min);
motor_out[CH_11] = max(motor_out[CH_11], out_min); motor_out[MOT_8] = max(motor_out[MOT_8], out_min);
#if CUT_MOTORS == ENABLED #if CUT_MOTORS == ENABLED
// if we are not sending a throttle output, we cut the motors // if we are not sending a throttle output, we cut the motors
if(g.rc_3.servo_out == 0){ if(g.rc_3.servo_out == 0){
motor_out[CH_1] = g.rc_3.radio_min; motor_out[MOT_1] = g.rc_3.radio_min;
motor_out[CH_2] = g.rc_3.radio_min; motor_out[MOT_2] = g.rc_3.radio_min;
motor_out[CH_3] = g.rc_3.radio_min; motor_out[MOT_3] = g.rc_3.radio_min;
motor_out[CH_4] = g.rc_3.radio_min; motor_out[MOT_4] = g.rc_3.radio_min;
motor_out[CH_7] = g.rc_3.radio_min; motor_out[MOT_5] = g.rc_3.radio_min;
motor_out[CH_8] = g.rc_3.radio_min; motor_out[MOT_6] = g.rc_3.radio_min;
motor_out[CH_10] = g.rc_3.radio_min; motor_out[MOT_7] = g.rc_3.radio_min;
motor_out[CH_11] = g.rc_3.radio_min; motor_out[MOT_8] = g.rc_3.radio_min;
} }
#endif #endif
// this filter slows the acceleration of motors vs the deceleration // this filter slows the acceleration of motors vs the deceleration
// Idea by Denny Rowland to help with his Yaw issue // Idea by Denny Rowland to help with his Yaw issue
for(int8_t i = CH_1; i <= CH_11; i++ ) { for(int8_t m = 0; m <= 8; m++ ) {
if(i == CH_5 || i == CH_6 || i == CH_9) int c = ch_of_mot(m);
continue; if(motor_filtered[c] < motor_out[c]){
if(motor_filtered[i] < motor_out[i]){ motor_filtered[c] = (motor_out[c] + motor_filtered[c]) / 2;
motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2;
}else{ }else{
// don't filter // 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(MOT_1, motor_filtered[MOT_1]);
APM_RC.OutputCh(CH_2, motor_filtered[CH_2]); APM_RC.OutputCh(MOT_2, motor_filtered[MOT_2]);
APM_RC.OutputCh(CH_3, motor_filtered[CH_3]); APM_RC.OutputCh(MOT_3, motor_filtered[MOT_3]);
APM_RC.OutputCh(CH_4, motor_filtered[CH_4]); APM_RC.OutputCh(MOT_4, motor_filtered[MOT_4]);
APM_RC.OutputCh(CH_7, motor_filtered[CH_7]); APM_RC.OutputCh(MOT_5, motor_filtered[MOT_5]);
APM_RC.OutputCh(CH_8, motor_filtered[CH_8]); APM_RC.OutputCh(MOT_6, motor_filtered[MOT_6]);
APM_RC.OutputCh(CH_10, motor_filtered[CH_10]); APM_RC.OutputCh(MOT_7, motor_filtered[MOT_7]);
APM_RC.OutputCh(CH_11, motor_filtered[CH_11]); APM_RC.OutputCh(MOT_8, motor_filtered[MOT_8]);
#if INSTANT_PWM == 1 #if INSTANT_PWM == 1
// InstantPWM // InstantPWM
@ -192,86 +191,86 @@ static void output_motors_disarmed()
} }
// Send commands to motors // Send commands to motors
APM_RC.OutputCh(CH_1, g.rc_3.radio_min); APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min); APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min); APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min); APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
APM_RC.OutputCh(CH_11, 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(MOT_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min); APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
APM_RC.OutputCh(CH_10, g.rc_3.radio_min); APM_RC.OutputCh(MOT_7, g.rc_3.radio_min);
} }
static void output_motor_test() static void output_motor_test()
{ {
if( g.frame_orientation == X_FRAME || g.frame_orientation == PLUS_FRAME ) if( g.frame_orientation == X_FRAME || g.frame_orientation == PLUS_FRAME )
{ {
APM_RC.OutputCh(CH_7, g.rc_3.radio_min); APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100); APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
delay(1000); delay(1000);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min); APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100); APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100);
delay(1000); delay(1000);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min); APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 100); APM_RC.OutputCh(MOT_8, g.rc_3.radio_min + 100);
delay(1000); delay(1000);
APM_RC.OutputCh(CH_11, g.rc_3.radio_min); APM_RC.OutputCh(MOT_8, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100); APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100);
delay(1000); delay(1000);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min); APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100); APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100);
delay(1000); delay(1000);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min); APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100); APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100);
delay(1000); delay(1000);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min); APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 100); APM_RC.OutputCh(MOT_7, g.rc_3.radio_min + 100);
delay(1000); delay(1000);
APM_RC.OutputCh(CH_10, g.rc_3.radio_min); APM_RC.OutputCh(MOT_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100); APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100);
delay(1000); delay(1000);
} }
if( g.frame_orientation == V_FRAME ) if( g.frame_orientation == V_FRAME )
{ {
APM_RC.OutputCh(CH_7, g.rc_3.radio_min); APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 100); APM_RC.OutputCh(MOT_7, g.rc_3.radio_min + 100);
delay(1000); delay(1000);
APM_RC.OutputCh(CH_10, g.rc_3.radio_min); APM_RC.OutputCh(MOT_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100); APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100);
delay(1000); delay(1000);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min); APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100); APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100);
delay(1000); delay(1000);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min); APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100); APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100);
delay(1000); delay(1000);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min); APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 100); APM_RC.OutputCh(MOT_8, g.rc_3.radio_min + 100);
delay(1000); delay(1000);
APM_RC.OutputCh(CH_11, 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 + 100); APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100);
delay(1000); delay(1000);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min); APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100); APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
delay(1000); delay(1000);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min); APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100); APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100);
delay(1000); delay(1000);
} }
} }

169
ArduCopter/motors_octa_quad.pde
View File

@ -5,8 +5,8 @@
static void init_motors_out() static void init_motors_out()
{ {
#if INSTANT_PWM == 0 #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)
| MSK_CH_7 | MSK_CH_8 | MSK_CH_10 | MSK_CH_11 ); | _BV(MOT_5) | _BV(MOT_6) | _BV(MOT_7) | _BV(MOT_8) );
#endif #endif
} }
@ -32,20 +32,20 @@ static void output_motors_armed()
pitch_out = (float)g.rc_2.pwm_out * .707; pitch_out = (float)g.rc_2.pwm_out * .707;
// Front Left // Front Left
motor_out[CH_7] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out + pitch_out); // CCW TOP motor_out[MOT_5] = ((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_6] = g.rc_3.radio_out + roll_out + pitch_out; // CW
// Front Right // Front Right
motor_out[CH_10] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out + pitch_out); // CCW TOP motor_out[MOT_7] = ((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_8] = g.rc_3.radio_out - roll_out + pitch_out; // CW
// Back Left // Back Left
motor_out[CH_3] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out - pitch_out); // CCW TOP motor_out[MOT_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_4] = g.rc_3.radio_out + roll_out - pitch_out; // CW
// Back Right // Back Right
motor_out[CH_1] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out - pitch_out); // CCW TOP motor_out[MOT_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_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; pitch_out = g.rc_2.pwm_out;
// Left // Left
motor_out[CH_7] = (g.rc_3.radio_out * g.top_bottom_ratio) - roll_out; // CCW TOP motor_out[MOT_5] = (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_6] = g.rc_3.radio_out - roll_out; // CW
// Right // Right
motor_out[CH_1] = (g.rc_3.radio_out * g.top_bottom_ratio) + roll_out; // CCW TOP motor_out[MOT_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_2] = g.rc_3.radio_out + roll_out; // CW
// Front // Front
motor_out[CH_10] = (g.rc_3.radio_out * g.top_bottom_ratio) + pitch_out; // CCW TOP motor_out[MOT_7] = (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_8] = g.rc_3.radio_out + pitch_out; // CW
// Back // Back
motor_out[CH_3] = (g.rc_3.radio_out * g.top_bottom_ratio) - pitch_out; // CCW TOP motor_out[MOT_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_4] = g.rc_3.radio_out - pitch_out; // CW
} }
// Yaw // Yaw
motor_out[CH_1] += g.rc_4.pwm_out; // CCW motor_out[MOT_1] += g.rc_4.pwm_out; // CCW
motor_out[CH_3] += g.rc_4.pwm_out; // CCW motor_out[MOT_3] += g.rc_4.pwm_out; // CCW
motor_out[CH_7] += g.rc_4.pwm_out; // CCW motor_out[MOT_5] += g.rc_4.pwm_out; // CCW
motor_out[CH_10] += 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[MOT_2] -= g.rc_4.pwm_out; // CW
motor_out[CH_4] -= g.rc_4.pwm_out; // CW motor_out[MOT_4] -= g.rc_4.pwm_out; // CW
motor_out[CH_8] -= g.rc_4.pwm_out; // CW motor_out[MOT_6] -= g.rc_4.pwm_out; // CW
motor_out[CH_11] -= g.rc_4.pwm_out; // CW motor_out[MOT_8] -= g.rc_4.pwm_out; // CW
// TODO add stability patch // TODO add stability patch
motor_out[CH_1] = min(motor_out[CH_1], out_max); motor_out[MOT_1] = min(motor_out[MOT_1], out_max);
motor_out[CH_2] = min(motor_out[CH_2], out_max); motor_out[MOT_2] = min(motor_out[MOT_2], out_max);
motor_out[CH_3] = min(motor_out[CH_3], out_max); motor_out[MOT_3] = min(motor_out[MOT_3], out_max);
motor_out[CH_4] = min(motor_out[CH_4], out_max); motor_out[MOT_4] = min(motor_out[MOT_4], out_max);
motor_out[CH_7] = min(motor_out[CH_7], out_max); motor_out[MOT_5] = min(motor_out[MOT_5], out_max);
motor_out[CH_8] = min(motor_out[CH_8], out_max); motor_out[MOT_6] = min(motor_out[MOT_6], out_max);
motor_out[CH_10] = min(motor_out[CH_10], out_max); motor_out[MOT_7] = min(motor_out[MOT_7], out_max);
motor_out[CH_11] = min(motor_out[CH_11], out_max); motor_out[MOT_8] = min(motor_out[MOT_8], out_max);
// limit output so motors don't stop // limit output so motors don't stop
motor_out[CH_1] = max(motor_out[CH_1], out_min); motor_out[MOT_1] = max(motor_out[MOT_1], out_min);
motor_out[CH_2] = max(motor_out[CH_2], out_min); motor_out[MOT_2] = max(motor_out[MOT_2], out_min);
motor_out[CH_3] = max(motor_out[CH_3], out_min); motor_out[MOT_3] = max(motor_out[MOT_3], out_min);
motor_out[CH_4] = max(motor_out[CH_4], out_min); motor_out[MOT_4] = max(motor_out[MOT_4], out_min);
motor_out[CH_7] = max(motor_out[CH_7], out_min); motor_out[MOT_5] = max(motor_out[MOT_5], out_min);
motor_out[CH_8] = max(motor_out[CH_8], out_min); motor_out[MOT_6] = max(motor_out[MOT_6], out_min);
motor_out[CH_10] = max(motor_out[CH_10], out_min); motor_out[MOT_7] = max(motor_out[MOT_7], out_min);
motor_out[CH_11] = max(motor_out[CH_11], out_min); motor_out[MOT_8] = max(motor_out[MOT_8], out_min);
#if CUT_MOTORS == ENABLED #if CUT_MOTORS == ENABLED
// if we are not sending a throttle output, we cut the motors // if we are not sending a throttle output, we cut the motors
if(g.rc_3.servo_out == 0){ if(g.rc_3.servo_out == 0){
motor_out[CH_1] = g.rc_3.radio_min; motor_out[MOT_1] = g.rc_3.radio_min;
motor_out[CH_2] = g.rc_3.radio_min; motor_out[MOT_2] = g.rc_3.radio_min;
motor_out[CH_3] = g.rc_3.radio_min; motor_out[MOT_3] = g.rc_3.radio_min;
motor_out[CH_4] = g.rc_3.radio_min; motor_out[MOT_4] = g.rc_3.radio_min;
motor_out[CH_7] = g.rc_3.radio_min; motor_out[MOT_5] = g.rc_3.radio_min;
motor_out[CH_8] = g.rc_3.radio_min; motor_out[MOT_6] = g.rc_3.radio_min;
motor_out[CH_10] = g.rc_3.radio_min; motor_out[MOT_7] = g.rc_3.radio_min;
motor_out[CH_11] = g.rc_3.radio_min; motor_out[MOT_8] = g.rc_3.radio_min;
} }
#endif #endif
// this filter slows the acceleration of motors vs the deceleration // this filter slows the acceleration of motors vs the deceleration
// Idea by Denny Rowland to help with his Yaw issue // Idea by Denny Rowland to help with his Yaw issue
for(int8_t i = CH_1; i <= CH_11; i++ ) { for(int8_t m = 0; m <= 8; m++ ) {
if(i == CH_5 || i == CH_6 || i == CH_9) int i = ch_of_mot(m);
continue;
if(motor_filtered[i] < motor_out[i]){ if(motor_filtered[i] < motor_out[i]){
motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2; motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2;
}else{ }else{
@ -129,14 +128,14 @@ static void output_motors_armed()
} }
} }
APM_RC.OutputCh(CH_1, motor_filtered[CH_1]); APM_RC.OutputCh(MOT_1, motor_filtered[MOT_1]);
APM_RC.OutputCh(CH_2, motor_filtered[CH_2]); APM_RC.OutputCh(MOT_2, motor_filtered[MOT_2]);
APM_RC.OutputCh(CH_3, motor_filtered[CH_3]); APM_RC.OutputCh(MOT_3, motor_filtered[MOT_3]);
APM_RC.OutputCh(CH_4, motor_filtered[CH_4]); APM_RC.OutputCh(MOT_4, motor_filtered[MOT_4]);
APM_RC.OutputCh(CH_7, motor_filtered[CH_7]); APM_RC.OutputCh(MOT_5, motor_filtered[MOT_5]);
APM_RC.OutputCh(CH_8, motor_filtered[CH_8]); APM_RC.OutputCh(MOT_6, motor_filtered[MOT_6]);
APM_RC.OutputCh(CH_10, motor_filtered[CH_10]); APM_RC.OutputCh(MOT_7, motor_filtered[MOT_7]);
APM_RC.OutputCh(CH_11, motor_filtered[CH_11]); APM_RC.OutputCh(MOT_8, motor_filtered[MOT_8]);
#if INSTANT_PWM == 1 #if INSTANT_PWM == 1
// InstantPWM // InstantPWM
@ -160,48 +159,48 @@ static void output_motors_disarmed()
} }
// Send commands to motors // Send commands to motors
APM_RC.OutputCh(CH_1, g.rc_3.radio_min); APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min); APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min); APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min); APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min); APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min); APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
APM_RC.OutputCh(CH_10, g.rc_3.radio_min); APM_RC.OutputCh(MOT_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_11, g.rc_3.radio_min); APM_RC.OutputCh(MOT_8, g.rc_3.radio_min);
} }
static void output_motor_test() static void output_motor_test()
{ {
APM_RC.OutputCh(CH_8, g.rc_3.radio_min); APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
APM_RC.OutputCh(CH_10, g.rc_3.radio_min + 100); APM_RC.OutputCh(MOT_7, g.rc_3.radio_min + 100);
delay(1000); delay(1000);
APM_RC.OutputCh(CH_10, g.rc_3.radio_min); APM_RC.OutputCh(MOT_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_11, g.rc_3.radio_min + 100); APM_RC.OutputCh(MOT_8, g.rc_3.radio_min + 100);
delay(1000); delay(1000);
APM_RC.OutputCh(CH_11, g.rc_3.radio_min); APM_RC.OutputCh(MOT_8, g.rc_3.radio_min);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 100); APM_RC.OutputCh(MOT_1, g.rc_3.radio_min + 100);
delay(1000); delay(1000);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min); APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 100); APM_RC.OutputCh(MOT_2, g.rc_3.radio_min + 100);
delay(1000); delay(1000);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min); APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 100); APM_RC.OutputCh(MOT_3, g.rc_3.radio_min + 100);
delay(1000); delay(1000);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min); APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 100); APM_RC.OutputCh(MOT_4, g.rc_3.radio_min + 100);
delay(1000); delay(1000);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min); APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 100); APM_RC.OutputCh(MOT_5, g.rc_3.radio_min + 100);
delay(1000); delay(1000);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min); APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 100); APM_RC.OutputCh(MOT_6, g.rc_3.radio_min + 100);
delay(1000); delay(1000);
} }

110
ArduCopter/motors_quad.pde
View File

@ -5,7 +5,7 @@
static void init_motors_out() static void init_motors_out()
{ {
#if INSTANT_PWM == 0 #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 #endif
} }
@ -33,12 +33,12 @@ static void output_motors_armed()
pitch_out = g.rc_2.pwm_out * .707; pitch_out = g.rc_2.pwm_out * .707;
// left // left
motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // FRONT motor_out[MOT_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_2] = g.rc_3.radio_out + roll_out - pitch_out; // BACK
// right // right
motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out; // FRONT motor_out[MOT_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_4] = g.rc_3.radio_out - roll_out - pitch_out; // BACK
}else{ }else{
@ -46,20 +46,20 @@ static void output_motors_armed()
pitch_out = g.rc_2.pwm_out; pitch_out = g.rc_2.pwm_out;
// right motor // 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 // 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 // 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 // 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 // Yaw input
motor_out[CH_1] += g.rc_4.pwm_out; // CCW motor_out[MOT_1] += g.rc_4.pwm_out; // CCW
motor_out[CH_2] += g.rc_4.pwm_out; // CCW motor_out[MOT_2] += g.rc_4.pwm_out; // CCW
motor_out[CH_3] -= g.rc_4.pwm_out; // CW motor_out[MOT_3] -= g.rc_4.pwm_out; // CW
motor_out[CH_4] -= 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 /* We need to clip motor output at out_max. When cipping a motors
* output we also need to compensate for the instability by * 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 * ensures that we retain control when one or more of the motors
* is at its maximum output * 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) { if (motor_out[i] > out_max) {
// note that i^1 is the opposite motor // note that i^1 is the opposite motor
motor_out[i^1] -= motor_out[i] - out_max; 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 // limit output so motors don't stop
motor_out[CH_1] = max(motor_out[CH_1], out_min); motor_out[MOT_1] = max(motor_out[MOT_1], out_min);
motor_out[CH_2] = max(motor_out[CH_2], out_min); motor_out[MOT_2] = max(motor_out[MOT_2], out_min);
motor_out[CH_3] = max(motor_out[CH_3], out_min); motor_out[MOT_3] = max(motor_out[MOT_3], out_min);
motor_out[CH_4] = max(motor_out[CH_4], out_min); motor_out[MOT_4] = max(motor_out[MOT_4], out_min);
#if CUT_MOTORS == ENABLED #if CUT_MOTORS == ENABLED
// if we are not sending a throttle output, we cut the motors // if we are not sending a throttle output, we cut the motors
if(g.rc_3.servo_out == 0){ if(g.rc_3.servo_out == 0){
motor_out[CH_1] = g.rc_3.radio_min; motor_out[MOT_1] = g.rc_3.radio_min;
motor_out[CH_2] = g.rc_3.radio_min; motor_out[MOT_2] = g.rc_3.radio_min;
motor_out[CH_3] = g.rc_3.radio_min; motor_out[MOT_3] = g.rc_3.radio_min;
motor_out[CH_4] = g.rc_3.radio_min; motor_out[MOT_4] = g.rc_3.radio_min;
} }
#endif #endif
// this filter slows the acceleration of motors vs the deceleration // this filter slows the acceleration of motors vs the deceleration
// Idea by Denny Rowland to help with his Yaw issue // 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]){ if(motor_filtered[i] < motor_out[i]){
motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2; motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2;
}else{ }else{
@ -102,10 +102,10 @@ static void output_motors_armed()
} }
} }
APM_RC.OutputCh(CH_1, motor_filtered[CH_1]); APM_RC.OutputCh(MOT_1, motor_filtered[MOT_1]);
APM_RC.OutputCh(CH_2, motor_filtered[CH_2]); APM_RC.OutputCh(MOT_2, motor_filtered[MOT_2]);
APM_RC.OutputCh(CH_3, motor_filtered[CH_3]); APM_RC.OutputCh(MOT_3, motor_filtered[MOT_3]);
APM_RC.OutputCh(CH_4, motor_filtered[CH_4]); APM_RC.OutputCh(MOT_4, motor_filtered[MOT_4]);
#if INSTANT_PWM == 1 #if INSTANT_PWM == 1
// InstantPWM // InstantPWM
@ -130,52 +130,52 @@ static void output_motors_disarmed()
} }
// Send commands to motors // Send commands to motors
APM_RC.OutputCh(CH_1, g.rc_3.radio_min); APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min); APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min); APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min); APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
} }
/* /*
//static void debug_motors() //static void debug_motors()
{ {
Serial.printf("1:%d\t2:%d\t3:%d\t4:%d\n", Serial.printf("1:%d\t2:%d\t3:%d\t4:%d\n",
motor_out[CH_1], motor_out[MOT_1],
motor_out[CH_2], motor_out[MOT_2],
motor_out[CH_3], motor_out[MOT_3],
motor_out[CH_4]); motor_out[MOT_4]);
} }
//*/ //*/
static void output_motor_test() static void output_motor_test()
{ {
motor_out[CH_1] = g.rc_3.radio_min; motor_out[MOT_1] = g.rc_3.radio_min;
motor_out[CH_2] = g.rc_3.radio_min; motor_out[MOT_2] = g.rc_3.radio_min;
motor_out[CH_3] = g.rc_3.radio_min; motor_out[MOT_3] = g.rc_3.radio_min;
motor_out[CH_4] = g.rc_3.radio_min; motor_out[MOT_4] = g.rc_3.radio_min;
if(g.frame_orientation == X_FRAME){ if(g.frame_orientation == X_FRAME){
// 31 // 31
// 24 // 24
if(g.rc_1.control_in > 3000){ if(g.rc_1.control_in > 3000){
motor_out[CH_1] += 100; motor_out[MOT_1] += 100;
motor_out[CH_4] += 100; motor_out[MOT_4] += 100;
} }
if(g.rc_1.control_in < -3000){ if(g.rc_1.control_in < -3000){
motor_out[CH_2] += 100; motor_out[MOT_2] += 100;
motor_out[CH_3] += 100; motor_out[MOT_3] += 100;
} }
if(g.rc_2.control_in > 3000){ if(g.rc_2.control_in > 3000){
motor_out[CH_2] += 100; motor_out[MOT_2] += 100;
motor_out[CH_4] += 100; motor_out[MOT_4] += 100;
} }
if(g.rc_2.control_in < -3000){ if(g.rc_2.control_in < -3000){
motor_out[CH_1] += 100; motor_out[MOT_1] += 100;
motor_out[CH_3] += 100; motor_out[MOT_3] += 100;
} }
}else{ }else{
@ -183,22 +183,22 @@ static void output_motor_test()
// 2 1 // 2 1
// 4 // 4
if(g.rc_1.control_in > 3000) if(g.rc_1.control_in > 3000)
motor_out[CH_1] += 100; motor_out[MOT_1] += 100;
if(g.rc_1.control_in < -3000) if(g.rc_1.control_in < -3000)
motor_out[CH_2] += 100; motor_out[MOT_2] += 100;
if(g.rc_2.control_in > 3000) if(g.rc_2.control_in > 3000)
motor_out[CH_4] += 100; motor_out[MOT_4] += 100;
if(g.rc_2.control_in < -3000) 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(MOT_1, motor_out[MOT_1]);
APM_RC.OutputCh(CH_2, motor_out[CH_2]); APM_RC.OutputCh(MOT_2, motor_out[MOT_2]);
APM_RC.OutputCh(CH_3, motor_out[CH_3]); APM_RC.OutputCh(MOT_3, motor_out[MOT_3]);
APM_RC.OutputCh(CH_4, motor_out[CH_4]); APM_RC.OutputCh(MOT_4, motor_out[MOT_4]);
} }
#endif #endif

76
ArduCopter/motors_tri.pde
View File

@ -4,7 +4,7 @@
static void init_motors_out() static void init_motors_out()
{ {
#if INSTANT_PWM == 0 #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 #endif
} }
@ -28,50 +28,50 @@ static void output_motors_armed()
int pitch_out = g.rc_2.pwm_out / 2; int pitch_out = g.rc_2.pwm_out / 2;
//left front //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 //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 // 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 // Tridge's stability patch
if (motor_out[CH_1] > out_max) { if (motor_out[MOT_1] > out_max) {
motor_out[CH_2] -= (motor_out[CH_1] - out_max) >> 1; motor_out[MOT_2] -= (motor_out[MOT_1] - out_max) >> 1;
motor_out[CH_4] -= (motor_out[CH_1] - out_max) >> 1; motor_out[MOT_4] -= (motor_out[MOT_1] - out_max) >> 1;
motor_out[CH_1] = out_max; motor_out[MOT_1] = out_max;
} }
if (motor_out[CH_2] > out_max) { if (motor_out[MOT_2] > out_max) {
motor_out[CH_1] -= (motor_out[CH_2] - out_max) >> 1; motor_out[MOT_1] -= (motor_out[MOT_2] - out_max) >> 1;
motor_out[CH_4] -= (motor_out[CH_2] - out_max) >> 1; motor_out[MOT_4] -= (motor_out[MOT_2] - out_max) >> 1;
motor_out[CH_2] = out_max; motor_out[MOT_2] = out_max;
} }
if (motor_out[CH_4] > out_max) { if (motor_out[MOT_4] > out_max) {
motor_out[CH_1] -= (motor_out[CH_4] - out_max) >> 1; motor_out[MOT_1] -= (motor_out[MOT_4] - out_max) >> 1;
motor_out[CH_2] -= (motor_out[CH_4] - out_max) >> 1; motor_out[MOT_2] -= (motor_out[MOT_4] - out_max) >> 1;
motor_out[CH_4] = out_max; motor_out[MOT_4] = out_max;
} }
// limit output so motors don't stop // limit output so motors don't stop
motor_out[CH_1] = max(motor_out[CH_1], out_min); motor_out[MOT_1] = max(motor_out[MOT_1], out_min);
motor_out[CH_2] = max(motor_out[CH_2], out_min); motor_out[MOT_2] = max(motor_out[MOT_2], out_min);
motor_out[CH_4] = max(motor_out[CH_4], out_min); motor_out[MOT_4] = max(motor_out[MOT_4], out_min);
#if CUT_MOTORS == ENABLED #if CUT_MOTORS == ENABLED
// if we are not sending a throttle output, we cut the motors // if we are not sending a throttle output, we cut the motors
if(g.rc_3.servo_out == 0){ if(g.rc_3.servo_out == 0){
motor_out[CH_1] = g.rc_3.radio_min; motor_out[MOT_1] = g.rc_3.radio_min;
motor_out[CH_2] = g.rc_3.radio_min; motor_out[MOT_2] = g.rc_3.radio_min;
motor_out[CH_4] = g.rc_3.radio_min; motor_out[MOT_4] = g.rc_3.radio_min;
} }
#endif #endif
APM_RC.OutputCh(CH_1, motor_out[CH_1]); APM_RC.OutputCh(MOT_1, motor_out[MOT_1]);
APM_RC.OutputCh(CH_2, motor_out[CH_2]); APM_RC.OutputCh(MOT_2, motor_out[MOT_2]);
APM_RC.OutputCh(CH_4, motor_out[CH_4]); APM_RC.OutputCh(MOT_4, motor_out[MOT_4]);
#if INSTANT_PWM == 1 #if INSTANT_PWM == 1
// InstantPWM // InstantPWM
@ -94,33 +94,33 @@ static void output_motors_disarmed()
} }
// Send commands to motors // Send commands to motors
APM_RC.OutputCh(CH_1, g.rc_3.radio_min); APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min); APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min); APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
} }
static void output_motor_test() static void output_motor_test()
{ {
motor_out[CH_1] = g.rc_3.radio_min; motor_out[MOT_1] = g.rc_3.radio_min;
motor_out[CH_2] = g.rc_3.radio_min; motor_out[MOT_2] = g.rc_3.radio_min;
motor_out[CH_4] = g.rc_3.radio_min; motor_out[MOT_4] = g.rc_3.radio_min;
if(g.rc_1.control_in > 3000){ // right 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 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 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(MOT_1, motor_out[MOT_1]);
APM_RC.OutputCh(CH_2, motor_out[CH_2]); APM_RC.OutputCh(MOT_2, motor_out[MOT_2]);
APM_RC.OutputCh(CH_4, motor_out[CH_4]); APM_RC.OutputCh(MOT_4, motor_out[MOT_4]);
} }
#endif #endif

153
ArduCopter/motors_y6.pde
View File

@ -7,8 +7,8 @@
static void init_motors_out() static void init_motors_out()
{ {
#if INSTANT_PWM == 0 #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)
| MSK_CH_7 | MSK_CH_8 ); | _BV(MOT_5) | _BV(MOT_6) );
#endif #endif
} }
@ -30,24 +30,24 @@ static void output_motors_armed()
// Multi-Wii Mix // Multi-Wii Mix
//left //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[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[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_3] = g.rc_3.radio_out + g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // BOTTOM_LEFT - CCW
//right //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[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[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_1] = g.rc_3.radio_out - g.rc_1.pwm_out + (g.rc_2.pwm_out * 2 / 3); // BOTTOM_RIGHT - CCW
//back //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[MOT_6] = (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_4] = g.rc_3.radio_out - (g.rc_2.pwm_out * 4 / 3); // BOTTOM_REAR - CW
//left //left
motor_out[CH_2] -= YAW_DIRECTION * g.rc_4.pwm_out; // LEFT TOP - CW motor_out[MOT_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_3] += YAW_DIRECTION * g.rc_4.pwm_out; // LEFT BOTTOM - CCW
//right //right
motor_out[CH_7] -= YAW_DIRECTION * g.rc_4.pwm_out; // RIGHT TOP - CW motor_out[MOT_5] -= 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_1] += YAW_DIRECTION * g.rc_4.pwm_out; // RIGHT BOTTOM - CCW
//back //back
motor_out[CH_8] += YAW_DIRECTION * g.rc_4.pwm_out; // REAR TOP - CCW motor_out[MOT_6] += 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_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; int pitch_out = g.rc_2.pwm_out / 2;
//left //left
motor_out[CH_2] = ((g.rc_3.radio_out * g.top_bottom_ratio) + roll_out + pitch_out); // CCW TOP motor_out[MOT_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_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW
//right //right
motor_out[CH_7] = ((g.rc_3.radio_out * g.top_bottom_ratio) - roll_out + pitch_out); // CCW TOP motor_out[MOT_5] = ((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_1] = g.rc_3.radio_out - roll_out + pitch_out; // CW
//back //back
motor_out[CH_8] = ((g.rc_3.radio_out * g.top_bottom_ratio) - g.rc_2.pwm_out); // CCW TOP motor_out[MOT_6] = ((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_4] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW
// Yaw // Yaw
//top //top
motor_out[CH_2] += g.rc_4.pwm_out; // CCW motor_out[MOT_2] += g.rc_4.pwm_out; // CCW
motor_out[CH_7] += g.rc_4.pwm_out; // CCW motor_out[MOT_5] += g.rc_4.pwm_out; // CCW
motor_out[CH_8] += g.rc_4.pwm_out; // CCW motor_out[MOT_6] += g.rc_4.pwm_out; // CCW
//bottom //bottom
motor_out[CH_3] -= g.rc_4.pwm_out; // CW motor_out[MOT_3] -= g.rc_4.pwm_out; // CW
motor_out[CH_1] -= g.rc_4.pwm_out; // CW motor_out[MOT_1] -= g.rc_4.pwm_out; // CW
motor_out[CH_4] -= g.rc_4.pwm_out; // CW motor_out[MOT_4] -= g.rc_4.pwm_out; // CW
*/ */
// TODO: add stability patch // TODO: add stability patch
motor_out[CH_1] = min(motor_out[CH_1], out_max); motor_out[MOT_1] = min(motor_out[MOT_1], out_max);
motor_out[CH_2] = min(motor_out[CH_2], out_max); motor_out[MOT_2] = min(motor_out[MOT_2], out_max);
motor_out[CH_3] = min(motor_out[CH_3], out_max); motor_out[MOT_3] = min(motor_out[MOT_3], out_max);
motor_out[CH_4] = min(motor_out[CH_4], out_max); motor_out[MOT_4] = min(motor_out[MOT_4], out_max);
motor_out[CH_7] = min(motor_out[CH_7], out_max); motor_out[MOT_5] = min(motor_out[MOT_5], out_max);
motor_out[CH_8] = min(motor_out[CH_8], out_max); motor_out[MOT_6] = min(motor_out[MOT_6], out_max);
// limit output so motors don't stop // limit output so motors don't stop
motor_out[CH_1] = max(motor_out[CH_1], out_min); motor_out[MOT_1] = max(motor_out[MOT_1], out_min);
motor_out[CH_2] = max(motor_out[CH_2], out_min); motor_out[MOT_2] = max(motor_out[MOT_2], out_min);
motor_out[CH_3] = max(motor_out[CH_3], out_min); motor_out[MOT_3] = max(motor_out[MOT_3], out_min);
motor_out[CH_4] = max(motor_out[CH_4], out_min); motor_out[MOT_4] = max(motor_out[MOT_4], out_min);
motor_out[CH_7] = max(motor_out[CH_7], out_min); motor_out[MOT_5] = max(motor_out[MOT_5], out_min);
motor_out[CH_8] = max(motor_out[CH_8], out_min); motor_out[MOT_6] = max(motor_out[MOT_6], out_min);
#if CUT_MOTORS == ENABLED #if CUT_MOTORS == ENABLED
// if we are not sending a throttle output, we cut the motors // if we are not sending a throttle output, we cut the motors
if(g.rc_3.servo_out == 0){ if(g.rc_3.servo_out == 0){
motor_out[CH_1] = g.rc_3.radio_min; motor_out[MOT_1] = g.rc_3.radio_min;
motor_out[CH_2] = g.rc_3.radio_min; motor_out[MOT_2] = g.rc_3.radio_min;
motor_out[CH_3] = g.rc_3.radio_min; motor_out[MOT_3] = g.rc_3.radio_min;
motor_out[CH_4] = g.rc_3.radio_min; motor_out[MOT_4] = g.rc_3.radio_min;
motor_out[CH_7] = g.rc_3.radio_min; motor_out[MOT_5] = g.rc_3.radio_min;
motor_out[CH_8] = g.rc_3.radio_min; motor_out[MOT_6] = g.rc_3.radio_min;
} }
#endif #endif
// this filter slows the acceleration of motors vs the deceleration // this filter slows the acceleration of motors vs the deceleration
// Idea by Denny Rowland to help with his Yaw issue // Idea by Denny Rowland to help with his Yaw issue
for(int8_t i = CH_1; i <= CH_8; i++ ) { for(int8_t m = 0; m <= 6; m++ ) {
if(i == CH_5 || i == CH_6) int i = ch_of_mot(m);
continue;
if(motor_filtered[i] < motor_out[i]){ if(motor_filtered[i] < motor_out[i]){
motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2; motor_filtered[i] = (motor_out[i] + motor_filtered[i]) / 2;
}else{ }else{
@ -119,12 +118,12 @@ static void output_motors_armed()
} }
} }
APM_RC.OutputCh(CH_1, motor_filtered[CH_1]); APM_RC.OutputCh(MOT_1, motor_filtered[MOT_1]);
APM_RC.OutputCh(CH_2, motor_filtered[CH_2]); APM_RC.OutputCh(MOT_2, motor_filtered[MOT_2]);
APM_RC.OutputCh(CH_3, motor_filtered[CH_3]); APM_RC.OutputCh(MOT_3, motor_filtered[MOT_3]);
APM_RC.OutputCh(CH_4, motor_filtered[CH_4]); APM_RC.OutputCh(MOT_4, motor_filtered[MOT_4]);
APM_RC.OutputCh(CH_7, motor_filtered[CH_7]); APM_RC.OutputCh(MOT_5, motor_filtered[MOT_5]);
APM_RC.OutputCh(CH_8, motor_filtered[CH_8]); APM_RC.OutputCh(MOT_6, motor_filtered[MOT_6]);
#if INSTANT_PWM == 1 #if INSTANT_PWM == 1
// InstantPWM // InstantPWM
@ -148,45 +147,45 @@ static void output_motors_disarmed()
} }
// Send commands to motors // Send commands to motors
APM_RC.OutputCh(CH_1, g.rc_3.radio_min); APM_RC.OutputCh(MOT_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min); APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min); APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min); APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min); APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min); APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
} }
static void output_motor_test() static void output_motor_test()
{ {
motor_out[CH_1] = g.rc_3.radio_min; motor_out[MOT_1] = g.rc_3.radio_min;
motor_out[CH_2] = g.rc_3.radio_min; motor_out[MOT_2] = g.rc_3.radio_min;
motor_out[CH_3] = g.rc_3.radio_min; motor_out[MOT_3] = g.rc_3.radio_min;
motor_out[CH_4] = g.rc_3.radio_min; motor_out[MOT_4] = g.rc_3.radio_min;
motor_out[CH_7] = g.rc_3.radio_min; motor_out[MOT_5] = g.rc_3.radio_min;
motor_out[CH_8] = g.rc_3.radio_min; motor_out[MOT_6] = g.rc_3.radio_min;
if(g.rc_1.control_in > 3000){ // right if(g.rc_1.control_in > 3000){ // right
motor_out[CH_1] += 100; motor_out[MOT_1] += 100;
motor_out[CH_7] += 100; motor_out[MOT_5] += 100;
} }
if(g.rc_1.control_in < -3000){ // left if(g.rc_1.control_in < -3000){ // left
motor_out[CH_2] += 100; motor_out[MOT_2] += 100;
motor_out[CH_3] += 100; motor_out[MOT_3] += 100;
} }
if(g.rc_2.control_in > 3000){ // back if(g.rc_2.control_in > 3000){ // back
motor_out[CH_8] += 100; motor_out[MOT_6] += 100;
motor_out[CH_4] += 100; motor_out[MOT_4] += 100;
} }
APM_RC.OutputCh(CH_1, motor_out[CH_1]); APM_RC.OutputCh(MOT_1, motor_out[MOT_1]);
APM_RC.OutputCh(CH_2, motor_out[CH_2]); APM_RC.OutputCh(MOT_2, motor_out[MOT_2]);
APM_RC.OutputCh(CH_3, motor_out[CH_4]); APM_RC.OutputCh(MOT_3, motor_out[MOT_4]);
APM_RC.OutputCh(CH_4, motor_out[CH_4]); APM_RC.OutputCh(MOT_4, motor_out[MOT_4]);
APM_RC.OutputCh(CH_7, motor_out[CH_7]); APM_RC.OutputCh(MOT_5, motor_out[MOT_5]);
APM_RC.OutputCh(CH_8, motor_out[CH_8]); APM_RC.OutputCh(MOT_6, motor_out[MOT_6]);
} }
#endif #endif

115
ArduCopter/navigation.pde
View File

@ -367,12 +367,125 @@ static void update_crosstrack(void)
} }
} }
static int32_t get_altitude_error() 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; 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) static int32_t wrap_360(int32_t error)
{ {
if (error > 36000) error -= 36000; if (error > 36000) error -= 36000;

32
ArduCopter/radio.pde
View File

@ -2,7 +2,7 @@
//Function that will read the radio data, limit servos and trigger a failsafe //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() static void default_dead_zones()
{ {
@ -55,8 +55,8 @@ static void init_rc_out()
init_motors_out(); init_motors_out();
// this is the camera pitch5 and roll6 // this is the camera pitch5 and roll6
APM_RC.OutputCh(CH_5, 1500); APM_RC.OutputCh(CH_CAM_PITCH, 1500);
APM_RC.OutputCh(CH_6, 1500); APM_RC.OutputCh(CH_CAM_ROLL, 1500);
for(byte i = 0; i < 5; i++){ for(byte i = 0; i < 5; i++){
delay(20); delay(20);
@ -103,18 +103,18 @@ void output_min()
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
heli_move_servos_to_mid(); heli_move_servos_to_mid();
#else #else
APM_RC.OutputCh(CH_1, g.rc_3.radio_min); // Initialization of servo outputs APM_RC.OutputCh(MOT_1, g.rc_3.radio_min); // Initialization of servo outputs
APM_RC.OutputCh(CH_2, g.rc_3.radio_min); APM_RC.OutputCh(MOT_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min); APM_RC.OutputCh(MOT_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min); APM_RC.OutputCh(MOT_4, g.rc_3.radio_min);
#endif #endif
APM_RC.OutputCh(CH_7, g.rc_3.radio_min); APM_RC.OutputCh(MOT_5, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min); APM_RC.OutputCh(MOT_6, g.rc_3.radio_min);
#if FRAME_CONFIG == OCTA_FRAME #if FRAME_CONFIG == OCTA_FRAME
APM_RC.OutputCh(CH_10, g.rc_3.radio_min); APM_RC.OutputCh(MOT_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_11, g.rc_3.radio_min); APM_RC.OutputCh(MOT_8, g.rc_3.radio_min);
#endif #endif
} }
@ -153,12 +153,16 @@ static void throttle_failsafe(uint16_t pwm)
// throttle has dropped below the mark // throttle has dropped below the mark
failsafeCounter++; failsafeCounter++;
if (failsafeCounter == 9){ if (failsafeCounter == 9){
SendDebug("MSG FS ON "); //
SendDebugln(pwm, DEC);
}else if(failsafeCounter == 10) { }else if(failsafeCounter == 10) {
// Don't enter Failsafe if we are not armed // 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); set_failsafe(true);
}
}else if (failsafeCounter > 10){ }else if (failsafeCounter > 10){
failsafeCounter = 11; failsafeCounter = 11;
} }

18
ArduCopter/setup.pde
View File

@ -271,7 +271,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
{ {
byte _switchPosition = 0; byte _switchPosition = 0;
byte _oldSwitchPosition = 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")); Serial.printf_P(PSTR("\nMode switch to edit, aileron: select modes, rudder: Simple on/off\n"));
print_hit_enter(); print_hit_enter();
@ -1102,16 +1102,16 @@ init_esc()
read_radio(); read_radio();
delay(100); delay(100);
dancing_light(); dancing_light();
APM_RC.OutputCh(CH_1, g.rc_3.radio_in); APM_RC.OutputCh(MOT_1, g.rc_3.radio_in);
APM_RC.OutputCh(CH_2, g.rc_3.radio_in); APM_RC.OutputCh(MOT_2, g.rc_3.radio_in);
APM_RC.OutputCh(CH_3, g.rc_3.radio_in); APM_RC.OutputCh(MOT_3, g.rc_3.radio_in);
APM_RC.OutputCh(CH_4, g.rc_3.radio_in); APM_RC.OutputCh(MOT_4, g.rc_3.radio_in);
APM_RC.OutputCh(CH_7, g.rc_3.radio_in); APM_RC.OutputCh(MOT_5, g.rc_3.radio_in);
APM_RC.OutputCh(CH_8, g.rc_3.radio_in); APM_RC.OutputCh(MOT_6, g.rc_3.radio_in);
#if FRAME_CONFIG == OCTA_FRAME #if FRAME_CONFIG == OCTA_FRAME
APM_RC.OutputCh(CH_10, g.rc_3.radio_in); APM_RC.OutputCh(MOT_7, g.rc_3.radio_in);
APM_RC.OutputCh(CH_11, g.rc_3.radio_in); APM_RC.OutputCh(MOT_8, g.rc_3.radio_in);
#endif #endif
} }

21
ArduCopter/system.pde
View File

@ -321,7 +321,7 @@ static void init_ardupilot()
#endif #endif
// initialise sonar // initialise sonar
#if HIL_MODE != HIL_MODE_ATTITUDE && CONFIG_SONAR == ENABLED #if CONFIG_SONAR == ENABLED
init_sonar(); init_sonar();
#endif #endif
@ -490,9 +490,7 @@ static void set_mode(byte mode)
roll_pitch_mode = ALT_HOLD_RP; roll_pitch_mode = ALT_HOLD_RP;
throttle_mode = ALT_HOLD_THR; throttle_mode = ALT_HOLD_THR;
next_WP = current_loc; set_next_WP(&current_loc);
// 1m is the alt hold limit
next_WP.alt = max(next_WP.alt, 100);
break; break;
case AUTO: case AUTO:
@ -508,9 +506,7 @@ static void set_mode(byte mode)
yaw_mode = CIRCLE_YAW; yaw_mode = CIRCLE_YAW;
roll_pitch_mode = CIRCLE_RP; roll_pitch_mode = CIRCLE_RP;
throttle_mode = CIRCLE_THR; throttle_mode = CIRCLE_THR;
next_WP = current_loc; set_next_WP(&current_loc);
// reset the desired circle angle
circle_angle = 0; circle_angle = 0;
break; break;
@ -518,14 +514,14 @@ static void set_mode(byte mode)
yaw_mode = LOITER_YAW; yaw_mode = LOITER_YAW;
roll_pitch_mode = LOITER_RP; roll_pitch_mode = LOITER_RP;
throttle_mode = LOITER_THR; throttle_mode = LOITER_THR;
next_WP = current_loc; set_next_WP(&current_loc);
break; break;
case POSITION: case POSITION:
yaw_mode = YAW_HOLD; yaw_mode = YAW_HOLD;
roll_pitch_mode = ROLL_PITCH_AUTO; roll_pitch_mode = ROLL_PITCH_AUTO;
throttle_mode = THROTTLE_MANUAL; throttle_mode = THROTTLE_MANUAL;
next_WP = current_loc; set_next_WP(&current_loc);
break; break;
case GUIDED: 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 // removes the navigation from roll and pitch commands, but leaves the wind compensation
reset_nav(); reset_nav();
// removes the navigation from roll and pitch commands, but leaves the wind compensation #if WIND_COMP_STAB == 1
if(GPS_enabled) if(GPS_enabled){
wp_control = NO_NAV_MODE; wp_control = NO_NAV_MODE;
update_nav_wp(); update_nav_wp();
}
#endif
} }
Log_Write_Mode(control_mode); Log_Write_Mode(control_mode);

10
ArduCopter/test.pde
View File

@ -167,7 +167,7 @@ test_eedump(uint8_t argc, const Menu::arg *argv)
g.rc_4.control_in, g.rc_4.control_in,
g.rc_4.radio_out); 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){ if(Serial.available() > 0){
return (0); return (0);
@ -765,10 +765,10 @@ test_current(uint8_t argc, const Menu::arg *argv)
current_amps, current_amps,
current_total); current_total);
APM_RC.OutputCh(CH_1, g.rc_3.radio_in); APM_RC.OutputCh(MOT_1, g.rc_3.radio_in);
APM_RC.OutputCh(CH_2, g.rc_3.radio_in); APM_RC.OutputCh(MOT_2, g.rc_3.radio_in);
APM_RC.OutputCh(CH_3, g.rc_3.radio_in); APM_RC.OutputCh(MOT_3, g.rc_3.radio_in);
APM_RC.OutputCh(CH_4, g.rc_3.radio_in); APM_RC.OutputCh(MOT_4, g.rc_3.radio_in);
if(Serial.available() > 0){ if(Serial.available() > 0){
return (0); return (0);

10
Tools/autotest/arducopter.py
View File

@ -168,7 +168,7 @@ def fly_RTL(mavproxy, mav, side=60):
print("# Enter RTL") print("# Enter RTL")
mavproxy.send('switch 3\n') mavproxy.send('switch 3\n')
tstart = time.time() tstart = time.time()
while time.time() < tstart + 120: while time.time() < tstart + 200:
m = mav.recv_match(type='VFR_HUD', blocking=True) m = mav.recv_match(type='VFR_HUD', blocking=True)
pos = current_location(mav) pos = current_location(mav)
#delta = get_distance(start, pos) #delta = get_distance(start, pos)
@ -208,7 +208,7 @@ def fly_simple(mavproxy, mav, side=60, timeout=120):
'''fly Simple, flying N then E''' '''fly Simple, flying N then E'''
mavproxy.send('switch 6\n') mavproxy.send('switch 6\n')
wait_mode(mav, 'STABILIZE') wait_mode(mav, 'STABILIZE')
mavproxy.send('rc 3 1450\n') mavproxy.send('rc 3 1440\n')
tstart = time.time() tstart = time.time()
failed = False failed = False
@ -246,7 +246,7 @@ def land(mavproxy, mav, timeout=60):
mavproxy.send('switch 2\n') mavproxy.send('switch 2\n')
wait_mode(mav, 'LAND') wait_mode(mav, 'LAND')
print("Entered Landing Mode") print("Entered Landing Mode")
ret = wait_altitude(mav, -5, 5) ret = wait_altitude(mav, -5, 1)
print("LANDING: ok= %s" % ret) print("LANDING: ok= %s" % ret)
return ret return ret
@ -437,8 +437,8 @@ def fly_ArduCopter(viewerip=None):
failed = True failed = True
# Loiter for 30 seconds # Loiter for 30 seconds
print("# Loiter for 30 seconds") print("# Loiter for 45 seconds")
if not loiter(mavproxy, mav, 30): if not loiter(mavproxy, mav, 45):
print("loiter failed") print("loiter failed")
failed = True failed = True

4
Tools/autotest/common.py
View File

@ -195,7 +195,9 @@ def wait_waypoint(mav, wpnum_start, wpnum_end, allow_skip=True, max_dist=2, time
seq = m.seq seq = m.seq
m = mav.recv_match(type='NAV_CONTROLLER_OUTPUT', blocking=True) m = mav.recv_match(type='NAV_CONTROLLER_OUTPUT', blocking=True)
wp_dist = m.wp_dist 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): if seq == current_wp+1 or (seq > current_wp+1 and allow_skip):
print("test: Starting new waypoint %u" % seq) print("test: Starting new waypoint %u" % seq)
tstart = time.time() tstart = time.time()

8
Tools/autotest/pysim/sim_multicopter.py
View File

@ -168,10 +168,10 @@ while True:
frame_count += 1 frame_count += 1
t = time.time() t = time.time()
if t - lastt > 1.0: if t - lastt > 1.0:
print("%.2f fps zspeed=%.2f zaccel=%.2f h=%.1f a=%.1f yaw=%.1f yawrate=%.1f" % ( #print("%.2f fps zspeed=%.2f zaccel=%.2f h=%.1f a=%.1f yaw=%.1f yawrate=%.1f" % (
frame_count/(t-lastt), # frame_count/(t-lastt),
a.velocity.z, a.accel.z, a.position.z, a.altitude, # a.velocity.z, a.accel.z, a.position.z, a.altitude,
a.yaw, a.yaw_rate)) # a.yaw, a.yaw_rate))
lastt = t lastt = t
frame_count = 0 frame_count = 0
frame_end = time.time() frame_end = time.time()

14
libraries/APM_RC/APM_RC.h
View File

@ -21,20 +21,6 @@
#define CH_10 9 #define CH_10 9
#define CH_11 10 #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 #define NUM_CHANNELS 8

8
libraries/APM_RC/APM_RC_APM1.cpp
View File

@ -216,16 +216,16 @@ void APM_RC_APM1::Force_Out6_Out7(void)
void APM_RC_APM1::SetFastOutputChannels(uint32_t chmask) 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); _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); _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); _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); _set_speed_ch7_ch8_ch11(OUTPUT_SPEED_400HZ);
} }

6
libraries/APM_RC/APM_RC_APM2.cpp
View File

@ -197,13 +197,13 @@ void APM_RC_APM2::Force_Out6_Out7(void) { }
void APM_RC_APM2::SetFastOutputChannels(uint32_t chmask) 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); _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); _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); _set_speed_ch6_ch7_ch8(OUTPUT_SPEED_400HZ);
} }

2
libraries/AP_ADC/AP_ADC_HIL.h
View File

@ -79,7 +79,7 @@ class AP_ADC_HIL : public AP_ADC
// @param index the axis for the accelerometer(0-x,1-y,2-z) // @param index the axis for the accelerometer(0-x,1-y,2-z)
inline void setAccel(uint8_t index, int16_t val) { inline void setAccel(uint8_t index, int16_t val) {
int16_t temp = val * accelScale[index] / 1000 + accelBias[index]; 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;
} }
/// ///

7
libraries/AP_Common/menu.cpp
View File

@ -30,7 +30,8 @@ Menu::Menu(const prog_char *prompt, const Menu::command *commands, uint8_t entri
void void
Menu::run(void) Menu::run(void)
{ {
uint8_t len, i, ret; int8_t ret;
uint8_t len, i;
uint8_t argc; uint8_t argc;
int c; int c;
char *s; char *s;
@ -123,12 +124,12 @@ Menu::run(void)
return; return;
} }
} }
if (cmd_found==false) if (cmd_found==false)
{ {
Serial.println("Invalid command, type 'help'"); Serial.println("Invalid command, type 'help'");
} }
} }
} }

128
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp
View File

@ -1,65 +1,65 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*-
/* /*
AP_RangeFinder_MaxsonarXL.cpp - Arduino Library for Sharpe GP2Y0A02YK0F AP_RangeFinder_MaxsonarXL.cpp - Arduino Library for Sharpe GP2Y0A02YK0F
infrared proximity sensor infrared proximity sensor
Code by Jose Julio and Randy Mackay. DIYDrones.com Code by Jose Julio and Randy Mackay. DIYDrones.com
This library is free software; you can redistribute it and/or This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version. version 2.1 of the License, or (at your option) any later version.
Sparkfun URL: http://www.sparkfun.com/products/9491 Sparkfun URL: http://www.sparkfun.com/products/9491
datasheet: http://www.sparkfun.com/datasheets/Sensors/Proximity/XL-EZ0-Datasheet.pdf datasheet: http://www.sparkfun.com/datasheets/Sensors/Proximity/XL-EZ0-Datasheet.pdf
Sensor should be connected to one of the analog ports Sensor should be connected to one of the analog ports
Variables: Variables:
int raw_value : raw value from the sensor int raw_value : raw value from the sensor
int distance : distance in cm int distance : distance in cm
int max_distance : maximum measurable distance (in cm) int max_distance : maximum measurable distance (in cm)
int min_distance : minimum measurable distance (in cm) int min_distance : minimum measurable distance (in cm)
Methods: Methods:
read() : read value from analog port and returns the distance (in cm) read() : read value from analog port and returns the distance (in cm)
*/ */
// AVR LibC Includes // AVR LibC Includes
#include "WConstants.h" #include "WConstants.h"
#include "AP_RangeFinder_MaxsonarXL.h" #include "AP_RangeFinder_MaxsonarXL.h"
// Constructor ////////////////////////////////////////////////////////////// // Constructor //////////////////////////////////////////////////////////////
AP_RangeFinder_MaxsonarXL::AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source, AP_RangeFinder_MaxsonarXL::AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source, ModeFilter *filter):
ModeFilter *filter) : RangeFinder(source, filter),
RangeFinder(source, filter), _scaler(AP_RANGEFINDER_MAXSONARXL_SCALER) _scaler(AP_RANGEFINDER_MAXSONARXL_SCALER)
{ {
max_distance = AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE; max_distance = AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE;
min_distance = AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE; min_distance = AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE;
} }
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
float AP_RangeFinder_MaxsonarXL::calculate_scaler(int sonar_type, float adc_refence_voltage) float AP_RangeFinder_MaxsonarXL::calculate_scaler(int sonar_type, float adc_refence_voltage)
{ {
float type_scaler = 1.0; float type_scaler = 1.0;
switch(sonar_type) { switch(sonar_type) {
case AP_RANGEFINDER_MAXSONARXL: case AP_RANGEFINDER_MAXSONARXL:
type_scaler = AP_RANGEFINDER_MAXSONARXL_SCALER; type_scaler = AP_RANGEFINDER_MAXSONARXL_SCALER;
min_distance = AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE; min_distance = AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE;
max_distance = AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE; max_distance = AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE;
break; break;
case AP_RANGEFINDER_MAXSONARLV: case AP_RANGEFINDER_MAXSONARLV:
type_scaler = AP_RANGEFINDER_MAXSONARLV_SCALER; type_scaler = AP_RANGEFINDER_MAXSONARLV_SCALER;
min_distance = AP_RANGEFINDER_MAXSONARLV_MIN_DISTANCE; min_distance = AP_RANGEFINDER_MAXSONARLV_MIN_DISTANCE;
max_distance = AP_RANGEFINDER_MAXSONARLV_MAX_DISTANCE; max_distance = AP_RANGEFINDER_MAXSONARLV_MAX_DISTANCE;
break; break;
case AP_RANGEFINDER_MAXSONARXLL: case AP_RANGEFINDER_MAXSONARXLL:
type_scaler = AP_RANGEFINDER_MAXSONARXLL_SCALER; type_scaler = AP_RANGEFINDER_MAXSONARXLL_SCALER;
min_distance = AP_RANGEFINDER_MAXSONARXLL_MIN_DISTANCE; min_distance = AP_RANGEFINDER_MAXSONARXLL_MIN_DISTANCE;
max_distance = AP_RANGEFINDER_MAXSONARXLL_MAX_DISTANCE; max_distance = AP_RANGEFINDER_MAXSONARXLL_MAX_DISTANCE;
break; break;
} }
_scaler = type_scaler * adc_refence_voltage / 5.0; _scaler = type_scaler * adc_refence_voltage / 5.0;
return _scaler; return _scaler;
} }

67
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h
View File

@ -1,33 +1,34 @@
#ifndef AP_RangeFinder_MaxsonarXL_H #ifndef AP_RangeFinder_MaxsonarXL_H
#define AP_RangeFinder_MaxsonarXL_H #define AP_RangeFinder_MaxsonarXL_H
#include "RangeFinder.h" #include "RangeFinder.h"
// XL-EZ0 (aka XL) // XL-EZ0 (aka XL)
#define AP_RANGEFINDER_MAXSONARXL 0 #define AP_RANGEFINDER_MAXSONARXL 0
#define AP_RANGEFINDER_MAXSONARXL_SCALER 1.0 #define AP_RANGEFINDER_MAXSONARXL_SCALER 1.0
#define AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE 20 #define AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE 20
#define AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE 765 #define AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE 765
// LV-EZ0 (aka LV) // LV-EZ0 (aka LV)
#define AP_RANGEFINDER_MAXSONARLV 1 #define AP_RANGEFINDER_MAXSONARLV 1
#define AP_RANGEFINDER_MAXSONARLV_SCALER (2.54/2.0) #define AP_RANGEFINDER_MAXSONARLV_SCALER (2.54/2.0)
#define AP_RANGEFINDER_MAXSONARLV_MIN_DISTANCE 15 #define AP_RANGEFINDER_MAXSONARLV_MIN_DISTANCE 15
#define AP_RANGEFINDER_MAXSONARLV_MAX_DISTANCE 645 #define AP_RANGEFINDER_MAXSONARLV_MAX_DISTANCE 645
// XL-EZL0 (aka XLL) // XL-EZL0 (aka XLL)
#define AP_RANGEFINDER_MAXSONARXLL 2 #define AP_RANGEFINDER_MAXSONARXLL 2
#define AP_RANGEFINDER_MAXSONARXLL_SCALER 2.0 #define AP_RANGEFINDER_MAXSONARXLL_SCALER 2.0
#define AP_RANGEFINDER_MAXSONARXLL_MIN_DISTANCE 20 #define AP_RANGEFINDER_MAXSONARXLL_MIN_DISTANCE 20
#define AP_RANGEFINDER_MAXSONARXLL_MAX_DISTANCE 1068 #define AP_RANGEFINDER_MAXSONARXLL_MAX_DISTANCE 1068
class AP_RangeFinder_MaxsonarXL : public RangeFinder class AP_RangeFinder_MaxsonarXL : public RangeFinder
{ {
public: public:
AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source, ModeFilter *filter); 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 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); float calculate_scaler(int sonar_type, float adc_refence_voltage);
private:
float _scaler; // used to account for different sonar types private:
}; float _scaler; // used to account for different sonar types
#endif };
#endif

81
libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp
View File

@ -1,42 +1,41 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*-
/* /*
AP_RangeFinder_SharpGP2Y.cpp - Arduino Library for Sharpe GP2Y0A02YK0F AP_RangeFinder_SharpGP2Y.cpp - Arduino Library for Sharpe GP2Y0A02YK0F
infrared proximity sensor infrared proximity sensor
Code by Jose Julio and Randy Mackay. DIYDrones.com Code by Jose Julio and Randy Mackay. DIYDrones.com
This library is free software; you can redistribute it and/or This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version. version 2.1 of the License, or (at your option) any later version.
Sensor should be conected to one of the analog ports Sensor should be conected to one of the analog ports
Sparkfun URL: http://www.sparkfun.com/products/8958 Sparkfun URL: http://www.sparkfun.com/products/8958
datasheet: http://www.sparkfun.com/datasheets/Sensors/Infrared/gp2y0a02yk_e.pdf datasheet: http://www.sparkfun.com/datasheets/Sensors/Infrared/gp2y0a02yk_e.pdf
Variables: Variables:
int raw_value : raw value from the sensor int raw_value : raw value from the sensor
int distance : distance in cm int distance : distance in cm
int max_distance : maximum measurable distance (in cm) int max_distance : maximum measurable distance (in cm)
int min_distance : minimum measurable distance (in cm) int min_distance : minimum measurable distance (in cm)
Methods: Methods:
read() : read value from analog port read() : read value from analog port
*/ */
// AVR LibC Includes // AVR LibC Includes
#include "WConstants.h" #include "WConstants.h"
#include "AP_RangeFinder_SharpGP2Y.h" #include "AP_RangeFinder_SharpGP2Y.h"
// Constructor ////////////////////////////////////////////////////////////// // Constructor //////////////////////////////////////////////////////////////
AP_RangeFinder_SharpGP2Y::AP_RangeFinder_SharpGP2Y(AP_AnalogSource *source, AP_RangeFinder_SharpGP2Y::AP_RangeFinder_SharpGP2Y(AP_AnalogSource *source, ModeFilter *filter) :
ModeFilter *filter) :
RangeFinder(source, filter) RangeFinder(source, filter)
{ {
max_distance = AP_RANGEFINDER_SHARPEGP2Y_MAX_DISTANCE; max_distance = AP_RANGEFINDER_SHARPEGP2Y_MAX_DISTANCE;
min_distance = AP_RANGEFINDER_SHARPEGP2Y_MIN_DISTANCE; min_distance = AP_RANGEFINDER_SHARPEGP2Y_MIN_DISTANCE;
} }
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////

35
libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h
View File

@ -1,16 +1,21 @@
#ifndef AP_RangeFinder_SharpGP2Y_H #ifndef AP_RangeFinder_SharpGP2Y_H
#define AP_RangeFinder_SharpGP2Y_H #define AP_RangeFinder_SharpGP2Y_H
#include "RangeFinder.h" #include "RangeFinder.h"
#define AP_RANGEFINDER_SHARPEGP2Y_MIN_DISTANCE 20 #define AP_RANGEFINDER_SHARPEGP2Y_MIN_DISTANCE 20
#define AP_RANGEFINDER_SHARPEGP2Y_MAX_DISTANCE 150 #define AP_RANGEFINDER_SHARPEGP2Y_MAX_DISTANCE 150
class AP_RangeFinder_SharpGP2Y : public RangeFinder class AP_RangeFinder_SharpGP2Y : public RangeFinder
{ {
public: public:
AP_RangeFinder_SharpGP2Y(AP_AnalogSource *source, ModeFilter *filter); 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 int convert_raw_to_distance(int _raw_value) {
if( _raw_value == 0 )
}; return max_distance;
#endif else
return 14500/_raw_value;
} // read value from analog port and return distance in cm
};
#endif

89
libraries/AP_RangeFinder/RangeFinder.cpp
View File

@ -1,46 +1,45 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*-
/* /*
AP_RangeFinder.cpp - Arduino Library for Sharpe GP2Y0A02YK0F AP_RangeFinder.cpp - Arduino Library for Sharpe GP2Y0A02YK0F
infrared proximity sensor infrared proximity sensor
Code by Jose Julio and Randy Mackay. DIYDrones.com Code by Jose Julio and Randy Mackay. DIYDrones.com
This library is free software; you can redistribute it and/or This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version. version 2.1 of the License, or (at your option) any later version.
This has the basic functions that all RangeFinders need implemented This has the basic functions that all RangeFinders need implemented
*/ */
// AVR LibC Includes // AVR LibC Includes
#include "WConstants.h" #include "WConstants.h"
#include "RangeFinder.h" #include "RangeFinder.h"
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
void RangeFinder::set_orientation(int x, int y, int z) void RangeFinder::set_orientation(int x, int y, int z)
{ {
orientation_x = x; orientation_x = x;
orientation_y = y; orientation_y = y;
orientation_z = z; orientation_z = z;
} }
// Read Sensor data - only the raw_value is filled in by this parent class // Read Sensor data - only the raw_value is filled in by this parent class
int RangeFinder::read() int RangeFinder::read()
{ {
int temp_dist; int temp_dist;
raw_value = _analog_source->read(); raw_value = _analog_source->read();
// convert analog value to distance in cm (using child implementation most likely)
// convert analog value to distance in cm (using child implementation most likely) temp_dist = convert_raw_to_distance(raw_value);
temp_dist = convert_raw_to_distance(raw_value);
// ensure distance is within min and max
// ensure distance is within min and max temp_dist = constrain(temp_dist, min_distance, max_distance);
temp_dist = constrain(temp_dist, min_distance, max_distance);
distance = _mode_filter->get_filtered_with_sample(temp_dist);
distance = _mode_filter->get_filtered_with_sample(temp_dist); return distance;
return distance; }
}

79
libraries/AP_RangeFinder/RangeFinder.h
View File

@ -1,44 +1,43 @@
#ifndef RangeFinder_h #ifndef RangeFinder_h
#define RangeFinder_h #define RangeFinder_h
#include <stdlib.h> #include <stdlib.h>
#include <inttypes.h> #include <inttypes.h>
#include "../AP_AnalogSource/AP_AnalogSource.h" #include "../AP_AnalogSource/AP_AnalogSource.h"
#include "../ModeFilter/ModeFilter.h" // ArduPilot Mega RC Library #include "../ModeFilter/ModeFilter.h" // ArduPilot Mega RC Library
/* /*
#define AP_RANGEFINDER_ORIENTATION_FRONT 0, 10, 0 #define AP_RANGEFINDER_ORIENTATION_FRONT 0, 10, 0
#define AP_RANGEFINDER_ORIENTATION_RIGHT -10, 0, 0 #define AP_RANGEFINDER_ORIENTATION_RIGHT -10, 0, 0
#define AP_RANGEFINDER_ORIENTATION_BACK 0,-10, 0 #define AP_RANGEFINDER_ORIENTATION_BACK 0,-10, 0
#define AP_RANGEFINDER_ORIENTATION_LEFT 10, 0, 0 #define AP_RANGEFINDER_ORIENTATION_LEFT 10, 0, 0
#define AP_RANGEFINDER_ORIENTATION_UP 0, 0,-10 #define AP_RANGEFINDER_ORIENTATION_UP 0, 0,-10
#define AP_RANGEFINDER_ORIENTATION_DOWN 0, 0, 10 #define AP_RANGEFINDER_ORIENTATION_DOWN 0, 0, 10
#define AP_RANGEFINDER_ORIENTATION_FRONT_RIGHT -5, -5, 0 #define AP_RANGEFINDER_ORIENTATION_FRONT_RIGHT -5, -5, 0
#define AP_RANGEFINDER_ORIENTATION_BACK_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_BACK_LEFT 5, -5, 0
#define AP_RANGEFINDER_ORIENTATION_FRONT_LEFT 5, 5, 0 #define AP_RANGEFINDER_ORIENTATION_FRONT_LEFT 5, 5, 0
*/ */
class RangeFinder class RangeFinder
{ {
protected: protected:
RangeFinder(AP_AnalogSource * source, ModeFilter *filter) : RangeFinder(AP_AnalogSource * source, ModeFilter *filter) :
_analog_source(source), _analog_source(source),
_mode_filter(filter) _mode_filter(filter) {}
{} public:
public:
int raw_value; // raw value from the sensor
int raw_value; // raw value from the sensor int distance; // distance in cm
int distance; // distance in cm int max_distance; // maximum measurable distance (in cm) - should be set in child's constructor
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 min_distance; // minimum measurable distance (in cm) - should be set in child's constructor int orientation_x, orientation_y, orientation_z;
int orientation_x, orientation_y, orientation_z;
virtual void set_orientation(int x, int y, int 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 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
virtual int read(); // read value from sensor and return distance in cm
AP_AnalogSource *_analog_source; AP_AnalogSource *_analog_source;
ModeFilter *_mode_filter; ModeFilter *_mode_filter;
}; };
#endif #endif

28
libraries/AP_RangeFinder/keywords.txt
View File

@ -1,14 +1,14 @@
RangeFinder KEYWORD1 RangeFinder KEYWORD1
AP_RangeFinder KEYWORD1 AP_RangeFinder KEYWORD1
AP_RangeFinder_SharpGP2Y KEYWORD1 AP_RangeFinder_SharpGP2Y KEYWORD1
AP_RangeFinder_MaxsonarXL KEYWORD1 AP_RangeFinder_MaxsonarXL KEYWORD1
read KEYWORD2 read KEYWORD2
set_orientation KEYWORD2 set_orientation KEYWORD2
convert_raw_to_distance KEYWORD2 convert_raw_to_distance KEYWORD2
raw_value KEYWORD2 raw_value KEYWORD2
distance KEYWORD2 distance KEYWORD2
max_distance KEYWORD2 max_distance KEYWORD2
min_distance KEYWORD2 min_distance KEYWORD2
orientation_x KEYWORD2 orientation_x KEYWORD2
orientation_y KEYWORD2 orientation_y KEYWORD2
orientation_z KEYWORD2 orientation_z KEYWORD2

2
libraries/Desktop/support/sitl.cpp
View File

@ -135,7 +135,7 @@ static void sitl_fdm_input(void)
count++; count++;
if (millis() - last_report > 1000) { if (millis() - last_report > 1000) {
printf("SIM %u FPS\n", count); //printf("SIM %u FPS\n", count);
count = 0; count = 0;
last_report = millis(); last_report = millis();
} }

3
libraries/Desktop/support/sitl_adc.cpp
View File

@ -111,11 +111,12 @@ void sitl_update_adc(float roll, float pitch, float yaw,
(fabs(roll - dcm.roll_sensor/100.0) > 5.0 || (fabs(roll - dcm.roll_sensor/100.0) > 5.0 ||
fabs(pitch - dcm.pitch_sensor/100.0) > 5.0))) { fabs(pitch - dcm.pitch_sensor/100.0) > 5.0))) {
last_report = tnow; 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, roll, dcm.roll_sensor/100.0,
pitch, dcm.pitch_sensor/100.0, pitch, dcm.pitch_sensor/100.0,
rollRate, ToDeg(omega.x), rollRate, ToDeg(omega.x),
pitchRate, ToDeg(omega.y)); pitchRate, ToDeg(omega.y));
*/
} }
} }