From f8b0c61e63343c8068ad09b909707a907d37749f Mon Sep 17 00:00:00 2001 From: jasonshort Date: Sat, 26 Mar 2011 06:35:52 +0000 Subject: [PATCH] updates - support new test mission - Liftoff, spin, land updated scripted Yaw control Public Beta candidate... git-svn-id: https://arducopter.googlecode.com/svn/trunk@1814 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/APM_Config.h | 5 +- ArduCopterMega/ArduCopterMega.pde | 27 ++++- ArduCopterMega/Attitude.pde | 9 +- ArduCopterMega/GCS.pde | 14 +-- ArduCopterMega/GCS_Mavlink.pde | 7 -- ArduCopterMega/Log.pde | 24 ++--- ArduCopterMega/Mavlink_Common.h | 2 - ArduCopterMega/command description.txt | 12 +-- ArduCopterMega/commands.pde | 8 +- ArduCopterMega/commands_logic.pde | 98 ++++++++++-------- ArduCopterMega/config.h | 4 +- ArduCopterMega/control_modes.pde | 36 ++++--- ArduCopterMega/defines.h | 2 +- ArduCopterMega/navigation.pde | 1 + ArduCopterMega/sensors.pde | 2 +- ArduCopterMega/setup.pde | 138 ++++++++++++++++++------- ArduCopterMega/test.pde | 80 ++++++++++---- 17 files changed, 290 insertions(+), 179 deletions(-) diff --git a/ArduCopterMega/APM_Config.h b/ArduCopterMega/APM_Config.h index a9e8b19fd7..50ff954c20 100644 --- a/ArduCopterMega/APM_Config.h +++ b/ArduCopterMega/APM_Config.h @@ -5,10 +5,9 @@ // GPS is auto-selected #define GCS_PROTOCOL GCS_PROTOCOL_NONE +//#define GCS_PORT 0 -//#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_UP_PINS_BACK - -# define SERIAL0_BAUD 38400 +#define SERIAL0_BAUD 38400 //# define STABILIZE_ROLL_P 0.4 //# define STABILIZE_PITCH_P 0.4 diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 742562d1c3..1002f24732 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -42,6 +42,8 @@ version 2.1 of the License, or (at your option) any later version. #define MAVLINK_COMM_NUM_BUFFERS 2 #include // MAVLink GCS definitions +//#include + // Configuration #include "config.h" @@ -175,6 +177,8 @@ GPS *g_gps; GCS_Class gcs; #endif +//GCS_SIMPLE gcs_simple(&Serial); + AP_RangeFinder_MaxsonarXL sonar; //////////////////////////////////////////////////////////////////////////////// @@ -337,6 +341,7 @@ long command_yaw_end; // what angle are we trying to be long command_yaw_delta; // how many degrees will we turn int command_yaw_speed; // how fast to turn byte command_yaw_dir; +byte command_yaw_relative; // Waypoints // --------- @@ -758,6 +763,19 @@ void super_slow_loop() gcs.send_message(MSG_HEARTBEAT); // XXX This is running at 3 1/3 Hz instead of 1 Hz // gcs.send_message(MSG_CPU_LOAD, load*100); + //if(gcs_simple.read()){ + // Serial.print("!"); + /* + Location temp; + temp.id = gcs_simple.id; + temp.p1 = gcs_simple.p1; + temp.alt = gcs_simple.altitude; + temp.lat = gcs_simple.latitude; + temp.lng = gcs_simple.longitude; + set_wp_with_index(temp, gcs_simple.index); + gcs_simple.ack(); + */ + //} } void update_GPS(void) @@ -1127,11 +1145,9 @@ void update_alt() // decide which sensor we're usings sonar_alt = sonar.read(); - if(baro_alt < 550){ + if(baro_alt < 500 && sonar_alt < 600){ altitude_sensor = SONAR; - } - - if(sonar_alt > 600){ + }else{ altitude_sensor = BARO; } @@ -1162,4 +1178,5 @@ void update_alt() // Amount of throttle to apply for hovering // ---------------------------------------- calc_nav_throttle(); -} +} + diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index 2fbd8bc281..5921b66741 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -89,7 +89,6 @@ clear_yaw_control() void output_yaw_with_hold(boolean hold) { - //digitalWrite(B_LED_PIN, LOW); if(hold){ // look to see if we have exited rate control properly - ie stopped turning if(rate_yaw_flag){ @@ -101,8 +100,6 @@ output_yaw_with_hold(boolean hold) }else{ - //digitalWrite(B_LED_PIN, HIGH); - // return to rate control until we slow down. hold = false; //Serial.print("D"); @@ -119,7 +116,6 @@ output_yaw_with_hold(boolean hold) } if(hold){ - //Serial.println("H"); // try and hold the current nav_yaw setting yaw_error = nav_yaw - dcm.yaw_sensor; // +- 60° yaw_error = wrap_180(yaw_error); @@ -139,11 +135,9 @@ output_yaw_with_hold(boolean hold) }else{ - //Serial.println("R"); // rate control long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377 rate = constrain(rate, -36000, 36000); // limit to something fun! - long error = ((long)g.rc_4.control_in * 6) - rate; // control is += 6000 * 6 = 36000 // -error = CCW, +error = CW @@ -161,10 +155,9 @@ output_yaw_with_hold(boolean hold) g.rc_4.servo_out = constrain(g.rc_4.servo_out, -1800, 1800); // limit to 24° } - } -// slight left rudder give right roll. +// slight left rudder gives right roll. void output_rate_roll() diff --git a/ArduCopterMega/GCS.pde b/ArduCopterMega/GCS.pde index c7b4f4649a..d8b8d4a3d0 100644 --- a/ArduCopterMega/GCS.pde +++ b/ArduCopterMega/GCS.pde @@ -16,7 +16,7 @@ B Checksum byte 2 */ - +/* #if GCS_PORT == 3 # define SendSerw Serial3.write # define SendSer Serial3.print @@ -80,23 +80,15 @@ void flush(byte id) byte mess_ck_a = 0; byte mess_ck_b = 0; byte i; - + SendSer("4D"); // This is the message preamble SendSerw(buff_pointer); // Length SendSerw(2); // id - //SendSerw(0x01); // Version for (i = 0; i < buff_pointer; i++) { SendSerw(mess_buffer[i]); } - - //for (i = 0; i < buff_pointer; i++) { - // mess_ck_a += mess_buffer[i]; // Calculates checksums - // mess_ck_b += mess_ck_a; - //} - - //SendSerw(mess_ck_a); - //SendSerw(mess_ck_b); buff_pointer = 0; } +*/ \ No newline at end of file diff --git a/ArduCopterMega/GCS_Mavlink.pde b/ArduCopterMega/GCS_Mavlink.pde index 708fcbb4fd..7604c99796 100644 --- a/ArduCopterMega/GCS_Mavlink.pde +++ b/ArduCopterMega/GCS_Mavlink.pde @@ -34,8 +34,6 @@ GCS_MAVLINK::update(void) { uint8_t c = comm_receive_ch(chan); - - // Try to get a new message if(mavlink_parse_char(chan, c, &msg, &status)) handleMessage(&msg); } @@ -43,11 +41,6 @@ GCS_MAVLINK::update(void) // Update packet drops counter packet_drops += status.packet_rx_drop_count; - - - - - // send out queued params/ waypoints _queued_send(); diff --git a/ArduCopterMega/Log.pde b/ArduCopterMega/Log.pde index 02ef7e1912..472752be71 100644 --- a/ArduCopterMega/Log.pde +++ b/ArduCopterMega/Log.pde @@ -22,10 +22,10 @@ static int8_t help_log(uint8_t argc, const Menu::arg *argv) { Serial.printf_P(PSTR("\n" "Commands:\n" - " dump dump log \n" - " erase erase all logs\n" - " enable |all enable logging or everything\n" - " disable |all disable logging or everything\n" + " dump " + " erase (all logs)\n" + " enable | all\n" + " disable | all\n" "\n")); } @@ -75,13 +75,13 @@ print_log_menu(void) Serial.println(); if (last_log_num == 0) { - Serial.printf_P(PSTR("\nNo logs available for download\n")); + Serial.printf_P(PSTR("\nNo logs\n")); }else{ - Serial.printf_P(PSTR("\n%d logs available for download\n"), last_log_num); + Serial.printf_P(PSTR("\n%d logs\n"), last_log_num); for(int i=1;i 0; i--) { - Serial.printf_P(PSTR("ATTENTION - Erasing log in %d seconds. Power off now to save log! \n"), i); + Serial.printf_P(PSTR("ATTENTION - Erasing log in %d seconds.\n"), i); delay(1000); } Serial.printf_P(PSTR("\nErasing log...\n")); @@ -253,7 +253,7 @@ void start_new_log(byte num_existing_logs) DataFlash.FinishWrite(); DataFlash.StartWrite(start_pages[num_existing_logs-1]); }else{ - gcs.send_text_P(SEVERITY_LOW,PSTR(" Logs full - logging discontinued")); + gcs.send_text_P(SEVERITY_LOW,PSTR(" Logs full")); } } @@ -708,7 +708,7 @@ void Log_Read(int start_page, int end_page) } page = DataFlash.GetPage(); } - Serial.printf_P(PSTR("Number of packets read: %d\n"), packet_count); + Serial.printf_P(PSTR("# of packets read: %d\n"), packet_count); } diff --git a/ArduCopterMega/Mavlink_Common.h b/ArduCopterMega/Mavlink_Common.h index ba2ba7fec9..604995b089 100644 --- a/ArduCopterMega/Mavlink_Common.h +++ b/ArduCopterMega/Mavlink_Common.h @@ -8,7 +8,6 @@ uint16_t system_type = MAV_FIXED_WING; byte mavdelay = 0; - // what does this do? static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid) { @@ -24,7 +23,6 @@ static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid) } } - void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, uint16_t packet_drops) { uint64_t timeStamp = micros(); diff --git a/ArduCopterMega/command description.txt b/ArduCopterMega/command description.txt index 68b297f2cc..622efaded9 100644 --- a/ArduCopterMega/command description.txt +++ b/ArduCopterMega/command description.txt @@ -23,22 +23,16 @@ Commands below MAV_CMD_NAV_LAST are commands that have a end criteria, eg "reach *********************************** Command ID Name Parameter 1 Altitude Latitude Longitude 0x10 / 16 MAV_CMD_NAV_WAYPOINT - altitude lat lon - -0x11 / 17 MAV_CMD_NAV_LOITER_UNLIM (indefinitely) altitude lat lon - +0x11 / 17 MAV_CMD_NAV_LOITER_UNLIM (forever) altitude lat lon 0x12 / 18 MAV_CMD_NAV_LOITER_TURNS turns altitude lat lon - 0x13 / 19 MAV_CMD_NAV_LOITER_TIME time (seconds*10) altitude lat lon - 0x14 / 20 MAV_CMD_NAV_RETURN_TO_LAUNCH - altitude lat lon - 0x15 / 21 MAV_CMD_NAV_LAND - altitude lat lon - 0x16 / 22 MAV_CMD_NAV_TAKEOFF takeoff pitch 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. - 0x17 / 23 MAV_CMD_NAV_TARGET - altitude lat lon + 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. + *********************************** May Commands - these commands are optional to finish diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde index ac289d5a26..7d91525e76 100644 --- a/ArduCopterMega/commands.pde +++ b/ArduCopterMega/commands.pde @@ -40,6 +40,9 @@ struct Location get_wp_with_index(int i) mem = (WP_START_BYTE) + (i * WP_SIZE); temp.id = eeprom_read_byte((uint8_t*)mem); + mem++; + temp.options = eeprom_read_byte((uint8_t*)mem); + mem++; temp.p1 = eeprom_read_byte((uint8_t*)mem); @@ -70,6 +73,9 @@ void set_wp_with_index(struct Location temp, int i) eeprom_write_byte((uint8_t *) mem, temp.id); + mem++; + eeprom_write_byte((uint8_t *) mem, temp.options); + mem++; eeprom_write_byte((uint8_t *) mem, temp.p1); @@ -194,7 +200,7 @@ void init_home() home.alt = max(g_gps->altitude, 0); home_is_set = true; - Serial.printf("gps alt: %ld", home.alt); + //Serial.printf_P(PSTR("gps alt: %ld\n"), home.alt); // Save Home to EEPROM // ------------------- diff --git a/ArduCopterMega/commands_logic.pde b/ArduCopterMega/commands_logic.pde index 90fb015d23..4889cce846 100644 --- a/ArduCopterMega/commands_logic.pde +++ b/ArduCopterMega/commands_logic.pde @@ -230,7 +230,8 @@ void do_land() velocity_land = 1000; Location temp = current_loc; - temp.alt = home.alt; + //temp.alt = home.alt; + temp.alt = -1000; set_next_WP(&temp); } @@ -269,17 +270,21 @@ bool verify_takeoff() bool verify_land() { - // XXX not sure - velocity_land = ((old_alt - current_loc.alt) *.2) + (velocity_land * .8); old_alt = current_loc.alt; - if((current_loc.alt < home.alt + 100) && velocity_land == 0){ - land_complete = true; - return true; - } + if(g.sonar_enabled){ + // decide which sensor we're usings + if(sonar_alt < 20){ + land_complete = true; + return true; + } + } else { + //land_complete = true; + //return true; + } - update_crosstrack(); + //update_crosstrack(); return false; } @@ -353,62 +358,67 @@ void do_within_distance() void do_yaw() { - // p1: bearing - // alt: speed - // lat: direction (-1,1), - // lng: rel (1) abs (0) +// { // CMD opt dir angle/deg deg/s relative +// Location t = {MAV_CMD_CONDITION_YAW, 0, 1, 360, 60, 1}; + // target angle in degrees command_yaw_start = nav_yaw; // current position command_yaw_start_time = millis(); - // which direction to turn - // 1 = clockwise, -1 = counterclockwise - command_yaw_dir = next_command.lat; + command_yaw_dir = next_command.p1; // 1 = clockwise, 0 = counterclockwise + command_yaw_relative = next_command.lng; // 1 = Relative, 0 = Absolute - // 1 = Relative or 0 = Absolute - if (next_command.lng == 1) { - // relative - command_yaw_dir = (command_yaw_end > 0) ? 1 : -1; - command_yaw_end += nav_yaw; - command_yaw_end = wrap_360(command_yaw_end); - }else{ - // absolute - command_yaw_end = next_command.p1 * 100; - } + command_yaw_speed = next_command.lat * 100; // if unspecified go 10° a second if(command_yaw_speed == 0) - command_yaw_speed = 10; + command_yaw_speed = 6000; - // if unspecified go clockwise + // if unspecified go counterclockwise if(command_yaw_dir == 0) - command_yaw_dir = 1; + command_yaw_dir = -1; - // calculate the delta travel - if(command_yaw_dir == 1){ - if(command_yaw_start > command_yaw_end){ - command_yaw_delta = 36000 - (command_yaw_start - command_yaw_end); - }else{ - command_yaw_delta = command_yaw_end - command_yaw_start; - } + if (command_yaw_relative){ + // relative + //command_yaw_dir = (command_yaw_end > 0) ? 1 : -1; + //command_yaw_end += nav_yaw; + //command_yaw_end = wrap_360(command_yaw_end); + command_yaw_delta = next_command.alt * 100; }else{ - if(command_yaw_start > command_yaw_end){ - command_yaw_delta = command_yaw_start - command_yaw_end; - }else{ - command_yaw_delta = 36000 + (command_yaw_start - command_yaw_end); - } + // absolute + command_yaw_end = next_command.alt * 100; + + // calculate the delta travel in deg * 100 + if(command_yaw_dir == 1){ + if(command_yaw_start >= command_yaw_end){ + command_yaw_delta = 36000 - (command_yaw_start - command_yaw_end); + }else{ + command_yaw_delta = command_yaw_end - command_yaw_start; + } + }else{ + if(command_yaw_start > command_yaw_end){ + command_yaw_delta = command_yaw_start - command_yaw_end; + }else{ + command_yaw_delta = 36000 + (command_yaw_start - command_yaw_end); + } + } + command_yaw_delta = wrap_360(command_yaw_delta); } - command_yaw_delta = wrap_360(command_yaw_delta); + // rate to turn deg per second - default is ten - command_yaw_speed = next_command.alt; command_yaw_time = command_yaw_delta / command_yaw_speed; + command_yaw_time *= 1000; + + + // //9000 turn in 10 seconds //command_yaw_time = 9000/ 10 = 900° per second } + /********************************************************************************/ // Verify Condition (May) commands /********************************************************************************/ @@ -452,13 +462,17 @@ bool verify_within_distance() bool verify_yaw() { if((millis() - command_yaw_start_time) > command_yaw_time){ + // time out nav_yaw = command_yaw_end; return true; + }else{ // else we need to be at a certain place // power is a ratio of the time : .5 = half done float power = (float)(millis() - command_yaw_start_time) / (float)command_yaw_time; + nav_yaw = command_yaw_start + ((float)command_yaw_delta * power * command_yaw_dir); + nav_yaw = wrap_360(nav_yaw); return false; } } diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index d669500c6e..f1578e9c99 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -121,7 +121,7 @@ // GCS_PORT // #ifndef GCS_PROTOCOL -# define GCS_PROTOCOL GCS_PROTOCOL_NONE +# define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK #endif ////////////////////////////////////////////////////////////////////////////// @@ -346,7 +346,7 @@ // YAW Control // #ifndef YAW_P -# define YAW_P 0.5 +# define YAW_P 0.25 #endif #ifndef YAW_I # define YAW_I 0.0 diff --git a/ArduCopterMega/control_modes.pde b/ArduCopterMega/control_modes.pde index 2af4e17b5f..4702018672 100644 --- a/ArduCopterMega/control_modes.pde +++ b/ArduCopterMega/control_modes.pde @@ -19,17 +19,17 @@ 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 + #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 if (pulsewidth > 1230 && pulsewidth <= 1360) return 1; if (pulsewidth > 1360 && pulsewidth <= 1490) return 2; @@ -75,14 +75,14 @@ void read_trim_switch() if(trim_flag){ // switch was just released if((millis() - trim_timer) > 2000){ -#if HIL_MODE != HIL_MODE_ATTITUDE + #if HIL_MODE != HIL_MODE_ATTITUDE imu.save(); -#endif + #endif }else{ // set the throttle nominal if(g.rc_3.control_in > 50){ g.throttle_cruise.set(g.rc_3.control_in); - Serial.printf("tnom %d\n", g.throttle_cruise.get()); + //Serial.printf("tnom %d\n", g.throttle_cruise.get()); //save_EEPROM_throttle_cruise(); g.throttle_cruise.save(); @@ -114,7 +114,11 @@ void trim_accel() imu.ax(imu.ax() - 1); } - Serial.printf_P(PSTR("r:%ld p:%ld ax:%f, ay:%f, az:%f\n"), dcm.roll_sensor, dcm.pitch_sensor, - (double)imu.ax(), (double)imu.ay(), (double)imu.az()); + /*Serial.printf_P(PSTR("r:%ld p:%ld ax:%f, ay:%f, az:%f\n"), + dcm.roll_sensor, + dcm.pitch_sensor, + (double)imu.ax(), + (double)imu.ay(), + (double)imu.az());*/ } diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h index febf8da346..3426718f09 100644 --- a/ArduCopterMega/defines.h +++ b/ArduCopterMega/defines.h @@ -255,4 +255,4 @@ #define EEPROM_MAX_ADDR 4096 // parameters get the first 1KiB of EEPROM, remainder is for waypoints #define WP_START_BYTE 0x400 // where in memory home WP is stored + all other WP -#define WP_SIZE 14 +#define WP_SIZE 15 diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde index 092998ec08..958341bbdd 100644 --- a/ArduCopterMega/navigation.pde +++ b/ArduCopterMega/navigation.pde @@ -208,6 +208,7 @@ long get_altitude_above_home(void) return current_loc.alt - home.alt; } +// distance is returned in meters long get_distance(struct Location *loc1, struct Location *loc2) { //if(loc1->lat == 0 || loc1->lng == 0) diff --git a/ArduCopterMega/sensors.pde b/ArduCopterMega/sensors.pde index e1efe7c4cf..7234fad6c1 100644 --- a/ArduCopterMega/sensors.pde +++ b/ArduCopterMega/sensors.pde @@ -20,7 +20,7 @@ void init_barometer(void) ground_pressure = barometer.Press; ground_temperature = barometer.Temp; delay(20); - Serial.printf("barometer.Press %ld\n", barometer.Press); + //Serial.printf("barometer.Press %ld\n", barometer.Press); } for(int i = 0; i < 30; i++){ // We take some readings... diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index f67733e996..12153d75b9 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -12,7 +12,6 @@ static int8_t setup_frame (uint8_t argc, const Menu::arg *argv); static int8_t setup_current (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); -//static int8_t setup_mag_offset (uint8_t argc, const Menu::arg *argv); static int8_t setup_declination (uint8_t argc, const Menu::arg *argv); static int8_t setup_show (uint8_t argc, const Menu::arg *argv); @@ -31,7 +30,6 @@ const struct Menu::command setup_menu_commands[] PROGMEM = { {"current", setup_current}, {"sonar", setup_sonar}, {"compass", setup_compass}, -// {"mag_offset", setup_mag_offset}, {"declination", setup_declination}, {"show", setup_show} }; @@ -44,12 +42,12 @@ int8_t setup_mode(uint8_t argc, const Menu::arg *argv) { // Give the user some guidance - Serial.printf_P(PSTR("Setup Mode\n" - "\n" - "IMPORTANT: if you have not previously set this system up, use the\n" - "'reset' command to initialize the EEPROM to sensible default values\n" - "and then the 'radio' command to configure for your radio.\n" - "\n")); + Serial.printf_P(PSTR("Setup Mode\n")); + //"\n" + //"IMPORTANT: if you have not previously set this system up, use the\n" + //"'reset' command to initialize the EEPROM to sensible default values\n" + //"and then the 'radio' command to configure for your radio.\n" + //"\n")); // Run the setup menu. When the menu exits, we will return to the main menu. setup_menu.run(); @@ -88,7 +86,7 @@ setup_factory(uint8_t argc, const Menu::arg *argv) uint8_t i; int c; - Serial.printf_P(PSTR("\nType 'Y' and hit Enter to perform factory reset, any other key to abort:\n")); + Serial.printf_P(PSTR("\n'Y' + Enter to factory reset, any other key to abort:\n")); do { c = Serial.read(); @@ -97,7 +95,7 @@ setup_factory(uint8_t argc, const Menu::arg *argv) if (('y' != c) && ('Y' != c)) return(-1); AP_Var::erase_all(); - Serial.printf_P(PSTR("\nFACTORY RESET complete - please reset APM to continue")); + Serial.printf_P(PSTR("\nFACTORY RESET complete - reboot APM")); delay(1000); default_log_bitmask(); @@ -124,7 +122,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv) if(g.rc_1.radio_in < 500){ while(1){ - Serial.printf_P(PSTR("\nNo radio; Check connectors.")); + //Serial.printf_P(PSTR("\nNo radio; Check connectors.")); delay(1000); // stop here } @@ -303,7 +301,7 @@ 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")); + //Serial.printf_P(PSTR("\nHold ArduCopter completely still and level.\n")); imu.init_accel(); print_accel_offsets(); @@ -318,35 +316,52 @@ 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("s_kp"))) { + }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); - save_EEPROM_PID(); + g.stabilize_dampener.set_and_save(argv[3].f); - }else if (!strcmp_P(argv[1].str, PSTR("s_kd"))) { - g.stabilize_dampener = argv[2].f; - save_EEPROM_PID(); + g.pid_stabilize_roll.save_gains(); + g.pid_stabilize_pitch.save_gains(); - }else if (!strcmp_P(argv[1].str, PSTR("y_kp"))) { + }else if (!strcmp_P(argv[1].str, PSTR("yaw"))) { g.pid_yaw.kP(argv[2].f); - save_EEPROM_PID(); - }else if (!strcmp_P(argv[1].str, PSTR("y_kd"))) { - g.pid_yaw.kD(argv[2].f); - save_EEPROM_PID(); + g.pid_yaw.save_gains(); + g.hold_yaw_dampener.set_and_save(argv[3].f); - }else if (!strcmp_P(argv[1].str, PSTR("t_kp"))) { + }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); - save_EEPROM_PID(); + 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 if (!strcmp_P(argv[1].str, PSTR("t_kd"))) { - g.pid_baro_throttle.kD(argv[2].f); - save_EEPROM_PID(); }else{ default_gains(); } - report_gains(); } @@ -379,7 +394,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv) } // look for stick input - if (radio_input_switch() == true){ + if (radio_input_switch() == true){ mode++; if(mode >= NUM_MODES) mode = 0; @@ -408,6 +423,7 @@ setup_declination(uint8_t argc, const Menu::arg *argv) report_compass(); } + static int8_t setup_erase(uint8_t argc, const Menu::arg *argv) { @@ -747,11 +763,34 @@ default_gains() /***************************************************************************/ // CLI reports /***************************************************************************/ +void report_wp(byte index = 255) +{ + if(index == 255){ + for(byte i = 0; i <= g.waypoint_total; i++){ + struct Location temp = get_wp_with_index(i); + print_wp(&temp, i); + } + }else{ + struct Location temp = get_wp_with_index(index); + print_wp(&temp, index); + } +} + +void print_wp(struct Location *cmd, byte index) +{ + Serial.printf_P(PSTR("command #: %d id:%d p1:%d p2:%ld p3:%ld p4:%ld \n"), + (int)index, + (int)cmd->id, + (int)cmd->p1, + cmd->alt, + cmd->lat, + cmd->lng); +} void report_current() { read_EEPROM_current(); - Serial.printf_P(PSTR("Current Sensor\n")); + Serial.printf_P(PSTR("Current \n")); print_divider(); print_enabled(g.current_enabled.get()); @@ -762,7 +801,7 @@ void report_current() void report_sonar() { g.sonar_enabled.load(); - Serial.printf_P(PSTR("Sonar Sensor\n")); + Serial.printf_P(PSTR("Sonar\n")); print_divider(); print_enabled(g.sonar_enabled.get()); print_blanks(2); @@ -822,8 +861,8 @@ void report_gains() Serial.printf_P(PSTR("yaw:\n")); print_PID(&g.pid_yaw); - Serial.printf_P(PSTR("Stabilize dampener: %4.3f\n"), (float)g.stabilize_dampener); - Serial.printf_P(PSTR("Yaw Dampener: %4.3f\n\n"), (float)g.hold_yaw_dampener); + Serial.printf_P(PSTR("Stab D: %4.3f\n"), (float)g.stabilize_dampener); + Serial.printf_P(PSTR("Yaw D: %4.3f\n\n"), (float)g.hold_yaw_dampener); // Nav Serial.printf_P(PSTR("Nav:\nlat:\n")); @@ -839,7 +878,7 @@ void report_gains() void report_xtrack() { - Serial.printf_P(PSTR("Crosstrack\n")); + Serial.printf_P(PSTR("XTrack\n")); print_divider(); // radio read_EEPROM_nav(); @@ -889,7 +928,7 @@ void report_compass() print_enabled(g.compass_enabled); // mag declination - Serial.printf_P(PSTR("Mag Delination: %4.4f\n"), + Serial.printf_P(PSTR("Mag Dec: %4.4f\n"), degrees(compass.get_declination())); Vector3f offsets = compass.get_offsets(); @@ -920,12 +959,35 @@ 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"), pid->kP(), pid->kI(), pid->kD(), (long)pid->imax()); + Serial.printf_P(PSTR("P: %4.3f, I:%4.3f, D:%4.3f, IMAX:%ld\n"), + pid->kP(), + pid->kI(), + pid->kD(), + (long)pid->imax()); } 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); @@ -934,6 +996,7 @@ print_radio_values() 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); + //*/ } void @@ -967,7 +1030,8 @@ print_divider(void) Serial.println(""); } -int8_t +// read at 50Hz +bool radio_input_switch(void) { static int8_t bouncer = 0; diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index 3057127122..3532160391 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -6,7 +6,7 @@ static int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv); static int8_t test_radio(uint8_t argc, const Menu::arg *argv); static int8_t test_failsafe(uint8_t argc, const Menu::arg *argv); static int8_t test_stabilize(uint8_t argc, const Menu::arg *argv); -static int8_t test_fbw(uint8_t argc, const Menu::arg *argv); +//static int8_t test_fbw(uint8_t argc, const Menu::arg *argv); static int8_t test_gps(uint8_t argc, const Menu::arg *argv); static int8_t test_adc(uint8_t argc, const Menu::arg *argv); static int8_t test_imu(uint8_t argc, const Menu::arg *argv); @@ -22,6 +22,7 @@ static int8_t test_sonar(uint8_t argc, const Menu::arg *argv); static int8_t test_xbee(uint8_t argc, const Menu::arg *argv); static int8_t test_eedump(uint8_t argc, const Menu::arg *argv); static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv); +static int8_t test_mission(uint8_t argc, const Menu::arg *argv); // This is the help function // PSTR is an AVR macro to read strings from flash memory @@ -47,7 +48,7 @@ const struct Menu::command test_menu_commands[] PROGMEM = { {"radio", test_radio}, {"failsafe", test_failsafe}, {"stabilize", test_stabilize}, - {"fbw", test_fbw}, +// {"fbw", test_fbw}, {"gps", test_gps}, #if HIL_MODE != HIL_MODE_ATTITUDE {"adc", test_adc}, @@ -67,6 +68,7 @@ const struct Menu::command test_menu_commands[] PROGMEM = { {"xbee", test_xbee}, {"eedump", test_eedump}, {"rawgps", test_rawgps}, + {"mission", test_mission}, }; // A Macro to create the Menu @@ -315,7 +317,7 @@ test_stabilize(uint8_t argc, const Menu::arg *argv) } } } - +/* static int8_t test_fbw(uint8_t argc, const Menu::arg *argv) { @@ -408,6 +410,7 @@ test_fbw(uint8_t argc, const Menu::arg *argv) } } } +*/ #if HIL_MODE != HIL_MODE_ATTITUDE static int8_t @@ -737,36 +740,22 @@ test_wp(uint8_t argc, const Menu::arg *argv) // save the alitude above home option + Serial.printf_P(PSTR("Hold altitude ")); if(g.RTL_altitude < 0){ - Serial.printf_P(PSTR("Hold current altitude\n")); + Serial.printf_P(PSTR("\n")); }else{ - Serial.printf_P(PSTR("Hold altitude of %dm\n"), (int)g.RTL_altitude / 100); + Serial.printf_P(PSTR("of %dm\n"), (int)g.RTL_altitude / 100); } Serial.printf_P(PSTR("%d waypoints\n"), (int)g.waypoint_total); Serial.printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius); - Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius); + //Serial.printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius); - for(byte i = 0; i <= g.waypoint_total; i++){ - struct Location temp = get_wp_with_index(i); - test_wp_print(&temp, i); - } + report_wp(); return (0); } -void -test_wp_print(struct Location *cmd, byte index) -{ - Serial.printf_P(PSTR("command #: %d id:%d p1:%d p2:%ld p3:%ld p4:%ld \n"), - (int)index, - (int)cmd->id, - (int)cmd->p1, - cmd->alt, - cmd->lat, - cmd->lng); -} - static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv) { @@ -925,6 +914,53 @@ test_sonar(uint8_t argc, const Menu::arg *argv) } +static int8_t +test_mission(uint8_t argc, const Menu::arg *argv) +{ + print_hit_enter(); + delay(1000); + //write out a basic mission to the EEPROM + Location t; +/*{ + uint8_t id; ///< command id + uint8_t options; ///< options bitmask (1<<0 = relative altitude) + uint8_t p1; ///< param 1 + int32_t alt; ///< param 2 - Altitude in centimeters (meters * 100) + int32_t lat; ///< param 3 - Lattitude * 10**7 + int32_t lng; ///< param 4 - Longitude * 10**7 +}*/ + + { + Location t = {0, 0, 0, 0, 0, 0}; + set_wp_with_index(t,0); + } + + { // CMD opt pitch alt/cm + Location t = {MAV_CMD_NAV_TAKEOFF, 0, 0, 300, 0, 0}; + set_wp_with_index(t,1); + } + { // CMD opt time/ms + Location t = {MAV_CMD_CONDITION_DELAY, 0, 0, 0, 3000, 0}; + set_wp_with_index(t,2); + + } + { // CMD opt dir angle/deg time/ms relative + Location t = {MAV_CMD_CONDITION_YAW, 0, 1, 360, 1000, 1}; + set_wp_with_index(t,3); + } + { // CMD opt + Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0}; + set_wp_with_index(t,4); + } + + g.RTL_altitude.set_and_save(300); + g.waypoint_total.set_and_save(4); + g.waypoint_radius.set_and_save(3); + + test_wp(NULL, NULL); + +} + void print_hit_enter() { Serial.printf_P(PSTR("Hit Enter to exit.\n\n"));