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

Work in progress for using 0 based counting

This commit is contained in:
Jason Short 2011-11-12 21:39:24 -08:00
parent 9e67c7efca
commit fd02ebca74
1 changed files with 10 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
ArduCopter/commands.pde
View File

@ -2,7 +2,7 @@
static void init_commands()
{
// zero is home, but we always load the next command (1), in the code.
// Zero is home, the curren command
g.command_index = 0;
// This are registers for the current may and must commands
@ -28,7 +28,7 @@ static struct Location get_cmd_with_index(int i)
// Find out proper location in memory by using the start_byte position + the index
// --------------------------------------------------------------------------------
if (i > g.command_total) {
if (i >= g.command_total) {
// we do not have a valid command to load
// return a WP with a "Blank" id
temp.id = CMD_BLANK;
@ -77,7 +77,13 @@ static struct Location get_cmd_with_index(int i)
// -------
static void set_command_with_index(struct Location temp, int i)
{
i = constrain(i, 0, g.command_total.get());
Serial.printf("set_command: %d with id: %d\n", i, temp.id);
i = constrain(i, 0, (g.command_total.get() -1));
// store home as 0 altitude!!!
if (i == 0)
temp.alt = 0;
uint32_t mem = WP_START_BYTE + (i * WP_SIZE);
eeprom_write_byte((uint8_t *) mem, temp.id);
@ -100,7 +106,7 @@ static void set_command_with_index(struct Location temp, int i)
static void increment_WP_index()
{
if (g.command_index < g.command_total) {
if (g.command_index < (g.command_total-1)) {
g.command_index++;
}
@ -129,18 +135,6 @@ static int32_t read_alt_to_hold()
// It's not currently used
//********************************************************************************
/*static Location get_LOITER_home_wp()
{
//so we know where we are navigating from
next_WP = current_loc;
// read home position
struct Location temp = get_cmd_with_index(0); // 0 = home
temp.id = MAV_CMD_NAV_LOITER_UNLIM;
temp.alt = read_alt_to_hold();
return temp;
}
*/
/*
This function sets the next waypoint command
It precalculates all the necessary stuff.