From 8f17f95cbcffd1949e27a5f38b7236c6001d55be Mon Sep 17 00:00:00 2001 From: jasonshort Date: Sun, 15 May 2011 02:02:09 +0000 Subject: [PATCH] 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 --- ArduCopterMega/ArduCopterMega.pde | 27 +- ArduCopterMega/Attitude.pde | 15 - ArduCopterMega/Log.pde | 9 +- ArduCopterMega/Parameters.h | 19 +- ArduCopterMega/command description.txt | 1 + ArduCopterMega/commands.pde | 6 +- ArduCopterMega/commands_logic.pde | 35 +- ArduCopterMega/commands_process.pde | 93 +++- ArduCopterMega/config.h | 4 +- ArduCopterMega/control_modes.pde | 18 +- ArduCopterMega/defines.h | 4 +- ArduCopterMega/motors.pde | 257 +-------- ArduCopterMega/motors_hexa_p.pde | 115 +++++ ArduCopterMega/motors_hexa_x.pde | 114 ++++ ArduCopterMega/motors_quad_p.pde | 97 ++++ ArduCopterMega/motors_quad_x.pde | 99 ++++ ArduCopterMega/motors_tri.pde | 89 ++++ ArduCopterMega/motors_y6.pde | 110 ++++ ArduCopterMega/read_commands.pde | 1 - ArduCopterMega/setup.pde | 687 ++----------------------- ArduCopterMega/system.pde | 68 +-- ArduCopterMega/test.pde | 22 +- 22 files changed, 822 insertions(+), 1068 deletions(-) create mode 100644 ArduCopterMega/motors_hexa_p.pde create mode 100644 ArduCopterMega/motors_hexa_x.pde create mode 100644 ArduCopterMega/motors_quad_p.pde create mode 100644 ArduCopterMega/motors_quad_x.pde create mode 100644 ArduCopterMega/motors_tri.pde create mode 100644 ArduCopterMega/motors_y6.pde diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index f32bc3cdae..e0fe7a7c43 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -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(¤t_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. diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index 2ffa1e2b20..b1c3ac1836 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -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 } diff --git a/ArduCopterMega/Log.pde b/ArduCopterMega/Log.pde index 135c81ce01..f4eee5ad33 100644 --- a/ArduCopterMega/Log.pde +++ b/ArduCopterMega/Log.pde @@ -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); diff --git a/ArduCopterMega/Parameters.h b/ArduCopterMega/Parameters.h index 3d501302d8..3ed32bfce3 100644 --- a/ArduCopterMega/Parameters.h +++ b/ArduCopterMega/Parameters.h @@ -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 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 //-------------------------------------------------------------------------------------------------------------------------------------------------------------------- diff --git a/ArduCopterMega/command description.txt b/ArduCopterMega/command description.txt index cad3b4efa9..5eb25caaa7 100644 --- a/ArduCopterMega/command description.txt +++ b/ArduCopterMega/command description.txt @@ -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. diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde index 05a743d65b..9c09ce0921 100644 --- a/ArduCopterMega/commands.pde +++ b/ArduCopterMega/commands.pde @@ -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 diff --git a/ArduCopterMega/commands_logic.pde b/ArduCopterMega/commands_logic.pde index c713649a96..c0d8480858 100644 --- a/ArduCopterMega/commands_logic.pde +++ b/ArduCopterMega/commands_logic.pde @@ -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(¤t_loc); - else - set_next_WP(&next_command); - + set_next_WP(¤t_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 diff --git a/ArduCopterMega/commands_process.pde b/ArduCopterMega/commands_process.pde index 0a04f9a505..440d5fc09c 100644 --- a/ArduCopterMega/commands_process.pde +++ b/ArduCopterMega/commands_process.pde @@ -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 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 new may "); - SendDebugln(next_command.id,DEC); + command_may_index = g.waypoint_index + 1; + + //SendDebug("MSG 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 new now "); - SendDebugln(next_command.id,DEC); + //SendDebug("MSG new now "); + //SendDebugln(next_command.id,DEC); if (g.log_bitmask & MASK_LOG_CMD) Log_Write_Cmd(g.waypoint_index, &next_command); diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index 1bed2f2765..411c6debf3 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -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 diff --git a/ArduCopterMega/control_modes.pde b/ArduCopterMega/control_modes.pde index 4c425e708b..83a6f8d4bf 100644 --- a/ArduCopterMega/control_modes.pde +++ b/ArduCopterMega/control_modes.pde @@ -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; } diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h index 473bf74529..61b84f7f11 100644 --- a/ArduCopterMega/defines.h +++ b/ArduCopterMega/defines.h @@ -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 diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde index 0167ee1791..206fd082cc 100644 --- a/ArduCopterMega/motors.pde +++ b/ArduCopterMega/motors.pde @@ -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); - } - } - + //} \ No newline at end of file diff --git a/ArduCopterMega/motors_hexa_p.pde b/ArduCopterMega/motors_hexa_p.pde new file mode 100644 index 0000000000..49661eea1c --- /dev/null +++ b/ArduCopterMega/motors_hexa_p.pde @@ -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 \ No newline at end of file diff --git a/ArduCopterMega/motors_hexa_x.pde b/ArduCopterMega/motors_hexa_x.pde new file mode 100644 index 0000000000..9dce63eb7e --- /dev/null +++ b/ArduCopterMega/motors_hexa_x.pde @@ -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 \ No newline at end of file diff --git a/ArduCopterMega/motors_quad_p.pde b/ArduCopterMega/motors_quad_p.pde new file mode 100644 index 0000000000..a28b425c0c --- /dev/null +++ b/ArduCopterMega/motors_quad_p.pde @@ -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 \ No newline at end of file diff --git a/ArduCopterMega/motors_quad_x.pde b/ArduCopterMega/motors_quad_x.pde new file mode 100644 index 0000000000..4f4dbc1b6e --- /dev/null +++ b/ArduCopterMega/motors_quad_x.pde @@ -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 \ No newline at end of file diff --git a/ArduCopterMega/motors_tri.pde b/ArduCopterMega/motors_tri.pde new file mode 100644 index 0000000000..00afa18d25 --- /dev/null +++ b/ArduCopterMega/motors_tri.pde @@ -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 \ No newline at end of file diff --git a/ArduCopterMega/motors_y6.pde b/ArduCopterMega/motors_y6.pde new file mode 100644 index 0000000000..8b95e69da4 --- /dev/null +++ b/ArduCopterMega/motors_y6.pde @@ -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 \ No newline at end of file diff --git a/ArduCopterMega/read_commands.pde b/ArduCopterMega/read_commands.pde index eb491e229d..265948a4f0 100644 --- a/ArduCopterMega/read_commands.pde +++ b/ArduCopterMega/read_commands.pde @@ -104,7 +104,6 @@ void parseCommand(char *buffer) g.stabilize_dampener.set_and_save((float)value / 1000); break; } - init_pids(); //*/ } } diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index 95aeb29ec9..5ce4109531 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -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); -} -*/ diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index 308a1ccf4f..b172557a06 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -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()); } - } + //} } diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index 1969fd8e84..1fb749f109 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -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