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

Moved motors to individual files. 

updated motor setup test to be sequencial pulses of the motors in CW order.
Fixed Mission scripting logic
fixed Free yaw error in neutral throttle
fixed D term issue with Baro hold - was too high
incremented firmware revision, removed frame var
removed setup show from startup
removed unused EEPROM functions
fixed broken demo mission
fixed non working loiter with delay



git-svn-id: https://arducopter.googlecode.com/svn/trunk@2275 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-05-15 02:02:09 +00:00
parent fda05ca4d0
commit 8f17f95cbc
22 changed files with 822 additions and 1068 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
ArduCopterMega/ArduCopterMega.pde
View File

@ -226,7 +226,7 @@ Vector3f omega;
boolean failsafe; // did our throttle dip below the failsafe value?
boolean ch3_failsafe;
boolean motor_armed;
boolean motor_auto_safe;
boolean motor_auto_armed; // if true,
// PIDs
// ----
@ -596,10 +596,11 @@ void medium_loop()
// ------------------------------------------------------
navigate();
// control mode specific updates to nav_bearing
// --------------------------------------------
update_navigation();
}
// control mode specific updates to nav_bearing
// --------------------------------------------
update_navigation();
break;
// command processing
@ -623,10 +624,10 @@ void medium_loop()
// perform next command
// --------------------
if(control_mode == AUTO || control_mode == GCS_AUTO){
if(home_is_set){
if(control_mode == AUTO){
//if(home_is_set){
update_commands();
}
//}
}
break;
@ -745,11 +746,11 @@ void fifty_hz_loop()
// XXX this should be absorbed into the above,
// or be a "GCS fast loop" interface
// Hack - had to move to 50hz loop to test a theory
if(g.frame_type == TRI_FRAME){
#if FRAME_CONFIG == TRI_FRAME
// Hack - had to move to 50hz loop to test a theory
// servo Yaw
APM_RC.OutputCh(CH_7, g.rc_4.radio_out);
}
#endif
}
@ -1137,7 +1138,7 @@ void update_navigation()
// ------------------------------------------------------------------------
// distance and bearing calcs only
if(control_mode == AUTO || control_mode == GCS_AUTO){
if(control_mode == AUTO){
verify_commands();
if(wp_control == LOITER_MODE){
@ -1161,6 +1162,9 @@ void update_navigation()
// this tracks a location so the copter is always pointing towards it.
if(yaw_tracking == MAV_ROI_LOCATION){
nav_yaw = get_bearing(&current_loc, &target_WP);
}else if(yaw_tracking == MAV_ROI_WPNEXT){
nav_yaw = target_bearing;
}
}else{
@ -1300,7 +1304,6 @@ void tuning(){
#if CHANNEL_6_TUNING == CH6_STABLIZE_KP
g.pid_stabilize_roll.kP((float)g.rc_6.control_in / 1000.0);
g.pid_stabilize_pitch.kP((float)g.rc_6.control_in / 1000.0);
init_pids();
#elif CHANNEL_6_TUNING == CH6_STABLIZE_KD
// uncomment me out to try in flight dampening, 0 = unflyable, .2 = unfun, .13 worked for me.

15
ArduCopterMega/Attitude.pde
View File

@ -1,15 +1,5 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
void
init_pids()
{
// create limits to how much dampening we'll allow
// this creates symmetry with the P gain value preventing oscillations
max_stabilize_dampener = g.pid_stabilize_roll.kP() * 2500; // = 0.6 * 2500 = 1500 or 15°
//max_yaw_dampener = g.pid_yaw.kP() * 6000; // = .35 * 6000 = 2100
}
void
control_nav_mixer()
{
@ -67,7 +57,6 @@ output_stabilize_roll()
// 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 -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500 based on kP
g.rc_1.servo_out -= dampener;
g.rc_1.servo_out = min(g.rc_1.servo_out, 2500);
@ -100,7 +89,6 @@ output_stabilize_pitch()
// 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 -= constrain(dampener, -max_stabilize_dampener, max_stabilize_dampener); // limit to 1500 based on kP
g.rc_2.servo_out -= dampener;
g.rc_2.servo_out = min(g.rc_2.servo_out, 2500);
@ -224,9 +212,6 @@ void output_manual_yaw()
void auto_yaw()
{
if(yaw_tracking == MAV_ROI_LOCATION){
nav_yaw = target_bearing;
}
output_yaw_with_hold(true); // hold yaw
}

9
ArduCopterMega/Log.pde
View File

@ -117,10 +117,11 @@ dump_log(uint8_t argc, const Menu::arg *argv)
static int8_t
erase_logs(uint8_t argc, const Menu::arg *argv)
{
for(int i = 10 ; i > 0; i--) {
Serial.printf_P(PSTR("ATTENTION - Erasing log in %d seconds.\n"), i);
delay(1000);
}
//for(int i = 10 ; i > 0; i--) {
// Serial.printf_P(PSTR("ATTENTION - Erasing log in %d seconds.\n"), i);
// delay(1000);
//}
Serial.printf_P(PSTR("\nErasing log...\n"));
for(int j = 1; j < 4096; j++)
DataFlash.PageErase(j);

19
ArduCopterMega/Parameters.h
View File

@ -17,7 +17,7 @@ public:
// The increment will prevent old parameters from being used incorrectly
// by newer code.
//
static const uint16_t k_format_version = 6;
static const uint16_t k_format_version = 7;
//
// Parameter identities.
@ -48,8 +48,7 @@ public:
// Misc
//
k_param_log_bitmask,
k_param_frame_type,
// 110: Telemetry control
//
k_param_streamrates_port0 = 110,
@ -94,7 +93,6 @@ public:
k_param_throttle_fs_action,
k_param_throttle_fs_value,
k_param_throttle_cruise,
k_param_flight_mode_channel,
k_param_flight_modes,
k_param_esc_calibrate,
@ -162,7 +160,6 @@ public:
// Flight modes
//
AP_Int8 flight_mode_channel;
AP_VarA<uint8_t,6> flight_modes;
// Radio settings
@ -180,7 +177,6 @@ public:
AP_Int16 ground_temperature;
AP_Int16 ground_pressure;
AP_Int16 RTL_altitude;
AP_Int8 frame_type;
AP_Int8 sonar_enabled;
AP_Int8 battery_monitoring; // 0=disabled, 1=3 cell lipo, 2=4 cell lipo, 3=total voltage only, 4=total voltage and current
@ -226,8 +222,6 @@ public:
crosstrack_gain (XTRACK_GAIN * 100, k_param_crosstrack_gain, PSTR("XTRK_GAIN")),
crosstrack_entry_angle (XTRACK_ENTRY_ANGLE * 100, k_param_crosstrack_entry_angle, PSTR("XTRACK_ANGLE")),
frame_type (FRAME_CONFIG, k_param_frame_type, PSTR("FRAME_CONFIG")),
sonar_enabled (DISABLED, k_param_sonar, PSTR("SONAR_ENABLE")),
battery_monitoring (DISABLED, k_param_battery_monitoring, PSTR("BATT_MONITOR")),
pack_capacity (HIGH_DISCHARGE, k_param_pack_capacity, PSTR("BATT_CAPACITY")),
@ -247,7 +241,6 @@ public:
throttle_fs_value (THROTTLE_FS_VALUE, k_param_throttle_fs_value, PSTR("THR_FS_VALUE")),
throttle_cruise (100, k_param_throttle_cruise, PSTR("TRIM_THROTTLE")),
flight_mode_channel (FLIGHT_MODE_CHANNEL+1, k_param_flight_mode_channel, PSTR("FLT_MODE_CH")),
flight_modes (k_param_flight_modes, PSTR("FLIGHT_MODES")),
pitch_max (PITCH_MAX * 100, k_param_pitch_max, PSTR("PITCH_MAX")),
@ -256,6 +249,7 @@ public:
ground_temperature (0, k_param_ground_temperature, PSTR("GND_TEMP")),
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")),
esc_calibrate (0, k_param_esc_calibrate, PSTR("ESC")),
// RC channel group key name
//----------------------------------------------------------------------
@ -267,11 +261,8 @@ public:
rc_6 (k_param_rc_6, PSTR("RC6_")),
rc_7 (k_param_rc_7, PSTR("RC7_")),
rc_8 (k_param_rc_8, PSTR("RC8_")),
rc_camera_pitch (k_param_rc_9, PSTR("RC9_")),
rc_camera_roll (k_param_rc_10, PSTR("RC10_")),
esc_calibrate (0, k_param_esc_calibrate, PSTR("ESC")),
rc_camera_pitch (k_param_rc_9, NULL),
rc_camera_roll (k_param_rc_10, NULL),
// PID controller group key name initial P initial I initial D initial imax
//--------------------------------------------------------------------------------------------------------------------------------------------------------------------

1
ArduCopterMega/command description.txt
View File

@ -27,6 +27,7 @@ Command ID Name Parameter 1 Altitude Latitude Longitude
0x14 / 20 MAV_CMD_NAV_RETURN_TO_LAUNCH - - - -
0x15 / 21 MAV_CMD_NAV_LAND - - - -
0x16 / 22 MAV_CMD_NAV_TAKEOFF - altitude - -
0x19 / 22 MAV_CMD_NAV_LOITER_TIME - altitude - -
NOTE: for command 0x16 the value takeoff pitch specifies the minimum pitch for the case with airspeed sensor and the target pitch for the case without.

6
ArduCopterMega/commands.pde
View File

@ -41,6 +41,8 @@ struct Location get_command_with_index(int i)
// Find out proper location in memory by using the start_byte position + the index
// --------------------------------------------------------------------------------
if (i > g.waypoint_total) {
Serial.println("XCD");
// we do not have a valid command to load
// return a WP with a "Blank" id
temp.id = CMD_BLANK;
@ -49,6 +51,7 @@ struct Location get_command_with_index(int i)
return temp;
}else{
Serial.println("LD");
// we can load a command, we don't process it yet
// read WP position
long mem = (WP_START_BYTE) + (i * WP_SIZE);
@ -73,8 +76,9 @@ struct Location get_command_with_index(int i)
// Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative
if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & WP_OPTION_ALT_RELATIVE){
temp.alt += home.alt;
//temp.alt += home.alt;
}
Serial.println("ADD ALT");
if(temp.options & WP_OPTION_RELATIVE){
// If were relative, just offset from home

35
ArduCopterMega/commands_logic.pde
View File

@ -27,7 +27,7 @@ void handle_process_must()
//do_loiter_turns();
break;
case MAV_CMD_NAV_LOITER_TIME:
case MAV_CMD_NAV_LOITER_TIME: // 19
do_loiter_time();
break;
@ -104,7 +104,7 @@ void handle_process_now()
void handle_no_commands()
{
// we don't want to RTL. Maybe this will change in the future. RTL is kinda dangerous.
// we don't want to RTL yet. Maybe this will change in the future. RTL is kinda dangerous.
// use landing commands
/*
switch (control_mode){
@ -114,6 +114,7 @@ void handle_no_commands()
}
return;
*/
Serial.println("Handle No CMDs");
}
/********************************************************************************/
@ -228,8 +229,10 @@ void do_takeoff()
// next_command.alt is a relative altitude!!!
if (next_command.options & WP_OPTION_ALT_RELATIVE) {
temp.alt = next_command.alt + home.alt;
//Serial.printf("rel alt: %ld",temp.alt);
} else {
temp.alt = next_command.alt;
//Serial.printf("abs alt: %ld",temp.alt);
}
takeoff_complete = false; // set flag to use g_gps ground course during TO. IMU will be doing yaw drift correction
@ -313,17 +316,13 @@ void do_loiter_turns()
void do_loiter_time()
{
/*
///*
wp_control = LOITER_MODE;
if(next_command.lat == 0)
set_next_WP(&current_loc);
else
set_next_WP(&next_command);
set_next_WP(&current_loc);
loiter_time = millis();
loiter_time_max = next_command.p1 * 1000; // units are (seconds)
Serial.printf("dlt %ld, max %ld\n",loiter_time, loiter_time_max);
*/
//*/
}
/********************************************************************************/
@ -332,11 +331,13 @@ void do_loiter_time()
bool verify_takeoff()
{
//Serial.printf("vt c_alt:%ld, n_alt:%ld\n", current_loc.alt, next_WP.alt);
// wait until we are ready!
if(g.rc_3.control_in == 0)
if(g.rc_3.control_in == 0){
return false;
}
Serial.printf("vt c_alt:%ld, n_alt:%ld\n", current_loc.alt, next_WP.alt);
if (current_loc.alt > next_WP.alt){
//Serial.println("Y");
@ -353,7 +354,7 @@ bool verify_takeoff()
bool verify_land()
{
// land at 1 meter per second
next_WP.alt = original_alt - ((millis() - land_start) / 5); // condition_value = our initial
next_WP.alt = original_alt - ((millis() - land_start) / 20); // condition_value = our initial
velocity_land = ((old_alt - current_loc.alt) *.2) + (velocity_land * .8);
old_alt = current_loc.alt;
@ -415,8 +416,8 @@ bool verify_nav_wp()
}
}
//if(wp_verify_byte >= 7){
if(wp_verify_byte & NAV_LOCATION){
if(wp_verify_byte >= 7){
//if(wp_verify_byte & NAV_LOCATION){
char message[30];
sprintf(message,"Reached Command #%i",command_must_index);
gcs.send_text(SEVERITY_LOW,message);
@ -437,7 +438,7 @@ bool verify_loiter_time()
//Serial.printf("vlt %ld\n",(millis() - loiter_time));
if ((millis() - loiter_time) > loiter_time_max) { // scale loiter_time_max from (sec*10) to milliseconds
gcs.send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete"));
//gcs.send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete"));
//Serial.println("vlt done");
return true;
}
@ -629,8 +630,8 @@ void do_jump()
struct Location temp;
if(next_command.lat > 0) {
command_must_index = 0;
command_may_index = 0;
command_must_index = NO_COMMAND;
command_may_index = NO_COMMAND;
temp = get_command_with_index(g.waypoint_index);
temp.lat = next_command.lat - 1; // Decrement repeat counter

93
ArduCopterMega/commands_process.pde
View File

@ -19,8 +19,10 @@ void change_command(uint8_t index)
// called by 10 Hz Medium loop
// ---------------------------
void update_commands(void)
void update_commands2(void)
{
//Serial.println("Upd CMDs");
// This function loads commands into three buffers
// when a new command is loaded, it is processed with process_XXX()
@ -32,34 +34,82 @@ void update_commands(void)
// fetch next command if the next command queue is empty
// -----------------------------------------------------
next_command = get_command_with_index(g.waypoint_index + 1);
Serial.printf("next CMD %d\n", next_command.id);
if(next_command.id == NO_COMMAND){
// if no commands were available from EEPROM
// And we have no nav commands
// --------------------------------------------
if (command_must_ID == NO_COMMAND)
if (command_must_ID == NO_COMMAND){
gcs.send_text_P(SEVERITY_LOW,PSTR("out of commands!"));
handle_no_commands();
gcs.send_text_P(SEVERITY_LOW,PSTR("out of commands!"));
}
} else {
// A command was loaded from EEPROM
// --------------------------------------------
// debug by outputing the Waypoint loaded
print_wp(&next_command, g.waypoint_index + 1);
// Set our current mission index + 1;
increment_WP_index();
Serial.printf("loaded cmd %d\n" , g.waypoint_index.get());
// debug by outputing the Waypoint loaded
print_wp(&next_command, g.waypoint_index);
// act on our new command
if (process_next_command()){
// invalidate command queue so a new one is loaded
// -----------------------------------------------
clear_command_queue();
}
}
}
// called by 10 Hz Medium loop
// ---------------------------
void update_commands(void)
{
//Serial.println("Upd CMDs");
// fill command queue with a new command if available
if(next_command.id == NO_COMMAND){
// fetch next command if the next command queue is empty
// -----------------------------------------------------
if (g.waypoint_index < g.waypoint_total) {
// only if we have a cmd stored in EEPROM
next_command = get_command_with_index(g.waypoint_index + 1);
Serial.printf("queue CMD %d\n", next_command.id);
}
}
// Are we out of must commands and the queue is empty?
if(next_command.id == NO_COMMAND && command_must_index == NO_COMMAND){
// if no commands were available from EEPROM
// And we have no nav commands
// --------------------------------------------
if (command_must_ID == NO_COMMAND){
gcs.send_text_P(SEVERITY_LOW,PSTR("out of commands!"));
handle_no_commands();
}
}
// check to see if we need to act on our command queue
if (process_next_command()){
Serial.printf("did PNC: %d\n", next_command.id);
// We acted on the queue - let's debug that
// ----------------------------------------
print_wp(&next_command, g.waypoint_index);
// invalidate command queue so a new one is loaded
// -----------------------------------------------
clear_command_queue();
// make sure we load the next command index
// ----------------------------------------
increment_WP_index();
}
}
@ -67,10 +117,14 @@ void update_commands(void)
void verify_commands(void)
{
if(verify_must()){
Serial.printf("verified must cmd %d\n" , command_must_index);
command_must_index = NO_COMMAND;
}else{
Serial.printf("verified must false %d\n" , command_must_index);
}
if(verify_may()){
Serial.printf("verified may cmd %d\n" , command_may_index);
command_may_index = NO_COMMAND;
command_may_ID = NO_COMMAND;
}
@ -85,13 +139,7 @@ process_next_command()
if (next_command.id < MAV_CMD_NAV_LAST ){
// we remember the index of our mission here
command_must_index = g.waypoint_index;
// dubugging output
SendDebug("MSG <pnc> new c_must_id ");
SendDebug(next_command.id,DEC);
SendDebug(" index:");
SendDebugln(command_must_index,DEC);
command_must_index = g.waypoint_index + 1;
// Save CMD to Log
if (g.log_bitmask & MASK_LOG_CMD)
@ -109,9 +157,10 @@ process_next_command()
if (next_command.id > MAV_CMD_NAV_LAST && next_command.id < MAV_CMD_CONDITION_LAST ){
// we remember the index of our mission here
command_may_index = g.waypoint_index;
SendDebug("MSG <pnc> new may ");
SendDebugln(next_command.id,DEC);
command_may_index = g.waypoint_index + 1;
//SendDebug("MSG <pnc> new may ");
//SendDebugln(next_command.id,DEC);
//Serial.print("new command_may_index ");
//Serial.println(command_may_index,DEC);
@ -126,8 +175,8 @@ process_next_command()
// these are Do/Now commands
// ---------------------------
if (next_command.id > MAV_CMD_CONDITION_LAST){
SendDebug("MSG <pnc> new now ");
SendDebugln(next_command.id,DEC);
//SendDebug("MSG <pnc> new now ");
//SendDebugln(next_command.id,DEC);
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(g.waypoint_index, &next_command);

4
ArduCopterMega/config.h
View File

@ -47,7 +47,7 @@
// FRAME_CONFIG
//
#ifndef FRAME_CONFIG
# define FRAME_CONFIG PLUS_FRAME
# define FRAME_CONFIG QUADP_FRAME
#endif
//////////////////////////////////////////////////////////////////////////////
@ -412,7 +412,7 @@
# define THROTTLE_BARO_I 0.1
#endif
#ifndef THROTTLE_BARO_D
# define THROTTLE_BARO_D 0.01
# define THROTTLE_BARO_D 0.03
#endif
#ifndef THROTTLE_BARO_IMAX
# define THROTTLE_BARO_IMAX 50

18
ArduCopterMega/control_modes.pde
View File

@ -19,17 +19,7 @@ void read_control_switch()
}
byte readSwitch(void){
#if FLIGHT_MODE_CHANNEL == CH_5
int pulsewidth = g.rc_5.radio_in; // default for Arducopter
#elif FLIGHT_MODE_CHANNEL == CH_6
int pulsewidth = g.rc_6.radio_in; //
#elif FLIGHT_MODE_CHANNEL == CH_7
int pulsewidth = g.rc_7.radio_in; //
#elif FLIGHT_MODE_CHANNEL == CH_8
int pulsewidth = g.rc_8.radio_in; // default for Ardupilot. Don't use for Arducopter! it has a hardware failsafe mux!
#else
# error Must define FLIGHT_MODE_CHANNEL as CH_5 - CH_8
#endif
int pulsewidth = g.rc_5.radio_in; // default for Arducopter
if (pulsewidth > 1230 && pulsewidth <= 1360) return 1;
if (pulsewidth > 1360 && pulsewidth <= 1490) return 2;
@ -83,12 +73,6 @@ void read_trim_switch()
g.throttle_cruise.set_and_save(g.rc_3.control_in);
//Serial.printf("tnom %d\n", g.throttle_cruise.get());
}
// this is a test for Max's tri-copter
if(g.frame_type == TRI_FRAME){
//g.rc_4.trim(); // yaw
//g.rc_4.save_eeprom();
}
}
trim_flag = false;
}

4
ArduCopterMega/defines.h
View File

@ -17,8 +17,8 @@
#define BARO 1
// Frame types
#define PLUS_FRAME 0
#define X_FRAME 1
#define QUADP_FRAME 0
#define QUADX_FRAME 1
#define TRI_FRAME 2
#define HEXAX_FRAME 3
#define Y6_FRAME 4

257
ArduCopterMega/motors.pde
View File

@ -40,193 +40,30 @@ void arm_motors()
}
/*****************************************
* Set the flight control servos based on the current calculated values
*****************************************/
void
set_servos_4()
{
static byte num;
int out_min;
// Quadcopter mix
if (motor_armed == true && motor_auto_safe == true) {
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;
//Serial.printf("out: %d %d %d %d\t\t", g.rc_1.servo_out, g.rc_2.servo_out, g.rc_3.servo_out, g.rc_4.servo_out);
if (motor_armed == true && motor_auto_armed == true) {
// creates the radio_out and pwm_out values
g.rc_1.calc_pwm();
g.rc_2.calc_pwm();
g.rc_3.calc_pwm();
g.rc_4.calc_pwm();
output_motors_armed();
} else{
output_motors_disarmed();
}
}
// limit Yaw control so we don't clip and loose altitude
// this is only a partial solution.
// g.rc_4.pwm_out = min(g.rc_4.pwm_out, (g.rc_3.radio_out - out_min));
//Serial.printf("out: %d %d %d %d\n", g.rc_1.radio_out, g.rc_2.radio_out, g.rc_3.radio_out, g.rc_4.radio_out);
//Serial.printf("yaw: %d ", g.rc_4.radio_out);
if(g.frame_type == PLUS_FRAME){
//Serial.println("P_FRAME");
motor_out[CH_1] = g.rc_3.radio_out - g.rc_1.pwm_out;
motor_out[CH_2] = g.rc_3.radio_out + g.rc_1.pwm_out;
motor_out[CH_3] = g.rc_3.radio_out + g.rc_2.pwm_out;
motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out;
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
}else if(g.frame_type == X_FRAME){
//Serial.println("X_FRAME");
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
//Serial.printf("\tb4: %d %d %d %d ", motor_out[CH_1], motor_out[CH_2], motor_out[CH_3], motor_out[CH_4]);
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
//Serial.printf("\tl8r: %d %d %d %d\n", motor_out[CH_1], motor_out[CH_2], motor_out[CH_3], motor_out[CH_4]);
}else if(g.frame_type == TRI_FRAME){
//Serial.println("TRI_FRAME");
// Tri-copter power distribution
int roll_out = (float)g.rc_1.pwm_out * .866;
int pitch_out = g.rc_2.pwm_out / 2;
//left front
motor_out[CH_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;
// rear
motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out;
// this is a compensation for the angle of the yaw motor. Its linear, but should work ok.
//motor_out[CH_4] += (float)(abs(g.rc_4.control_in)) * .013;
/*****************************************
* Set the flight control servos based on the current calculated values
*****************************************/
}else if (g.frame_type == HEXAX_FRAME) {
//Serial.println("6_FRAME");
int roll_out = (float)g.rc_1.pwm_out * .866;
int pitch_out = g.rc_2.pwm_out / 2;
//left side
motor_out[CH_2] = g.rc_3.radio_out + g.rc_1.pwm_out; // CCW
motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW
motor_out[CH_8] = g.rc_3.radio_out + roll_out - pitch_out; // CW
//right side
motor_out[CH_1] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW
motor_out[CH_7] = g.rc_3.radio_out - roll_out + pitch_out; // CCW
motor_out[CH_4] = g.rc_3.radio_out - roll_out - pitch_out; // CCW
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[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
}else if (g.frame_type == HEXAP_FRAME) {
//Serial.println("HEXAP_FRAME");
int roll_out = g.rc_1.pwm_out;
int pitch_out = (float)g.rc_2.pwm_out * .5;
//Back side
motor_out[CH_8] = g.rc_3.radio_out - g.rc_2.pwm_out; // CCW BACK
motor_out[CH_1] = g.rc_3.radio_out + roll_out - pitch_out; // CW, BACK LEFT
motor_out[CH_3] = g.rc_3.radio_out - roll_out - pitch_out; // CW BACK RIGHT
//Front side
motor_out[CH_7] = g.rc_3.radio_out + g.rc_2.pwm_out; // CW FRONT
motor_out[CH_2] = 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[CH_8] += g.rc_4.pwm_out; // CCW
motor_out[CH_2] += g.rc_4.pwm_out; // CCW
motor_out[CH_4] += g.rc_4.pwm_out; // CCW
motor_out[CH_1] -= g.rc_4.pwm_out; // CW
motor_out[CH_7] -= g.rc_4.pwm_out; // CW
motor_out[CH_3] -= g.rc_4.pwm_out; // CW
}else if (g.frame_type == Y6_FRAME) {
//Serial.println("Y6_FRAME");
int roll_out = (float)g.rc_1.pwm_out * .866;
int pitch_out = g.rc_2.pwm_out / 2;
//left
motor_out[CH_2] = ((g.rc_3.radio_out * Y6_scaling) + roll_out + pitch_out); // CCW TOP
motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW
//right
motor_out[CH_7] = ((g.rc_3.radio_out * Y6_scaling) - roll_out + pitch_out); // CCW TOP
motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out; // CW
//back
motor_out[CH_8] = ((g.rc_3.radio_out * Y6_scaling) - g.rc_2.pwm_out); // CCW TOP
motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW
//yaw
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[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
}else{
//Serial.print("frame error");
}
// limit output so motors don't stop
motor_out[CH_1] = constrain(motor_out[CH_1], out_min, g.rc_3.radio_max.get());
motor_out[CH_2] = constrain(motor_out[CH_2], out_min, g.rc_3.radio_max.get());
motor_out[CH_3] = constrain(motor_out[CH_3], out_min, g.rc_3.radio_max.get());
motor_out[CH_4] = constrain(motor_out[CH_4], out_min, g.rc_3.radio_max.get());
if ((g.frame_type == HEXAX_FRAME) || (g.frame_type == HEXAP_FRAME) || (g.frame_type == Y6_FRAME)) {
motor_out[CH_7] = constrain(motor_out[CH_7], out_min, g.rc_3.radio_max.get());
motor_out[CH_8] = constrain(motor_out[CH_8], out_min, g.rc_3.radio_max.get());
}
if (num++ > 25){
num = 0;
//if (num++ > 25){
// num = 0;
//Serial.print("kP: ");
//Serial.println(g.pid_stabilize_roll.kP(),3);
@ -304,74 +141,4 @@ set_servos_4()
nav_throttle,
angle_boost());
*/
}
// 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();
if ((g.frame_type == HEXAX_FRAME) || (g.frame_type == Y6_FRAME)) {
APM_RC.OutputCh(CH_7, motor_out[CH_7]);
APM_RC.OutputCh(CH_8, motor_out[CH_8]);
APM_RC.Force_Out6_Out7();
}
}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);
// InstantPWM
APM_RC.Force_Out0_Out1();
APM_RC.Force_Out2_Out3();
if ((g.frame_type == HEXAX_FRAME) || (g.frame_type == Y6_FRAME)) {
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
APM_RC.Force_Out6_Out7();
}
}
}else{
// our motor is unarmed, we're on the ground
//reset_I();
if(g.rc_3.control_in > 0){
// we have pushed up the throttle
// remove safety
motor_auto_safe = 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, 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]);
if ((g.frame_type == HEXAX_FRAME) || (g.frame_type == Y6_FRAME)){
APM_RC.OutputCh(CH_7, motor_out[CH_7]);
APM_RC.OutputCh(CH_8, motor_out[CH_8]);
}
// reset I terms of PID controls
//reset_I();
// Initialize yaw command to actual yaw when throttle is down...
g.rc_4.control_in = ToDeg(dcm.yaw);
}
}
//}

115
ArduCopterMega/motors_hexa_p.pde Normal file
View File

@ -0,0 +1,115 @@
/// -*- 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_8] = g.rc_3.radio_out - g.rc_2.pwm_out; // CCW BACK
motor_out[CH_1] = g.rc_3.radio_out + roll_out - pitch_out; // CW, BACK LEFT
motor_out[CH_3] = g.rc_3.radio_out - roll_out - pitch_out; // CW BACK RIGHT
//Front side
motor_out[CH_7] = g.rc_3.radio_out + g.rc_2.pwm_out; // CW FRONT
motor_out[CH_2] = 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_8] += g.rc_4.pwm_out; // CCW
motor_out[CH_2] += g.rc_4.pwm_out; // CCW
motor_out[CH_4] += g.rc_4.pwm_out; // CCW
motor_out[CH_1] -= g.rc_4.pwm_out; // CW
motor_out[CH_7] -= g.rc_4.pwm_out; // CW
motor_out[CH_3] -= 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_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 50);
delay(1000);
APM_RC.OutputCh(CH_7, 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_3, g.rc_3.radio_min + 50);
delay(1000);
APM_RC.OutputCh(CH_3, 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_1, g.rc_3.radio_min + 50);
delay(1000);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 50);
delay(1000);
}
#endif

114
ArduCopterMega/motors_hexa_x.pde Normal file
View File

@ -0,0 +1,114 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if FRAME_CONFIG == HEXAX_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 = (float)g.rc_1.pwm_out * .866;
int pitch_out = g.rc_2.pwm_out / 2;
//left side
motor_out[CH_2] = g.rc_3.radio_out + g.rc_1.pwm_out; // CCW
motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW
motor_out[CH_8] = g.rc_3.radio_out + roll_out - pitch_out; // CW
//right side
motor_out[CH_1] = g.rc_3.radio_out - g.rc_1.pwm_out; // CW
motor_out[CH_7] = g.rc_3.radio_out - roll_out + pitch_out; // CCW
motor_out[CH_4] = g.rc_3.radio_out - roll_out - pitch_out; // CCW
// 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[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
// 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_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);
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);
}
#endif

97
ArduCopterMega/motors_quad_p.pde Normal file
View File

@ -0,0 +1,97 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if FRAME_CONFIG == QUADP_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();
// left
motor_out[CH_1] = g.rc_3.radio_out - g.rc_1.pwm_out;
// right
motor_out[CH_2] = g.rc_3.radio_out + g.rc_1.pwm_out;
// front
motor_out[CH_3] = g.rc_3.radio_out + g.rc_2.pwm_out;
// back
motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_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
// 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

99
ArduCopterMega/motors_quad_x.pde Normal file
View File

@ -0,0 +1,99 @@
/// -*- 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

89
ArduCopterMega/motors_tri.pde Normal file
View File

@ -0,0 +1,89 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if FRAME_CONFIG == TRI_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 = (float)g.rc_1.pwm_out * .866;
int pitch_out = g.rc_2.pwm_out / 2;
//left front
motor_out[CH_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;
// rear
motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out;
//motor_out[CH_4] += (float)(abs(g.rc_4.control_in)) * .013;
// 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_2, 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

110
ArduCopterMega/motors_y6.pde Normal file
View File

@ -0,0 +1,110 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#if FRAME_CONFIG == Y6_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 = (float)g.rc_1.pwm_out * .866;
int pitch_out = g.rc_2.pwm_out / 2;
//left
motor_out[CH_2] = ((g.rc_3.radio_out * Y6_scaling) + roll_out + pitch_out); // CCW TOP
motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW
//right
motor_out[CH_7] = ((g.rc_3.radio_out * Y6_scaling) - roll_out + pitch_out); // CCW TOP
motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out; // CW
//back
motor_out[CH_8] = ((g.rc_3.radio_out * Y6_scaling) - g.rc_2.pwm_out); // CCW TOP
motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW
// Yaw
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[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
// 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_2, g.rc_3.radio_min);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min + 50);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min + 50);
delay(1000);
APM_RC.OutputCh(CH_7, g.rc_3.radio_min);
APM_RC.OutputCh(CH_1, g.rc_3.radio_min);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min + 50);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min + 50);
delay(1000);
APM_RC.OutputCh(CH_8, g.rc_3.radio_min);
APM_RC.OutputCh(CH_4, g.rc_3.radio_min);
APM_RC.OutputCh(CH_2, g.rc_3.radio_min + 50);
APM_RC.OutputCh(CH_3, g.rc_3.radio_min + 50);
delay(1000);
}
#endif

1
ArduCopterMega/read_commands.pde
View File

@ -104,7 +104,6 @@ void parseCommand(char *buffer)
g.stabilize_dampener.set_and_save((float)value / 1000);
break;
}
init_pids();
//*/
}
}

687
ArduCopterMega/setup.pde
View File

@ -7,8 +7,6 @@ static int8_t setup_accel (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_flightmodes (uint8_t argc, const Menu::arg *argv);
static int8_t setup_pid (uint8_t argc, const Menu::arg *argv);
static int8_t setup_frame (uint8_t argc, const Menu::arg *argv);
static int8_t setup_batt_monitor (uint8_t argc, const Menu::arg *argv);
static int8_t setup_sonar (uint8_t argc, const Menu::arg *argv);
static int8_t setup_compass (uint8_t argc, const Menu::arg *argv);
@ -22,13 +20,11 @@ const struct Menu::command setup_menu_commands[] PROGMEM = {
// ======= ===============
{"erase", setup_erase},
{"reset", setup_factory},
{"pid", setup_pid},
{"radio", setup_radio},
{"motors", setup_motors},
{"esc", setup_esc},
{"level", setup_accel},
{"modes", setup_flightmodes},
{"frame", setup_frame},
{"battery", setup_batt_monitor},
{"sonar", setup_sonar},
{"compass", setup_compass},
@ -185,7 +181,15 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
delay(20);
Serial.flush();
save_EEPROM_radio();
g.rc_1.save_eeprom();
g.rc_2.save_eeprom();
g.rc_3.save_eeprom();
g.rc_4.save_eeprom();
g.rc_5.save_eeprom();
g.rc_6.save_eeprom();
g.rc_7.save_eeprom();
g.rc_8.save_eeprom();
print_done();
break;
}
@ -215,7 +219,6 @@ setup_esc(uint8_t argc, const Menu::arg *argv)
void
init_esc()
{
//Serial.printf_P(PSTR("\nCalibrate ESC"));
g.esc_calibrate.set_and_save(0);
while(1){
read_radio();
@ -233,104 +236,11 @@ init_esc()
static int8_t
setup_motors(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
report_frame();
//init_rc_in();
//delay(1000);
int out_min = g.rc_3.radio_min + 70;
while(1){
delay(20);
read_radio();
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;
if(g.frame_type == PLUS_FRAME){
if(g.rc_1.control_in > 0){
motor_out[CH_1] = out_min;
Serial.println("0");
}else if(g.rc_1.control_in < 0){
motor_out[CH_2] = out_min;
Serial.println("1");
}
if(g.rc_2.control_in > 0){
motor_out[CH_4] = out_min;
Serial.println("3");
}else if(g.rc_2.control_in < 0){
motor_out[CH_3] = out_min;
Serial.println("2");
}
}else if(g.frame_type == X_FRAME){
// lower right
if((g.rc_1.control_in > 0) && (g.rc_2.control_in > 0)){
motor_out[CH_4] = out_min;
Serial.println("3");
// lower left
}else if((g.rc_1.control_in < 0) && (g.rc_2.control_in > 0)){
motor_out[CH_2] = out_min;
Serial.println("1");
// upper left
}else if((g.rc_1.control_in < 0) && (g.rc_2.control_in < 0)){
motor_out[CH_3] = out_min;
Serial.println("2");
// upper right
}else if((g.rc_1.control_in > 0) && (g.rc_2.control_in < 0)){
motor_out[CH_1] = out_min;
Serial.println("0");
}
}else if(g.frame_type == TRI_FRAME){
if(g.rc_1.control_in > 0){
motor_out[CH_1] = out_min;
}else if(g.rc_1.control_in < 0){
motor_out[CH_2] = out_min;
}
if(g.rc_2.control_in > 0){
motor_out[CH_4] = out_min;
}
if(g.rc_4.control_in > 0){
g.rc_4.servo_out = 2000;
}else if(g.rc_4.control_in < 0){
g.rc_4.servo_out = -2000;
}
g.rc_4.calc_pwm();
// servo Yaw
APM_RC.OutputCh(CH_7, g.rc_4.radio_out);
}
if(g.rc_3.control_in > 0){
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);
}else{
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]);
}
output_motor_test();
if(Serial.available() > 0){
return (0);
g.esc_calibrate.set_and_save(0);
return(0);
}
}
}
@ -338,71 +248,13 @@ setup_motors(uint8_t argc, const Menu::arg *argv)
static int8_t
setup_accel(uint8_t argc, const Menu::arg *argv)
{
//Serial.printf_P(PSTR("\nHold ArduCopter completely still and level.\n"));
imu.init_accel();
print_accel_offsets();
report_imu();
return(0);
}
static int8_t
setup_pid(uint8_t argc, const Menu::arg *argv)
{
if (!strcmp_P(argv[1].str, PSTR("default"))) {
default_gains();
}else if (!strcmp_P(argv[1].str, PSTR("stabilize"))) {
g.pid_stabilize_roll.kP(argv[2].f);
g.pid_stabilize_pitch.kP(argv[2].f);
g.stabilize_dampener.set_and_save(argv[3].f);
g.pid_stabilize_roll.save_gains();
g.pid_stabilize_pitch.save_gains();
}else if (!strcmp_P(argv[1].str, PSTR("yaw"))) {
g.pid_yaw.kP(argv[2].f);
g.pid_yaw.save_gains();
g.hold_yaw_dampener.set_and_save(argv[3].f);
}else if (!strcmp_P(argv[1].str, PSTR("nav"))) {
g.pid_nav_lat.kP(argv[2].f);
g.pid_nav_lat.kI(argv[3].f);
g.pid_nav_lat.imax(argv[4].i);
g.pid_nav_lon.kP(argv[2].f);
g.pid_nav_lon.kI(argv[3].f);
g.pid_nav_lon.imax(argv[4].i);
g.pid_nav_lon.save_gains();
g.pid_nav_lat.save_gains();
}else if (!strcmp_P(argv[1].str, PSTR("baro"))) {
g.pid_baro_throttle.kP(argv[2].f);
g.pid_baro_throttle.kI(argv[3].f);
g.pid_baro_throttle.kD(0);
g.pid_baro_throttle.imax(argv[4].i);
g.pid_baro_throttle.save_gains();
}else if (!strcmp_P(argv[1].str, PSTR("sonar"))) {
g.pid_sonar_throttle.kP(argv[2].f);
g.pid_sonar_throttle.kI(argv[3].f);
g.pid_sonar_throttle.kD(argv[4].f);
g.pid_sonar_throttle.imax(argv[5].i);
g.pid_sonar_throttle.save_gains();
}else{
default_gains();
}
report_gains();
}
static int8_t
setup_flightmodes(uint8_t argc, const Menu::arg *argv)
{
@ -489,37 +341,6 @@ setup_compass(uint8_t argc, const Menu::arg *argv)
return 0;
}
static int8_t
setup_frame(uint8_t argc, const Menu::arg *argv)
{
if (!strcmp_P(argv[1].str, PSTR("+"))) {
g.frame_type = PLUS_FRAME;
} else if (!strcmp_P(argv[1].str, PSTR("x"))) {
g.frame_type = X_FRAME;
} else if (!strcmp_P(argv[1].str, PSTR("tri"))) {
g.frame_type = TRI_FRAME;
} else if (!strcmp_P(argv[1].str, PSTR("hexax"))) {
g.frame_type = HEXAX_FRAME;
} else if (!strcmp_P(argv[1].str, PSTR("y6"))) {
g.frame_type = Y6_FRAME;
} else if (!strcmp_P(argv[1].str, PSTR("hexa+"))) {
g.frame_type = HEXAP_FRAME;
}else{
Serial.printf_P(PSTR("\nOptions:[+, x, tri, hexa+, hexax, y6]\n"));
report_frame();
return 0;
}
g.frame_type.save();
report_frame();
return 0;
}
static int8_t
setup_batt_monitor(uint8_t argc, const Menu::arg *argv)
{
@ -553,155 +374,12 @@ setup_sonar(uint8_t argc, const Menu::arg *argv)
return 0;
}
/*static int8_t
setup_mag_offset(uint8_t argc, const Menu::arg *argv)
{
Serial.printf_P(PSTR("\nRotate/Pitch/Roll your ArduCopter until the offset variables stop changing.\n"));
print_hit_enter();
Serial.printf_P(PSTR("Starting in 3 secs.\n"));
delay(3000);
compass.init(); // Initialization
compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft
//compass.set_offsets(0, 0, 0); // set offsets to account for surrounding interference
//int counter = 0;
float _min[3], _max[3], _offset[3];
while(1){
static float min[3], _max[3], offset[3];
if (millis() - fast_loopTimer > 100) {
delta_ms_fast_loop = millis() - fast_loopTimer;
fast_loopTimer = millis();
G_Dt = (float)delta_ms_fast_loop / 1000.f;
compass.read();
compass.calculate(0, 0); // roll = 0, pitch = 0 for this example
// capture min
if(compass.mag_x < _min[0]) _min[0] = compass.mag_x;
if(compass.mag_y < _min[1]) _min[1] = compass.mag_y;
if(compass.mag_z < _min[2]) _min[2] = compass.mag_z;
// capture max
if(compass.mag_x > _max[0]) _max[0] = compass.mag_x;
if(compass.mag_y > _max[1]) _max[1] = compass.mag_y;
if(compass.mag_z > _max[2]) _max[2] = compass.mag_z;
// calculate offsets
offset[0] = -(_max[0] + _min[0]) / 2;
offset[1] = -(_max[1] + _min[1]) / 2;
offset[2] = -(_max[2] + _min[2]) / 2;
// display all to user
Serial.printf_P(PSTR("Heading: "));
Serial.print(ToDeg(compass.heading));
Serial.print(" \t(");
Serial.print(compass.mag_x);
Serial.print(",");
Serial.print(compass.mag_y);
Serial.print(",");
Serial.print(compass.mag_z);
Serial.print(")\t offsets(");
Serial.print(offset[0]);
Serial.print(",");
Serial.print(offset[1]);
Serial.print(",");
Serial.print(offset[2]);
Serial.println(")");
if(Serial.available() > 0){
//mag_offset_x = offset[0];
//mag_offset_y = offset[1];
//mag_offset_z = offset[2];
//setup_mag_offset();
// set offsets to account for surrounding interference
//compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z);
report_compass();
break;
}
}
}
}
*/
/***************************************************************************/
// CLI defaults
/***************************************************************************/
void default_waypoint_info()
{
g.waypoint_radius = 4; //TODO: Replace this quick fix with a real way to define wp_radius
g.loiter_radius = 30; //TODO: Replace this quick fix with a real way to define loiter_radius
save_EEPROM_waypoint_info();
}
void
default_nav()
{
// nav control
g.crosstrack_gain = XTRACK_GAIN * 100;
g.crosstrack_entry_angle = XTRACK_ENTRY_ANGLE * 100;
g.pitch_max = PITCH_MAX * 100;
save_EEPROM_nav();
}
void
default_alt_hold()
{
g.RTL_altitude.set_and_save(-1);
}
void
default_frame()
{
g.frame_type.set_and_save(PLUS_FRAME);
}
/*void
default_current()
{
g.milliamp_hours = 2000;
g.current_enabled.set(false);
save_EEPROM_current();
}
*/
void
default_flight_modes()
{
g.flight_modes[0] = FLIGHT_MODE_1;
g.flight_modes[1] = FLIGHT_MODE_2;
g.flight_modes[2] = FLIGHT_MODE_3;
g.flight_modes[3] = FLIGHT_MODE_4;
g.flight_modes[4] = FLIGHT_MODE_5;
g.flight_modes[5] = FLIGHT_MODE_6;
g.flight_modes.save();
}
void
default_throttle()
{
g.throttle_min = 0;
g.throttle_max = 1000;
g.throttle_cruise = 100;
g.throttle_fs_enabled = THROTTLE_FAILSAFE;
g.throttle_fs_action = THROTTLE_FAILSAFE_ACTION;
g.throttle_fs_value = THROTTLE_FS_VALUE;
save_EEPROM_throttle();
}
void default_log_bitmask()
{
// convenience macro for testing LOG_* and setting LOGBIT_*
#define LOGBIT(_s) (LOG_##_s ? MASK_LOG_##_s : 0)
@ -721,96 +399,22 @@ void default_log_bitmask()
g.log_bitmask.save();
}
void
default_gains()
{
/*
// acro, angular rate
g.pid_acro_rate_roll.kP(ACRO_RATE_ROLL_P);
g.pid_acro_rate_roll.kI(ACRO_RATE_ROLL_I);
g.pid_acro_rate_roll.kD(0);
g.pid_acro_rate_roll.imax(ACRO_RATE_ROLL_IMAX * 100);
g.pid_acro_rate_pitch.kP(ACRO_RATE_PITCH_P);
g.pid_acro_rate_pitch.kI(ACRO_RATE_PITCH_I);
g.pid_acro_rate_pitch.kD(0);
g.pid_acro_rate_pitch.imax(ACRO_RATE_PITCH_IMAX * 100);
g.pid_acro_rate_yaw.kP(ACRO_RATE_YAW_P);
g.pid_acro_rate_yaw.kI(ACRO_RATE_YAW_I);
g.pid_acro_rate_yaw.kD(0);
g.pid_acro_rate_yaw.imax(ACRO_RATE_YAW_IMAX * 100);
// stabilize, angle error
g.pid_stabilize_roll.kP(STABILIZE_ROLL_P);
g.pid_stabilize_roll.kI(STABILIZE_ROLL_I);
g.pid_stabilize_roll.kD(0);
g.pid_stabilize_roll.imax(STABILIZE_ROLL_IMAX * 100);
g.pid_stabilize_pitch.kP(STABILIZE_PITCH_P);
g.pid_stabilize_pitch.kI(STABILIZE_PITCH_I);
g.pid_stabilize_pitch.kD(0);
g.pid_stabilize_pitch.imax(STABILIZE_PITCH_IMAX * 100);
// YAW hold
g.pid_yaw.kP(YAW_P);
g.pid_yaw.kI(YAW_I);
g.pid_yaw.kD(0);
g.pid_yaw.imax(YAW_IMAX * 100);
// custom dampeners
// roll pitch
g.stabilize_dampener = STABILIZE_ROLL_D;
//yaw
g.hold_yaw_dampener = YAW_D;
// navigation
g.pid_nav_lat.kP(NAV_P);
g.pid_nav_lat.kI(NAV_I);
g.pid_nav_lat.kD(NAV_D);
g.pid_nav_lat.imax(NAV_IMAX);
g.pid_nav_lon.kP(NAV_P);
g.pid_nav_lon.kI(NAV_I);
g.pid_nav_lon.kD(NAV_D);
g.pid_nav_lon.imax(NAV_IMAX);
g.pid_baro_throttle.kP(THROTTLE_BARO_P);
g.pid_baro_throttle.kI(THROTTLE_BARO_I);
g.pid_baro_throttle.kD(THROTTLE_BARO_D);
g.pid_baro_throttle.imax(THROTTLE_BARO_IMAX);
g.pid_sonar_throttle.kP(THROTTLE_SONAR_P);
g.pid_sonar_throttle.kI(THROTTLE_SONAR_I);
g.pid_sonar_throttle.kD(THROTTLE_SONAR_D);
g.pid_sonar_throttle.imax(THROTTLE_SONAR_IMAX);
save_EEPROM_PID();
*/
}
/***************************************************************************/
// CLI reports
/***************************************************************************/
void report_batt_monitor()
{
//print_blanks(2);
Serial.printf_P(PSTR("Batt Mointor\n"));
Serial.printf_P(PSTR("\nBatt Mointor\n"));
print_divider();
if(g.battery_monitoring == 0) Serial.printf_P(PSTR("Batt mon. disabled"));
if(g.battery_monitoring == 0) print_enabled(false);
if(g.battery_monitoring == 1) Serial.printf_P(PSTR("3 cells"));
if(g.battery_monitoring == 2) Serial.printf_P(PSTR("4 cells"));
if(g.battery_monitoring == 3) Serial.printf_P(PSTR("batt volts"));
if(g.battery_monitoring == 4) Serial.printf_P(PSTR("volts and cur"));
print_blanks(2);
}
void report_wp(byte index = 255)
{
if(index == 255){
@ -836,18 +440,6 @@ void print_wp(struct Location *cmd, byte index)
cmd->lng);
}
/*
void report_current()
{
//read_EEPROM_current();
Serial.printf_P(PSTR("Current \n"));
print_divider();
print_enabled(g.current_enabled.get());
Serial.printf_P(PSTR("mah: %d"),(int)g.milliamp_hours.get());
print_blanks(2);
}
*/
void report_gps()
{
Serial.printf_P(PSTR("\nGPS\n"));
@ -872,27 +464,25 @@ void report_version()
print_blanks(2);
}
void report_frame()
{
Serial.printf_P(PSTR("Frame\n"));
print_divider();
#if FRAME_CONFIG == QUADX_FRAME
Serial.printf_P(PSTR("X frame\n"));
#elif FRAME_CONFIG == QUADP_FRAME
Serial.printf_P(PSTR("Plus frame\n"));
#elif FRAME_CONFIG == TRI_FRAME
Serial.printf_P(PSTR("TRI frame\n"));
#elif FRAME_CONFIG == HEXAX_FRAME
Serial.printf_P(PSTR("HexaX frame\n"));
#elif FRAME_CONFIG == HEXAP_FRAME
Serial.printf_P(PSTR("HexaP frame\n"));
#elif FRAME_CONFIG == Y6_FRAME
Serial.printf_P(PSTR("Y6 frame\n"));
#endif
if(g.frame_type == X_FRAME)
Serial.printf_P(PSTR("X "));
else if(g.frame_type == PLUS_FRAME)
Serial.printf_P(PSTR("Plus "));
else if(g.frame_type == TRI_FRAME)
Serial.printf_P(PSTR("TRI "));
else if(g.frame_type == HEXAX_FRAME)
Serial.printf_P(PSTR("HEXA X"));
else if(g.frame_type == Y6_FRAME)
Serial.printf_P(PSTR("Y6 "));
else if(g.frame_type == HEXAP_FRAME)
Serial.printf_P(PSTR("HEXA +"));
Serial.printf_P(PSTR("frame (%d)"), (int)g.frame_type);
print_blanks(2);
}
@ -901,7 +491,6 @@ void report_radio()
Serial.printf_P(PSTR("Radio\n"));
print_divider();
// radio
//read_EEPROM_radio();
print_radio_values();
print_blanks(2);
}
@ -911,8 +500,6 @@ void report_gains()
Serial.printf_P(PSTR("Gains\n"));
print_divider();
//read_EEPROM_PID();
// Acro
Serial.printf_P(PSTR("Acro:\nroll:\n"));
print_PID(&g.pid_acro_rate_roll);
@ -949,7 +536,6 @@ void report_xtrack()
Serial.printf_P(PSTR("XTrack\n"));
print_divider();
// radio
//read_EEPROM_nav();
Serial.printf_P(PSTR("XTRACK: %4.2f\n"
"XTRACK angle: %d\n"
"PITCH_MAX: %ld"),
@ -964,7 +550,6 @@ void report_throttle()
Serial.printf_P(PSTR("Throttle\n"));
print_divider();
//read_EEPROM_throttle();
Serial.printf_P(PSTR("min: %d\n"
"max: %d\n"
"cruise: %d\n"
@ -1027,7 +612,7 @@ void report_flight_modes()
void
print_PID(PID * pid)
{
Serial.printf_P(PSTR("P: %4.3f, I:%4.3f, D:%4.3f, IMAX:%ld\n"),
Serial.printf_P(PSTR("P: %4.2f, I:%4.2f, D:%4.2f, IMAX:%ld\n"),
pid->kP(),
pid->kI(),
pid->kD(),
@ -1037,25 +622,7 @@ print_PID(PID * pid)
void
print_radio_values()
{
/*Serial.printf_P(PSTR( "CH1: %d | %d\n"
"CH2: %d | %d\n"
"CH3: %d | %d\n"
"CH4: %d | %d\n"
"CH5: %d | %d\n"
"CH6: %d | %d\n"
"CH7: %d | %d\n"
"CH8: %d | %d\n"),
g.rc_1.radio_min, g.rc_1.radio_max,
g.rc_2.radio_min, g.rc_2.radio_max,
g.rc_3.radio_min, g.rc_3.radio_max,
g.rc_4.radio_min, g.rc_4.radio_max,
g.rc_5.radio_min, g.rc_5.radio_max,
g.rc_6.radio_min, g.rc_6.radio_max,
g.rc_7.radio_min, g.rc_7.radio_max,
g.rc_8.radio_min, g.rc_8.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("CH3: %d | %d\n"), (int)g.rc_3.radio_min, (int)g.rc_3.radio_max);
@ -1063,8 +630,7 @@ print_radio_values()
Serial.printf_P(PSTR("CH5: %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_max);
Serial.printf_P(PSTR("CH6: %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_max);
Serial.printf_P(PSTR("CH7: %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_max);
Serial.printf_P(PSTR("CH8: %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_max);
//*/
//Serial.printf_P(PSTR("CH8: %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_max);
}
void
@ -1162,196 +728,3 @@ print_gyro_offsets(void)
(float)imu.gz());
}
/***************************************************************************/
// EEPROM convenience functions
/***************************************************************************/
void read_EEPROM_waypoint_info(void)
{
g.waypoint_total.load();
g.waypoint_radius.load();
g.loiter_radius.load();
}
void save_EEPROM_waypoint_info(void)
{
g.waypoint_total.save();
g.waypoint_radius.save();
g.loiter_radius.save();
}
/********************************************************************************/
void read_EEPROM_nav(void)
{
g.crosstrack_gain.load();
g.crosstrack_entry_angle.load();
g.pitch_max.load();
}
void save_EEPROM_nav(void)
{
g.crosstrack_gain.save();
g.crosstrack_entry_angle.save();
g.pitch_max.save();
}
/********************************************************************************/
void read_EEPROM_PID(void)
{
g.pid_acro_rate_roll.load_gains();
g.pid_acro_rate_pitch.load_gains();
g.pid_acro_rate_yaw.load_gains();
g.pid_stabilize_roll.load_gains();
g.pid_stabilize_pitch.load_gains();
g.pid_yaw.load_gains();
g.pid_nav_lon.load_gains();
g.pid_nav_lat.load_gains();
g.pid_baro_throttle.load_gains();
g.pid_sonar_throttle.load_gains();
// roll pitch
g.stabilize_dampener.load();
// yaw
g.hold_yaw_dampener.load();
init_pids();
}
void save_EEPROM_PID(void)
{
/*
g.pid_acro_rate_roll.save_gains();
g.pid_acro_rate_pitch.save_gains();
g.pid_acro_rate_yaw.save_gains();
g.pid_stabilize_roll.save_gains();
g.pid_stabilize_pitch.save_gains();
g.pid_yaw.save_gains();
g.pid_nav_lon.save_gains();
g.pid_nav_lat.save_gains();
g.pid_baro_throttle.save_gains();
g.pid_sonar_throttle.save_gains();
// roll pitch
g.stabilize_dampener.save();
// yaw
g.hold_yaw_dampener.save();
*/
}
/********************************************************************************/
/*void save_EEPROM_current(void)
{
g.current_enabled.save();
//g.milliamp_hours.save();
}
void read_EEPROM_current(void)
{
g.current_enabled.load();
g.milliamp_hours.load();
}
*/
/********************************************************************************/
void read_EEPROM_radio(void)
{
g.rc_1.load_eeprom();
g.rc_2.load_eeprom();
g.rc_3.load_eeprom();
g.rc_4.load_eeprom();
g.rc_5.load_eeprom();
g.rc_6.load_eeprom();
g.rc_7.load_eeprom();
g.rc_8.load_eeprom();
}
void save_EEPROM_radio(void)
{
g.rc_1.save_eeprom();
g.rc_2.save_eeprom();
g.rc_3.save_eeprom();
g.rc_4.save_eeprom();
g.rc_5.save_eeprom();
g.rc_6.save_eeprom();
g.rc_7.save_eeprom();
g.rc_8.save_eeprom();
}
/********************************************************************************/
// configs are the basics
void read_EEPROM_throttle(void)
{
g.throttle_min.load();
g.throttle_max.load();
g.throttle_cruise.load();
g.throttle_fs_enabled.load();
g.throttle_fs_action.load();
g.throttle_fs_value.load();
}
void save_EEPROM_throttle(void)
{
g.throttle_min.load();
g.throttle_max.load();
g.throttle_cruise.save();
g.throttle_fs_enabled.load();
g.throttle_fs_action.load();
g.throttle_fs_value.load();
}
/********************************************************************************/
/*
float
read_EE_float(int address)
{
union {
byte bytes[4];
float value;
} _floatOut;
for (int i = 0; i < 4; i++)
_floatOut.bytes[i] = eeprom_read_byte((uint8_t *) (address + i));
return _floatOut.value;
}
void write_EE_float(float value, int address)
{
union {
byte bytes[4];
float value;
} _floatIn;
_floatIn.value = value;
for (int i = 0; i < 4; i++)
eeprom_write_byte((uint8_t *) (address + i), _floatIn.bytes[i]);
}
*/
/********************************************************************************/
/*
float
read_EE_compressed_float(int address, byte places)
{
float scale = pow(10, places);
int temp = eeprom_read_word((uint16_t *) address);
return ((float)temp / scale);
}
void write_EE_compressed_float(float value, int address, byte places)
{
float scale = pow(10, places);
int temp = value * scale;
eeprom_write_word((uint16_t *) address, temp);
}
*/

68
ArduCopterMega/system.pde
View File

@ -37,7 +37,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
};
// Create the top-level menu object.
MENU(main_menu, "AC 2.0.4 Beta", main_menu_commands);
MENU(main_menu, "AC 2.0.5 Beta", main_menu_commands);
void init_ardupilot()
{
@ -225,7 +225,7 @@ void init_ardupilot()
// set the correct flight mode
// ---------------------------
reset_control_switch();
//reset_control_switch();
// Logging:
// --------
@ -252,29 +252,24 @@ void startup_ground(void)
delay(GROUND_START_DELAY * 1000);
#endif
setup_show(NULL,NULL);
// setup up PID value max
init_pids();
// Output waypoints for confirmation
// --------------------------------
for(int i = 1; i < g.waypoint_total + 1; i++) {
gcs.send_message(MSG_COMMAND_LIST, i);
}
#if HIL_MODE != HIL_MODE_ATTITUDE
// Warm up and read Gyro offsets
// -----------------------------
imu.init_gyro();
report_imu();
#endif
#if HIL_MODE != HIL_MODE_ATTITUDE
// Warm up and read Gyro offsets
// -----------------------------
imu.init_gyro();
report_imu();
#endif
#if HIL_MODE != HIL_MODE_ATTITUDE
// read Baro pressure at ground
//-----------------------------
init_barometer();
#endif
#if HIL_MODE != HIL_MODE_ATTITUDE
// read Baro pressure at ground
//-----------------------------
init_barometer();
#endif
// initialize commands
// -------------------
@ -311,23 +306,19 @@ void set_mode(byte mode)
control_mode = mode;
control_mode = constrain(control_mode, 0, NUM_MODES - 1);
// XXX temporary
//if (g.log_bitmask & MASK_LOG_MODE)
Log_Write_Mode(control_mode);
Log_Write_Mode(control_mode);
// used to stop fly_aways
if(g.rc_1.control_in == 0){
if(g.rc_3.control_in == 0){ // throttle is 0
// we are on the ground is this is true
// disarm motors temp
motor_auto_safe = false;
// disarm motors for Auto
motor_auto_armed = false;
}
// clear our tracking behaviors
yaw_tracking = MAV_ROI_NONE;
//send_text_P(SEVERITY_LOW,PSTR("control mode"));
//Serial.printf("set mode: %d old: %d\n", (int)mode, (int)control_mode);
//Serial.printf("set mode: %d\n",control_mode);
Serial.println(flight_mode_strings[control_mode]);
switch(control_mode)
{
case ACRO:
@ -469,10 +460,6 @@ void update_esc_light()
void resetPerfData(void) {
mainLoop_count = 0;
G_Dt_max = 0;
//gyro_sat_count = 0;
//adc_constraints = 0;
//renorm_sqrt_count = 0;
//renorm_blowup_count = 0;
gps_fix_count = 0;
perf_mon_timer = millis();
}
@ -481,7 +468,7 @@ void
init_compass()
{
dcm.set_compass(&compass);
bool junkbool = compass.init();
bool junkbool = compass.init();
compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft
Vector3f junkvector = compass.get_offsets(); // load offsets to account for airframe magnetic interference
}
@ -506,24 +493,15 @@ void
init_simple_bearing()
{
initial_simple_bearing = dcm.yaw_sensor;
//if(simple_bearing_is_set == false){
//if(g.rc_3.control_in == 0){
// we are on the ground
// initial_simple_bearing = dcm.yaw_sensor;
// simple_bearing_is_set = true;
//}
//}
}
void
init_throttle_cruise()
{
if(set_throttle_cruise_flag == false){
//if(set_throttle_cruise_flag == false){
if(g.rc_3.control_in > 200){
//set_throttle_cruise_flag = true;
g.throttle_cruise.set_and_save(g.rc_3.control_in);
//Serial.printf("throttle_cruise: %d\n", g.throttle_cruise.get());
}
}
//}
}

22
ArduCopterMega/test.pde
View File

@ -264,7 +264,7 @@ test_stabilize(uint8_t argc, const Menu::arg *argv)
Serial.printf_P(PSTR("g.pid_stabilize_roll.kP: %4.4f\n"), g.pid_stabilize_roll.kP());
Serial.printf_P(PSTR("max_stabilize_dampener:%d\n\n "), max_stabilize_dampener);
motor_auto_safe = false;
motor_auto_armed = false;
motor_armed = true;
while(1){
@ -696,8 +696,6 @@ static int8_t
test_wp(uint8_t argc, const Menu::arg *argv)
{
delay(1000);
//read_EEPROM_waypoint_info();
// save the alitude above home option
Serial.printf_P(PSTR("Hold altitude "));
@ -723,15 +721,10 @@ test_rawgps(uint8_t argc, const Menu::arg *argv)
delay(1000);
while(1){
if (Serial3.available()){
digitalWrite(B_LED_PIN, HIGH); // Blink Yellow LED if we are sending data to GPS
Serial1.write(Serial3.read());
digitalWrite(B_LED_PIN, LOW);
}
if (Serial1.available()){
digitalWrite(C_LED_PIN, HIGH); // Blink Red LED if we are receiving data from GPS
Serial3.write(Serial1.read());
digitalWrite(C_LED_PIN, LOW);
digitalWrite(B_LED_PIN, HIGH); // Blink Yellow LED if we are sending data to GPS
Serial.write(Serial1.read());
digitalWrite(B_LED_PIN, LOW);
}
if(Serial.available() > 0){
return (0);
@ -851,13 +844,14 @@ test_mission(uint8_t argc, const Menu::arg *argv)
int32_t lat; ///< param 3 - Lattitude * 10**7
int32_t lng; ///< param 4 - Longitude * 10**7
}*/
byte alt_rel = 1;
// clear home
{Location t = {0, 0, 0, 0, 0, 0};
set_command_with_index(t,0);}
// CMD opt pitch alt/cm
{Location t = {MAV_CMD_NAV_TAKEOFF, 0, 0, 100, 0, 0};
// CMD opt pitch alt/cm
{Location t = {MAV_CMD_NAV_TAKEOFF, WP_OPTION_RELATIVE, 0, 100, 0, 0};
set_command_with_index(t,1);}
if (!strcmp_P(argv[1].str, PSTR("wp"))) {
@ -876,7 +870,7 @@ test_mission(uint8_t argc, const Menu::arg *argv)
} else {
//2250 = 25 meteres
// CMD opt p1 //alt //NS //WE
{Location t = {MAV_CMD_NAV_LOITER_TIME, 0, 0, 100, 1100, 0};
{Location t = {MAV_CMD_NAV_LOITER_TIME, 0, 10, 0, 0, 0}; // 19
set_command_with_index(t,2);}
// CMD opt dir angle/deg deg/s relative