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
|
|
|
|
|
|
|
void init_commands()
|
|
|
|
{
|
2011-02-19 03:21:42 -04:00
|
|
|
//read_EEPROM_waypoint_info();
|
2011-02-17 03:25:31 -04:00
|
|
|
g.waypoint_index.set_and_save(0);
|
2010-12-19 12:40:33 -04:00
|
|
|
command_must_index = 0;
|
|
|
|
command_may_index = 0;
|
|
|
|
next_command.id = CMD_BLANK;
|
|
|
|
}
|
|
|
|
|
2011-04-03 18:11:14 -03:00
|
|
|
void init_auto()
|
2010-12-19 12:40:33 -04:00
|
|
|
{
|
2011-02-19 03:21:42 -04:00
|
|
|
if (g.waypoint_index == g.waypoint_total) {
|
2011-04-03 18:11:14 -03:00
|
|
|
Serial.println("ia_f");
|
2011-03-02 22:32:50 -04:00
|
|
|
do_RTL();
|
2010-12-19 12:40:33 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2011-03-02 22:32:50 -04:00
|
|
|
// this is only used by an air-start
|
2011-04-03 18:11:14 -03:00
|
|
|
/*void reload_commands_airstart()
|
2010-12-19 12:40:33 -04:00
|
|
|
{
|
|
|
|
init_commands();
|
2011-02-17 03:09:13 -04:00
|
|
|
g.waypoint_index.load(); // XXX can we assume it's been loaded already by ::load_all?
|
|
|
|
decrement_WP_index();
|
|
|
|
}
|
2011-04-03 18:11:14 -03:00
|
|
|
*/
|
2011-02-17 03:09:13 -04:00
|
|
|
|
2010-12-19 12:40:33 -04:00
|
|
|
// Getters
|
|
|
|
// -------
|
|
|
|
struct Location get_wp_with_index(int i)
|
|
|
|
{
|
|
|
|
struct Location temp;
|
|
|
|
long mem;
|
|
|
|
|
|
|
|
// Find out proper location in memory by using the start_byte position + the index
|
|
|
|
// --------------------------------------------------------------------------------
|
2011-02-17 03:09:13 -04:00
|
|
|
if (i > g.waypoint_total) {
|
2010-12-19 12:40:33 -04:00
|
|
|
temp.id = CMD_BLANK;
|
|
|
|
}else{
|
2011-02-17 05:36:33 -04:00
|
|
|
// read WP position
|
2010-12-19 12:40:33 -04:00
|
|
|
mem = (WP_START_BYTE) + (i * WP_SIZE);
|
|
|
|
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-04-20 02:37:05 -03:00
|
|
|
temp.alt = (long)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-04-20 02:37:05 -03:00
|
|
|
temp.lat = (long)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-04-20 02:37:05 -03:00
|
|
|
temp.lng = (long)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
|
|
|
|
|
|
|
// Add on home altitude if we are a nav command
|
2011-03-09 02:37:09 -04:00
|
|
|
if(temp.id < 0x70){
|
2011-03-13 03:25:38 -03:00
|
|
|
temp.alt += home.alt;
|
2011-03-05 00:56:58 -04:00
|
|
|
}
|
2011-03-02 22:32:50 -04:00
|
|
|
|
2011-04-08 16:13:31 -03:00
|
|
|
if(temp.options & WP_OPTION_RELATIVE){
|
|
|
|
temp.lat += home.lat;
|
|
|
|
temp.lng += home.lng;
|
|
|
|
}
|
|
|
|
|
|
|
|
// XXX this is a little awkward. We have two methods to control Yaw tracking
|
|
|
|
// one is global and one is per waypoint.
|
|
|
|
if(temp.options & WP_OPTION_YAW){
|
|
|
|
yaw_tracking = TRACK_NEXT_WP;
|
|
|
|
}
|
|
|
|
|
2011-04-11 23:32:24 -03:00
|
|
|
// this is a hack for now, until we get GUI support
|
|
|
|
yaw_tracking = TRACK_NEXT_WP;
|
|
|
|
|
2010-12-19 12:40:33 -04:00
|
|
|
return temp;
|
|
|
|
}
|
|
|
|
|
|
|
|
// Setters
|
|
|
|
// -------
|
|
|
|
void set_wp_with_index(struct Location temp, int i)
|
|
|
|
{
|
2011-02-19 03:21:42 -04:00
|
|
|
i = constrain(i, 0, g.waypoint_total.get());
|
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-03-27 18:26:34 -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;
|
|
|
|
eeprom_write_dword((uint32_t *) mem, temp.lat);
|
|
|
|
|
|
|
|
mem += 4;
|
|
|
|
eeprom_write_dword((uint32_t *) mem, temp.lng);
|
|
|
|
}
|
|
|
|
|
|
|
|
void increment_WP_index()
|
|
|
|
{
|
2011-02-19 03:21:42 -04:00
|
|
|
if (g.waypoint_index < g.waypoint_total) {
|
2011-02-17 03:25:31 -04:00
|
|
|
g.waypoint_index.set_and_save(g.waypoint_index + 1);
|
2011-02-19 03:21:42 -04:00
|
|
|
SendDebug("MSG <increment_WP_index> WP index is incremented to ");
|
2010-12-19 12:40:33 -04:00
|
|
|
}else{
|
2011-02-19 03:21:42 -04:00
|
|
|
//SendDebug("MSG <increment_WP_index> Failed to increment WP index of ");
|
|
|
|
// This message is used excessively at the end of a mission
|
2010-12-19 12:40:33 -04:00
|
|
|
}
|
2011-02-17 03:25:31 -04:00
|
|
|
SendDebugln(g.waypoint_index,DEC);
|
2010-12-19 12:40:33 -04:00
|
|
|
}
|
2011-02-17 03:25:31 -04:00
|
|
|
|
2010-12-19 12:40:33 -04:00
|
|
|
void decrement_WP_index()
|
|
|
|
{
|
2011-02-19 03:21:42 -04:00
|
|
|
if (g.waypoint_index > 0) {
|
2011-02-17 03:25:31 -04:00
|
|
|
g.waypoint_index.set_and_save(g.waypoint_index - 1);
|
2011-02-19 03:21:42 -04:00
|
|
|
}
|
2010-12-19 12:40:33 -04:00
|
|
|
}
|
|
|
|
|
2011-02-17 03:25:31 -04:00
|
|
|
long read_alt_to_hold()
|
2011-02-17 05:36:33 -04:00
|
|
|
{
|
2011-02-25 01:33:39 -04:00
|
|
|
if(g.RTL_altitude < 0)
|
2011-02-17 03:25:31 -04:00
|
|
|
return current_loc.alt;
|
|
|
|
else
|
|
|
|
return g.RTL_altitude + home.alt;
|
|
|
|
}
|
2010-12-19 12:40:33 -04:00
|
|
|
|
2011-02-24 01:56:59 -04:00
|
|
|
|
2010-12-19 12:40:33 -04:00
|
|
|
//********************************************************************************
|
|
|
|
//This function sets the waypoint and modes for Return to Launch
|
|
|
|
//********************************************************************************
|
|
|
|
|
2011-03-02 22:32:50 -04:00
|
|
|
Location get_LOITER_home_wp()
|
2010-12-19 12:40:33 -04:00
|
|
|
{
|
2011-03-13 03:25:38 -03:00
|
|
|
//so we know where we are navigating from
|
|
|
|
next_WP = current_loc;
|
|
|
|
|
2011-02-17 05:36:33 -04:00
|
|
|
// read home position
|
|
|
|
struct Location temp = get_wp_with_index(0);
|
2011-02-19 03:21:42 -04:00
|
|
|
temp.id = MAV_CMD_NAV_LOITER_UNLIM;
|
2011-03-02 22:32:50 -04:00
|
|
|
temp.alt = read_alt_to_hold();
|
2010-12-19 12:40:33 -04:00
|
|
|
return temp;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
This function stores waypoint commands
|
|
|
|
It looks to see what the next command type is and finds the last command.
|
|
|
|
*/
|
2011-02-24 01:56:59 -04:00
|
|
|
void set_next_WP(struct Location *wp)
|
2010-12-19 12:40:33 -04:00
|
|
|
{
|
2011-03-19 07:14:15 -03:00
|
|
|
//GCS.send_text_P(SEVERITY_LOW,PSTR("load WP"));
|
2011-02-19 03:21:42 -04:00
|
|
|
SendDebug("MSG <set_next_wp> wp_index: ");
|
|
|
|
SendDebugln(g.waypoint_index, DEC);
|
|
|
|
gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index);
|
2010-12-19 12:40:33 -04:00
|
|
|
|
|
|
|
// copy the current WP into the OldWP slot
|
|
|
|
// ---------------------------------------
|
2011-02-19 03:21:42 -04:00
|
|
|
prev_WP = next_WP;
|
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-08 16:13:31 -03:00
|
|
|
// used to control and limit the rate of climb
|
2010-12-19 12:40:33 -04:00
|
|
|
// -----------------------------------------------
|
2011-02-24 01:56:59 -04:00
|
|
|
target_altitude = current_loc.alt;
|
|
|
|
|
2010-12-19 12:40:33 -04:00
|
|
|
// zero out our loiter vals to watch for missed waypoints
|
2011-02-19 03:21:42 -04:00
|
|
|
loiter_delta = 0;
|
|
|
|
loiter_sum = 0;
|
|
|
|
loiter_total = 0;
|
2010-12-19 12:40:33 -04:00
|
|
|
|
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-03-02 22:32:50 -04:00
|
|
|
wp_totalDistance = get_distance(¤t_loc, &next_WP);
|
|
|
|
wp_distance = wp_totalDistance;
|
2010-12-19 12:40:33 -04:00
|
|
|
target_bearing = get_bearing(¤t_loc, &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
|
|
|
|
// ----------------------------
|
2010-12-19 12:40:33 -04:00
|
|
|
old_target_bearing = target_bearing;
|
2011-02-17 05:36:33 -04:00
|
|
|
|
2010-12-19 12:40:33 -04:00
|
|
|
// set a new crosstrack bearing
|
|
|
|
// ----------------------------
|
|
|
|
reset_crosstrack();
|
2011-02-19 03:21:42 -04:00
|
|
|
|
|
|
|
gcs.print_current_waypoints();
|
2010-12-19 12:40:33 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// run this at setup on the ground
|
|
|
|
// -------------------------------
|
|
|
|
void init_home()
|
|
|
|
{
|
2011-02-19 03:21:42 -04:00
|
|
|
SendDebugln("MSG: <init_home> init home");
|
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();
|
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-02-21 00:30:56 -04:00
|
|
|
home.alt = max(g_gps->altitude, 0);
|
2010-12-19 12:40:33 -04:00
|
|
|
home_is_set = true;
|
|
|
|
|
2011-03-26 03:35:52 -03:00
|
|
|
//Serial.printf_P(PSTR("gps alt: %ld\n"), home.alt);
|
2011-02-21 00:30:56 -04:00
|
|
|
|
2010-12-19 12:40:33 -04:00
|
|
|
// Save Home to EEPROM
|
|
|
|
// -------------------
|
|
|
|
set_wp_with_index(home, 0);
|
|
|
|
|
|
|
|
// Save prev loc
|
|
|
|
// -------------
|
2011-02-19 03:21:42 -04:00
|
|
|
next_WP = prev_WP = home;
|
2010-12-19 12:40:33 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|