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
|
|
|
{
|
2011-11-17 01:28:56 -04:00
|
|
|
g.command_index = NO_COMMAND;
|
|
|
|
command_nav_index = NO_COMMAND;
|
|
|
|
command_cond_index = NO_COMMAND;
|
|
|
|
prev_nav_index = NO_COMMAND;
|
2011-11-16 04:16:09 -04:00
|
|
|
command_cond_queue.id = NO_COMMAND;
|
|
|
|
command_nav_queue.id = NO_COMMAND;
|
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
|
|
|
{
|
|
|
|
struct Location temp;
|
|
|
|
|
|
|
|
// Find out proper location in memory by using the start_byte position + the index
|
|
|
|
// --------------------------------------------------------------------------------
|
2011-11-13 01:39:24 -04:00
|
|
|
if (i >= g.command_total) {
|
2011-04-25 02:12:59 -03:00
|
|
|
// we do not have a valid command to load
|
|
|
|
// return a WP with a "Blank" id
|
2010-12-19 12:40:33 -04:00
|
|
|
temp.id = CMD_BLANK;
|
2011-04-25 02:12:59 -03:00
|
|
|
|
|
|
|
// no reason to carry on
|
|
|
|
return temp;
|
|
|
|
|
2010-12-19 12:40:33 -04:00
|
|
|
}else{
|
2011-04-25 02:12:59 -03:00
|
|
|
// we can load a command, we don't process it yet
|
2011-02-17 05:36:33 -04:00
|
|
|
// read WP position
|
2011-11-07 18:24:32 -04:00
|
|
|
int32_t mem = (WP_START_BYTE) + (i * WP_SIZE);
|
2011-04-25 02:12:59 -03:00
|
|
|
|
2010-12-19 12:40:33 -04:00
|
|
|
temp.id = eeprom_read_byte((uint8_t*)mem);
|
2011-03-05 00:56:58 -04:00
|
|
|
|
2011-03-26 03:35:52 -03:00
|
|
|
mem++;
|
|
|
|
temp.options = eeprom_read_byte((uint8_t*)mem);
|
|
|
|
|
2010-12-19 12:40:33 -04:00
|
|
|
mem++;
|
|
|
|
temp.p1 = eeprom_read_byte((uint8_t*)mem);
|
2011-03-05 00:56:58 -04:00
|
|
|
|
2010-12-19 12:40:33 -04:00
|
|
|
mem++;
|
2011-11-07 18:24:32 -04:00
|
|
|
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
|
|
|
|
2010-12-19 12:40:33 -04:00
|
|
|
mem += 4;
|
2011-11-07 18:24:32 -04:00
|
|
|
temp.lat = eeprom_read_dword((uint32_t*)mem); // lat is stored in decimal * 10,000,000
|
2011-03-05 00:56:58 -04:00
|
|
|
|
2010-12-19 12:40:33 -04:00
|
|
|
mem += 4;
|
2011-11-07 18:24:32 -04:00
|
|
|
temp.lng = eeprom_read_dword((uint32_t*)mem); // lon is stored in decimal * 10,000,000
|
2010-12-19 12:40:33 -04:00
|
|
|
}
|
2011-03-02 22:32:50 -04:00
|
|
|
|
2011-05-11 08:55:13 -03:00
|
|
|
// Add on home altitude if we are a nav command (or other command with altitude) and stored alt is relative
|
2011-11-20 03:21:49 -04:00
|
|
|
//if((temp.id < MAV_CMD_NAV_LAST || temp.id == MAV_CMD_CONDITION_CHANGE_ALT) && temp.options & MASK_OPTIONS_RELATIVE_ALT){
|
2011-05-14 23:02:09 -03:00
|
|
|
//temp.alt += home.alt;
|
2011-09-12 16:57:36 -03:00
|
|
|
//}
|
2011-05-11 08:55:13 -03:00
|
|
|
|
2011-11-20 04:20:36 -04:00
|
|
|
if(temp.options & WP_OPTION_RELATIVE){
|
2011-04-25 02:12:59 -03:00
|
|
|
// If were relative, just offset from home
|
2011-04-08 16:13:31 -03:00
|
|
|
temp.lat += home.lat;
|
|
|
|
temp.lng += home.lng;
|
|
|
|
}
|
|
|
|
|
2010-12-19 12:40:33 -04:00
|
|
|
return temp;
|
|
|
|
}
|
|
|
|
|
|
|
|
// 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
|
|
|
|
2011-11-13 17:40:09 -04:00
|
|
|
i = constrain(i, 0, g.command_total.get());
|
2011-11-14 02:54:18 -04:00
|
|
|
//Serial.printf("set_command: %d with id: %d\n", i, temp.id);
|
2011-11-13 01:39:24 -04:00
|
|
|
|
|
|
|
// store home as 0 altitude!!!
|
2011-11-14 02:54:18 -04:00
|
|
|
// Home is always a MAV_CMD_NAV_WAYPOINT (16)
|
|
|
|
if (i == 0){
|
2011-11-13 01:39:24 -04:00
|
|
|
temp.alt = 0;
|
2011-11-14 02:54:18 -04:00
|
|
|
temp.id = MAV_CMD_NAV_WAYPOINT;
|
|
|
|
}
|
2011-11-13 01:39:24 -04:00
|
|
|
|
2010-12-19 12:40:33 -04:00
|
|
|
uint32_t mem = WP_START_BYTE + (i * WP_SIZE);
|
|
|
|
|
|
|
|
eeprom_write_byte((uint8_t *) mem, temp.id);
|
|
|
|
|
2011-03-26 03:35:52 -03:00
|
|
|
mem++;
|
|
|
|
eeprom_write_byte((uint8_t *) mem, temp.options);
|
|
|
|
|
2010-12-19 12:40:33 -04:00
|
|
|
mem++;
|
|
|
|
eeprom_write_byte((uint8_t *) mem, temp.p1);
|
|
|
|
|
|
|
|
mem++;
|
2011-04-25 02:12:59 -03:00
|
|
|
eeprom_write_dword((uint32_t *) mem, temp.alt); // Alt is stored in CM!
|
2010-12-19 12:40:33 -04:00
|
|
|
|
|
|
|
mem += 4;
|
2011-04-25 02:12:59 -03:00
|
|
|
eeprom_write_dword((uint32_t *) mem, temp.lat); // Lat is stored in decimal degrees * 10^7
|
2010-12-19 12:40:33 -04:00
|
|
|
|
|
|
|
mem += 4;
|
2011-04-25 02:12:59 -03:00
|
|
|
eeprom_write_dword((uint32_t *) mem, temp.lng); // Long is stored in decimal degrees * 10^7
|
2011-11-13 17:40:09 -04:00
|
|
|
|
|
|
|
// Make sure our WP_total
|
|
|
|
if(g.command_total <= i)
|
|
|
|
g.command_total.set_and_save(i+1);
|
2010-12-19 12:40:33 -04:00
|
|
|
}
|
|
|
|
|
2011-11-16 04:16:09 -04:00
|
|
|
/*
|
|
|
|
//static void increment_WP_index()
|
2010-12-19 12:40:33 -04:00
|
|
|
{
|
2011-11-13 01:39:24 -04:00
|
|
|
if (g.command_index < (g.command_total-1)) {
|
2011-11-05 01:41:51 -03:00
|
|
|
g.command_index++;
|
2010-12-19 12:40:33 -04:00
|
|
|
}
|
2011-04-27 02:08:24 -03:00
|
|
|
|
2011-11-05 01:41:51 -03:00
|
|
|
SendDebugln(g.command_index,DEC);
|
2010-12-19 12:40:33 -04:00
|
|
|
}
|
2011-11-16 04:16:09 -04:00
|
|
|
*/
|
2011-09-05 02:09:07 -03:00
|
|
|
/*
|
2011-11-16 04:16:09 -04:00
|
|
|
//static void decrement_WP_index()
|
2010-12-19 12:40:33 -04:00
|
|
|
{
|
2011-11-05 01:41:51 -03:00
|
|
|
if (g.command_index > 0) {
|
|
|
|
g.command_index.set_and_save(g.command_index - 1);
|
2011-02-19 03:21:42 -04:00
|
|
|
}
|
2011-09-05 02:09:07 -03:00
|
|
|
}*/
|
2010-12-19 12:40:33 -04:00
|
|
|
|
2011-11-07 18:24:32 -04:00
|
|
|
static int32_t read_alt_to_hold()
|
2011-02-17 05:36:33 -04:00
|
|
|
{
|
2011-11-16 04:16:09 -04:00
|
|
|
if(g.RTL_altitude <= 0)
|
2011-02-17 03:25:31 -04:00
|
|
|
return current_loc.alt;
|
|
|
|
else
|
2011-08-13 19:49:20 -03:00
|
|
|
return g.RTL_altitude;// + home.alt;
|
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
|
|
|
//********************************************************************************
|
|
|
|
|
|
|
|
/*
|
2011-04-25 02:12:59 -03:00
|
|
|
This function sets the next waypoint command
|
|
|
|
It precalculates all the necessary stuff.
|
2010-12-19 12:40:33 -04:00
|
|
|
*/
|
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
|
|
|
{
|
2011-07-02 19:44:59 -03:00
|
|
|
//SendDebug("MSG <set_next_wp> wp_index: ");
|
2011-11-05 01:41:51 -03:00
|
|
|
//SendDebugln(g.command_index, DEC);
|
2010-12-19 12:40:33 -04:00
|
|
|
|
|
|
|
// copy the current WP into the OldWP slot
|
|
|
|
// ---------------------------------------
|
2011-11-28 14:32:58 -04:00
|
|
|
if (next_WP.lat == 0 || command_nav_index <= 1){
|
|
|
|
prev_WP = current_loc;
|
|
|
|
}else{
|
|
|
|
if (get_distance(¤t_loc, &next_WP) < 10)
|
|
|
|
prev_WP = next_WP;
|
|
|
|
else
|
|
|
|
prev_WP = current_loc;
|
|
|
|
}
|
|
|
|
|
|
|
|
//Serial.printf("set_next_WP #%d, ", command_nav_index);
|
|
|
|
//print_wp(&prev_WP, command_nav_index -1);
|
2011-02-17 05:36:33 -04:00
|
|
|
|
2010-12-19 12:40:33 -04:00
|
|
|
// Load the next_WP slot
|
|
|
|
// ---------------------
|
|
|
|
next_WP = *wp;
|
2011-02-17 05:36:33 -04:00
|
|
|
|
2011-04-25 02:12:59 -03:00
|
|
|
// used to control and limit the rate of climb - not used right now!
|
|
|
|
// -----------------------------------------------------------------
|
2011-02-24 01:56:59 -04:00
|
|
|
target_altitude = current_loc.alt;
|
|
|
|
|
2011-02-19 03:21:42 -04:00
|
|
|
// this is used to offset the shrinking longitude as we go towards the poles
|
|
|
|
float rads = (abs(next_WP.lat)/t7) * 0.0174532925;
|
|
|
|
scaleLongDown = cos(rads);
|
|
|
|
scaleLongUp = 1.0f/cos(rads);
|
2010-12-19 12:40:33 -04:00
|
|
|
|
|
|
|
// this is handy for the groundstation
|
2011-10-03 14:37:56 -03:00
|
|
|
// -----------------------------------
|
2011-03-02 22:32:50 -04:00
|
|
|
wp_totalDistance = get_distance(¤t_loc, &next_WP);
|
|
|
|
wp_distance = wp_totalDistance;
|
2011-11-28 14:32:58 -04:00
|
|
|
target_bearing = get_bearing(&prev_WP, &next_WP);
|
2011-02-19 03:21:42 -04:00
|
|
|
|
2011-03-02 22:32:50 -04:00
|
|
|
// to check if we have missed the WP
|
2011-10-03 14:37:56 -03:00
|
|
|
// ---------------------------------
|
2011-09-21 17:19:36 -03:00
|
|
|
original_target_bearing = target_bearing;
|
2011-02-19 03:21:42 -04:00
|
|
|
|
2011-10-03 14:37:56 -03:00
|
|
|
// reset speed governer
|
|
|
|
// --------------------
|
|
|
|
waypoint_speed_gov = 0;
|
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
|
|
|
{
|
2011-11-09 02:43:55 -04:00
|
|
|
home_is_set = true;
|
|
|
|
|
2010-12-19 12:40:33 -04:00
|
|
|
// block until we get a good fix
|
|
|
|
// -----------------------------
|
2011-02-17 05:36:33 -04:00
|
|
|
while (!g_gps->new_data || !g_gps->fix) {
|
|
|
|
g_gps->update();
|
2011-11-09 02:43:55 -04:00
|
|
|
// we need GCS update while waiting for GPS, to ensure
|
|
|
|
// we react to HIL mavlink
|
|
|
|
gcs_update();
|
2010-12-19 12:40:33 -04:00
|
|
|
}
|
2011-02-17 05:36:33 -04:00
|
|
|
|
2011-02-19 03:21:42 -04:00
|
|
|
home.id = MAV_CMD_NAV_WAYPOINT;
|
2011-02-17 05:36:33 -04:00
|
|
|
home.lng = g_gps->longitude; // Lon * 10**7
|
|
|
|
home.lat = g_gps->latitude; // Lat * 10**7
|
2011-06-16 14:03:26 -03:00
|
|
|
//home.alt = max(g_gps->altitude, 0); // we sometimes get negatives from GPS, not valid
|
2011-09-12 16:57:36 -03:00
|
|
|
home.alt = 0; // Home is always 0
|
2010-12-19 12:40:33 -04:00
|
|
|
|
2011-05-23 02:53:00 -03:00
|
|
|
// to point yaw towards home until we set it with Mavlink
|
|
|
|
target_WP = home;
|
|
|
|
|
2010-12-19 12:40:33 -04:00
|
|
|
// Save Home to EEPROM
|
|
|
|
// -------------------
|
2011-08-13 19:49:20 -03:00
|
|
|
// no need to save this to EPROM
|
2011-11-20 03:21:49 -04:00
|
|
|
set_cmd_with_index(home, 0);
|
2011-04-21 02:15:45 -03:00
|
|
|
print_wp(&home, 0);
|
|
|
|
|
2011-04-25 02:12:59 -03:00
|
|
|
// Save prev loc this makes the calcs look better before commands are loaded
|
2011-07-04 03:37:29 -03:00
|
|
|
prev_WP = home;
|
2011-09-12 16:57:36 -03:00
|
|
|
|
2011-12-11 03:26:10 -04:00
|
|
|
//
|
|
|
|
next_WP = home;
|
2011-09-12 16:57:36 -03:00
|
|
|
|
|
|
|
// Load home for a default guided_WP
|
|
|
|
// -------------
|
|
|
|
guided_WP = home;
|
|
|
|
guided_WP.alt += g.RTL_altitude;
|
2010-12-19 12:40:33 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|