ardupilot/ArduCopter/commands.pde

143 lines
4.0 KiB
Plaintext
Raw Normal View History

// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
static void init_commands()
{
2012-11-10 01:55:51 -04:00
g.command_index = NO_COMMAND;
command_nav_index = NO_COMMAND;
command_cond_index = NO_COMMAND;
prev_nav_index = NO_COMMAND;
2012-08-16 21:50:03 -03:00
command_cond_queue.id = NO_COMMAND;
command_nav_queue.id = NO_COMMAND;
}
// Getters
// -------
static struct Location get_cmd_with_index(int i)
{
2012-08-16 21:50:03 -03:00
struct Location temp;
2012-08-16 21:50:03 -03:00
// 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;
2012-08-16 21:50:03 -03:00
// no reason to carry on
return temp;
2012-08-16 21:50:03 -03:00
}else{
// we can load a command, we don't process it yet
// read WP position
2012-12-12 21:25:09 -04:00
uint16_t mem = (WP_START_BYTE) + (i * WP_SIZE);
2012-12-12 21:25:09 -04:00
temp.id = hal.storage->read_byte(mem);
2012-08-16 21:50:03 -03:00
mem++;
2012-12-12 21:25:09 -04:00
temp.options = hal.storage->read_byte(mem);
2012-08-16 21:50:03 -03:00
mem++;
2012-12-12 21:25:09 -04:00
temp.p1 = hal.storage->read_byte(mem);
2012-08-16 21:50:03 -03:00
mem++;
2012-12-12 21:25:09 -04:00
temp.alt = hal.storage->read_dword(mem); // alt is stored in CM! Alt is stored relative!
2012-08-16 21:50:03 -03:00
mem += 4;
2012-12-12 21:25:09 -04:00
temp.lat = hal.storage->read_dword(mem); // lat is stored in decimal * 10,000,000
2012-08-16 21:50:03 -03:00
mem += 4;
2012-12-12 21:25:09 -04:00
temp.lng = hal.storage->read_dword(mem); // lon is stored in decimal * 10,000,000
2012-08-16 21:50:03 -03:00
}
2012-08-16 21:50:03 -03:00
// 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)){
2012-08-16 21:50:03 -03:00
//temp.alt += home.alt;
//}
2012-08-16 21:50:03 -03:00
if(temp.options & WP_OPTION_RELATIVE) {
// If were relative, just offset from home
temp.lat += home.lat;
temp.lng += home.lng;
}
2012-08-16 21:50:03 -03:00
return temp;
}
// Setters
// -------
2011-11-20 03:21:49 -04:00
static void set_cmd_with_index(struct Location temp, int i)
{
2011-11-20 04:20:36 -04:00
i = constrain_int16(i, 0, g.command_total.get());
2012-08-16 21:50:03 -03:00
// 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;
}
2012-12-12 21:25:09 -04:00
uint16_t mem = WP_START_BYTE + (i * WP_SIZE);
2012-12-12 21:25:09 -04:00
hal.storage->write_byte(mem, temp.id);
2012-08-16 21:50:03 -03:00
mem++;
2012-12-12 21:25:09 -04:00
hal.storage->write_byte(mem, temp.options);
2012-08-16 21:50:03 -03:00
mem++;
2012-12-12 21:25:09 -04:00
hal.storage->write_byte(mem, temp.p1);
2012-08-16 21:50:03 -03:00
mem++;
2012-12-12 21:25:09 -04:00
hal.storage->write_dword(mem, temp.alt); // Alt is stored in CM!
2012-08-16 21:50:03 -03:00
mem += 4;
2012-12-12 21:25:09 -04:00
hal.storage->write_dword(mem, temp.lat); // Lat is stored in decimal degrees * 10^7
2012-08-16 21:50:03 -03:00
mem += 4;
2012-12-12 21:25:09 -04:00
hal.storage->write_dword(mem, temp.lng); // Long is stored in decimal degrees * 10^7
2011-11-13 17:40:09 -04:00
2012-08-16 21:50:03 -03:00
// 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);
2012-08-16 21:50:03 -03:00
}else{
return g.rtl_altitude;
2012-08-16 21:50:03 -03:00
}
}
// run this at setup on the ground
// -------------------------------
static void init_home()
{
2012-11-10 01:55:51 -04:00
set_home_is_set(true);
2012-08-16 21:50:03 -03:00
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_home_position(g_gps->longitude, g_gps->latitude);
2012-08-16 21:50:03 -03:00
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;
}