Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 14:08:45 -04:00
Code Issues Packages Projects Releases Wiki Activity

2.0.6 Beta updates. Frame options + X moved to AP_Var.

Browse Source 
Updated nav code.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2345 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-05-18 23:38:24 +00:00
parent 886032141a
commit 4764bf9b63
16 changed files with 204 additions and 331 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
ArduCopterMega/APM_Config.h
View File

@ -6,19 +6,24 @@
//#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD //#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
#define NAV_TEST 0 // 0 = traditional, 1 = rate controlled #define NAV_TEST 1 // 0 = traditional, 1 = rate controlled
#define FRAME_CONFIG QUADX_FRAME #define FRAME_CONFIG QUAD_FRAME
/* /*
options: options:
QUADP_FRAME // the classic plus configuration QUAD_FRAME
QUADX_FRAME // the superior X configuration TRI_FRAME
TRI_FRAME // three props with a servo on the tail for yaw HEXA_FRAME
HEXAP_FRAME // you have more motors than sense (6) Y6_FRAME
HEXAX_FRAME // you like scaring children in the park
Y6_FRAME // the motors are stacked on a Tri frame
*/ */
#define FRAME_ORIENTATION X_FRAME
/*
PLUS_FRAME
X_FRAME
*/
#define CHANNEL_6_TUNING CH6_NONE #define CHANNEL_6_TUNING CH6_NONE
/* /*
CH6_NONE CH6_NONE
@ -53,3 +58,11 @@
# define LOG_CMD ENABLED # define LOG_CMD ENABLED
# define LOG_CURRENT DISABLED # define LOG_CURRENT DISABLED
#define MOTOR_LEDS 0 // 0 = off, 1 = on
#define FR_LED AN12 // Mega PE4 pin, OUT7
#define RE_LED AN14 // Mega PE5 pin, OUT6
#define RI_LED AN10 // Mega PH4 pin, OUT5
#define LE_LED AN8 // Mega PH5 pin, OUT4

3
ArduCopterMega/APM_Config.h.reference
View File

@ -188,9 +188,6 @@
// switch to produce 1165, 1425, and 1815 microseconds and configure // switch to produce 1165, 1425, and 1815 microseconds and configure
// FLIGHT_MODE 1 & 2, 3 & 4 and 5 & 6 to be the same. This is the default. // FLIGHT_MODE 1 & 2, 3 & 4 and 5 & 6 to be the same. This is the default.
// //
// If you have FLIGHT_MODE_CHANNEL set to 8 (the default) and your control
// channel connected to input channel 8, the hardware failsafe mode will
// activate for any control input over 1750ms.
// //
// For more modes (up to six), set your switch(es) to produce any of 1165, // For more modes (up to six), set your switch(es) to produce any of 1165,
// 1295, 1425, 1555, 1685, and 1815 microseconds. // 1295, 1425, 1555, 1685, and 1815 microseconds.

12
ArduCopterMega/ArduCopterMega.pde
View File

@ -230,8 +230,8 @@ boolean motor_auto_armed; // if true,
// PIDs // PIDs
// ---- // ----
int max_stabilize_dampener; // //int max_stabilize_dampener; //
int max_yaw_dampener; // //int max_yaw_dampener; //
boolean rate_yaw_flag; // used to transition yaw control from Rate control to Yaw hold boolean rate_yaw_flag; // used to transition yaw control from Rate control to Yaw hold
byte yaw_debug; byte yaw_debug;
bool did_clear_yaw_control; bool did_clear_yaw_control;
@ -818,6 +818,11 @@ void slow_loop()
if(baro_alt_offset > 0) baro_alt_offset--; if(baro_alt_offset > 0) baro_alt_offset--;
if(baro_alt_offset < 0) baro_alt_offset++; if(baro_alt_offset < 0) baro_alt_offset++;
#if MOTOR_LEDS == 1
update_motor_light();
#endif
break; break;
default: default:
@ -1310,7 +1315,8 @@ void tuning(){
#elif CHANNEL_6_TUNING == CH6_STABLIZE_KD #elif CHANNEL_6_TUNING == CH6_STABLIZE_KD
// uncomment me out to try in flight dampening, 0 = unflyable, .2 = unfun, .13 worked for me. // uncomment me out to try in flight dampening, 0 = unflyable, .2 = unfun, .13 worked for me.
// use test,radio to get the value to use in your config. // use test,radio to get the value to use in your config.
g.stabilize_dampener.set((float)g.rc_6.control_in / 1000.0); g.pid_stabilize_pitch.kD((float)g.rc_6.control_in / 1000.0);
g.pid_stabilize_roll.kD((float)g.rc_6.control_in / 1000.0);
#elif CHANNEL_6_TUNING == CH6_BARO_KP #elif CHANNEL_6_TUNING == CH6_BARO_KP
g.pid_baro_throttle.kP((float)g.rc_6.control_in / 1000.0); g.pid_baro_throttle.kP((float)g.rc_6.control_in / 1000.0);

41
ArduCopterMega/Attitude.pde
View File

@ -30,35 +30,21 @@ limit_nav_pitch_roll(long pmax)
void void
output_stabilize_roll() output_stabilize_roll()
{ {
float error, rate; float error;//, rate;
int dampener; //int dampener;
error = g.rc_1.servo_out - dcm.roll_sensor; error = g.rc_1.servo_out - dcm.roll_sensor;
// limit the error we're feeding to the PID // limit the error we're feeding to the PID
error = constrain(error, -2500, 2500); error = constrain(error, -2500, 2500);
// only buildup I if we are trying to roll hard
//if(abs(g.rc_1.servo_out) < 1000){
// smoother alternative to reset?
//if(g.pid_stabilize_roll.kI() != 0){
// g.pid_stabilize_roll.kI(g.pid_stabilize_roll.kI() * .8);
//}
// g.pid_stabilize_roll.reset_I();
//}
// write out angles back to servo out - this will be converted to PWM by RC_Channel // write out angles back to servo out - this will be converted to PWM by RC_Channel
g.rc_1.servo_out = g.pid_stabilize_roll.get_pid(error, delta_ms_fast_loop, 1.0); // 2500 * .7 = 1750 g.rc_1.servo_out = g.pid_stabilize_roll.get_pi(error, delta_ms_fast_loop, 1.0); // 2500 * .7 = 1750
// We adjust the output by the rate of rotation: // We adjust the output by the rate of rotation:
// Rate control through bias corrected gyro rates // Rate control through bias corrected gyro rates
// omega is the raw gyro reading // omega is the raw gyro reading
g.rc_1.servo_out -= degrees(omega.x) * 100.0 * g.pid_stabilize_roll.kD();
// Limit dampening to be equal to propotional term for symmetry
rate = degrees(omega.x) * 100.0; // 6rad = 34377
dampener = rate * g.stabilize_dampener; // 34377 * .175 = 6000
g.rc_1.servo_out -= dampener;
g.rc_1.servo_out = min(g.rc_1.servo_out, 2500); g.rc_1.servo_out = min(g.rc_1.servo_out, 2500);
g.rc_1.servo_out = max(g.rc_1.servo_out, -2500); g.rc_1.servo_out = max(g.rc_1.servo_out, -2500);
} }
@ -74,23 +60,13 @@ output_stabilize_pitch()
// limit the error we're feeding to the PID // limit the error we're feeding to the PID
error = constrain(error, -2500, 2500); error = constrain(error, -2500, 2500);
// only buildup I if we are trying to roll hard
//if(abs(g.rc_2.servo_out) < 1500){
// g.pid_stabilize_pitch.reset_I();
//}
// write out angles back to servo out - this will be converted to PWM by RC_Channel // write out angles back to servo out - this will be converted to PWM by RC_Channel
g.rc_2.servo_out = g.pid_stabilize_pitch.get_pid(error, delta_ms_fast_loop, 1.0); g.rc_2.servo_out = g.pid_stabilize_pitch.get_pi(error, delta_ms_fast_loop, 1.0);
// We adjust the output by the rate of rotation: // We adjust the output by the rate of rotation:
// Rate control through bias corrected gyro rates // Rate control through bias corrected gyro rates
// omega is the raw gyro reading // omega is the raw gyro reading
g.rc_2.servo_out -= degrees(omega.y) * 100.0 * g.pid_stabilize_pitch.kD();
// Limit dampening to be equal to propotional term for symmetry
rate = degrees(omega.y) * 100.0; // 6rad = 34377
dampener = rate * g.stabilize_dampener; // 34377 * .175 = 6000
g.rc_2.servo_out -= dampener;
g.rc_2.servo_out = min(g.rc_2.servo_out, 2500); g.rc_2.servo_out = min(g.rc_2.servo_out, 2500);
g.rc_2.servo_out = max(g.rc_2.servo_out, -2500); g.rc_2.servo_out = max(g.rc_2.servo_out, -2500);
} }
@ -125,8 +101,6 @@ void
reset_I(void) reset_I(void)
{ {
// I removed these, they don't seem to be needed. // I removed these, they don't seem to be needed.
//g.pid_nav_lat.reset_I();
//g.pid_nav_lon.reset_I();
} }
@ -212,7 +186,6 @@ void output_manual_yaw()
void auto_yaw() void auto_yaw()
{ {
output_yaw_with_hold(true); // hold yaw output_yaw_with_hold(true); // hold yaw
} }
@ -233,7 +206,7 @@ output_yaw_with_hold(boolean hold)
// rate control // rate control
long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377 long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377
rate = constrain(rate, -36000, 36000); // limit to something fun! rate = constrain(rate, -36000, 36000); // limit to something fun!
int dampener = rate * g.hold_yaw_dampener; // 34377 * .175 = 6000 int dampener = rate * g.pid_yaw.kD(); // 34377 * .175 = 6000
if(hold){ if(hold){
// look to see if we have exited rate control properly - ie stopped turning // look to see if we have exited rate control properly - ie stopped turning

23
ArduCopterMega/Parameters.h
View File

@ -17,7 +17,7 @@ public:
// The increment will prevent old parameters from being used incorrectly // The increment will prevent old parameters from being used incorrectly
// by newer code. // by newer code.
// //
static const uint16_t k_format_version = 7; static const uint16_t k_format_version = 8;
// //
// Parameter identities. // Parameter identities.
@ -65,6 +65,7 @@ public:
k_param_compass_enabled, k_param_compass_enabled,
k_param_compass, k_param_compass,
k_param_sonar, k_param_sonar,
k_param_frame_orientation,
// //
// 160: Navigation parameters // 160: Navigation parameters
@ -124,9 +125,6 @@ public:
k_param_pid_nav_wp, k_param_pid_nav_wp,
k_param_pid_baro_throttle, k_param_pid_baro_throttle,
k_param_pid_sonar_throttle, k_param_pid_sonar_throttle,
// special D term alternatives
k_param_stabilize_dampener,
k_param_hold_yaw_dampener,
// 255: reserved // 255: reserved
@ -183,6 +181,7 @@ public:
AP_Int16 pack_capacity; // Battery pack capacity less reserve AP_Int16 pack_capacity; // Battery pack capacity less reserve
AP_Int8 compass_enabled; AP_Int8 compass_enabled;
AP_Int8 esc_calibrate; AP_Int8 esc_calibrate;
AP_Int8 frame_orientation;
// RC channels // RC channels
RC_Channel rc_1; RC_Channel rc_1;
@ -209,9 +208,6 @@ public:
PID pid_baro_throttle; PID pid_baro_throttle;
PID pid_sonar_throttle; PID pid_sonar_throttle;
AP_Float stabilize_dampener;
AP_Float hold_yaw_dampener;
uint8_t junk; uint8_t junk;
Parameters() : Parameters() :
@ -249,7 +245,9 @@ public:
ground_temperature (0, k_param_ground_temperature, PSTR("GND_TEMP")), ground_temperature (0, k_param_ground_temperature, PSTR("GND_TEMP")),
ground_pressure (0, k_param_ground_pressure, PSTR("GND_ABS_PRESS")), ground_pressure (0, k_param_ground_pressure, PSTR("GND_ABS_PRESS")),
RTL_altitude (ALT_HOLD_HOME * 100, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")), RTL_altitude (ALT_HOLD_HOME * 100, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")),
esc_calibrate (0, k_param_esc_calibrate, PSTR("ESC")), esc_calibrate (0, k_param_esc_calibrate, PSTR("ESC")),
frame_orientation (FRAME_ORIENTATION, k_param_frame_orientation, PSTR("FRAME")),
// RC channel group key name // RC channel group key name
//---------------------------------------------------------------------- //----------------------------------------------------------------------
@ -270,9 +268,9 @@ public:
pid_acro_rate_pitch (k_param_pid_acro_rate_pitch, PSTR("ACR_PIT_"), ACRO_RATE_PITCH_P, ACRO_RATE_PITCH_I, ACRO_RATE_PITCH_D, ACRO_RATE_PITCH_IMAX * 100), pid_acro_rate_pitch (k_param_pid_acro_rate_pitch, PSTR("ACR_PIT_"), ACRO_RATE_PITCH_P, ACRO_RATE_PITCH_I, ACRO_RATE_PITCH_D, ACRO_RATE_PITCH_IMAX * 100),
pid_acro_rate_yaw (k_param_pid_acro_rate_yaw, PSTR("ACR_YAW_"), ACRO_RATE_YAW_P, ACRO_RATE_YAW_I, ACRO_RATE_YAW_D, ACRO_RATE_YAW_IMAX * 100), pid_acro_rate_yaw (k_param_pid_acro_rate_yaw, PSTR("ACR_YAW_"), ACRO_RATE_YAW_P, ACRO_RATE_YAW_I, ACRO_RATE_YAW_D, ACRO_RATE_YAW_IMAX * 100),
pid_stabilize_roll (k_param_pid_stabilize_roll, PSTR("STB_RLL_"), STABILIZE_ROLL_P, STABILIZE_ROLL_I, 0, STABILIZE_ROLL_IMAX * 100), pid_stabilize_roll (k_param_pid_stabilize_roll, PSTR("STB_RLL_"), STABILIZE_ROLL_P, STABILIZE_ROLL_I, STABILIZE_ROLL_D, STABILIZE_ROLL_IMAX * 100),
pid_stabilize_pitch (k_param_pid_stabilize_pitch, PSTR("STB_PIT_"), STABILIZE_PITCH_P, STABILIZE_PITCH_I, 0, STABILIZE_PITCH_IMAX * 100), pid_stabilize_pitch (k_param_pid_stabilize_pitch, PSTR("STB_PIT_"), STABILIZE_PITCH_P, STABILIZE_PITCH_I, STABILIZE_PITCH_D, STABILIZE_PITCH_IMAX * 100),
pid_yaw (k_param_pid_yaw, PSTR("STB_YAW_"), YAW_P, YAW_I, 0, YAW_IMAX * 100), pid_yaw (k_param_pid_yaw, PSTR("STB_YAW_"), YAW_P, YAW_I, YAW_D, YAW_IMAX * 100),
pid_nav_lat (k_param_pid_nav_lat, PSTR("NAV_LAT_"), NAV_LOITER_P, NAV_LOITER_I, NAV_LOITER_D, NAV_LOITER_IMAX * 100), pid_nav_lat (k_param_pid_nav_lat, PSTR("NAV_LAT_"), NAV_LOITER_P, NAV_LOITER_I, NAV_LOITER_D, NAV_LOITER_IMAX * 100),
pid_nav_lon (k_param_pid_nav_lon, PSTR("NAV_LON_"), NAV_LOITER_P, NAV_LOITER_I, NAV_LOITER_D, NAV_LOITER_IMAX * 100), pid_nav_lon (k_param_pid_nav_lon, PSTR("NAV_LON_"), NAV_LOITER_P, NAV_LOITER_I, NAV_LOITER_D, NAV_LOITER_IMAX * 100),
@ -281,9 +279,6 @@ public:
pid_baro_throttle (k_param_pid_baro_throttle, PSTR("THR_BAR_"), THROTTLE_BARO_P, THROTTLE_BARO_I, THROTTLE_BARO_D, THROTTLE_BARO_IMAX), pid_baro_throttle (k_param_pid_baro_throttle, PSTR("THR_BAR_"), THROTTLE_BARO_P, THROTTLE_BARO_I, THROTTLE_BARO_D, THROTTLE_BARO_IMAX),
pid_sonar_throttle (k_param_pid_sonar_throttle, PSTR("THR_SON_"), THROTTLE_SONAR_P, THROTTLE_SONAR_I, THROTTLE_SONAR_D, THROTTLE_SONAR_IMAX), pid_sonar_throttle (k_param_pid_sonar_throttle, PSTR("THR_SON_"), THROTTLE_SONAR_P, THROTTLE_SONAR_I, THROTTLE_SONAR_D, THROTTLE_SONAR_IMAX),
stabilize_dampener (STABILIZE_ROLL_D, k_param_stabilize_dampener, PSTR("STB_DAMP")),
hold_yaw_dampener (YAW_D, k_param_hold_yaw_dampener, PSTR("YAW_DAMP")),
junk(0) // XXX just so that we can add things without worrying about the trailing comma junk(0) // XXX just so that we can add things without worrying about the trailing comma
{ {
} }

17
ArduCopterMega/config.h
View File

@ -47,8 +47,12 @@
// FRAME_CONFIG // FRAME_CONFIG
// //
#ifndef FRAME_CONFIG #ifndef FRAME_CONFIG
# define FRAME_CONFIG QUADP_FRAME # define FRAME_CONFIG QUAD_FRAME
#endif #endif
#ifndef FRAME_ORIENTATION
# define FRAME_ORIENTATION PLUS_FRAME
#endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// Sonar // Sonar
@ -117,7 +121,7 @@
// setup may override the GPS configuration. // setup may override the GPS configuration.
// //
#ifndef GPS_PROTOCOL #ifndef GPS_PROTOCOL
# define GPS_PROTOCOL GPS_PROTOCOL_AUTO # define GPS_PROTOCOL GPS_PROTOCOL_AUTO
#endif #endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
@ -197,16 +201,7 @@
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// FLIGHT_MODE // FLIGHT_MODE
// FLIGHT_MODE_CHANNEL
// //
#ifndef FLIGHT_MODE_CHANNEL
# define FLIGHT_MODE_CHANNEL CH_5
#endif
#if (FLIGHT_MODE_CHANNEL != CH_5) && (FLIGHT_MODE_CHANNEL != CH_6) && (FLIGHT_MODE_CHANNEL != CH_7) && (FLIGHT_MODE_CHANNEL != CH_8)
# error XXX
# error XXX You must set FLIGHT_MODE_CHANNEL to 5, 6, 7 or 8
# error XXX
#endif
#if !defined(FLIGHT_MODE_1) #if !defined(FLIGHT_MODE_1)
# define FLIGHT_MODE_1 STABILIZE # define FLIGHT_MODE_1 STABILIZE

13
ArduCopterMega/defines.h
View File

@ -17,12 +17,13 @@
#define BARO 1 #define BARO 1
// Frame types // Frame types
#define QUADP_FRAME 0 #define QUAD_FRAME 0
#define QUADX_FRAME 1 #define TRI_FRAME 1
#define TRI_FRAME 2 #define HEXA_FRAME 2
#define HEXAX_FRAME 3 #define Y6_FRAME 3
#define Y6_FRAME 4
#define HEXAP_FRAME 5 #define PLUS_FRAME 0
#define X_FRAME 1
// Internal defines, don't edit and expect things to work // Internal defines, don't edit and expect things to work
// ------------------------------------------------------- // -------------------------------------------------------

39
ArduCopterMega/motors_hexa_x.pde → ArduCopterMega/motors_hexa.pde
View File

@ -1,9 +1,10 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if FRAME_CONFIG == HEXAX_FRAME #if FRAME_CONFIG == HEXA_FRAME
void output_motors_armed() void output_motors_armed()
{ {
int roll_out, pitch_out;
int out_min = g.rc_3.radio_min; int out_min = g.rc_3.radio_min;
// Throttle is 0 to 1000 only // Throttle is 0 to 1000 only
@ -17,18 +18,34 @@ void output_motors_armed()
g.rc_3.calc_pwm(); g.rc_3.calc_pwm();
g.rc_4.calc_pwm(); g.rc_4.calc_pwm();
int roll_out = (float)g.rc_1.pwm_out * .866; if(g.frame_orientation == X_FRAME){
int pitch_out = g.rc_2.pwm_out / 2; roll_out = (float)g.rc_1.pwm_out * .866;
pitch_out = g.rc_2.pwm_out / 2;
//left side //left side
motor_out[CH_2] = g.rc_3.radio_out + g.rc_1.pwm_out; // CCW 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 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 motor_out[CH_8] = g.rc_3.radio_out + roll_out - pitch_out; // CW Back
//right side //right side
motor_out[CH_1] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW 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 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 motor_out[CH_4] = g.rc_3.radio_out - roll_out - pitch_out; // CCW Back
}else{
roll_out = g.rc_1.pwm_out;
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
//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
}
// Yaw // Yaw
motor_out[CH_2] += g.rc_4.pwm_out; // CCW motor_out[CH_2] += g.rc_4.pwm_out; // CCW

114
ArduCopterMega/motors_hexa_p.pde
View File

@ -1,114 +0,0 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if FRAME_CONFIG == HEXAP_FRAME
void output_motors_armed()
{
int out_min = g.rc_3.radio_min;
// Throttle is 0 to 1000 only
g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000);
if(g.rc_3.servo_out > 0)
out_min = g.rc_3.radio_min + 90;
g.rc_1.calc_pwm();
g.rc_2.calc_pwm();
g.rc_3.calc_pwm();
g.rc_4.calc_pwm();
int roll_out = g.rc_1.pwm_out;
int pitch_out = (float)g.rc_2.pwm_out * .5;
//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
//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
// Yaw
motor_out[CH_1] += g.rc_4.pwm_out; // CCW
motor_out[CH_3] += g.rc_4.pwm_out; // CCW
motor_out[CH_8] += 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_7] -= g.rc_4.pwm_out; // CW
// Send commands to motors
if(g.rc_3.servo_out > 0){
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]);
// InstantPWM
APM_RC.Force_Out0_Out1();
APM_RC.Force_Out6_Out7();
APM_RC.Force_Out2_Out3();
}else{
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);
}
}
void output_motors_disarmed()
{
if(g.rc_3.control_in > 0){
// we have pushed up the throttle
// remove safety
motor_auto_armed = true;
}
// fill the motor_out[] array for HIL use
for (unsigned char i = 0; i < 8; i++) {
motor_out[i] = g.rc_3.radio_min;
}
// 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);
}
void output_motor_test()
{
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 50);
delay(1000);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 50);
delay(1000);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 50);
delay(1000);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 50);
delay(1000);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 50);
delay(1000);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 50);
delay(1000);
}
#endif

36
ArduCopterMega/motors_quad_p.pde → ArduCopterMega/motors_quad.pde
View File

@ -1,9 +1,10 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if FRAME_CONFIG == QUADP_FRAME #if FRAME_CONFIG == QUAD_FRAME
void output_motors_armed() void output_motors_armed()
{ {
int roll_out, pitch_out;
int out_min = g.rc_3.radio_min; int out_min = g.rc_3.radio_min;
// Throttle is 0 to 1000 only // Throttle is 0 to 1000 only
@ -17,14 +18,31 @@ void output_motors_armed()
g.rc_3.calc_pwm(); g.rc_3.calc_pwm();
g.rc_4.calc_pwm(); g.rc_4.calc_pwm();
// left if(g.frame_orientation == X_FRAME){
motor_out[CH_1] = g.rc_3.radio_out - g.rc_1.pwm_out; roll_out = g.rc_1.pwm_out * .707;
// right pitch_out = g.rc_2.pwm_out * .707;
motor_out[CH_2] = g.rc_3.radio_out + g.rc_1.pwm_out;
// front // left
motor_out[CH_3] = g.rc_3.radio_out + g.rc_2.pwm_out; motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // FRONT
// back motor_out[CH_2] = g.rc_3.radio_out + roll_out - pitch_out; // BACK
motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out;
// 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
}else{
roll_out = g.rc_1.pwm_out;
pitch_out = g.rc_2.pwm_out;
// left
motor_out[CH_1] = g.rc_3.radio_out - roll_out;
// right
motor_out[CH_2] = g.rc_3.radio_out + roll_out;
// front
motor_out[CH_3] = g.rc_3.radio_out + pitch_out;
// back
motor_out[CH_4] = g.rc_3.radio_out - pitch_out;
}
// Yaw input // Yaw input
motor_out[CH_1] += g.rc_4.pwm_out; // CCW motor_out[CH_1] += g.rc_4.pwm_out; // CCW

99
ArduCopterMega/motors_quad_x.pde
View File

@ -1,99 +0,0 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if FRAME_CONFIG == QUADX_FRAME
void output_motors_armed()
{
int out_min = g.rc_3.radio_min;
// Throttle is 0 to 1000 only
g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000);
if(g.rc_3.servo_out > 0)
out_min = g.rc_3.radio_min + 90;
g.rc_1.calc_pwm();
g.rc_2.calc_pwm();
g.rc_3.calc_pwm();
g.rc_4.calc_pwm();
int roll_out = g.rc_1.pwm_out * .707;
int 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
// 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
// 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
// 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);
// Send commands to motors
if(g.rc_3.servo_out > 0){
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]);
// InstantPWM
APM_RC.Force_Out0_Out1();
APM_RC.Force_Out2_Out3();
}else{
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);
}
}
void output_motors_disarmed()
{
if(g.rc_3.control_in > 0){
// we have pushed up the throttle
// remove safety
motor_auto_armed = true;
}
// fill the motor_out[] array for HIL use
for (unsigned char i = 0; i < 8; i++) {
motor_out[i] = g.rc_3.radio_min;
}
// 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);
}
void output_motor_test()
{
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 50);
delay(1000);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 50);
delay(1000);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 50);
delay(1000);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 50);
delay(1000);
}
#endif

5
ArduCopterMega/navigation.pde
View File

@ -167,9 +167,10 @@ void calc_rate_nav()
// change to rate error // change to rate error
// we want to be going 450cm/s // we want to be going 450cm/s
int error = constrain(WAYPOINT_SPEED - groundspeed, -1000, 1000); int error = constrain(WAYPOINT_SPEED - groundspeed, -1000, 1000);
// Scale response by kP // Scale response by kP
long nav_lat = g.pid_nav_wp.get_pid(error, dTnav, 1.0); nav_lat = g.pid_nav_wp.get_pid(error, dTnav, 1.0);
Serial.printf("dTnav: %ld, gs: %d, err: %d, int: %d, pitch: %ld", dTnav, groundspeed, error, (int)g.pid_nav_wp.get_integrator(), (long)nav_lat);
// limit our output // limit our output
nav_lat = constrain(nav_lat, -4000, 4000); // +- max error nav_lat = constrain(nav_lat, -4000, 4000); // +- max error

8
ArduCopterMega/read_commands.pde
View File

@ -1,4 +1,5 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if 0
#define INPUT_BUF_LEN 40 #define INPUT_BUF_LEN 40
char input_buffer[INPUT_BUF_LEN]; char input_buffer[INPUT_BUF_LEN];
@ -101,14 +102,11 @@ void parseCommand(char *buffer)
break; break;
case 'R': case 'R':
g.stabilize_dampener.set_and_save((float)value / 1000); //g.stabilize_dampener.set_and_save((float)value / 1000);
break; break;
} }
//*/ //*/
} }
} }
#endif

40
ArduCopterMega/setup.pde
View File

@ -4,6 +4,7 @@
static int8_t setup_radio (uint8_t argc, const Menu::arg *argv); static int8_t setup_radio (uint8_t argc, const Menu::arg *argv);
static int8_t setup_motors (uint8_t argc, const Menu::arg *argv); static int8_t setup_motors (uint8_t argc, const Menu::arg *argv);
static int8_t setup_accel (uint8_t argc, const Menu::arg *argv); static int8_t setup_accel (uint8_t argc, const Menu::arg *argv);
static int8_t setup_frame (uint8_t argc, const Menu::arg *argv);
static int8_t setup_factory (uint8_t argc, const Menu::arg *argv); static int8_t setup_factory (uint8_t argc, const Menu::arg *argv);
static int8_t setup_erase (uint8_t argc, const Menu::arg *argv); static int8_t setup_erase (uint8_t argc, const Menu::arg *argv);
static int8_t setup_flightmodes (uint8_t argc, const Menu::arg *argv); static int8_t setup_flightmodes (uint8_t argc, const Menu::arg *argv);
@ -21,6 +22,7 @@ const struct Menu::command setup_menu_commands[] PROGMEM = {
{"erase", setup_erase}, {"erase", setup_erase},
{"reset", setup_factory}, {"reset", setup_factory},
{"radio", setup_radio}, {"radio", setup_radio},
{"frame", setup_frame},
{"motors", setup_motors}, {"motors", setup_motors},
{"esc", setup_esc}, {"esc", setup_esc},
{"level", setup_accel}, {"level", setup_accel},
@ -254,6 +256,22 @@ setup_accel(uint8_t argc, const Menu::arg *argv)
return(0); return(0);
} }
static int8_t
setup_frame(uint8_t argc, const Menu::arg *argv)
{
if (!strcmp_P(argv[1].str, PSTR("x"))) {
} else if (!strcmp_P(argv[1].str, PSTR("p"))) {
}else{
Serial.printf_P(PSTR("\nOptions:[x,p]\n"));
report_frame();
return 0;
}
report_frame();
return 0;
}
static int8_t static int8_t
setup_flightmodes(uint8_t argc, const Menu::arg *argv) setup_flightmodes(uint8_t argc, const Menu::arg *argv)
@ -469,20 +487,21 @@ void report_frame()
Serial.printf_P(PSTR("Frame\n")); Serial.printf_P(PSTR("Frame\n"));
print_divider(); print_divider();
#if FRAME_CONFIG == QUADX_FRAME #if FRAME_CONFIG == QUAD_FRAME
Serial.printf_P(PSTR("X frame\n")); Serial.printf_P(PSTR("Quad frame\n"));
#elif FRAME_CONFIG == QUADP_FRAME
Serial.printf_P(PSTR("Plus frame\n"));
#elif FRAME_CONFIG == TRI_FRAME #elif FRAME_CONFIG == TRI_FRAME
Serial.printf_P(PSTR("TRI frame\n")); Serial.printf_P(PSTR("TRI frame\n"));
#elif FRAME_CONFIG == HEXAX_FRAME #elif FRAME_CONFIG == HEXA_FRAME
Serial.printf_P(PSTR("HexaX frame\n")); Serial.printf_P(PSTR("Hexa frame\n"));
#elif FRAME_CONFIG == HEXAP_FRAME
Serial.printf_P(PSTR("HexaP frame\n"));
#elif FRAME_CONFIG == Y6_FRAME #elif FRAME_CONFIG == Y6_FRAME
Serial.printf_P(PSTR("Y6 frame\n")); Serial.printf_P(PSTR("Y6 frame\n"));
#endif #endif
if(g.frame_orientation == X_FRAME)
Serial.printf_P(PSTR("X mode\n"));
else if(g.frame_orientation == PLUS_FRAME)
Serial.printf_P(PSTR("+ mode\n"));
print_blanks(2); print_blanks(2);
} }
@ -516,8 +535,8 @@ void report_gains()
Serial.printf_P(PSTR("yaw:\n")); Serial.printf_P(PSTR("yaw:\n"));
print_PID(&g.pid_yaw); print_PID(&g.pid_yaw);
Serial.printf_P(PSTR("Stab D: %4.3f\n"), (float)g.stabilize_dampener); //Serial.printf_P(PSTR("Stab D: %4.3f\n"), (float)g.stabilize_dampener);
Serial.printf_P(PSTR("Yaw D: %4.3f\n\n"), (float)g.hold_yaw_dampener); //Serial.printf_P(PSTR("Yaw D: %4.3f\n\n"), (float)g.hold_yaw_dampener);
// Nav // Nav
Serial.printf_P(PSTR("Nav:\nlat:\n")); Serial.printf_P(PSTR("Nav:\nlat:\n"));
@ -622,7 +641,6 @@ print_PID(PID * pid)
void void
print_radio_values() print_radio_values()
{ {
Serial.printf_P(PSTR("CH1: %d | %d\n"), (int)g.rc_1.radio_min, (int)g.rc_1.radio_max); Serial.printf_P(PSTR("CH1: %d | %d\n"), (int)g.rc_1.radio_min, (int)g.rc_1.radio_max);
Serial.printf_P(PSTR("CH2: %d | %d\n"), (int)g.rc_2.radio_min, (int)g.rc_2.radio_max); Serial.printf_P(PSTR("CH2: %d | %d\n"), (int)g.rc_2.radio_min, (int)g.rc_2.radio_max);
Serial.printf_P(PSTR("CH3: %d | %d\n"), (int)g.rc_3.radio_min, (int)g.rc_3.radio_max); Serial.printf_P(PSTR("CH3: %d | %d\n"), (int)g.rc_3.radio_min, (int)g.rc_3.radio_max);

26
ArduCopterMega/system.pde
View File

@ -37,7 +37,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
}; };
// Create the top-level menu object. // Create the top-level menu object.
MENU(main_menu, "AC 2.0.5 Beta", main_menu_commands); MENU(main_menu, "AC 2.0.6 Beta", main_menu_commands);
void init_ardupilot() void init_ardupilot()
{ {
@ -189,6 +189,13 @@ void init_ardupilot()
pinMode(PUSHBUTTON_PIN, INPUT); // unused pinMode(PUSHBUTTON_PIN, INPUT); // unused
DDRL |= B00000100; // Set Port L, pin 2 to output for the relay DDRL |= B00000100; // Set Port L, pin 2 to output for the relay
#if MOTOR_LEDS == 1
pinMode(FR_LED, OUTPUT); // GPS status LED
pinMode(RE_LED, OUTPUT); // GPS status LED
pinMode(RI_LED, OUTPUT); // GPS status LED
pinMode(LE_LED, OUTPUT); // GPS status LED
#endif
// Logging: // Logging:
// -------- // --------
DataFlash.Init(); // DataFlash log initialization DataFlash.Init(); // DataFlash log initialization
@ -383,6 +390,23 @@ void set_failsafe(boolean mode)
} }
} }
#if MOTOR_LEDS == 1
void update_motor_light(void)
{
// blink rear
static bool blink;
if (blink){
blink = false;
digitalWrite(RE_LED, LOW);
}else{
blink = true;
digitalWrite(RE_LED, HIGH);
}
}
#endif
void update_GPS_light(void) void update_GPS_light(void)
{ {
// GPS LED on if we have a fix or Blink GPS LED if we are receiving data // GPS LED on if we have a fix or Blink GPS LED if we are receiving data

30
ArduCopterMega/test.pde
View File

@ -13,6 +13,7 @@ static int8_t test_imu(uint8_t argc, const Menu::arg *argv);
//static int8_t test_dcm(uint8_t argc, const Menu::arg *argv); //static int8_t test_dcm(uint8_t argc, const Menu::arg *argv);
//static int8_t test_omega(uint8_t argc, const Menu::arg *argv); //static int8_t test_omega(uint8_t argc, const Menu::arg *argv);
static int8_t test_battery(uint8_t argc, const Menu::arg *argv); static int8_t test_battery(uint8_t argc, const Menu::arg *argv);
//static int8_t test_wp_nav(uint8_t argc, const Menu::arg *argv);
static int8_t test_tuning(uint8_t argc, const Menu::arg *argv); static int8_t test_tuning(uint8_t argc, const Menu::arg *argv);
static int8_t test_current(uint8_t argc, const Menu::arg *argv); static int8_t test_current(uint8_t argc, const Menu::arg *argv);
static int8_t test_relay(uint8_t argc, const Menu::arg *argv); static int8_t test_relay(uint8_t argc, const Menu::arg *argv);
@ -71,6 +72,7 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
{"eedump", test_eedump}, {"eedump", test_eedump},
{"rawgps", test_rawgps}, {"rawgps", test_rawgps},
{"mission", test_mission}, {"mission", test_mission},
// {"wp", test_wp_nav},
}; };
// A Macro to create the Menu // A Macro to create the Menu
@ -197,6 +199,34 @@ test_radio(uint8_t argc, const Menu::arg *argv)
} }
} }
/*
static int8_t
test_wp_nav(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
g.rc_6.set_range(0,900);
g.rc_4.set_angle(9000);
dTnav = 200;
//dTnav: 0, gs: 305, err: 145, int: 0, pitch: 28508160 gps_GC: 0, gps_GS: 305
while(1){
delay(20);
read_radio();
target_bearing = 0;
g_gps->ground_course = g.rc_4.control_in;
g_gps->ground_speed = g.rc_6.control_in;
calc_rate_nav();
Serial.printf(" gps_GC: %ld, gps_GS: %d\n", g_gps->ground_course, g_gps->ground_speed);
if(Serial.available() > 0){
return (0);
}
}
}
*/
static int8_t static int8_t
test_failsafe(uint8_t argc, const Menu::arg *argv) test_failsafe(uint8_t argc, const Menu::arg *argv)