ardupilot/ArduCopter/commands.pde

144 lines
4.1 KiB
Plaintext

// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
static void init_commands()
{
g.command_index = NO_COMMAND;
command_nav_index = NO_COMMAND;
command_cond_index = NO_COMMAND;
prev_nav_index = NO_COMMAND;
command_cond_queue.id = NO_COMMAND;
command_nav_queue.id = NO_COMMAND;
}
// Getters
// -------
static struct Location get_cmd_with_index(int i)
{
struct Location temp;
// Find out proper location in memory by using the start_byte position + the index
// --------------------------------------------------------------------------------
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;
// no reason to carry on
return temp;
}else{
// we can load a command, we don't process it yet
// read WP position
uint16_t mem = (WP_START_BYTE) + (i * WP_SIZE);
temp.id = hal.storage->read_byte(mem);
mem++;
temp.options = hal.storage->read_byte(mem);
mem++;
temp.p1 = hal.storage->read_byte(mem);
mem++;
temp.alt = hal.storage->read_dword(mem); // alt is stored in CM! Alt is stored relative!
mem += 4;
temp.lat = hal.storage->read_dword(mem); // lat is stored in decimal * 10,000,000
mem += 4;
temp.lng = hal.storage->read_dword(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
//if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && (temp.options & MASK_OPTIONS_RELATIVE_ALT)){
//temp.alt += home.alt;
//}
if(temp.options & WP_OPTION_RELATIVE) {
// If were relative, just offset from home
temp.lat += home.lat;
temp.lng += home.lng;
}
return temp;
}
// Setters
// -------
static void set_cmd_with_index(struct Location temp, int i)
{
i = constrain_int16(i, 0, g.command_total.get());
//cliSerial->printf("set_command: %d with id: %d\n", i, temp.id);
// store home as 0 altitude!!!
// Home is always a MAV_CMD_NAV_WAYPOINT (16)
if (i == 0) {
temp.alt = 0;
temp.id = MAV_CMD_NAV_WAYPOINT;
}
uint16_t mem = WP_START_BYTE + (i * WP_SIZE);
hal.storage->write_byte(mem, temp.id);
mem++;
hal.storage->write_byte(mem, temp.options);
mem++;
hal.storage->write_byte(mem, temp.p1);
mem++;
hal.storage->write_dword(mem, temp.alt); // Alt is stored in CM!
mem += 4;
hal.storage->write_dword(mem, temp.lat); // Lat is stored in decimal degrees * 10^7
mem += 4;
hal.storage->write_dword(mem, temp.lng); // Long is stored in decimal degrees * 10^7
// Make sure our WP_total
if(g.command_total < (i+1))
g.command_total.set_and_save(i+1);
}
static int32_t get_RTL_alt()
{
if(g.rtl_altitude <= 0) {
return min(current_loc.alt, RTL_ALT_MAX);
}else if (g.rtl_altitude < current_loc.alt) {
return min(current_loc.alt, RTL_ALT_MAX);
}else{
return g.rtl_altitude;
}
}
// run this at setup on the ground
// -------------------------------
static void init_home()
{
set_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 = 0; // Home is always 0
// Save Home to EEPROM
// -------------------
// no need to save this to EPROM
set_cmd_with_index(home, 0);
// set inertial nav's home position
inertial_nav.set_current_position(g_gps->longitude, g_gps->latitude);
if (g.log_bitmask & MASK_LOG_CMD)
Log_Write_Cmd(0, &home);
// update navigation scalers. used to offset the shrinking longitude as we go towards the poles
scaleLongDown = longitude_scale(&home);
scaleLongUp = 1.0f/scaleLongDown;
}