mirror of https://github.com/ArduPilot/ardupilot
144 lines
4.1 KiB
Plaintext
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;
|
|
}
|
|
|
|
|
|
|