diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 4881a787bf..c3f10000a3 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -313,7 +313,7 @@ boolean land_complete; int landing_distance; // meters; long old_alt; // used for managing altitude rates int velocity_land; -bool nav_yaw_towards_wp; // point at the next WP +byte yaw_tracking = TRACK_NONE; // no tracking, point at next wp, or at a target // Loiter management // ----------------- @@ -372,6 +372,7 @@ struct Location home; // home location struct Location prev_WP; // last waypoint struct Location current_loc; // current location struct Location next_WP; // next waypoint +struct Location target_WP; // where do we want to you towards? struct Location tell_command; // command for telemetry struct Location next_command; // command preloaded long target_altitude; // used for @@ -447,6 +448,9 @@ void loop() G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator mainLoop_count++; + if(delta_ms_fast_loop > 11){ + Serial.println(delta_ms_fast_loop,DEC); + } // Execute the fast loop // --------------------- fast_loop(); @@ -1084,6 +1088,10 @@ void update_navigation() if(control_mode == AUTO || control_mode == GCS_AUTO){ verify_commands(); + if(yaw_tracking & TRACK_TARGET_WP){ + nav_yaw = get_bearing(¤t_loc, &target_WP); + } + }else{ switch(control_mode){ case RTL: diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index 5921b66741..19d8e4bbc3 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -269,8 +269,9 @@ void output_manual_yaw() void auto_yaw() { - if(nav_yaw_towards_wp){ + if(yaw_tracking & TRACK_NEXT_WP){ nav_yaw = target_bearing; } + output_yaw_with_hold(true); // hold yaw } diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde index 7d91525e76..8eb9b24770 100644 --- a/ArduCopterMega/commands.pde +++ b/ArduCopterMega/commands.pde @@ -47,7 +47,7 @@ struct Location get_wp_with_index(int i) temp.p1 = eeprom_read_byte((uint8_t*)mem); mem++; - temp.alt = (long)eeprom_read_dword((uint32_t*)mem); + temp.alt = (long)eeprom_read_dword((uint32_t*)mem); // alt is stored in CM! mem += 4; temp.lat = (long)eeprom_read_dword((uint32_t*)mem); @@ -80,7 +80,7 @@ void set_wp_with_index(struct Location temp, int i) eeprom_write_byte((uint8_t *) mem, temp.p1); mem++; - eeprom_write_dword((uint32_t *) mem, temp.alt); + eeprom_write_dword((uint32_t *) mem, temp.alt); // alt is stored in CM! mem += 4; eeprom_write_dword((uint32_t *) mem, temp.lat); diff --git a/ArduCopterMega/commands_logic.pde b/ArduCopterMega/commands_logic.pde index 4889cce846..934777aab3 100644 --- a/ArduCopterMega/commands_logic.pde +++ b/ArduCopterMega/commands_logic.pde @@ -38,7 +38,6 @@ void handle_process_must() case MAV_CMD_NAV_RETURN_TO_LAUNCH: do_RTL(); break; - default: break; } @@ -100,6 +99,9 @@ void handle_process_now() case MAV_CMD_DO_REPEAT_RELAY: do_repeat_relay(); break; + + case MAV_CMD_NAV_ORIENTATION_TARGET: + do_target_yaw(); } } @@ -358,9 +360,7 @@ void do_within_distance() void do_yaw() { -// { // CMD opt dir angle/deg deg/s relative -// Location t = {MAV_CMD_CONDITION_YAW, 0, 1, 360, 60, 1}; - + yaw_tracking = TRACK_NONE; // target angle in degrees command_yaw_start = nav_yaw; // current position @@ -481,6 +481,16 @@ bool verify_yaw() // Do (Now) commands /********************************************************************************/ +void do_target_yaw() +{ + yaw_tracking = next_command.p1; + + if(yaw_tracking & TRACK_TARGET_WP){ + target_WP = next_command; + } + +} + void do_loiter_at_location() { next_WP = current_loc; diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h index 3426718f09..3fdf077fe6 100644 --- a/ArduCopterMega/defines.h +++ b/ArduCopterMega/defines.h @@ -100,6 +100,11 @@ #define CMD_BLANK 0 // there is no command stored in the mem location requested #define NO_COMMAND 0 +// Nav Yaw Tracking +#define TRACK_NONE 1 +#define TRACK_NEXT_WP 2 +#define TRACK_TARGET_WP 4 + //repeating events #define NO_REPEAT 0 #define CH_5_TOGGLE 1 diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index fd55719b5e..683e15fc38 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -276,6 +276,11 @@ void set_mode(byte mode) // disarm motors temp motor_auto_safe = false; } + + + // clear our tracking behaviors + yaw_tracking = TRACK_NONE; + //send_text_P(SEVERITY_LOW,PSTR("control mode")); //Serial.printf("set mode: %d old: %d\n", (int)mode, (int)control_mode); switch(control_mode) diff --git a/ArduCopterMega/test.pde b/ArduCopterMega/test.pde index 3532160391..3d891bea02 100644 --- a/ArduCopterMega/test.pde +++ b/ArduCopterMega/test.pde @@ -309,8 +309,8 @@ test_stabilize(uint8_t argc, const Menu::arg *argv) if(Serial.available() > 0){ if(g.compass_enabled){ compass.save_offsets(); + report_compass(); } - report_compass(); return (0); } @@ -930,28 +930,26 @@ test_mission(uint8_t argc, const Menu::arg *argv) int32_t lng; ///< param 4 - Longitude * 10**7 }*/ - { - Location t = {0, 0, 0, 0, 0, 0}; - set_wp_with_index(t,0); - } + // clear home + {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 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);} - } - { // 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);