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

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

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

132
ArduCopter/ArduCopter.pde
View File

@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#define THISFIRMWARE "ArduCopter V2.1.1r8 alpha"
#define THISFIRMWARE "ArduCopter V2.1.1r9 alpha"
/*
ArduCopter Version 2.0 Beta
Authors: Jason Short
@ -80,6 +80,7 @@ http://code.google.com/p/ardupilot-mega/downloads/list
// Configuration
#include "defines.h"
#include "config.h"
#include "config_channels.h"
// Local modules
#include "Parameters.h"
@ -278,7 +279,6 @@ ModeFilter sonar_mode_filter;
////////////////////////////////////////////////////////////////////////////////
// Global variables
////////////////////////////////////////////////////////////////////////////////
static const char *comma = ",";
static const char* flight_mode_strings[] = {
"STABILIZE",
@ -327,7 +327,7 @@ static int16_t y_rate_error;
////////////////////////////////////////////////////////////////////////////////
// This is the state of the flight control system
// There are multiple states defined such as STABILIZE, ACRO,
static byte control_mode = STABILIZE;
static int8_t control_mode = STABILIZE;
// This is the state of simple mode.
// Set in the control_mode.pde file when the control switch is read
static bool do_simple = false;
@ -443,8 +443,6 @@ static int32_t target_bearing;
// This is the angle from the copter to the "next_WP" location
// with the addition of Crosstrack error in degrees * 100
static int32_t nav_bearing;
// This is the angle from the copter to the "home" location in degrees * 100
static int32_t home_bearing;
// Status of the Waypoint tracking mode. Options include:
// NO_NAV_MODE, WP_MODE, LOITER_MODE, CIRCLE_MODE
static byte wp_control;
@ -572,7 +570,8 @@ static int32_t baro_alt;
static int32_t old_baro_alt;
// The climb_rate as reported by Baro in cm/s
static int16_t baro_rate;
//
static boolean reset_throttle_flag;
////////////////////////////////////////////////////////////////////////////////
// flight modes
@ -681,12 +680,23 @@ static int16_t nav_throttle; // 0-1000 for throttle control
static uint32_t throttle_integrator;
// This is a future value for replacing the throttle_cruise setup procedure. It's an average of throttle control
// that is generated when the climb rate is within a certain threshold
static float throttle_avg = THROTTLE_CRUISE;
//static float throttle_avg = THROTTLE_CRUISE;
// This is a flag used to trigger the updating of nav_throttle at 10hz
static bool invalid_throttle;
// Used to track the altitude offset for climbrate control
static int32_t target_altitude;
//static int32_t target_altitude;
////////////////////////////////////////////////////////////////////////////////
// Climb rate control
////////////////////////////////////////////////////////////////////////////////
// Time when we intiated command in millis - used for controlling decent rate
// The orginal altitude used to base our new altitude during decent
static int32_t original_altitude;
// Used to track the altitude offset for climbrate control
static int32_t target_altitude;
static uint32_t alt_change_timer;
static int8_t alt_change_flag;
static uint32_t alt_change;
////////////////////////////////////////////////////////////////////////////////
// Navigation Yaw control
@ -737,18 +747,8 @@ static int16_t event_undo_value;
////////////////////////////////////////////////////////////////////////////////
// Delay Mission Scripting Command
////////////////////////////////////////////////////////////////////////////////
static int32_t condition_value; // used in condition commands (eg delay, change alt, etc.)
static int32_t condition_start;
////////////////////////////////////////////////////////////////////////////////
// Auto Landing
////////////////////////////////////////////////////////////////////////////////
// Time when we intiated command in millis - used for controlling decent rate
static int32_t land_start;
// The orginal altitude used to base our new altitude during decent
static int32_t original_alt;
static int32_t condition_value; // used in condition commands (eg delay, change alt, etc.)
static uint32_t condition_start;
////////////////////////////////////////////////////////////////////////////////
@ -794,6 +794,8 @@ static float dTnav;
static int16_t superslow_loopCounter;
// RTL Autoland Timer
static uint32_t auto_land_timer;
// disarms the copter while in Acro or Stabilzie mode after 30 seconds of no flight
static uint8_t auto_disarming_counter;
// Tracks if GPS is enabled based on statup routine
@ -802,7 +804,7 @@ static bool GPS_enabled = false;
// Set true if we have new PWM data to act on from the Radio
static bool new_radio_frame;
// Used to auto exit the in-flight leveler
static int16_t auto_level_counter;
static int16_t auto_level_counter;
// Reference to the AP relay object - APM1 only
AP_Relay relay;
@ -865,6 +867,7 @@ void loop()
counter_one_herz++;
// trgger our 1 hz loop
if(counter_one_herz >= 50){
super_slow_loop();
counter_one_herz = 0;
@ -1129,7 +1132,7 @@ static void fifty_hz_loop()
#if FRAME_CONFIG == TRI_FRAME
// servo Yaw
g.rc_4.calc_pwm();
APM_RC.OutputCh(CH_7, g.rc_4.radio_out);
APM_RC.OutputCh(CH_TRI_YAW, g.rc_4.radio_out);
#endif
}
@ -1193,16 +1196,28 @@ static void slow_loop()
default:
slow_loopCounter = 0;
break;
}
}
#define AUTO_ARMING_DELAY 60
// 1Hz loop
static void super_slow_loop()
{
if (g.log_bitmask & MASK_LOG_CUR)
Log_Write_Current();
// this function disarms the copter if it has been sitting on the ground for any moment of time greater than 30s
// but only of the control mode is manual
if((control_mode <= ACRO) && (g.rc_3.control_in == 0)){
auto_disarming_counter++;
if(auto_disarming_counter == AUTO_ARMING_DELAY){
init_disarm_motors();
}else if (auto_disarming_counter > AUTO_ARMING_DELAY){
auto_disarming_counter = AUTO_ARMING_DELAY + 1;
}
}else{
auto_disarming_counter = 0;
}
gcs_send_message(MSG_HEARTBEAT);
gcs_data_stream_send(1,3);
// agmatthews - USERHOOKS
@ -1558,18 +1573,27 @@ void update_throttle_mode(void)
throttle_out = g.throttle_cruise + angle_boost + manual_boost;
#endif
// reset next_WP.alt and don't go below 1 meter
next_WP.alt = max(current_loc.alt, 100);
//force a reset of the altitude change
clear_new_altitude();
/*
Serial.printf("tar_alt: %d, actual_alt: %d \talt_err: %d, \t manb: %d\n",
int16_t iterm = g.pi_alt_hold.get_integrator();
Serial.printf("tar_alt: %d, actual_alt: %d \talt_err: %d, \t manb: %d, iterm %d\n",
next_WP.alt,
current_loc.alt,
altitude_error,
manual_boost);
manual_boost,
iterm);
//*/
reset_throttle_flag = true;
}else{
if(reset_throttle_flag) {
set_new_altitude(max(current_loc.alt, 100));
reset_throttle_flag = false;
}
// 10hz, don't run up i term
if(invalid_throttle && motor_auto_armed == true){
@ -1582,14 +1606,13 @@ void update_throttle_mode(void)
// clear the new data flag
invalid_throttle = false;
/*
Serial.printf("tar_alt: %d, actual_alt: %d \talt_err: %d, \tnav_thr: %d, \talt Int: %d, \trate_int %d \n",
Serial.printf("tar_alt: %d, actual_alt: %d \talt_err: %d, \tnav_thr: %d, \talt Int: %d\n",
next_WP.alt,
current_loc.alt,
altitude_error,
nav_throttle,
(int16_t)g.pi_alt_hold.get_integrator(),
(int16_t) g.pi_throttle.get_integrator());
*/
(int16_t)g.pi_alt_hold.get_integrator());
//*/
}
#if FRAME_CONFIG == HELI_FRAME
@ -1637,23 +1660,26 @@ static void update_navigation()
break;
case RTL:
// We have reached Home
if((wp_distance <= g.waypoint_radius) || check_missed_wp()){
// if this value > 0, we are set to trigger auto_land after 30 seconds
set_mode(LOITER);
auto_land_timer = millis();
break;
}
}else if(current_loc.alt < (next_WP.alt - 300)){
// don't navigate if we are below our target alt
wp_control = LOITER_MODE;
// We wait until we've reached out new altitude before coming home
// Arg doesn't work, it
//if(alt_change_flag != REACHED_ALT){
// wp_control = NO_NAV_MODE;
//}else{
wp_control = WP_MODE;
}else{
// calculates desired Yaw
#if FRAME_CONFIG == HELI_FRAME
update_auto_yaw();
#endif
wp_control = WP_MODE;
}
//}
// calculates the desired Roll and Pitch
update_nav_wp();
@ -1689,12 +1715,9 @@ static void update_navigation()
case LAND:
wp_control = LOITER_MODE;
if(verify_land()) { // JLN fix for auto land in RTL
set_mode(STABILIZE);
} else {
// calculates the desired Roll and Pitch
update_nav_wp();
}
// verify land will override WP_control if we are below
// a certain altitude
verify_land();
// calculates the desired Roll and Pitch
update_nav_wp();
@ -1827,7 +1850,8 @@ static void update_altitude()
scale = (sonar_alt - 400) / 200;
scale = constrain(scale, 0, 1);
current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt;
// solve for a blended altitude
current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt;
// solve for a blended climb_rate
climb_rate = ((float)sonar_rate * (1.0 - scale)) + (float)baro_rate * scale;
@ -1840,7 +1864,6 @@ static void update_altitude()
}
}else{
// NO Sonar case
current_loc.alt = baro_alt + home.alt;
climb_rate = baro_rate;
@ -1848,25 +1871,14 @@ static void update_altitude()
// manage bad data
climb_rate = constrain(climb_rate, -300, 300);
// update the target altitude
next_WP.alt = get_new_altitude();
}
static void
adjust_altitude()
{
/*
// old vert control
if(g.rc_3.control_in <= 200){
next_WP.alt -= 1; // 1 meter per second
next_WP.alt = max(next_WP.alt, (current_loc.alt - 500)); // don't go less than 4 meters below current location
next_WP.alt = max(next_WP.alt, 100); // don't go less than 1 meter
//manual_boost = (g.rc_3.control_in == 0) ? -20 : 0;
}else if (g.rc_3.control_in > 700){
next_WP.alt += 1; // 1 meter per second
next_WP.alt = min(next_WP.alt, (current_loc.alt + 500)); // don't go more than 4 meters below current location
//manual_boost = (g.rc_3.control_in == 800) ? 20 : 0;
}*/
if(g.rc_3.control_in <= 180){
// we remove 0 to 100 PWM from hover
manual_boost = g.rc_3.control_in - 180;

29
ArduCopter/Attitude.pde
View File

@ -170,9 +170,6 @@ get_nav_throttle(int32_t z_error)
int16_t rate_error;
int16_t output;
// XXX HACK, need a better way not to ramp this i term in large altitude changes.
float dt = (abs(z_error) < 400) ? .1 : 0.0;
// limit error to prevent I term run up
z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX);
@ -180,7 +177,7 @@ get_nav_throttle(int32_t z_error)
rate_error = g.pi_alt_hold.get_p(z_error); //_p = .85
// experiment to pipe iterm directly into the output
int16_t iterm = g.pi_alt_hold.get_i(z_error, dt);
int16_t iterm = g.pi_alt_hold.get_i(z_error, .1);
// calculate rate error
rate_error = rate_error - climb_rate;
@ -228,23 +225,37 @@ get_rate_yaw(int32_t target_rate)
// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
// Keeps outdated data out of our calculations
static void reset_hold_I(void)
{
g.pi_loiter_lat.reset_I();
g.pi_loiter_lon.reset_I();
}
//static void reset_hold_I(void)
//{
// g.pi_loiter_lat.reset_I();
// g.pi_loiter_lon.reset_I();
//}
// Keeps old data out of our calculation / logs
static void reset_nav(void)
{
// forces us to update nav throttle
invalid_throttle = true;
nav_throttle = 0;
// always start Circle mode at same angle
circle_angle = 0;
// We must be heading to a new WP, so XTrack must be 0
crosstrack_error = 0;
// Will be set by new command
target_bearing = 0;
// Will be set by new command
wp_distance = 0;
// Will be set by new command, used by loiter
long_error = 0;
lat_error = 0;
// Will be set by new command, used by loiter
next_WP.alt = 0;
}
static void reset_rate_I()

4
ArduCopter/Camera.pde
View File

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

2
ArduCopter/Log.pde
View File

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

13
ArduCopter/commands.pde
View File

@ -147,9 +147,13 @@ static void set_next_WP(struct Location *wp)
// ---------------------
next_WP = *wp;
// used to control and limit the rate of climb - not used right now!
// -----------------------------------------------------------------
target_altitude = current_loc.alt;
// used to control and limit the rate of climb
// -------------------------------------------
// We don't set next WP below 1m
next_WP.alt = max(next_WP.alt, 100);
// Save new altitude so we can track it for climb_rate
set_new_altitude(next_WP.alt);
// this is used to offset the shrinking longitude as we go towards the poles
float rads = (fabs((float)next_WP.lat)/t7) * 0.0174532925;
@ -162,6 +166,9 @@ static void set_next_WP(struct Location *wp)
target_bearing = get_bearing(&prev_WP, &next_WP);
nav_bearing = target_bearing;
// calc the location error:
calc_location_error(&next_WP);
// to check if we have missed the WP
// ---------------------------------
original_target_bearing = target_bearing;

63
ArduCopter/commands_logic.pde
View File

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

12
ArduCopter/commands_process.pde
View File

@ -39,7 +39,7 @@ static void update_commands()
//uint8_t tmp = g.command_index.get();
//Serial.printf("command_index %u \n", tmp);
if (g.command_total <= 1 || g.command_index == 255)
if (g.command_total <= 1 || g.command_index > 127)
return;
if(command_nav_queue.id == NO_COMMAND){
@ -62,8 +62,14 @@ static void update_commands()
g.command_index = command_nav_index = 255;
// if we are on the ground, enter stabilize, else Land
if (land_complete == true){
set_mode(STABILIZE);
// disarm motors
// So what state does this leave us in?
// We are still in the same mode as what landed us,
// so maybe we try to continue to descend just in case we are still in the air
// This will also drive down the Iterm to -300
set_new_altitude(-10000);
// We can't disarm the motors easily. We could very well be wrong
//
//init_disarm_motors();
} else {
set_mode(LAND);

6
ArduCopter/config.h
View File

@ -1,5 +1,7 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//
#ifndef __ARDUCOPTER_CONFIG_H__
#define __ARDUCOPTER_CONFIG_H__
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
//
@ -855,3 +857,7 @@
#ifndef MAVLINK_TELEMETRY_PORT_DELAY
# define MAVLINK_TELEMETRY_PORT_DELAY 2000
#endif
#endif // __ARDUCOPTER_CONFIG_H__

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_SENSORS 2
#define ASCENDING 1
#define DESCENDING -1
#define REACHED_ALT 0
// Auto Pilot modes
// ----------------
#define STABILIZE 0 // hold level position

19
ArduCopter/events.pde
View File

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

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()
{
#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
}

15
ArduCopter/motors.pde
View File

@ -142,3 +142,18 @@ set_servos_4()
output_motors_disarmed();
}
}
int ch_of_mot( int mot ) {
switch (mot) {
case 1: return MOT_1;
case 2: return MOT_2;
case 3: return MOT_3;
case 4: return MOT_4;
case 5: return MOT_5;
case 6: return MOT_6;
case 7: return MOT_7;
case 8: return MOT_8;
}
return (-1);
}

184
ArduCopter/motors_hexa.pde
View File

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

223
ArduCopter/motors_octa.pde
View File

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

169
ArduCopter/motors_octa_quad.pde
View File

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

110
ArduCopter/motors_quad.pde
View File

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

76
ArduCopter/motors_tri.pde
View File

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

153
ArduCopter/motors_y6.pde
View File

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

115
ArduCopter/navigation.pde
View File

@ -367,12 +367,125 @@ static void update_crosstrack(void)
}
}
static int32_t get_altitude_error()
{
// Next_WP alt is our target alt
// It changes based on climb rate
// until it reaches the target_altitude
return next_WP.alt - current_loc.alt;
}
static void clear_new_altitude()
{
alt_change_flag = REACHED_ALT;
}
static void set_new_altitude(int32_t _new_alt)
{
// just to be clear
next_WP.alt = current_loc.alt;
// for calculating the delta time
alt_change_timer = millis();
// save the target altitude
target_altitude = _new_alt;
// reset our altitude integrator
alt_change = 0;
// save the original altitude
original_altitude = current_loc.alt;
// to decide if we have reached the target altitude
if(target_altitude > original_altitude){
// we are below, going up
alt_change_flag = ASCENDING;
Serial.printf("go up\n");
}else if(target_altitude < original_altitude){
// we are above, going down
alt_change_flag = DESCENDING;
Serial.printf("go down\n");
}else{
// No Change
alt_change_flag = REACHED_ALT;
Serial.printf("reached alt\n");
}
//Serial.printf("new alt: %d Org alt: %d\n", target_altitude, original_altitude);
}
static int32_t get_new_altitude()
{
// returns a new next_WP.alt
if(alt_change_flag == ASCENDING){
// we are below, going up
if(current_loc.alt >= target_altitude){
alt_change_flag = REACHED_ALT;
}
// we shouldn't command past our target
if(next_WP.alt >= target_altitude){
return target_altitude;
}
}else if (alt_change_flag == DESCENDING){
// we are above, going down
if(current_loc.alt <= target_altitude)
alt_change_flag = REACHED_ALT;
// we shouldn't command past our target
if(next_WP.alt <= target_altitude){
return target_altitude;
}
}
// if we have reached our target altitude, return the target alt
if(alt_change_flag == REACHED_ALT){
return target_altitude;
}
int32_t diff = abs(next_WP.alt - target_altitude);
int8_t _scale = 4;
if (next_WP.alt < target_altitude){
// we are below the target alt
if(diff < 200){
// we are going up
_scale = 5;
} else {
_scale = 4;
}
}else {
// we are above the target
// stay at 16 for speed
//_scale = 16;
if(diff < 400){
// we are going down
_scale = 5;
}else if(diff < 100){
_scale = 6;
}
}
int32_t change = (millis() - alt_change_timer) >> _scale;
if(alt_change_flag == ASCENDING){
alt_change += change;
}else{
alt_change -= change;
}
// for generating delta time
alt_change_timer = millis();
return original_altitude + alt_change;
}
static int32_t wrap_360(int32_t error)
{
if (error > 36000) error -= 36000;

32
ArduCopter/radio.pde
View File

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

18
ArduCopter/setup.pde
View File

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

21
ArduCopter/system.pde
View File

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

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

10
Tools/autotest/arducopter.py
View File

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

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
m = mav.recv_match(type='NAV_CONTROLLER_OUTPUT', blocking=True)
wp_dist = m.wp_dist
print("test: WP %u (wp_dist=%u), current_wp: %u, wpnum_end: %u" % (seq, wp_dist, current_wp, wpnum_end))
m = mav.recv_match(type='VFR_HUD', blocking=True)
print("test: WP %u (wp_dist=%u Alt=%d), current_wp: %u, wpnum_end: %u" % (seq, wp_dist, m.alt, current_wp, wpnum_end))
if seq == current_wp+1 or (seq > current_wp+1 and allow_skip):
print("test: Starting new waypoint %u" % seq)
tstart = time.time()

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

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

14
libraries/APM_RC/APM_RC.h
View File

@ -21,20 +21,6 @@
#define CH_10 9
#define CH_11 10
#define MSK_CH_1 (1 << CH_1)
#define MSK_CH_2 (1 << CH_2)
#define MSK_CH_3 (1 << CH_3)
#define MSK_CH_4 (1 << CH_4)
#define MSK_CH_5 (1 << CH_5)
#define MSK_CH_6 (1 << CH_6)
#define MSK_CH_7 (1 << CH_7)
#define MSK_CH_8 (1 << CH_8)
#define MSK_CH_9 (1 << CH_9)
#define MSK_CH_10 (1 << CH_10)
#define MSK_CH_11 (1 << CH_11)
#define NUM_CHANNELS 8

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)
{
if ((chmask & ( MSK_CH_1 | MSK_CH_2 | MSK_CH_9)) != 0)
if ((chmask & ( _BV(CH_1) | _BV(CH_2) | _BV(CH_9))) != 0)
_set_speed_ch1_ch2_ch9(OUTPUT_SPEED_400HZ);
if ((chmask & ( MSK_CH_3 | MSK_CH_4 | MSK_CH_10 )) != 0)
if ((chmask & ( _BV(CH_3) | _BV(CH_4) | _BV(CH_10))) != 0)
_set_speed_ch3_ch4_ch10(OUTPUT_SPEED_400HZ);
if ((chmask & ( MSK_CH_5 | MSK_CH_6 )) != 0)
if ((chmask & ( _BV(CH_5) | _BV(CH_6))) != 0)
_set_speed_ch5_ch6(OUTPUT_SPEED_400HZ);
if ((chmask & ( MSK_CH_7 | MSK_CH_8 | MSK_CH_11 )) != 0)
if ((chmask & ( _BV(CH_7) | _BV(CH_8) | _BV(CH_11))) != 0)
_set_speed_ch7_ch8_ch11(OUTPUT_SPEED_400HZ);
}

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)
{
if ((chmask & ( MSK_CH_1 | MSK_CH_2 )) != 0)
if ((chmask & ( _BV(CH_1) | _BV(CH_2))) != 0)
_set_speed_ch1_ch2(OUTPUT_SPEED_400HZ);
if ((chmask & ( MSK_CH_3 | MSK_CH_4 | MSK_CH_5 )) != 0)
if ((chmask & ( _BV(CH_3) | _BV(CH_4) | _BV(CH_5))) != 0)
_set_speed_ch3_ch4_ch5(OUTPUT_SPEED_400HZ);
if ((chmask & ( MSK_CH_6 | MSK_CH_7 | MSK_CH_8 )) != 0)
if ((chmask & ( _BV(CH_6) | _BV(CH_7) | _BV(CH_8))) != 0)
_set_speed_ch6_ch7_ch8(OUTPUT_SPEED_400HZ);
}

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)
inline void setAccel(uint8_t index, int16_t val) {
int16_t temp = val * accelScale[index] / 1000 + accelBias[index];
adcValue[sensors[index+3]] = (sensors[index+3] < 0) ? -temp : temp;
adcValue[sensors[index+3]] = (sensorSign[index+3] < 0) ? -temp : temp;
}
///

7
libraries/AP_Common/menu.cpp
View File

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

128
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.cpp
View File

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

67
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarXL.h
View File

@ -1,33 +1,34 @@
#ifndef AP_RangeFinder_MaxsonarXL_H
#define AP_RangeFinder_MaxsonarXL_H
#include "RangeFinder.h"
// XL-EZ0 (aka XL)
#define AP_RANGEFINDER_MAXSONARXL 0
#define AP_RANGEFINDER_MAXSONARXL_SCALER 1.0
#define AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE 20
#define AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE 765
// LV-EZ0 (aka LV)
#define AP_RANGEFINDER_MAXSONARLV 1
#define AP_RANGEFINDER_MAXSONARLV_SCALER (2.54/2.0)
#define AP_RANGEFINDER_MAXSONARLV_MIN_DISTANCE 15
#define AP_RANGEFINDER_MAXSONARLV_MAX_DISTANCE 645
// XL-EZL0 (aka XLL)
#define AP_RANGEFINDER_MAXSONARXLL 2
#define AP_RANGEFINDER_MAXSONARXLL_SCALER 2.0
#define AP_RANGEFINDER_MAXSONARXLL_MIN_DISTANCE 20
#define AP_RANGEFINDER_MAXSONARXLL_MAX_DISTANCE 1068
class AP_RangeFinder_MaxsonarXL : public RangeFinder
{
public:
AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source, ModeFilter *filter);
int convert_raw_to_distance(int _raw_value) { return _raw_value * _scaler; } // read value from analog port and return distance in cm
float calculate_scaler(int sonar_type, float adc_refence_voltage);
private:
float _scaler; // used to account for different sonar types
};
#endif
#ifndef AP_RangeFinder_MaxsonarXL_H
#define AP_RangeFinder_MaxsonarXL_H
#include "RangeFinder.h"
// XL-EZ0 (aka XL)
#define AP_RANGEFINDER_MAXSONARXL 0
#define AP_RANGEFINDER_MAXSONARXL_SCALER 1.0
#define AP_RANGEFINDER_MAXSONARXL_MIN_DISTANCE 20
#define AP_RANGEFINDER_MAXSONARXL_MAX_DISTANCE 765
// LV-EZ0 (aka LV)
#define AP_RANGEFINDER_MAXSONARLV 1
#define AP_RANGEFINDER_MAXSONARLV_SCALER (2.54/2.0)
#define AP_RANGEFINDER_MAXSONARLV_MIN_DISTANCE 15
#define AP_RANGEFINDER_MAXSONARLV_MAX_DISTANCE 645
// XL-EZL0 (aka XLL)
#define AP_RANGEFINDER_MAXSONARXLL 2
#define AP_RANGEFINDER_MAXSONARXLL_SCALER 2.0
#define AP_RANGEFINDER_MAXSONARXLL_MIN_DISTANCE 20
#define AP_RANGEFINDER_MAXSONARXLL_MAX_DISTANCE 1068
class AP_RangeFinder_MaxsonarXL : public RangeFinder
{
public:
AP_RangeFinder_MaxsonarXL(AP_AnalogSource *source, ModeFilter *filter);
int convert_raw_to_distance(int _raw_value) { return _raw_value * _scaler; } // read value from analog port and return distance in cm
float calculate_scaler(int sonar_type, float adc_refence_voltage);
private:
float _scaler; // used to account for different sonar types
};
#endif

81
libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.cpp
View File

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

35
libraries/AP_RangeFinder/AP_RangeFinder_SharpGP2Y.h
View File

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

89
libraries/AP_RangeFinder/RangeFinder.cpp
View File

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

79
libraries/AP_RangeFinder/RangeFinder.h
View File

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

28
libraries/AP_RangeFinder/keywords.txt
View File

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

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

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

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(pitch - dcm.pitch_sensor/100.0) > 5.0))) {
last_report = tnow;
printf("roll=%5.1f / %5.1f pitch=%5.1f / %5.1f rr=%5.2f / %5.2f pr=%5.2f / %5.2f\n",
/*printf("roll=%5.1f / %5.1f pitch=%5.1f / %5.1f rr=%5.2f / %5.2f pr=%5.2f / %5.2f\n",
roll, dcm.roll_sensor/100.0,
pitch, dcm.pitch_sensor/100.0,
rollRate, ToDeg(omega.x),
pitchRate, ToDeg(omega.y));
*/
}
}