2011-03-19 07:20:11 -03:00
|
|
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
2010-12-19 12:40:33 -04:00
|
|
|
|
2011-07-17 07:32:00 -03:00
|
|
|
static void init_commands()
|
2010-12-19 12:40:33 -04:00
|
|
|
{
|
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;
|
2011-12-31 03:19:42 -04:00
|
|
|
|
2012-11-10 01:55:51 -04:00
|
|
|
ap.fast_corner = false;
|
2012-10-21 18:32:39 -03:00
|
|
|
reset_desired_speed();
|
2011-09-07 01:44:01 -03:00
|
|
|
}
|
|
|
|
|
2010-12-19 12:40:33 -04:00
|
|
|
// Getters
|
|
|
|
// -------
|
2011-11-05 01:41:51 -03:00
|
|
|
static struct Location get_cmd_with_index(int i)
|
2010-12-19 12:40:33 -04:00
|
|
|
{
|
2012-08-16 21:50:03 -03:00
|
|
|
struct Location temp;
|
2010-12-19 12:40:33 -04:00
|
|
|
|
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;
|
2011-04-25 02:12:59 -03:00
|
|
|
|
2012-08-16 21:50:03 -03:00
|
|
|
// no reason to carry on
|
|
|
|
return temp;
|
2011-04-25 02:12:59 -03:00
|
|
|
|
2012-08-16 21:50:03 -03:00
|
|
|
}else{
|
|
|
|
// we can load a command, we don't process it yet
|
|
|
|
// read WP position
|
2012-11-18 17:27:51 -04:00
|
|
|
uintptr_t mem = (WP_START_BYTE) + (i * WP_SIZE);
|
2011-04-25 02:12:59 -03:00
|
|
|
|
2012-08-16 21:50:03 -03:00
|
|
|
temp.id = eeprom_read_byte((uint8_t*)mem);
|
2011-03-05 00:56:58 -04:00
|
|
|
|
2012-08-16 21:50:03 -03:00
|
|
|
mem++;
|
|
|
|
temp.options = eeprom_read_byte((uint8_t*)mem);
|
2011-03-26 03:35:52 -03:00
|
|
|
|
2012-08-16 21:50:03 -03:00
|
|
|
mem++;
|
|
|
|
temp.p1 = eeprom_read_byte((uint8_t*)mem);
|
2011-03-05 00:56:58 -04:00
|
|
|
|
2012-08-16 21:50:03 -03:00
|
|
|
mem++;
|
|
|
|
temp.alt = eeprom_read_dword((uint32_t*)mem); // alt is stored in CM! Alt is stored relative!
|
2011-03-05 00:56:58 -04:00
|
|
|
|
2012-08-16 21:50:03 -03:00
|
|
|
mem += 4;
|
|
|
|
temp.lat = eeprom_read_dword((uint32_t*)mem); // lat is stored in decimal * 10,000,000
|
2011-03-05 00:56:58 -04:00
|
|
|
|
2012-08-16 21:50:03 -03:00
|
|
|
mem += 4;
|
|
|
|
temp.lng = eeprom_read_dword((uint32_t*)mem); // lon is stored in decimal * 10,000,000
|
|
|
|
}
|
2011-03-02 22:32:50 -04: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){
|
|
|
|
//temp.alt += home.alt;
|
|
|
|
//}
|
2011-05-11 08:55:13 -03:00
|
|
|
|
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;
|
|
|
|
}
|
2011-04-08 16:13:31 -03:00
|
|
|
|
2012-08-16 21:50:03 -03:00
|
|
|
return temp;
|
2010-12-19 12:40:33 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// Setters
|
|
|
|
// -------
|
2011-11-20 03:21:49 -04:00
|
|
|
static void set_cmd_with_index(struct Location temp, int i)
|
2010-12-19 12:40:33 -04:00
|
|
|
{
|
2011-11-20 04:20:36 -04:00
|
|
|
|
2012-08-16 21:50:03 -03:00
|
|
|
i = constrain(i, 0, g.command_total.get());
|
2012-11-21 02:08:03 -04:00
|
|
|
//cliSerial->printf("set_command: %d with id: %d\n", i, temp.id);
|
2011-11-13 01:39:24 -04:00
|
|
|
|
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;
|
|
|
|
}
|
2011-11-13 01:39:24 -04:00
|
|
|
|
2012-11-18 17:27:51 -04:00
|
|
|
uintptr_t mem = WP_START_BYTE + (i * WP_SIZE);
|
2010-12-19 12:40:33 -04:00
|
|
|
|
2012-08-16 21:50:03 -03:00
|
|
|
eeprom_write_byte((uint8_t *) mem, temp.id);
|
2010-12-19 12:40:33 -04:00
|
|
|
|
2012-08-16 21:50:03 -03:00
|
|
|
mem++;
|
|
|
|
eeprom_write_byte((uint8_t *) mem, temp.options);
|
2011-03-26 03:35:52 -03:00
|
|
|
|
2012-08-16 21:50:03 -03:00
|
|
|
mem++;
|
|
|
|
eeprom_write_byte((uint8_t *) mem, temp.p1);
|
2010-12-19 12:40:33 -04:00
|
|
|
|
2012-08-16 21:50:03 -03:00
|
|
|
mem++;
|
|
|
|
eeprom_write_dword((uint32_t *) mem, temp.alt); // Alt is stored in CM!
|
2010-12-19 12:40:33 -04:00
|
|
|
|
2012-08-16 21:50:03 -03:00
|
|
|
mem += 4;
|
|
|
|
eeprom_write_dword((uint32_t *) mem, temp.lat); // Lat is stored in decimal degrees * 10^7
|
2010-12-19 12:40:33 -04:00
|
|
|
|
2012-08-16 21:50:03 -03:00
|
|
|
mem += 4;
|
|
|
|
eeprom_write_dword((uint32_t *) 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);
|
2010-12-19 12:40:33 -04:00
|
|
|
}
|
|
|
|
|
2012-06-30 19:34:55 -03:00
|
|
|
static int32_t get_RTL_alt()
|
2011-02-17 05:36:33 -04:00
|
|
|
{
|
2012-11-29 08:08:19 -04:00
|
|
|
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{
|
2012-11-29 08:08:19 -04:00
|
|
|
return g.rtl_altitude;
|
2012-08-16 21:50:03 -03:00
|
|
|
}
|
2011-02-17 03:25:31 -04:00
|
|
|
}
|
2010-12-19 12:40:33 -04:00
|
|
|
|
2011-02-24 01:56:59 -04:00
|
|
|
|
2010-12-19 12:40:33 -04:00
|
|
|
//********************************************************************************
|
2011-04-25 02:12:59 -03:00
|
|
|
// This function sets the waypoint and modes for Return to Launch
|
|
|
|
// It's not currently used
|
2010-12-19 12:40:33 -04:00
|
|
|
//********************************************************************************
|
|
|
|
|
|
|
|
/*
|
2012-08-16 21:50:03 -03:00
|
|
|
* This function sets the next waypoint command
|
|
|
|
* It precalculates all the necessary stuff.
|
|
|
|
*/
|
2011-04-25 02:12:59 -03:00
|
|
|
|
2011-07-17 07:32:00 -03:00
|
|
|
static void set_next_WP(struct Location *wp)
|
2010-12-19 12:40:33 -04:00
|
|
|
{
|
2012-08-16 21:50:03 -03:00
|
|
|
// copy the current WP into the OldWP slot
|
|
|
|
// ---------------------------------------
|
|
|
|
if (next_WP.lat == 0 || command_nav_index <= 1) {
|
|
|
|
prev_WP = current_loc;
|
|
|
|
}else{
|
2012-11-07 06:03:30 -04:00
|
|
|
if (get_distance_cm(¤t_loc, &next_WP) < 500)
|
2012-08-16 21:50:03 -03:00
|
|
|
prev_WP = next_WP;
|
|
|
|
else
|
|
|
|
prev_WP = current_loc;
|
|
|
|
}
|
|
|
|
|
|
|
|
// Load the next_WP slot
|
|
|
|
// ---------------------
|
2013-01-14 12:19:23 -04:00
|
|
|
next_WP.options = wp->options;
|
|
|
|
next_WP.lat = wp->lat;
|
|
|
|
next_WP.lng = wp->lng;
|
2012-08-16 21:50:03 -03:00
|
|
|
|
2013-01-14 12:19:23 -04:00
|
|
|
// Set new target altitude so we can track it for climb_rate
|
|
|
|
// To-Do: remove the restriction on negative altitude targets (after testing)
|
|
|
|
set_new_altitude(max(wp->alt,100));
|
2012-08-16 21:50:03 -03:00
|
|
|
|
|
|
|
// this is handy for the groundstation
|
|
|
|
// -----------------------------------
|
2012-11-07 06:03:30 -04:00
|
|
|
wp_distance = get_distance_cm(¤t_loc, &next_WP);
|
2012-12-08 01:23:32 -04:00
|
|
|
wp_bearing = get_bearing_cd(&prev_WP, &next_WP);
|
2012-08-16 21:50:03 -03:00
|
|
|
|
|
|
|
// calc the location error:
|
|
|
|
calc_location_error(&next_WP);
|
|
|
|
|
|
|
|
// to check if we have missed the WP
|
|
|
|
// ---------------------------------
|
2012-12-08 01:23:32 -04:00
|
|
|
original_wp_bearing = wp_bearing;
|
2010-12-19 12:40:33 -04:00
|
|
|
}
|
|
|
|
|
2011-07-02 19:44:59 -03:00
|
|
|
|
2010-12-19 12:40:33 -04:00
|
|
|
// run this at setup on the ground
|
|
|
|
// -------------------------------
|
2011-07-17 07:32:00 -03:00
|
|
|
static void init_home()
|
2010-12-19 12:40:33 -04:00
|
|
|
{
|
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);
|
|
|
|
|
2012-11-28 10:31:02 -04:00
|
|
|
#if INERTIAL_NAV_XY == ENABLED
|
2012-11-07 06:03:30 -04:00
|
|
|
// set inertial nav's home position
|
|
|
|
inertial_nav.set_current_position(g_gps->longitude, g_gps->latitude);
|
|
|
|
#endif
|
|
|
|
|
2012-08-16 21:50:03 -03:00
|
|
|
if (g.log_bitmask & MASK_LOG_CMD)
|
|
|
|
Log_Write_Cmd(0, &home);
|
|
|
|
|
2012-12-17 03:54:03 -04:00
|
|
|
// update navigation scalers. used to offset the shrinking longitude as we go towards the poles
|
2013-01-31 08:39:26 -04:00
|
|
|
float rads = (fabs((float)home.lat)/t7) * 0.0174532925;
|
2012-12-17 03:54:03 -04:00
|
|
|
scaleLongDown = cos(rads);
|
|
|
|
scaleLongUp = 1.0f/cos(rads);
|
|
|
|
|
2012-08-16 21:50:03 -03:00
|
|
|
// Save prev loc this makes the calcs look better before commands are loaded
|
|
|
|
prev_WP = home;
|
|
|
|
|
|
|
|
// Load home for a default guided_WP
|
|
|
|
// -------------
|
|
|
|
guided_WP = home;
|
2012-11-29 08:08:19 -04:00
|
|
|
guided_WP.alt += g.rtl_altitude;
|
2010-12-19 12:40:33 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|