Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

uncrustify ArduPlane/commands.pde

Browse Source
This commit is contained in:
uncrustify 2012-08-21 19:19:51 -07:00 committed by Pat Hickey
parent 43991712be
commit ff4afa767b
1 changed files with 144 additions and 144 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

288
ArduPlane/commands.pde
View File

@ -1,117 +1,117 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/* /*
logic for dealing with the current command in the mission and home location * logic for dealing with the current command in the mission and home location
*/ */
static void init_commands() static void init_commands()
{ {
g.command_index.set_and_save(0); g.command_index.set_and_save(0);
nav_command_ID = NO_COMMAND; nav_command_ID = NO_COMMAND;
non_nav_command_ID = NO_COMMAND; non_nav_command_ID = NO_COMMAND;
next_nav_command.id = CMD_BLANK; next_nav_command.id = CMD_BLANK;
} }
static void update_auto() static void update_auto()
{ {
if (g.command_index >= g.command_total) { if (g.command_index >= g.command_total) {
handle_no_commands(); handle_no_commands();
if(g.command_total == 0) { if(g.command_total == 0) {
next_WP.lat = home.lat + 1000; // so we don't have bad calcs next_WP.lat = home.lat + 1000; // so we don't have bad calcs
next_WP.lng = home.lng + 1000; // so we don't have bad calcs next_WP.lng = home.lng + 1000; // so we don't have bad calcs
} }
} else { } else {
if(g.command_index != 0) { if(g.command_index != 0) {
g.command_index = nav_command_index; g.command_index = nav_command_index;
nav_command_index--; nav_command_index--;
} }
nav_command_ID = NO_COMMAND; nav_command_ID = NO_COMMAND;
non_nav_command_ID = NO_COMMAND; non_nav_command_ID = NO_COMMAND;
next_nav_command.id = CMD_BLANK; next_nav_command.id = CMD_BLANK;
process_next_command(); process_next_command();
} }
} }
// this is only used by an air-start // this is only used by an air-start
static void reload_commands_airstart() static void reload_commands_airstart()
{ {
init_commands(); init_commands();
decrement_cmd_index(); decrement_cmd_index();
} }
// Getters // Getters
// ------- // -------
static struct Location get_cmd_with_index(int16_t i) static struct Location get_cmd_with_index(int16_t i)
{ {
struct Location temp; struct Location temp;
int32_t mem; int32_t mem;
// Find out proper location in memory by using the start_byte position + the index // Find out proper location in memory by using the start_byte position + the index
// -------------------------------------------------------------------------------- // --------------------------------------------------------------------------------
if (i > g.command_total) { if (i > g.command_total) {
memset(&temp, 0, sizeof(temp)); memset(&temp, 0, sizeof(temp));
temp.id = CMD_BLANK; temp.id = CMD_BLANK;
}else{ }else{
// read WP position // read WP position
mem = (WP_START_BYTE) + (i * WP_SIZE); mem = (WP_START_BYTE) + (i * WP_SIZE);
temp.id = eeprom_read_byte((uint8_t*)mem); temp.id = eeprom_read_byte((uint8_t*)mem);
mem++; mem++;
temp.options = eeprom_read_byte((uint8_t*)mem); temp.options = eeprom_read_byte((uint8_t*)mem);
mem++; mem++;
temp.p1 = eeprom_read_byte((uint8_t*)mem); temp.p1 = eeprom_read_byte((uint8_t*)mem);
mem++; mem++;
temp.alt = (long)eeprom_read_dword((uint32_t*)mem); temp.alt = (long)eeprom_read_dword((uint32_t*)mem);
mem += 4; mem += 4;
temp.lat = (long)eeprom_read_dword((uint32_t*)mem); temp.lat = (long)eeprom_read_dword((uint32_t*)mem);
mem += 4; mem += 4;
temp.lng = (long)eeprom_read_dword((uint32_t*)mem); temp.lng = (long)eeprom_read_dword((uint32_t*)mem);
} }
// Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative // Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative
if ((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && if ((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) &&
(temp.options & MASK_OPTIONS_RELATIVE_ALT) && (temp.options & MASK_OPTIONS_RELATIVE_ALT) &&
(temp.lat != 0 || temp.lng != 0)) { (temp.lat != 0 || temp.lng != 0)) {
temp.alt += home.alt; temp.alt += home.alt;
} }
return temp; return temp;
} }
// Setters // Setters
// ------- // -------
static void set_cmd_with_index(struct Location temp, int16_t i) static void set_cmd_with_index(struct Location temp, int16_t i)
{ {
i = constrain(i, 0, g.command_total.get()); i = constrain(i, 0, g.command_total.get());
intptr_t mem = WP_START_BYTE + (i * WP_SIZE); intptr_t mem = WP_START_BYTE + (i * WP_SIZE);
// Set altitude options bitmask // Set altitude options bitmask
// XXX What is this trying to do? // XXX What is this trying to do?
if (temp.options & MASK_OPTIONS_RELATIVE_ALT && i != 0){ if (temp.options & MASK_OPTIONS_RELATIVE_ALT && i != 0) {
temp.options = MASK_OPTIONS_RELATIVE_ALT; temp.options = MASK_OPTIONS_RELATIVE_ALT;
} else { } else {
temp.options = 0; temp.options = 0;
} }
eeprom_write_byte((uint8_t *) mem, temp.id); eeprom_write_byte((uint8_t *) mem, temp.id);
mem++; mem++;
eeprom_write_byte((uint8_t *) mem, temp.options); eeprom_write_byte((uint8_t *) mem, temp.options);
mem++; mem++;
eeprom_write_byte((uint8_t *) mem, temp.p1); eeprom_write_byte((uint8_t *) mem, temp.p1);
mem++; mem++;
eeprom_write_dword((uint32_t *) mem, temp.alt); eeprom_write_dword((uint32_t *) mem, temp.alt);
mem += 4; mem += 4;
eeprom_write_dword((uint32_t *) mem, temp.lat); eeprom_write_dword((uint32_t *) mem, temp.lat);
mem += 4; mem += 4;
eeprom_write_dword((uint32_t *) mem, temp.lng); eeprom_write_dword((uint32_t *) mem, temp.lng);
} }
static void decrement_cmd_index() static void decrement_cmd_index()
@ -123,26 +123,26 @@ static void decrement_cmd_index()
static int32_t read_alt_to_hold() static int32_t read_alt_to_hold()
{ {
if (g.RTL_altitude_cm < 0) { if (g.RTL_altitude_cm < 0) {
return current_loc.alt; return current_loc.alt;
} }
return g.RTL_altitude_cm + home.alt; return g.RTL_altitude_cm + home.alt;
} }
/* /*
This function stores waypoint commands * This function stores waypoint commands
It looks to see what the next command type is and finds the last command. * It looks to see what the next command type is and finds the last command.
*/ */
static void set_next_WP(struct Location *wp) static void set_next_WP(struct Location *wp)
{ {
// copy the current WP into the OldWP slot // copy the current WP into the OldWP slot
// --------------------------------------- // ---------------------------------------
prev_WP = next_WP; prev_WP = next_WP;
// Load the next_WP slot // Load the next_WP slot
// --------------------- // ---------------------
next_WP = *wp; next_WP = *wp;
// if lat and lon is zero, then use current lat/lon // if lat and lon is zero, then use current lat/lon
// this allows a mission to contain a "loiter on the spot" // this allows a mission to contain a "loiter on the spot"
@ -167,96 +167,96 @@ static void set_next_WP(struct Location *wp)
prev_WP = current_loc; prev_WP = current_loc;
} }
// used to control FBW and limit the rate of climb // used to control FBW and limit the rate of climb
// ----------------------------------------------- // -----------------------------------------------
target_altitude_cm = current_loc.alt; target_altitude_cm = current_loc.alt;
if(prev_WP.id != MAV_CMD_NAV_TAKEOFF && prev_WP.alt != home.alt && (next_WP.id == MAV_CMD_NAV_WAYPOINT || next_WP.id == MAV_CMD_NAV_LAND)) if(prev_WP.id != MAV_CMD_NAV_TAKEOFF && prev_WP.alt != home.alt && (next_WP.id == MAV_CMD_NAV_WAYPOINT || next_WP.id == MAV_CMD_NAV_LAND))
offset_altitude_cm = next_WP.alt - prev_WP.alt; offset_altitude_cm = next_WP.alt - prev_WP.alt;
else else
offset_altitude_cm = 0; offset_altitude_cm = 0;
// zero out our loiter vals to watch for missed waypoints // zero out our loiter vals to watch for missed waypoints
loiter_delta = 0; loiter_delta = 0;
loiter_sum = 0; loiter_sum = 0;
loiter_total = 0; loiter_total = 0;
// this is handy for the groundstation // this is handy for the groundstation
wp_totalDistance = get_distance(&current_loc, &next_WP); wp_totalDistance = get_distance(&current_loc, &next_WP);
wp_distance = wp_totalDistance; wp_distance = wp_totalDistance;
target_bearing_cd = get_bearing_cd(&current_loc, &next_WP); target_bearing_cd = get_bearing_cd(&current_loc, &next_WP);
nav_bearing_cd = target_bearing_cd; nav_bearing_cd = target_bearing_cd;
// to check if we have missed the WP // to check if we have missed the WP
// ---------------------------- // ----------------------------
old_target_bearing_cd = target_bearing_cd; old_target_bearing_cd = target_bearing_cd;
// set a new crosstrack bearing // set a new crosstrack bearing
// ---------------------------- // ----------------------------
reset_crosstrack(); reset_crosstrack();
} }
static void set_guided_WP(void) static void set_guided_WP(void)
{ {
// copy the current location into the OldWP slot // copy the current location into the OldWP slot
// --------------------------------------- // ---------------------------------------
prev_WP = current_loc; prev_WP = current_loc;
// Load the next_WP slot // Load the next_WP slot
// --------------------- // ---------------------
next_WP = guided_WP; next_WP = guided_WP;
// used to control FBW and limit the rate of climb // used to control FBW and limit the rate of climb
// ----------------------------------------------- // -----------------------------------------------
target_altitude_cm = current_loc.alt; target_altitude_cm = current_loc.alt;
offset_altitude_cm = next_WP.alt - prev_WP.alt; offset_altitude_cm = next_WP.alt - prev_WP.alt;
// this is handy for the groundstation // this is handy for the groundstation
wp_totalDistance = get_distance(&current_loc, &next_WP); wp_totalDistance = get_distance(&current_loc, &next_WP);
wp_distance = wp_totalDistance; wp_distance = wp_totalDistance;
target_bearing_cd = get_bearing_cd(&current_loc, &next_WP); target_bearing_cd = get_bearing_cd(&current_loc, &next_WP);
// to check if we have missed the WP // to check if we have missed the WP
// ---------------------------- // ----------------------------
old_target_bearing_cd = target_bearing_cd; old_target_bearing_cd = target_bearing_cd;
// set a new crosstrack bearing // set a new crosstrack bearing
// ---------------------------- // ----------------------------
reset_crosstrack(); reset_crosstrack();
} }
// run this at setup on the ground // run this at setup on the ground
// ------------------------------- // -------------------------------
void init_home() void init_home()
{ {
gcs_send_text_P(SEVERITY_LOW, PSTR("init home")); gcs_send_text_P(SEVERITY_LOW, PSTR("init home"));
// block until we get a good fix // block until we get a good fix
// ----------------------------- // -----------------------------
while (!g_gps->new_data || !g_gps->fix) { while (!g_gps->new_data || !g_gps->fix) {
g_gps->update(); g_gps->update();
} }
home.id = MAV_CMD_NAV_WAYPOINT; home.id = MAV_CMD_NAV_WAYPOINT;
home.lng = g_gps->longitude; // Lon * 10**7 home.lng = g_gps->longitude; // Lon * 10**7
home.lat = g_gps->latitude; // Lat * 10**7 home.lat = g_gps->latitude; // Lat * 10**7
home.alt = max(g_gps->altitude, 0); home.alt = max(g_gps->altitude, 0);
home_is_set = true; home_is_set = true;
gcs_send_text_fmt(PSTR("gps alt: %lu"), (unsigned long)home.alt); gcs_send_text_fmt(PSTR("gps alt: %lu"), (unsigned long)home.alt);
// Save Home to EEPROM - Command 0 // Save Home to EEPROM - Command 0
// ------------------- // -------------------
set_cmd_with_index(home, 0); set_cmd_with_index(home, 0);
// Save prev loc // Save prev loc
// ------------- // -------------
next_WP = prev_WP = home; next_WP = prev_WP = home;
// Load home for a default guided_WP // Load home for a default guided_WP
// ------------- // -------------
guided_WP = home; guided_WP = home;
guided_WP.alt += g.RTL_altitude_cm; guided_WP.alt += g.RTL_altitude_cm;
} }