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 7dd3ae1d70 db1fafaaec
commit 80a342fbf8
41 changed files with 1138 additions and 918 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

126
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
@ -738,17 +748,7 @@ 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
@ -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,6 +1850,7 @@ static void update_altitude()
scale = (sonar_alt - 400) / 200; scale = (sonar_alt - 400) / 200;
scale = constrain(scale, 0, 1); scale = constrain(scale, 0, 1);
// solve for a blended altitude
current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt; 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
@ -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;

59
ArduCopter/commands_logic.pde
View File

@ -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,7 +567,7 @@ 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;
@ -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

17
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);
// 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);
}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;
} }
/// ///

3
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;

10
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp
View File

@ -31,13 +31,13 @@
// 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)

1
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h
View File

@ -27,6 +27,7 @@ class AP_RangeFinder_MaxsonarXL : public RangeFinder
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: private:
float _scaler; // used to account for different sonar types float _scaler; // used to account for different sonar types
}; };

3
libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp
View File

@ -31,8 +31,7 @@
// 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;

7
libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h
View File

@ -10,7 +10,12 @@ 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;
else
return 14500/_raw_value;
} // read value from analog port and return distance in cm
}; };
#endif #endif

1
libraries/AP_RangeFinder/RangeFinder.cpp
View File

@ -33,7 +33,6 @@ 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);

3
libraries/AP_RangeFinder/RangeFinder.h
View File

@ -24,8 +24,7 @@ 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

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));
*/
} }
} }