mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 02:18:29 -04:00
uncrustify ArduPlane/commands.pde
This commit is contained in:
uncrustify
Pat Hickey
parent
f2c523b6f0
commit
57f1bd123f
@ -1,117 +1,117 @@
|
||||
// -*- 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()
|
||||
{
|
||||
g.command_index.set_and_save(0);
|
||||
nav_command_ID = NO_COMMAND;
|
||||
non_nav_command_ID = NO_COMMAND;
|
||||
next_nav_command.id = CMD_BLANK;
|
||||
nav_command_ID = NO_COMMAND;
|
||||
non_nav_command_ID = NO_COMMAND;
|
||||
next_nav_command.id = CMD_BLANK;
|
||||
}
|
||||
|
||||
static void update_auto()
|
||||
{
|
||||
if (g.command_index >= g.command_total) {
|
||||
handle_no_commands();
|
||||
if(g.command_total == 0) {
|
||||
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
|
||||
}
|
||||
} else {
|
||||
if(g.command_index != 0) {
|
||||
g.command_index = nav_command_index;
|
||||
nav_command_index--;
|
||||
}
|
||||
nav_command_ID = NO_COMMAND;
|
||||
non_nav_command_ID = NO_COMMAND;
|
||||
next_nav_command.id = CMD_BLANK;
|
||||
process_next_command();
|
||||
}
|
||||
if (g.command_index >= g.command_total) {
|
||||
handle_no_commands();
|
||||
if(g.command_total == 0) {
|
||||
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
|
||||
}
|
||||
} else {
|
||||
if(g.command_index != 0) {
|
||||
g.command_index = nav_command_index;
|
||||
nav_command_index--;
|
||||
}
|
||||
nav_command_ID = NO_COMMAND;
|
||||
non_nav_command_ID = NO_COMMAND;
|
||||
next_nav_command.id = CMD_BLANK;
|
||||
process_next_command();
|
||||
}
|
||||
}
|
||||
|
||||
// this is only used by an air-start
|
||||
static void reload_commands_airstart()
|
||||
{
|
||||
init_commands();
|
||||
decrement_cmd_index();
|
||||
init_commands();
|
||||
decrement_cmd_index();
|
||||
}
|
||||
|
||||
// Getters
|
||||
// -------
|
||||
static struct Location get_cmd_with_index(int16_t i)
|
||||
{
|
||||
struct Location temp;
|
||||
int32_t mem;
|
||||
struct Location temp;
|
||||
int32_t mem;
|
||||
|
||||
// Find out proper location in memory by using the start_byte position + the index
|
||||
// --------------------------------------------------------------------------------
|
||||
if (i > g.command_total) {
|
||||
memset(&temp, 0, sizeof(temp));
|
||||
temp.id = CMD_BLANK;
|
||||
}else{
|
||||
// read WP position
|
||||
mem = (WP_START_BYTE) + (i * WP_SIZE);
|
||||
temp.id = eeprom_read_byte((uint8_t*)mem);
|
||||
// Find out proper location in memory by using the start_byte position + the index
|
||||
// --------------------------------------------------------------------------------
|
||||
if (i > g.command_total) {
|
||||
memset(&temp, 0, sizeof(temp));
|
||||
temp.id = CMD_BLANK;
|
||||
}else{
|
||||
// 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.options = eeprom_read_byte((uint8_t*)mem);
|
||||
|
||||
mem++;
|
||||
temp.p1 = 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++;
|
||||
temp.alt = (long)eeprom_read_dword((uint32_t*)mem);
|
||||
|
||||
mem += 4;
|
||||
temp.lat = (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);
|
||||
}
|
||||
mem += 4;
|
||||
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
|
||||
if ((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) &&
|
||||
(temp.options & MASK_OPTIONS_RELATIVE_ALT) &&
|
||||
// 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) &&
|
||||
(temp.lat != 0 || temp.lng != 0)) {
|
||||
temp.alt += home.alt;
|
||||
}
|
||||
temp.alt += home.alt;
|
||||
}
|
||||
|
||||
return temp;
|
||||
return temp;
|
||||
}
|
||||
|
||||
// Setters
|
||||
// -------
|
||||
static void set_cmd_with_index(struct Location temp, int16_t i)
|
||||
{
|
||||
i = constrain(i, 0, g.command_total.get());
|
||||
intptr_t mem = WP_START_BYTE + (i * WP_SIZE);
|
||||
i = constrain(i, 0, g.command_total.get());
|
||||
intptr_t mem = WP_START_BYTE + (i * WP_SIZE);
|
||||
|
||||
// Set altitude options bitmask
|
||||
// XXX What is this trying to do?
|
||||
if (temp.options & MASK_OPTIONS_RELATIVE_ALT && i != 0){
|
||||
temp.options = MASK_OPTIONS_RELATIVE_ALT;
|
||||
} else {
|
||||
temp.options = 0;
|
||||
}
|
||||
// Set altitude options bitmask
|
||||
// XXX What is this trying to do?
|
||||
if (temp.options & MASK_OPTIONS_RELATIVE_ALT && i != 0) {
|
||||
temp.options = MASK_OPTIONS_RELATIVE_ALT;
|
||||
} else {
|
||||
temp.options = 0;
|
||||
}
|
||||
|
||||
eeprom_write_byte((uint8_t *) mem, temp.id);
|
||||
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.options);
|
||||
|
||||
mem++;
|
||||
eeprom_write_byte((uint8_t *) mem, temp.p1);
|
||||
mem++;
|
||||
eeprom_write_byte((uint8_t *) mem, temp.p1);
|
||||
|
||||
mem++;
|
||||
eeprom_write_dword((uint32_t *) mem, temp.alt);
|
||||
mem++;
|
||||
eeprom_write_dword((uint32_t *) mem, temp.alt);
|
||||
|
||||
mem += 4;
|
||||
eeprom_write_dword((uint32_t *) mem, temp.lat);
|
||||
mem += 4;
|
||||
eeprom_write_dword((uint32_t *) mem, temp.lat);
|
||||
|
||||
mem += 4;
|
||||
eeprom_write_dword((uint32_t *) mem, temp.lng);
|
||||
mem += 4;
|
||||
eeprom_write_dword((uint32_t *) mem, temp.lng);
|
||||
}
|
||||
|
||||
static void decrement_cmd_index()
|
||||
@ -123,26 +123,26 @@ static void decrement_cmd_index()
|
||||
|
||||
static int32_t read_alt_to_hold()
|
||||
{
|
||||
if (g.RTL_altitude_cm < 0) {
|
||||
return current_loc.alt;
|
||||
if (g.RTL_altitude_cm < 0) {
|
||||
return current_loc.alt;
|
||||
}
|
||||
return g.RTL_altitude_cm + home.alt;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
This function stores waypoint commands
|
||||
It looks to see what the next command type is and finds the last command.
|
||||
*/
|
||||
* This function stores waypoint commands
|
||||
* It looks to see what the next command type is and finds the last command.
|
||||
*/
|
||||
static void set_next_WP(struct Location *wp)
|
||||
{
|
||||
// copy the current WP into the OldWP slot
|
||||
// ---------------------------------------
|
||||
prev_WP = next_WP;
|
||||
// copy the current WP into the OldWP slot
|
||||
// ---------------------------------------
|
||||
prev_WP = next_WP;
|
||||
|
||||
// Load the next_WP slot
|
||||
// ---------------------
|
||||
next_WP = *wp;
|
||||
// Load the next_WP slot
|
||||
// ---------------------
|
||||
next_WP = *wp;
|
||||
|
||||
// if lat and lon is zero, then use current lat/lon
|
||||
// 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;
|
||||
}
|
||||
|
||||
// used to control FBW and limit the rate of climb
|
||||
// -----------------------------------------------
|
||||
target_altitude_cm = current_loc.alt;
|
||||
// used to control FBW and limit the rate of climb
|
||||
// -----------------------------------------------
|
||||
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))
|
||||
offset_altitude_cm = next_WP.alt - prev_WP.alt;
|
||||
else
|
||||
offset_altitude_cm = 0;
|
||||
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;
|
||||
else
|
||||
offset_altitude_cm = 0;
|
||||
|
||||
// zero out our loiter vals to watch for missed waypoints
|
||||
loiter_delta = 0;
|
||||
loiter_sum = 0;
|
||||
loiter_total = 0;
|
||||
// zero out our loiter vals to watch for missed waypoints
|
||||
loiter_delta = 0;
|
||||
loiter_sum = 0;
|
||||
loiter_total = 0;
|
||||
|
||||
// this is handy for the groundstation
|
||||
wp_totalDistance = get_distance(¤t_loc, &next_WP);
|
||||
wp_distance = wp_totalDistance;
|
||||
target_bearing_cd = get_bearing_cd(¤t_loc, &next_WP);
|
||||
nav_bearing_cd = target_bearing_cd;
|
||||
// this is handy for the groundstation
|
||||
wp_totalDistance = get_distance(¤t_loc, &next_WP);
|
||||
wp_distance = wp_totalDistance;
|
||||
target_bearing_cd = get_bearing_cd(¤t_loc, &next_WP);
|
||||
nav_bearing_cd = target_bearing_cd;
|
||||
|
||||
// to check if we have missed the WP
|
||||
// ----------------------------
|
||||
old_target_bearing_cd = target_bearing_cd;
|
||||
// to check if we have missed the WP
|
||||
// ----------------------------
|
||||
old_target_bearing_cd = target_bearing_cd;
|
||||
|
||||
// set a new crosstrack bearing
|
||||
// ----------------------------
|
||||
reset_crosstrack();
|
||||
// set a new crosstrack bearing
|
||||
// ----------------------------
|
||||
reset_crosstrack();
|
||||
}
|
||||
|
||||
static void set_guided_WP(void)
|
||||
{
|
||||
// copy the current location into the OldWP slot
|
||||
// ---------------------------------------
|
||||
prev_WP = current_loc;
|
||||
// copy the current location into the OldWP slot
|
||||
// ---------------------------------------
|
||||
prev_WP = current_loc;
|
||||
|
||||
// Load the next_WP slot
|
||||
// ---------------------
|
||||
next_WP = guided_WP;
|
||||
// Load the next_WP slot
|
||||
// ---------------------
|
||||
next_WP = guided_WP;
|
||||
|
||||
// used to control FBW and limit the rate of climb
|
||||
// -----------------------------------------------
|
||||
target_altitude_cm = current_loc.alt;
|
||||
offset_altitude_cm = next_WP.alt - prev_WP.alt;
|
||||
// used to control FBW and limit the rate of climb
|
||||
// -----------------------------------------------
|
||||
target_altitude_cm = current_loc.alt;
|
||||
offset_altitude_cm = next_WP.alt - prev_WP.alt;
|
||||
|
||||
// this is handy for the groundstation
|
||||
wp_totalDistance = get_distance(¤t_loc, &next_WP);
|
||||
wp_distance = wp_totalDistance;
|
||||
target_bearing_cd = get_bearing_cd(¤t_loc, &next_WP);
|
||||
// this is handy for the groundstation
|
||||
wp_totalDistance = get_distance(¤t_loc, &next_WP);
|
||||
wp_distance = wp_totalDistance;
|
||||
target_bearing_cd = get_bearing_cd(¤t_loc, &next_WP);
|
||||
|
||||
// to check if we have missed the WP
|
||||
// ----------------------------
|
||||
old_target_bearing_cd = target_bearing_cd;
|
||||
// to check if we have missed the WP
|
||||
// ----------------------------
|
||||
old_target_bearing_cd = target_bearing_cd;
|
||||
|
||||
// set a new crosstrack bearing
|
||||
// ----------------------------
|
||||
reset_crosstrack();
|
||||
// set a new crosstrack bearing
|
||||
// ----------------------------
|
||||
reset_crosstrack();
|
||||
}
|
||||
|
||||
// run this at setup on the ground
|
||||
// -------------------------------
|
||||
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
|
||||
// -----------------------------
|
||||
while (!g_gps->new_data || !g_gps->fix) {
|
||||
g_gps->update();
|
||||
}
|
||||
// block until we get a good fix
|
||||
// -----------------------------
|
||||
while (!g_gps->new_data || !g_gps->fix) {
|
||||
g_gps->update();
|
||||
}
|
||||
|
||||
home.id = MAV_CMD_NAV_WAYPOINT;
|
||||
home.lng = g_gps->longitude; // Lon * 10**7
|
||||
home.lat = g_gps->latitude; // Lat * 10**7
|
||||
home.alt = max(g_gps->altitude, 0);
|
||||
home_is_set = true;
|
||||
home.id = MAV_CMD_NAV_WAYPOINT;
|
||||
home.lng = g_gps->longitude; // Lon * 10**7
|
||||
home.lat = g_gps->latitude; // Lat * 10**7
|
||||
home.alt = max(g_gps->altitude, 0);
|
||||
home_is_set = true;
|
||||
|
||||
gcs_send_text_fmt(PSTR("gps alt: %lu"), (unsigned long)home.alt);
|
||||
|
||||
// Save Home to EEPROM - Command 0
|
||||
// -------------------
|
||||
set_cmd_with_index(home, 0);
|
||||
// Save Home to EEPROM - Command 0
|
||||
// -------------------
|
||||
set_cmd_with_index(home, 0);
|
||||
|
||||
// Save prev loc
|
||||
// -------------
|
||||
next_WP = prev_WP = home;
|
||||
// Save prev loc
|
||||
// -------------
|
||||
next_WP = prev_WP = home;
|
||||
|
||||
// Load home for a default guided_WP
|
||||
// -------------
|
||||
guided_WP = home;
|
||||
guided_WP.alt += g.RTL_altitude_cm;
|
||||
// Load home for a default guided_WP
|
||||
// -------------
|
||||
guided_WP = home;
|
||||
guided_WP.alt += g.RTL_altitude_cm;
|
||||
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user