Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: integrate AP_Mission library

Browse Source
This commit is contained in:
Randy Mackay 2014-03-03 22:50:45 +09:00
parent ddba6f5c19
commit 9446e9fd2e
10 changed files with 231 additions and 518 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
ArduPlane/ArduPlane.pde
View File

@ -70,6 +70,7 @@
#include <AP_SpdHgtControl.h>
#include <AP_TECS.h>
#include <AP_NavEKF.h>
#include <AP_Mission.h> // Mission command library
#include <AP_Notify.h> // Notify library
#include <AP_BattMonitor.h> // Battery monitor library
@ -464,10 +465,6 @@ static bool have_position;
////////////////////////////////////////////////////////////////////////////////
// There may be two active commands in Auto mode.
// This indicates the active navigation command by index number
static uint8_t nav_command_index;
// This indicates the active non-navigation command by index number
static uint8_t non_nav_command_index;
// This is the command type (eg navigate to waypoint) of the active navigation command
static uint8_t nav_command_ID = NO_COMMAND;
static uint8_t non_nav_command_ID = NO_COMMAND;
@ -584,6 +581,15 @@ static int32_t nav_roll_cd;
// The instantaneous desired pitch angle. Hundredths of a degree
static int32_t nav_pitch_cd;
////////////////////////////////////////////////////////////////////////////////
// Mission library
// forward declaration to keep compiler happy
////////////////////////////////////////////////////////////////////////////////
static bool start_command(const AP_Mission::Mission_Command& cmd);
static bool verify_command(const AP_Mission::Mission_Command& cmd);
static void exit_mission();
AP_Mission mission(ahrs, &start_command, &verify_command, &exit_mission);
////////////////////////////////////////////////////////////////////////////////
// Waypoint distances
////////////////////////////////////////////////////////////////////////////////
@ -639,17 +645,17 @@ static const struct Location &home = ahrs.get_home();
// Flag for if we have g_gps lock and have set the home location in AHRS
static bool home_is_set;
// The location of the previous waypoint. Used for track following and altitude ramp calculations
static struct Location prev_WP;
static struct AP_Mission::Mission_Command prev_WP;
// The plane's current location
static struct Location current_loc;
// The location of the current/active waypoint. Used for altitude ramp, track following and loiter calculations.
static struct Location next_WP;
static struct AP_Mission::Mission_Command next_WP;
// The location of the active waypoint in Guided mode.
static struct Location guided_WP;
// The location structure information from the Nav command being processed
static struct Location next_nav_command;
static struct AP_Mission::Mission_Command next_nav_command;
// The location structure information from the Non-Nav command being processed
static struct Location next_nonnav_command;
static struct AP_Mission::Mission_Command next_nonnav_command;
////////////////////////////////////////////////////////////////////////////////
// Altitude / Climb rate control
@ -731,7 +737,6 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ read_airspeed, 5, 1200 },
{ update_alt, 5, 3400 },
{ calc_altitude_error, 5, 1000 },
{ update_commands, 5, 5000 },
{ obc_fs_check, 5, 1000 },
{ gcs_update, 1, 1700 },
{ gcs_data_stream_send, 1, 3000 },
@ -1118,7 +1123,7 @@ static void update_GPS_10Hz(void)
*/
static void handle_auto_mode(void)
{
switch(nav_command_ID) {
switch(mission.get_current_do_cmd().id) {
case MAV_CMD_NAV_TAKEOFF:
if (steer_state.hold_course_cd == -1) {
// we don't yet have a heading to hold - just level
@ -1345,7 +1350,7 @@ static void update_navigation()
// distance and bearing calcs only
switch(control_mode) {
case AUTO:
verify_commands();
update_commands();
break;
case LOITER:

146
ArduPlane/commands.pde
View File

@ -3,123 +3,6 @@
* logic for dealing with the current command in the mission and home location
*/
static void init_commands()
{
g.command_index.set_and_save(0);
nav_command_ID = NO_COMMAND;
non_nav_command_ID = NO_COMMAND;
next_nav_command.id = CMD_BLANK;
nav_command_index = 0;
}
static void update_auto()
{
if (g.command_index >= g.command_total) {
handle_no_commands();
if(g.command_total == 0) {
next_WP.lat = home.lat + 10;
next_WP.lng = home.lng + 10;
}
} else {
if(g.command_index != 0) {
g.command_index = nav_command_index;
nav_command_index--;
}
nav_command_ID = NO_COMMAND;
non_nav_command_ID = NO_COMMAND;
next_nav_command.id = CMD_BLANK;
process_next_command();
}
}
/*
fetch a mission item from EEPROM
*/
static struct Location get_cmd_with_index_raw(int16_t i)
{
struct Location temp;
uint16_t mem;
// Find out proper location in memory by using the start_byte position + the index
// --------------------------------------------------------------------------------
if (i > g.command_total) {
memset(&temp, 0, sizeof(temp));
temp.id = CMD_BLANK;
}else{
// read WP position
mem = (WP_START_BYTE) + (i * WP_SIZE);
temp.id = hal.storage->read_byte(mem);
mem++;
temp.options = hal.storage->read_byte(mem);
mem++;
temp.p1 = hal.storage->read_byte(mem);
mem++;
temp.alt = hal.storage->read_dword(mem);
mem += 4;
temp.lat = hal.storage->read_dword(mem);
mem += 4;
temp.lng = hal.storage->read_dword(mem);
}
return temp;
}
/*
fetch a mission item from EEPROM. Adjust altitude to be absolute
*/
static struct Location get_cmd_with_index(int16_t i)
{
struct Location temp;
temp = get_cmd_with_index_raw(i);
// 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 & LOCATION_MASK_OPTIONS_RELATIVE_ALT) &&
(temp.lat != 0 || temp.lng != 0 || temp.alt != 0)) {
temp.alt += home.alt;
}
return temp;
}
// Setters
// -------
static void set_cmd_with_index(struct Location temp, int16_t i)
{
i = constrain_int16(i, 0, g.command_total.get());
uint16_t mem = WP_START_BYTE + (i * WP_SIZE);
// force home wp to absolute height
if (i == 0) {
temp.options &= ~(LOCATION_MASK_OPTIONS_RELATIVE_ALT);
}
// zero unused bits
temp.options &= (LOCATION_MASK_OPTIONS_RELATIVE_ALT | LOCATION_MASK_OPTIONS_LOITER_DIRECTION);
hal.storage->write_byte(mem, temp.id);
mem++;
hal.storage->write_byte(mem, temp.options);
mem++;
hal.storage->write_byte(mem, temp.p1);
mem++;
hal.storage->write_dword(mem, temp.alt);
mem += 4;
hal.storage->write_dword(mem, temp.lat);
mem += 4;
hal.storage->write_dword(mem, temp.lng);
}
static int32_t read_alt_to_hold()
{
if (g.RTL_altitude_cm < 0) {
@ -133,7 +16,7 @@ static int32_t read_alt_to_hold()
* This function stores waypoint commands
* It looks to see what the next command type is and finds the last command.
*/
static void set_next_WP(const struct Location *wp)
static void set_next_WP(const AP_Mission::Mission_Command& cmd)
{
// copy the current WP into the OldWP slot
// ---------------------------------------
@ -141,17 +24,17 @@ static void set_next_WP(const struct Location *wp)
// Load the next_WP slot
// ---------------------
next_WP = *wp;
next_WP = cmd;
// if lat and lon is zero, then use current lat/lon
// this allows a mission to contain a "loiter on the spot"
// command
if (next_WP.lat == 0 && next_WP.lng == 0) {
next_WP.lat = current_loc.lat;
next_WP.lng = current_loc.lng;
if (next_WP.content.location.lat == 0 && next_WP.content.location.lng == 0) {
next_WP.content.location.lat = current_loc.lat;
next_WP.content.location.lng = current_loc.lng;
// additionally treat zero altitude as current altitude
if (next_WP.alt == 0) {
next_WP.alt = current_loc.alt;
if (next_WP.content.location.alt == 0) {
next_WP.content.location.alt = current_loc.alt;
}
}
@ -161,9 +44,9 @@ static void set_next_WP(const struct Location *wp)
// past the waypoint when we start on a leg, then use the current
// location as the previous waypoint, to prevent immediately
// considering the waypoint complete
if (location_passed_point(current_loc, prev_WP, next_WP)) {
if (location_passed_point(current_loc, prev_WP.content.location, next_WP.content.location)) {
gcs_send_text_P(SEVERITY_LOW, PSTR("Resetting prev_WP"));
prev_WP = current_loc;
prev_WP.content.location = current_loc;
}
// used to control FBW and limit the rate of climb
@ -188,11 +71,11 @@ static void set_guided_WP(void)
// copy the current location into the OldWP slot
// ---------------------------------------
prev_WP = current_loc;
prev_WP.content.location = current_loc;
// Load the next_WP slot
// ---------------------
next_WP = guided_WP;
next_WP.content.location = guided_WP;
// used to control FBW and limit the rate of climb
// -----------------------------------------------
@ -224,13 +107,12 @@ static void init_home()
gcs_send_text_fmt(PSTR("gps alt: %lu"), (unsigned long)home.alt);
// Save Home to EEPROM - Command 0
// -------------------
set_cmd_with_index(home, 0);
// Save Home to EEPROM
mission.write_home_to_storage();
// Save prev loc
// -------------
next_WP = prev_WP = home;
next_WP.content.location = prev_WP.content.location = home;
// Load home for a default guided_WP
// -------------

317
ArduPlane/commands_logic.pde
View File

@ -1,110 +1,114 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// forward declarations to make compiler happy
static void do_takeoff(const AP_Mission::Mission_Command& cmd);
static void do_nav_wp(const AP_Mission::Mission_Command& cmd);
static void do_land(const AP_Mission::Mission_Command& cmd);
static void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
static void do_loiter_turns(const AP_Mission::Mission_Command& cmd);
static void do_loiter_time(const AP_Mission::Mission_Command& cmd);
static void do_wait_delay(const AP_Mission::Mission_Command& cmd);
static void do_within_distance(const AP_Mission::Mission_Command& cmd);
static void do_change_alt(const AP_Mission::Mission_Command& cmd);
static void do_change_speed(const AP_Mission::Mission_Command& cmd);
static void do_set_home(const AP_Mission::Mission_Command& cmd);
static AP_Mission::Mission_Command rally_find_best_cmd(const Location &myloc, const Location &homeloc);
/********************************************************************************/
// Command Event Handlers
/********************************************************************************/
static void
handle_process_nav_cmd()
static bool
start_command(const AP_Mission::Mission_Command& cmd)
{
// log when new commands start
if (g.log_bitmask & MASK_LOG_CMD) {
Log_Write_Cmd(cmd);
}
// special handling for nav vs non-nav commands
if (AP_Mission::is_nav_cmd(cmd)) {
// set land_complete to false to stop us zeroing the throttle
land_complete = false;
// set takeoff_complete to true so we don't add extra evevator
// except in a takeoff
// except in a takeoff
takeoff_complete = true;
gcs_send_text_fmt(PSTR("Executing nav command ID #%i"),next_nav_command.id);
switch(next_nav_command.id) {
gcs_send_text_fmt(PSTR("Executing nav command ID #%i"),cmd.id);
}else{
gcs_send_text_fmt(PSTR("Executing command ID #%i"),cmd.id);
}
switch(cmd.id) {
case MAV_CMD_NAV_TAKEOFF:
do_takeoff();
do_takeoff(cmd);
break;
case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint
do_nav_wp();
do_nav_wp(cmd);
break;
case MAV_CMD_NAV_LAND: // LAND to Waypoint
do_land();
do_land(cmd);
break;
case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely
do_loiter_unlimited();
do_loiter_unlimited(cmd);
break;
case MAV_CMD_NAV_LOITER_TURNS: // Loiter N Times
do_loiter_turns();
do_loiter_turns(cmd);
break;
case MAV_CMD_NAV_LOITER_TIME:
do_loiter_time();
do_loiter_time(cmd);
break;
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
set_mode(RTL);
break;
default:
break;
}
}
static void
handle_process_condition_command()
{
gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nonnav_command.id);
switch(next_nonnav_command.id) {
// Conditional commands
case MAV_CMD_CONDITION_DELAY:
do_wait_delay();
do_wait_delay(cmd);
break;
case MAV_CMD_CONDITION_DISTANCE:
do_within_distance();
do_within_distance(cmd);
break;
case MAV_CMD_CONDITION_CHANGE_ALT:
do_change_alt();
do_change_alt(cmd);
break;
default:
break;
}
}
static void handle_process_do_command()
{
gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nonnav_command.id);
switch(next_nonnav_command.id) {
case MAV_CMD_DO_JUMP:
do_jump();
break;
// Do commands
case MAV_CMD_DO_CHANGE_SPEED:
do_change_speed();
do_change_speed(cmd);
break;
case MAV_CMD_DO_SET_HOME:
do_set_home();
do_set_home(cmd);
break;
case MAV_CMD_DO_SET_SERVO:
ServoRelayEvents.do_set_servo(next_nonnav_command.p1, next_nonnav_command.alt);
ServoRelayEvents.do_set_servo(cmd.p1, cmd.content.location.alt);
break;
case MAV_CMD_DO_SET_RELAY:
ServoRelayEvents.do_set_relay(next_nonnav_command.p1, next_nonnav_command.alt);
ServoRelayEvents.do_set_relay(cmd.p1, cmd.content.location.alt);
break;
case MAV_CMD_DO_REPEAT_SERVO:
ServoRelayEvents.do_repeat_servo(next_nonnav_command.p1, next_nonnav_command.alt,
next_nonnav_command.lat, next_nonnav_command.lng);
ServoRelayEvents.do_repeat_servo(cmd.p1, cmd.content.location.alt,
cmd.content.location.lat, cmd.content.location.lng);
break;
case MAV_CMD_DO_REPEAT_RELAY:
ServoRelayEvents.do_repeat_relay(next_nonnav_command.p1, next_nonnav_command.alt,
next_nonnav_command.lat);
ServoRelayEvents.do_repeat_relay(cmd.p1, cmd.content.location.alt,
cmd.content.location.lat);
break;
#if CAMERA == ENABLED
@ -119,7 +123,7 @@ static void handle_process_do_command()
break;
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
camera.set_trigger_distance(next_nonnav_command.alt);
camera.set_trigger_distance(cmd.content.location.alt);
break;
#endif
@ -132,7 +136,7 @@ static void handle_process_do_command()
case MAV_CMD_NAV_ROI:
#if 0
// send the command to the camera mount
camera_mount.set_roi_cmd(&command_nav_queue);
camera_mount.set_roi_cmd(cmd);
#else
gcs_send_text_P(SEVERITY_LOW, PSTR("DO_SET_ROI not supported"));
#endif
@ -147,16 +151,20 @@ static void handle_process_do_command()
break;
#endif
}
return true;
}
static void handle_no_commands()
static void exit_mission()
{
gcs_send_text_fmt(PSTR("Returning to Home"));
next_nav_command = rally_find_best_location(current_loc, home);
next_nav_command = rally_find_best_cmd(current_loc, home);
// To-Do: fix this by converting next_nav_command to cmd?
next_nav_command.id = MAV_CMD_NAV_LOITER_UNLIM;
nav_command_ID = MAV_CMD_NAV_LOITER_UNLIM;
non_nav_command_ID = WAIT_COMMAND;
handle_process_nav_cmd();
//nav_command_ID = MAV_CMD_NAV_LOITER_UNLIM;
//non_nav_command_ID = WAIT_COMMAND;
//handle_process_nav_cmd();
start_command(next_nav_command);
}
/*******************************************************************************
@ -167,9 +175,9 @@ operation returns true when the mission element has completed and we
should move onto the next mission element.
*******************************************************************************/
static bool verify_nav_command() // Returns true if command complete
static bool verify_command(const AP_Mission::Mission_Command& cmd) // Returns true if command complete
{
switch(nav_command_ID) {
switch(cmd.id) {
case MAV_CMD_NAV_TAKEOFF:
return verify_takeoff();
@ -192,17 +200,7 @@ static bool verify_nav_command() // Returns true if command complete
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
return verify_RTL();
default:
gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_nav: Invalid or no current Nav cmd"));
}
return false;
}
static bool verify_condition_command() // Returns true if command complete
{
switch(non_nav_command_ID) {
case NO_COMMAND:
break;
// Conditional commands
case MAV_CMD_CONDITION_DELAY:
return verify_wait_delay();
@ -217,15 +215,35 @@ static bool verify_condition_command() // Returns true if command compl
break;
case WAIT_COMMAND:
return 0;
return false;
break;
// do commands (always return true)
case MAV_CMD_DO_CHANGE_SPEED:
case MAV_CMD_DO_SET_HOME:
case MAV_CMD_DO_SET_SERVO:
case MAV_CMD_DO_SET_RELAY:
case MAV_CMD_DO_REPEAT_SERVO:
case MAV_CMD_DO_REPEAT_RELAY:
case MAV_CMD_DO_CONTROL_VIDEO:
case MAV_CMD_DO_DIGICAM_CONFIGURE:
case MAV_CMD_DO_DIGICAM_CONTROL:
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
case MAV_CMD_NAV_ROI:
case MAV_CMD_DO_MOUNT_CONFIGURE:
case MAV_CMD_DO_MOUNT_CONTROL:
return true;
default:
// error message
if (AP_Mission::is_nav_cmd(cmd)) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_nav: Invalid or no current Nav cmd"));
}else{
gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Invalid or no current Condition cmd"));
break;
}
return false;
// return true so that we do not get stuck at this command
return true;
}
}
/********************************************************************************/
@ -235,8 +253,8 @@ static bool verify_condition_command() // Returns true if command compl
static void do_RTL(void)
{
control_mode = RTL;
prev_WP = current_loc;
next_WP = rally_find_best_location(current_loc, home);
prev_WP.content.location = current_loc;
next_WP = rally_find_best_cmd(current_loc, home);
if (g.loiter_radius < 0) {
loiter.direction = -1;
@ -250,57 +268,57 @@ static void do_RTL(void)
Log_Write_Mode(control_mode);
}
static void do_takeoff()
static void do_takeoff(const AP_Mission::Mission_Command& cmd)
{
set_next_WP(&next_nav_command);
set_next_WP(cmd);
// pitch in deg, airspeed m/s, throttle %, track WP 1 or 0
takeoff_pitch_cd = (int)next_nav_command.p1 * 100;
takeoff_altitude_cm = next_nav_command.alt;
next_WP.lat = home.lat + 10;
next_WP.lng = home.lng + 10;
takeoff_pitch_cd = (int)cmd.p1 * 100;
takeoff_altitude_cm = cmd.content.location.alt;
next_WP.content.location.lat = home.lat + 10;
next_WP.content.location.lng = home.lng + 10;
takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction
// Flag also used to override "on the ground" throttle disable
}
static void do_nav_wp()
static void do_nav_wp(const AP_Mission::Mission_Command& cmd)
{
set_next_WP(&next_nav_command);
set_next_WP(cmd);
}
static void do_land()
static void do_land(const AP_Mission::Mission_Command& cmd)
{
set_next_WP(&next_nav_command);
set_next_WP(cmd);
}
static void loiter_set_direction_wp(const struct Location *nav_command)
static void loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd)
{
if (nav_command->options & LOCATION_MASK_OPTIONS_LOITER_DIRECTION) {
if (cmd.content.location.flags.loiter_ccw) {
loiter.direction = -1;
} else {
loiter.direction = 1;
}
}
static void do_loiter_unlimited()
static void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
{
set_next_WP(&next_nav_command);
loiter_set_direction_wp(&next_nav_command);
set_next_WP(cmd);
loiter_set_direction_wp(cmd);
}
static void do_loiter_turns()
static void do_loiter_turns(const AP_Mission::Mission_Command& cmd)
{
set_next_WP(&next_nav_command);
loiter.total_cd = next_nav_command.p1 * 36000UL;
loiter_set_direction_wp(&next_nav_command);
set_next_WP(cmd);
loiter.total_cd = cmd.p1 * 36000UL;
loiter_set_direction_wp(cmd);
}
static void do_loiter_time()
static void do_loiter_time(const AP_Mission::Mission_Command& cmd)
{
set_next_WP(&next_nav_command);
set_next_WP(cmd);
// we set start_time_ms when we reach the waypoint
loiter.start_time_ms = 0;
loiter.time_max_ms = next_nav_command.p1 * (uint32_t)1000; // units are seconds
loiter_set_direction_wp(&next_nav_command);
loiter.time_max_ms = cmd.p1 * (uint32_t)1000; // units are seconds
loiter_set_direction_wp(cmd);
}
/********************************************************************************/
@ -327,7 +345,7 @@ static bool verify_takeoff()
if (adjusted_altitude_cm() > takeoff_altitude_cm) {
steer_state.hold_course_cd = -1;
takeoff_complete = true;
next_WP = prev_WP = current_loc;
next_WP.content.location = prev_WP.content.location = current_loc;
return true;
} else {
return false;
@ -344,7 +362,7 @@ static bool verify_land()
// Set land_complete if we are within 2 seconds distance or within
// 3 meters altitude of the landing point
if ((wp_distance <= (g.land_flare_sec*g_gps->ground_speed_cm*0.01f))
|| (adjusted_altitude_cm() <= next_WP.alt + g.land_flare_alt*100)) {
|| (adjusted_altitude_cm() <= next_WP.content.location.alt + g.land_flare_alt*100)) {
land_complete = true;
@ -377,7 +395,7 @@ static bool verify_land()
// recalc bearing error with hold_course;
nav_controller->update_heading_hold(steer_state.hold_course_cd);
} else {
nav_controller->update_waypoint(prev_WP, next_WP);
nav_controller->update_waypoint(prev_WP.content.location, next_WP.content.location);
}
return false;
}
@ -386,29 +404,29 @@ static bool verify_nav_wp()
{
steer_state.hold_course_cd = -1;
nav_controller->update_waypoint(prev_WP, next_WP);
nav_controller->update_waypoint(prev_WP.content.location, next_WP.content.location);
// see if the user has specified a maximum distance to waypoint
if (g.waypoint_max_radius > 0 && wp_distance > (uint16_t)g.waypoint_max_radius) {
if (location_passed_point(current_loc, prev_WP, next_WP)) {
if (location_passed_point(current_loc, prev_WP.content.location, next_WP.content.location)) {
// this is needed to ensure completion of the waypoint
prev_WP = current_loc;
prev_WP.content.location = current_loc;
}
return false;
}
if (wp_distance <= nav_controller->turn_distance(g.waypoint_radius)) {
gcs_send_text_fmt(PSTR("Reached Waypoint #%i dist %um"),
(unsigned)nav_command_index,
(unsigned)get_distance(current_loc, next_WP));
(unsigned)mission.get_current_nav_cmd().index,
(unsigned)get_distance(current_loc, next_WP.content.location));
return true;
}
// have we flown past the waypoint?
if (location_passed_point(current_loc, prev_WP, next_WP)) {
if (location_passed_point(current_loc, prev_WP.content.location, next_WP.content.location)) {
gcs_send_text_fmt(PSTR("Passed Waypoint #%i dist %um"),
(unsigned)nav_command_index,
(unsigned)get_distance(current_loc, next_WP));
(unsigned)mission.get_current_nav_cmd().index,
(unsigned)get_distance(current_loc, next_WP.content.location));
return true;
}
@ -464,27 +482,27 @@ static bool verify_RTL()
// Condition (May) commands
/********************************************************************************/
static void do_wait_delay()
static void do_wait_delay(const AP_Mission::Mission_Command& cmd)
{
condition_start = millis();
condition_value = next_nonnav_command.lat * 1000; // convert to milliseconds
condition_value = cmd.content.location.lat * 1000; // convert to milliseconds
}
static void do_change_alt()
static void do_change_alt(const AP_Mission::Mission_Command& cmd)
{
condition_rate = labs((int)next_nonnav_command.lat);
condition_value = next_nonnav_command.alt;
condition_rate = labs((int)cmd.content.location.lat);
condition_value = cmd.content.location.alt;
if (condition_value < adjusted_altitude_cm()) {
condition_rate = -condition_rate;
}
target_altitude_cm = adjusted_altitude_cm() + (condition_rate / 10); // Divide by ten for 10Hz update
next_WP.alt = condition_value; // For future nav calculations
next_WP.content.location.alt = condition_value; // For future nav calculations
offset_altitude_cm = 0; // For future nav calculations
}
static void do_within_distance()
static void do_within_distance(const AP_Mission::Mission_Command& cmd)
{
condition_value = next_nonnav_command.lat;
condition_value = cmd.content.location.lat;
}
/********************************************************************************/
@ -531,84 +549,37 @@ static void do_loiter_at_location()
} else {
loiter.direction = 1;
}
next_WP = current_loc;
next_WP.content.location = current_loc;
}
static void do_jump()
static void do_change_speed(const AP_Mission::Mission_Command& cmd)
{
if (next_nonnav_command.lat == 0) {
// the jump counter has reached zero - ignore
gcs_send_text_fmt(PSTR("Jumps left: 0 - skipping"));
return;
}
if (next_nonnav_command.p1 >= g.command_total) {
gcs_send_text_fmt(PSTR("Skipping invalid jump to %i"), next_nonnav_command.p1);
return;
}
struct Location temp;
temp = get_cmd_with_index(g.command_index);
gcs_send_text_fmt(PSTR("Jump to WP %u. Jumps left: %d"),
(unsigned)next_nonnav_command.p1,
(int)next_nonnav_command.lat);
if (next_nonnav_command.lat > 0) {
// Decrement repeat counter
temp.lat = next_nonnav_command.lat - 1;
set_cmd_with_index(temp, g.command_index);
}
nav_command_ID = NO_COMMAND;
next_nav_command.id = NO_COMMAND;
non_nav_command_ID = NO_COMMAND;
gcs_send_text_fmt(PSTR("setting command index: %i"), next_nonnav_command.p1);
g.command_index.set_and_save(next_nonnav_command.p1);
nav_command_index = next_nonnav_command.p1;
// Need to back "next_WP" up as it was set to the next waypoint following the jump
next_WP = prev_WP;
temp = get_cmd_with_index(g.command_index);
next_nav_command = temp;
nav_command_ID = next_nav_command.id;
non_nav_command_index = g.command_index;
non_nav_command_ID = WAIT_COMMAND;
if (should_log(MASK_LOG_CMD)) {
Log_Write_Cmd(g.command_index, &next_nav_command);
}
handle_process_nav_cmd();
}
static void do_change_speed()
{
switch (next_nonnav_command.p1)
switch (cmd.p1)
{
case 0: // Airspeed
if (next_nonnav_command.alt > 0) {
g.airspeed_cruise_cm.set(next_nonnav_command.alt * 100);
gcs_send_text_fmt(PSTR("Set airspeed %u m/s"), (unsigned)next_nonnav_command.alt);
if (cmd.content.location.alt > 0) {
g.airspeed_cruise_cm.set(cmd.content.location.alt * 100);
gcs_send_text_fmt(PSTR("Set airspeed %u m/s"), (unsigned)cmd.content.location.alt);
}
break;
case 1: // Ground speed
gcs_send_text_fmt(PSTR("Set groundspeed %u"), (unsigned)next_nonnav_command.alt);
g.min_gndspeed_cm.set(next_nonnav_command.alt * 100);
gcs_send_text_fmt(PSTR("Set groundspeed %u"), (unsigned)cmd.content.location.alt);
g.min_gndspeed_cm.set(cmd.content.location.alt * 100);
break;
}
if (next_nonnav_command.lat > 0) {
gcs_send_text_fmt(PSTR("Set throttle %u"), (unsigned)next_nonnav_command.lat);
aparm.throttle_cruise.set(next_nonnav_command.lat);
if (cmd.content.location.lat > 0) {
gcs_send_text_fmt(PSTR("Set throttle %u"), (unsigned)cmd.content.location.lat);
aparm.throttle_cruise.set(cmd.content.location.lat);
}
}
static void do_set_home()
static void do_set_home(const AP_Mission::Mission_Command& cmd)
{
if (next_nonnav_command.p1 == 1 && g_gps->status() == GPS::GPS_OK_FIX_3D) {
if (cmd.p1 == 1 && g_gps->status() == GPS::GPS_OK_FIX_3D) {
init_home();
} else {
ahrs.set_home(next_nonnav_command.lat, next_nonnav_command.lng, next_nonnav_command.alt*100.0f);
ahrs.set_home(cmd.content.location.lat, cmd.content.location.lng, cmd.content.location.alt);
home_is_set = true;
}
}

149
ArduPlane/commands_process.pde
View File

@ -1,156 +1,13 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// For changing active command mid-mission
//----------------------------------------
void change_command(uint8_t cmd_index)
{
struct Location temp;
if (cmd_index == 0) {
init_commands();
gcs_send_text_fmt(PSTR("Received Request - reset mission"));
return;
}
temp = get_cmd_with_index(cmd_index);
if (temp.id > MAV_CMD_NAV_LAST ) {
gcs_send_text_fmt(PSTR("Cannot change to non-Nav cmd %u"), (unsigned)cmd_index);
} else {
gcs_send_text_fmt(PSTR("Received Request - jump to command #%i"),cmd_index);
nav_command_ID = NO_COMMAND;
next_nav_command.id = NO_COMMAND;
non_nav_command_ID = NO_COMMAND;
/*
if we are in AUTO then we need to set the nav_command_index
to one less than the requested index as
process_next_command() will increment the index before using
it. If not in AUTO then we just set the index as give.
Thanks to Michael Day for finding this!
*/
if (control_mode == AUTO) {
nav_command_index = cmd_index - 1;
} else {
nav_command_index = cmd_index;
}
g.command_index.set_and_save(cmd_index);
update_commands();
}
}
// called by 10 Hz loop
// called by update navigation at 10Hz
// --------------------
static void update_commands(void)
{
if(control_mode == AUTO) {
if(home_is_set == true && g.command_total > 1) {
process_next_command();
if(home_is_set == true && mission.num_commands() > 1) {
mission.update();
}
}
}
static void verify_commands(void)
{
if(verify_nav_command()) {
nav_command_ID = NO_COMMAND;
}
if(verify_condition_command()) {
non_nav_command_ID = NO_COMMAND;
}
}
static void process_next_command()
{
// This function makes sure that we always have a current navigation command
// and loads conditional or immediate commands if applicable
struct Location temp;
uint8_t old_index = nav_command_index;
// these are Navigation/Must commands
// ---------------------------------
if (nav_command_ID == NO_COMMAND) { // no current navigation command loaded
temp.id = MAV_CMD_NAV_LAST;
while(temp.id >= MAV_CMD_NAV_LAST && nav_command_index <= g.command_total) {
nav_command_index++;
temp = get_cmd_with_index(nav_command_index);
}
gcs_send_text_fmt(PSTR("Nav command index updated to #%i"),nav_command_index);
if(nav_command_index > g.command_total) {
// we are out of commands!
gcs_send_text_P(SEVERITY_LOW,PSTR("out of commands!"));
handle_no_commands();
} else {
next_nav_command = temp;
nav_command_ID = next_nav_command.id;
non_nav_command_index = NO_COMMAND; // This will cause the next intervening non-nav command (if any) to be loaded
non_nav_command_ID = NO_COMMAND;
if (should_log(MASK_LOG_CMD)) {
Log_Write_Cmd(g.command_index, &next_nav_command);
}
handle_process_nav_cmd();
}
}
// these are Condition/May and Do/Now commands
// -------------------------------------------
if (non_nav_command_index == NO_COMMAND) { // If the index is NO_COMMAND then we have just loaded a nav command
non_nav_command_index = old_index + 1;
//gcs_send_text_fmt(PSTR("Non-Nav command index #%i"),non_nav_command_index);
} else if (non_nav_command_ID == NO_COMMAND) { // If the ID is NO_COMMAND then we have just completed a non-nav command
non_nav_command_index++;
}
//gcs_send_text_fmt(PSTR("Nav command index #%i"),nav_command_index);
//gcs_send_text_fmt(PSTR("Non-Nav command index #%i"),non_nav_command_index);
//gcs_send_text_fmt(PSTR("Non-Nav command ID #%i"),non_nav_command_ID);
if (nav_command_index <= (int)g.command_total && non_nav_command_ID == NO_COMMAND) {
temp = get_cmd_with_index(non_nav_command_index);
if (temp.id <= MAV_CMD_NAV_LAST) {
// The next command is a nav command. No non-nav commands to do
g.command_index.set_and_save(nav_command_index);
non_nav_command_index = nav_command_index;
non_nav_command_ID = WAIT_COMMAND;
gcs_send_text_fmt(PSTR("Non-Nav command ID updated to #%i idx=%u"),
(unsigned)non_nav_command_ID,
(unsigned)non_nav_command_index);
} else {
// The next command is a non-nav command. Prepare to execute it.
g.command_index.set_and_save(non_nav_command_index);
next_nonnav_command = temp;
non_nav_command_ID = next_nonnav_command.id;
gcs_send_text_fmt(PSTR("(2) Non-Nav command ID updated to #%i idx=%u"),
(unsigned)non_nav_command_ID, (unsigned)non_nav_command_index);
if (should_log(MASK_LOG_CMD)) {
Log_Write_Cmd(g.command_index, &next_nonnav_command);
}
process_non_nav_command();
}
}
}
static void process_non_nav_command()
{
//gcs_send_text_P(SEVERITY_LOW,PSTR("new non-nav command loaded"));
if(non_nav_command_ID < MAV_CMD_CONDITION_LAST) {
handle_process_condition_command();
} else {
handle_process_do_command();
// flag command ID so a new one is loaded
// -----------------------------------------
if (non_nav_command_ID != WAIT_COMMAND) {
non_nav_command_ID = NO_COMMAND;
}
}
}

9
ArduPlane/control_modes.pde
View File

@ -43,14 +43,15 @@ static void read_control_switch()
set_mode((enum FlightMode)(flight_modes[switchPosition].get()));
oldSwitchPosition = switchPosition;
prev_WP = current_loc;
prev_WP.content.location = current_loc;
}
if (g.reset_mission_chan != 0 &&
hal.rcin->read(g.reset_mission_chan-1) > RESET_SWITCH_CHAN_PWM) {
// reset to first waypoint in mission
prev_WP = current_loc;
change_command(0);
if (mission.set_current_cmd(0)) {
// reset to first waypoint in mission
prev_WP.content.location = current_loc;
}
}
switch_debouncer = false;

2
ArduPlane/geofence.pde
View File

@ -277,8 +277,6 @@ static void geofence_check(bool altitude_check_only)
// min and max
guided_WP.alt = home.alt + 100.0*(g.fence_minalt + g.fence_maxalt)/2;
}
guided_WP.id = 0;
guided_WP.p1 = 0;
guided_WP.options = 0;
guided_WP.lat = geofence_state->boundary[0].x;
guided_WP.lng = geofence_state->boundary[0].y;

39
ArduPlane/navigation.pde
View File

@ -54,13 +54,13 @@ static void navigate()
return;
}
if (next_WP.lat == 0) {
if (next_WP.content.location.lat == 0) {
return;
}
// waypoint distance from plane
// ----------------------------
wp_distance = get_distance(current_loc, next_WP);
wp_distance = get_distance(current_loc, next_WP.content.location);
if (wp_distance < 0) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("WP error - distance < 0"));
@ -75,7 +75,6 @@ static void navigate()
update_navigation();
}
static void calc_airspeed_errors()
{
float aspeed_cm = airspeed.get_airspeed_cm();
@ -136,19 +135,19 @@ static void calc_altitude_error()
if (nav_controller->reached_loiter_target()) {
// once we reach a loiter target then lock to the final
// altitude target
target_altitude_cm = next_WP.alt;
target_altitude_cm = next_WP.content.location.alt;
} else if (offset_altitude_cm != 0) {
// control climb/descent rate
target_altitude_cm = next_WP.alt - (offset_altitude_cm*((float)(wp_distance-30) / (float)(wp_totalDistance-30)));
target_altitude_cm = next_WP.content.location.alt - (offset_altitude_cm*((float)(wp_distance-30) / (float)(wp_totalDistance-30)));
// stay within a certain range
if (prev_WP.alt > next_WP.alt) {
target_altitude_cm = constrain_int32(target_altitude_cm, next_WP.alt, prev_WP.alt);
if (prev_WP.content.location.alt > next_WP.content.location.alt) {
target_altitude_cm = constrain_int32(target_altitude_cm, next_WP.content.location.alt, prev_WP.content.location.alt);
}else{
target_altitude_cm = constrain_int32(target_altitude_cm, prev_WP.alt, next_WP.alt);
target_altitude_cm = constrain_int32(target_altitude_cm, prev_WP.content.location.alt, next_WP.content.location.alt);
}
} else if (non_nav_command_ID != MAV_CMD_CONDITION_CHANGE_ALT) {
target_altitude_cm = next_WP.alt;
} else if (mission.get_current_do_cmd().id != MAV_CMD_CONDITION_CHANGE_ALT) {
target_altitude_cm = next_WP.content.location.alt;
}
altitude_error_cm = target_altitude_cm - adjusted_altitude_cm();
@ -156,7 +155,7 @@ static void calc_altitude_error()
static void update_loiter()
{
nav_controller->update_loiter(next_WP, abs(g.loiter_radius), loiter.direction);
nav_controller->update_loiter(next_WP.content.location, abs(g.loiter_radius), loiter.direction);
}
/*
@ -181,15 +180,15 @@ static void update_cruise()
cruise_state.locked_heading = true;
cruise_state.lock_timer_ms = 0;
cruise_state.locked_heading_cd = g_gps->ground_course_cd;
prev_WP = current_loc;
prev_WP.content.location = current_loc;
}
if (cruise_state.locked_heading) {
next_WP = prev_WP;
// always look 1km ahead
location_update(next_WP,
location_update(next_WP.content.location,
cruise_state.locked_heading_cd*0.01f,
get_distance(prev_WP, current_loc) + 1000);
nav_controller->update_waypoint(prev_WP, next_WP);
get_distance(prev_WP.content.location, current_loc) + 1000);
nav_controller->update_waypoint(prev_WP.content.location, next_WP.content.location);
}
}
@ -233,7 +232,7 @@ static void setup_glide_slope(void)
{
// establish the distance we are travelling to the next waypoint,
// for calculating out rate of change of altitude
wp_totalDistance = get_distance(current_loc, next_WP);
wp_totalDistance = get_distance(current_loc, next_WP.content.location);
wp_distance = wp_totalDistance;
/*
@ -247,8 +246,8 @@ static void setup_glide_slope(void)
rapidly if below it. See
https://github.com/diydrones/ardupilot/issues/39
*/
if (current_loc.alt > next_WP.alt) {
offset_altitude_cm = next_WP.alt - current_loc.alt;
if (current_loc.alt > next_WP.content.location.alt) {
offset_altitude_cm = next_WP.content.location.alt - current_loc.alt;
} else {
offset_altitude_cm = 0;
}
@ -256,9 +255,9 @@ static void setup_glide_slope(void)
case AUTO:
if (prev_WP.id != MAV_CMD_NAV_TAKEOFF &&
prev_WP.alt != home.alt &&
prev_WP.content.location.alt != home.alt &&
(next_WP.id == MAV_CMD_NAV_WAYPOINT || next_WP.id == MAV_CMD_NAV_LAND)) {
offset_altitude_cm = next_WP.alt - prev_WP.alt;
offset_altitude_cm = next_WP.content.location.alt - prev_WP.content.location.alt;
} else {
offset_altitude_cm = 0;
}

17
ArduPlane/rally.pde
View File

@ -63,12 +63,10 @@ static bool find_best_rally_point(const Location &myloc, const Location &homeloc
}
// translate a RallyLocation to a Location
static Location rally_location_to_location(const RallyLocation &r_loc, const Location &homeloc)
static Location rally_location_to_location(const RallyLocation &r_loc, const Location &homeloc)
{
Location ret = {};
ret.id = MAV_CMD_NAV_LOITER_UNLIM;
ret.options = LOCATION_MASK_OPTIONS_RELATIVE_ALT;
ret.flags.relative_alt = true;
//Currently can't do true AGL on the APM. Relative altitudes are
//relative to HOME point's altitude. Terrain on the board is inbound
@ -82,17 +80,18 @@ static Location rally_location_to_location(const RallyLocation &r_loc, const Loc
}
// return best RTL location from current position
static Location rally_find_best_location(const Location &myloc, const Location &homeloc)
static AP_Mission::Mission_Command rally_find_best_cmd(const Location &myloc, const Location &homeloc)
{
RallyLocation ral_loc = {};
Location ret = {};
AP_Mission::Mission_Command ret = {};
ret.id = MAV_CMD_NAV_LOITER_UNLIM;
if (find_best_rally_point(myloc, home, ral_loc)) {
//we have setup Rally points: use them instead of Home for RTL
ret = rally_location_to_location(ral_loc, home);
ret.content.location = rally_location_to_location(ral_loc, home);
} else {
ret = homeloc;
ret.content.location = homeloc;
// Altitude to hold over home
ret.alt = read_alt_to_hold();
ret.content.location.alt = read_alt_to_hold();
}
return ret;
}

15
ArduPlane/system.pde
View File

@ -268,10 +268,6 @@ static void startup_ground(void)
// ------------------------------------
//save_EEPROM_groundstart();
// initialize commands
// -------------------
init_commands();
// Makes the servos wiggle - 3 times signals ready to fly
// -----------------------
if (!g.skip_gyro_cal) {
@ -337,16 +333,17 @@ static void set_mode(enum FlightMode mode)
case CIRCLE:
// the altitude to circle at is taken from the current altitude
next_WP.alt = current_loc.alt;
next_WP.content.location.alt = current_loc.alt;
break;
case AUTO:
prev_WP = current_loc;
update_auto();
prev_WP.content.location = current_loc;
// start the mission
mission.start();
break;
case RTL:
prev_WP = current_loc;
prev_WP.content.location = current_loc;
do_RTL();
break;
@ -360,7 +357,7 @@ static void set_mode(enum FlightMode mode)
break;
default:
prev_WP = current_loc;
prev_WP.content.location = current_loc;
do_RTL();
break;
}

28
ArduPlane/test.pde
View File

@ -27,6 +27,9 @@ static int8_t test_logging(uint8_t argc, const Menu::arg *argv);
static int8_t test_shell(uint8_t argc, const Menu::arg *argv);
#endif
// forward declaration to keep the compiler happy
static void test_wp_print(const AP_Mission::Mission_Command& cmd);
// Creates a constant array of structs representing menu options
// and stores them in Flash memory, not RAM.
// User enters the string in the console to call the functions on the right.
@ -282,29 +285,30 @@ test_wp(uint8_t argc, const Menu::arg *argv)
cliSerial->printf_P(PSTR("Hold altitude of %dm\n"), (int)g.RTL_altitude_cm/100);
}
cliSerial->printf_P(PSTR("%d waypoints\n"), (int)g.command_total);
cliSerial->printf_P(PSTR("%d waypoints\n"), (int)mission.num_commands());
cliSerial->printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius);
cliSerial->printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius);
for(uint8_t i = 0; i <= g.command_total; i++) {
struct Location temp = get_cmd_with_index(i);
test_wp_print(&temp, i);
for(uint8_t i = 0; i <= mission.num_commands(); i++) {
AP_Mission::Mission_Command temp_cmd;
mission.read_cmd_from_storage(i,temp_cmd);
test_wp_print(temp_cmd);
}
return (0);
}
static void
test_wp_print(const struct Location *cmd, uint8_t wp_index)
test_wp_print(const AP_Mission::Mission_Command& cmd)
{
cliSerial->printf_P(PSTR("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
(int)wp_index,
(int)cmd->id,
(int)cmd->options,
(int)cmd->p1,
(long)cmd->alt,
(long)cmd->lat,
(long)cmd->lng);
(int)cmd.index,
(int)cmd.id,
(int)cmd.content.location.options,
(int)cmd.p1,
(long)cmd.content.location.alt,
(long)cmd.content.location.lat,
(long)cmd.content.location.lng);
}
static int8_t