Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Added support for WP options

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1742 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-03-05 04:56:58 +00:00
parent b8e904bd54
commit 8fbe11fca5
3 changed files with 33 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
ArduCopterMega/commands.pde
View File

@ -39,19 +39,27 @@ struct Location get_wp_with_index(int i)
// read WP position
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);
mem++;
temp.alt = (long)eeprom_read_dword((uint32_t*)mem);
mem += 4;
temp.lat = (long)eeprom_read_dword((uint32_t*)mem);
mem += 4;
temp.lng = (long)eeprom_read_dword((uint32_t*)mem);
}
// Add on home altitude if we are a nav command
if(temp.id < 50)
if(temp.options & WP_OPT_ALT_RELATIVE){
temp.alt += home.alt;
}
return temp;
}
@ -62,15 +70,25 @@ void set_wp_with_index(struct Location temp, int i)
{
i = constrain(i, 0, g.waypoint_total.get());
if(i > 0 && temp.id < 50){
//if(i > 0 && temp.id < 50){
// temp.options & WP_OPT_LOCATION
// remove home altitude if we are a nav command
// temp.alt -= home.alt;
//}
// Store the location relatove to home
if(temp.options & WP_OPT_ALT_RELATIVE){
temp.alt -= home.alt;
}
uint32_t mem = WP_START_BYTE + (i * WP_SIZE);
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);

10
ArduCopterMega/defines.h
View File

@ -94,7 +94,15 @@
#define LAND 9 // controlled decent rate
#define NUM_MODES 10
// Commands - Note that APM now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library
#define WP_OPT_ALT_RELATIVE (1<<0)
#define WP_OPT_ALT_CHANGE (1<<1)
#define WP_OPT_YAW (1<<2)
// ..
#define WP_OPT_CMD_WAIT (1<<7)
// Commands - Note that APM (1<<9)now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library
#define CMD_BLANK 0 // there is no command stored in the mem location requested
#define NO_COMMAND 0

7
ArduCopterMega/navigation.pde
View File

@ -100,7 +100,10 @@ void calc_bearing_error()
void calc_altitude_error()
{
if(control_mode == AUTO && offset_altitude != 0) {
//if(control_mode == AUTO && offset_altitude != 0) {
if(next_WP.options & WP_OPT_ALT_CHANGE) {
target_altitude = next_WP.alt;
}else{
// limit climb rates - we draw a straight line between first location and edge of waypoint_radius
target_altitude = next_WP.alt - ((wp_distance * offset_altitude) / (wp_totalDistance - g.waypoint_radius));
@ -110,8 +113,6 @@ void calc_altitude_error()
}else{
target_altitude = constrain(target_altitude, prev_WP.alt, next_WP.alt);
}
}else{
target_altitude = next_WP.alt;
}
altitude_error = target_altitude - current_loc.alt;