diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index 0c15d14124..2776296862 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -2,70 +2,70 @@ 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; + 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; - fast_corner = false; + fast_corner = false; - // default Yaw tracking - yaw_tracking = MAV_ROI_WPNEXT; + // default Yaw tracking + yaw_tracking = MAV_ROI_WPNEXT; } // Getters // ------- static struct Location get_cmd_with_index(int i) { - struct Location temp; + 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; + // 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; + // no reason to carry on + return temp; - }else{ - // we can load a command, we don't process it yet - // read WP position - int32_t mem = (WP_START_BYTE) + (i * WP_SIZE); + }else{ + // we can load a command, we don't process it yet + // read WP position + int32_t mem = (WP_START_BYTE) + (i * WP_SIZE); - temp.id = eeprom_read_byte((uint8_t*)mem); + temp.id = eeprom_read_byte((uint8_t*)mem); - mem++; - temp.options = eeprom_read_byte((uint8_t*)mem); + mem++; + temp.options = eeprom_read_byte((uint8_t*)mem); - mem++; - temp.p1 = eeprom_read_byte((uint8_t*)mem); + mem++; + temp.p1 = eeprom_read_byte((uint8_t*)mem); - mem++; - temp.alt = eeprom_read_dword((uint32_t*)mem); // alt is stored in CM! Alt is stored relative! + mem++; + temp.alt = eeprom_read_dword((uint32_t*)mem); // alt is stored in CM! Alt is stored relative! - mem += 4; - temp.lat = eeprom_read_dword((uint32_t*)mem); // lat is stored in decimal * 10,000,000 + mem += 4; + temp.lat = eeprom_read_dword((uint32_t*)mem); // lat is stored in decimal * 10,000,000 - mem += 4; - temp.lng = eeprom_read_dword((uint32_t*)mem); // lon is stored in decimal * 10,000,000 - } + mem += 4; + temp.lng = eeprom_read_dword((uint32_t*)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; - //} + // 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; - } + if(temp.options & WP_OPTION_RELATIVE) { + // If were relative, just offset from home + temp.lat += home.lat; + temp.lng += home.lng; + } - return temp; + return temp; } // Setters @@ -73,49 +73,49 @@ static struct Location get_cmd_with_index(int i) static void set_cmd_with_index(struct Location temp, int i) { - i = constrain(i, 0, g.command_total.get()); - //Serial.printf("set_command: %d with id: %d\n", i, temp.id); + i = constrain(i, 0, g.command_total.get()); + //Serial.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; - } + // 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; + } - uint32_t mem = WP_START_BYTE + (i * WP_SIZE); + uint32_t mem = WP_START_BYTE + (i * WP_SIZE); - eeprom_write_byte((uint8_t *) mem, temp.id); + eeprom_write_byte((uint8_t *) mem, temp.id); - mem++; - eeprom_write_byte((uint8_t *) mem, temp.options); + mem++; + eeprom_write_byte((uint8_t *) mem, temp.options); - mem++; - eeprom_write_byte((uint8_t *) mem, temp.p1); + mem++; + eeprom_write_byte((uint8_t *) mem, temp.p1); - mem++; - eeprom_write_dword((uint32_t *) mem, temp.alt); // Alt is stored in CM! + mem++; + eeprom_write_dword((uint32_t *) mem, temp.alt); // Alt is stored in CM! - mem += 4; - eeprom_write_dword((uint32_t *) mem, temp.lat); // Lat is stored in decimal degrees * 10^7 + mem += 4; + eeprom_write_dword((uint32_t *) mem, temp.lat); // Lat is stored in decimal degrees * 10^7 - mem += 4; - eeprom_write_dword((uint32_t *) mem, temp.lng); // Long is stored in decimal degrees * 10^7 + mem += 4; + eeprom_write_dword((uint32_t *) 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); + // 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 current_loc.alt; - }else if (g.RTL_altitude < current_loc.alt){ - return current_loc.alt; - }else{ - return g.RTL_altitude; - } + if(g.RTL_altitude <= 0) { + return current_loc.alt; + }else if (g.RTL_altitude < current_loc.alt) { + return current_loc.alt; + }else{ + return g.RTL_altitude; + } } @@ -125,61 +125,61 @@ static int32_t get_RTL_alt() //******************************************************************************** /* -This function sets the next waypoint command -It precalculates all the necessary stuff. -*/ + * This function sets the next waypoint command + * It precalculates all the necessary stuff. + */ static void set_next_WP(struct Location *wp) { - //SendDebug("MSG wp_index: "); - //SendDebugln(g.command_index, DEC); + //SendDebug("MSG wp_index: "); + //SendDebugln(g.command_index, DEC); - // copy the current WP into the OldWP slot - // --------------------------------------- - if (next_WP.lat == 0 || command_nav_index <= 1){ - prev_WP = current_loc; - }else{ - if (get_distance_cm(&filtered_loc, &next_WP) < 500) - prev_WP = next_WP; - else - prev_WP = current_loc; - } + // copy the current WP into the OldWP slot + // --------------------------------------- + if (next_WP.lat == 0 || command_nav_index <= 1) { + prev_WP = current_loc; + }else{ + if (get_distance_cm(&filtered_loc, &next_WP) < 500) + 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); + //Serial.printf("set_next_WP #%d, ", command_nav_index); + //print_wp(&prev_WP, command_nav_index -1); - // Load the next_WP slot - // --------------------- - next_WP = *wp; + // Load the next_WP slot + // --------------------- + next_WP = *wp; - // used to control and limit the rate of climb - // ------------------------------------------- - // We don't set next WP below 1m - next_WP.alt = max(next_WP.alt, 100); + // used to control and limit the rate of climb + // ------------------------------------------- + // We don't set next WP below 1m + next_WP.alt = max(next_WP.alt, 100); - // Save new altitude so we can track it for climb_rate - set_new_altitude(next_WP.alt); + // Save new altitude so we can track it for climb_rate + set_new_altitude(next_WP.alt); - // this is used to offset the shrinking longitude as we go towards the poles - float rads = (fabs((float)next_WP.lat)/t7) * 0.0174532925; - scaleLongDown = cos(rads); - scaleLongUp = 1.0f/cos(rads); + // this is used to offset the shrinking longitude as we go towards the poles + float rads = (fabs((float)next_WP.lat)/t7) * 0.0174532925; + scaleLongDown = cos(rads); + scaleLongUp = 1.0f/cos(rads); - // this is handy for the groundstation - // ----------------------------------- - wp_distance = get_distance_cm(&filtered_loc, &next_WP); - target_bearing = get_bearing_cd(&prev_WP, &next_WP); + // this is handy for the groundstation + // ----------------------------------- + wp_distance = get_distance_cm(&filtered_loc, &next_WP); + target_bearing = get_bearing_cd(&prev_WP, &next_WP); - // calc the location error: - calc_location_error(&next_WP); + // calc the location error: + calc_location_error(&next_WP); - // to check if we have missed the WP - // --------------------------------- - original_target_bearing = target_bearing; + // to check if we have missed the WP + // --------------------------------- + original_target_bearing = target_bearing; - // reset speed governer - // -------------------- - waypoint_speed_gov = g_gps->ground_speed; + // reset speed governer + // -------------------- + waypoint_speed_gov = g_gps->ground_speed; } @@ -187,34 +187,34 @@ static void set_next_WP(struct Location *wp) // ------------------------------- static void init_home() { - 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 + 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 - // to point yaw towards home until we set it with Mavlink - target_WP = home; + // to point yaw towards home until we set it with Mavlink + target_WP = home; - // Save Home to EEPROM - // ------------------- - // no need to save this to EPROM - set_cmd_with_index(home, 0); - //print_wp(&home, 0); + // Save Home to EEPROM + // ------------------- + // no need to save this to EPROM + set_cmd_with_index(home, 0); + //print_wp(&home, 0); - if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Cmd(0, &home); + if (g.log_bitmask & MASK_LOG_CMD) + Log_Write_Cmd(0, &home); - // Save prev loc this makes the calcs look better before commands are loaded - prev_WP = home; + // Save prev loc this makes the calcs look better before commands are loaded + prev_WP = home; - // in case we RTL - next_WP = home; + // in case we RTL + next_WP = home; - // Load home for a default guided_WP - // ------------- - guided_WP = home; - guided_WP.alt += g.RTL_altitude; + // Load home for a default guided_WP + // ------------- + guided_WP = home; + guided_WP.alt += g.RTL_altitude; }