Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 12:14:10 -04:00
Code Issues Packages Projects Releases Wiki Activity

uncrustify ArduCopter/commands.pde

Browse Source
This commit is contained in:
uncrustify 2012-08-16 17:50:03 -07:00 committed by Pat Hickey
parent a75d477071
commit 2d398aa663
1 changed files with 138 additions and 138 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

276
ArduCopter/commands.pde
View File

@ -2,70 +2,70 @@
static void init_commands() static void init_commands()
{ {
g.command_index = NO_COMMAND; g.command_index = NO_COMMAND;
command_nav_index = NO_COMMAND; command_nav_index = NO_COMMAND;
command_cond_index = NO_COMMAND; command_cond_index = NO_COMMAND;
prev_nav_index = NO_COMMAND; prev_nav_index = NO_COMMAND;
command_cond_queue.id = NO_COMMAND; command_cond_queue.id = NO_COMMAND;
command_nav_queue.id = NO_COMMAND; command_nav_queue.id = NO_COMMAND;
fast_corner = false; fast_corner = false;
// default Yaw tracking // default Yaw tracking
yaw_tracking = MAV_ROI_WPNEXT; yaw_tracking = MAV_ROI_WPNEXT;
} }
// Getters // Getters
// ------- // -------
static struct Location get_cmd_with_index(int i) static struct Location get_cmd_with_index(int i)
{ {
struct Location temp; struct Location temp;
// 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) {
// we do not have a valid command to load // we do not have a valid command to load
// return a WP with a "Blank" id // return a WP with a "Blank" id
temp.id = CMD_BLANK; temp.id = CMD_BLANK;
// no reason to carry on // no reason to carry on
return temp; return temp;
}else{ }else{
// we can load a command, we don't process it yet // we can load a command, we don't process it yet
// read WP position // read WP position
int32_t mem = (WP_START_BYTE) + (i * WP_SIZE); int32_t 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 = eeprom_read_dword((uint32_t*)mem); // alt is stored in CM! Alt is stored relative! temp.alt = eeprom_read_dword((uint32_t*)mem); // alt is stored in CM! Alt is stored relative!
mem += 4; mem += 4;
temp.lat = eeprom_read_dword((uint32_t*)mem); // lat is stored in decimal * 10,000,000 temp.lat = eeprom_read_dword((uint32_t*)mem); // lat is stored in decimal * 10,000,000
mem += 4; mem += 4;
temp.lng = eeprom_read_dword((uint32_t*)mem); // lon is stored in decimal * 10,000,000 temp.lng = eeprom_read_dword((uint32_t*)mem); // lon is stored in decimal * 10,000,000
} }
// 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) && temp.options & MASK_OPTIONS_RELATIVE_ALT){ //if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & MASK_OPTIONS_RELATIVE_ALT){
//temp.alt += home.alt; //temp.alt += home.alt;
//} //}
if(temp.options & WP_OPTION_RELATIVE){ if(temp.options & WP_OPTION_RELATIVE) {
// If were relative, just offset from home // If were relative, just offset from home
temp.lat += home.lat; temp.lat += home.lat;
temp.lng += home.lng; temp.lng += home.lng;
} }
return temp; return temp;
} }
// Setters // Setters
@ -73,49 +73,49 @@ static struct Location get_cmd_with_index(int i)
static void set_cmd_with_index(struct Location temp, int i) static void set_cmd_with_index(struct Location temp, int i)
{ {
i = constrain(i, 0, g.command_total.get()); i = constrain(i, 0, g.command_total.get());
//Serial.printf("set_command: %d with id: %d\n", i, temp.id); //Serial.printf("set_command: %d with id: %d\n", i, temp.id);
// store home as 0 altitude!!! // store home as 0 altitude!!!
// Home is always a MAV_CMD_NAV_WAYPOINT (16) // Home is always a MAV_CMD_NAV_WAYPOINT (16)
if (i == 0){ if (i == 0) {
temp.alt = 0; temp.alt = 0;
temp.id = MAV_CMD_NAV_WAYPOINT; temp.id = MAV_CMD_NAV_WAYPOINT;
} }
uint32_t mem = WP_START_BYTE + (i * WP_SIZE); uint32_t mem = WP_START_BYTE + (i * WP_SIZE);
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); // Alt is stored in CM! eeprom_write_dword((uint32_t *) mem, temp.alt); // Alt is stored in CM!
mem += 4; mem += 4;
eeprom_write_dword((uint32_t *) mem, temp.lat); // Lat is stored in decimal degrees * 10^7 eeprom_write_dword((uint32_t *) mem, temp.lat); // Lat is stored in decimal degrees * 10^7
mem += 4; mem += 4;
eeprom_write_dword((uint32_t *) mem, temp.lng); // Long is stored in decimal degrees * 10^7 eeprom_write_dword((uint32_t *) mem, temp.lng); // Long is stored in decimal degrees * 10^7
// Make sure our WP_total // Make sure our WP_total
if(g.command_total < (i+1)) if(g.command_total < (i+1))
g.command_total.set_and_save(i+1); g.command_total.set_and_save(i+1);
} }
static int32_t get_RTL_alt() static int32_t get_RTL_alt()
{ {
if(g.RTL_altitude <= 0){ if(g.RTL_altitude <= 0) {
return current_loc.alt; return current_loc.alt;
}else if (g.RTL_altitude < current_loc.alt){ }else if (g.RTL_altitude < current_loc.alt) {
return current_loc.alt; return current_loc.alt;
}else{ }else{
return g.RTL_altitude; return g.RTL_altitude;
} }
} }
@ -125,61 +125,61 @@ static int32_t get_RTL_alt()
//******************************************************************************** //********************************************************************************
/* /*
This function sets the next waypoint command * This function sets the next waypoint command
It precalculates all the necessary stuff. * It precalculates all the necessary stuff.
*/ */
static void set_next_WP(struct Location *wp) static void set_next_WP(struct Location *wp)
{ {
//SendDebug("MSG <set_next_wp> wp_index: "); //SendDebug("MSG <set_next_wp> wp_index: ");
//SendDebugln(g.command_index, DEC); //SendDebugln(g.command_index, DEC);
// copy the current WP into the OldWP slot // copy the current WP into the OldWP slot
// --------------------------------------- // ---------------------------------------
if (next_WP.lat == 0 || command_nav_index <= 1){ if (next_WP.lat == 0 || command_nav_index <= 1) {
prev_WP = current_loc; prev_WP = current_loc;
}else{ }else{
if (get_distance_cm(&filtered_loc, &next_WP) < 500) if (get_distance_cm(&filtered_loc, &next_WP) < 500)
prev_WP = next_WP; prev_WP = next_WP;
else else
prev_WP = current_loc; prev_WP = current_loc;
} }
//Serial.printf("set_next_WP #%d, ", command_nav_index); //Serial.printf("set_next_WP #%d, ", command_nav_index);
//print_wp(&prev_WP, command_nav_index -1); //print_wp(&prev_WP, command_nav_index -1);
// Load the next_WP slot // Load the next_WP slot
// --------------------- // ---------------------
next_WP = *wp; next_WP = *wp;
// used to control and limit the rate of climb // used to control and limit the rate of climb
// ------------------------------------------- // -------------------------------------------
// We don't set next WP below 1m // We don't set next WP below 1m
next_WP.alt = max(next_WP.alt, 100); next_WP.alt = max(next_WP.alt, 100);
// Save new altitude so we can track it for climb_rate // Save new altitude so we can track it for climb_rate
set_new_altitude(next_WP.alt); set_new_altitude(next_WP.alt);
// this is used to offset the shrinking longitude as we go towards the poles // this is used to offset the shrinking longitude as we go towards the poles
float rads = (fabs((float)next_WP.lat)/t7) * 0.0174532925; float rads = (fabs((float)next_WP.lat)/t7) * 0.0174532925;
scaleLongDown = cos(rads); scaleLongDown = cos(rads);
scaleLongUp = 1.0f/cos(rads); scaleLongUp = 1.0f/cos(rads);
// this is handy for the groundstation // this is handy for the groundstation
// ----------------------------------- // -----------------------------------
wp_distance = get_distance_cm(&filtered_loc, &next_WP); wp_distance = get_distance_cm(&filtered_loc, &next_WP);
target_bearing = get_bearing_cd(&prev_WP, &next_WP); target_bearing = get_bearing_cd(&prev_WP, &next_WP);
// calc the location error: // calc the location error:
calc_location_error(&next_WP); calc_location_error(&next_WP);
// to check if we have missed the WP // to check if we have missed the WP
// --------------------------------- // ---------------------------------
original_target_bearing = target_bearing; original_target_bearing = target_bearing;
// reset speed governer // reset speed governer
// -------------------- // --------------------
waypoint_speed_gov = g_gps->ground_speed; waypoint_speed_gov = g_gps->ground_speed;
} }
@ -187,34 +187,34 @@ static void set_next_WP(struct Location *wp)
// ------------------------------- // -------------------------------
static void init_home() static void init_home()
{ {
home_is_set = true; home_is_set = true;
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 = 0; // Home is always 0 home.alt = 0; // Home is always 0
// to point yaw towards home until we set it with Mavlink // to point yaw towards home until we set it with Mavlink
target_WP = home; target_WP = home;
// Save Home to EEPROM // Save Home to EEPROM
// ------------------- // -------------------
// no need to save this to EPROM // no need to save this to EPROM
set_cmd_with_index(home, 0); set_cmd_with_index(home, 0);
//print_wp(&home, 0); //print_wp(&home, 0);
if (g.log_bitmask & MASK_LOG_CMD) if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(0, &home); Log_Write_Cmd(0, &home);
// Save prev loc this makes the calcs look better before commands are loaded // Save prev loc this makes the calcs look better before commands are loaded
prev_WP = home; prev_WP = home;
// in case we RTL // in case we RTL
next_WP = home; next_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; guided_WP.alt += g.RTL_altitude;
} }