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

implementing yaw tracking. 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1821 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-03-27 21:26:34 +00:00
parent 2c17164de4
commit eee391e6d9
7 changed files with 56 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduCopterMega/ArduCopterMega.pde
View File

@ -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(&current_loc, &target_WP);
}
}else{
switch(control_mode){
case RTL:

3
ArduCopterMega/Attitude.pde
View File

@ -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
}

4
ArduCopterMega/commands.pde
View File

@ -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);

18
ArduCopterMega/commands_logic.pde
View File

@ -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;

5
ArduCopterMega/defines.h
View File

@ -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

5
ArduCopterMega/system.pde
View File

@ -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)

40
ArduCopterMega/test.pde
View File

@ -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();
}
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);