Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Scripting: allow to get/set/create arbitrary mission items

Browse Source 
fetch item/s by their index, and review wp data, etc.  
AP_Mission: ran mission files through approved astyle  as they were non-compliant before this( astyle --options=Tools/CodeStyle/astylerc   )
This commit is contained in:
Buzz 2020-05-05 10:27:56 +10:00 committed by Andrew Tridgell
parent 59a2667870
commit 963b25059d
4 changed files with 240 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

124
libraries/AP_Mission/AP_Mission.cpp
View File

@ -258,7 +258,7 @@ void AP_Mission::update()
complete();
return;
}
}else{
} else {
// run the active nav command
if (verify_command(_nav_cmd)) {
// market _nav_cmd as complete (it will be started on the next iteration)
@ -275,7 +275,7 @@ void AP_Mission::update()
// check if we have an active do command
if (!_flags.do_cmd_loaded) {
advance_current_do_cmd();
}else{
} else {
// check the active do command
if (verify_command(_do_cmd)) {
// mark _do_cmd as complete
@ -474,7 +474,7 @@ bool AP_Mission::set_current_cmd(uint16_t index, bool rewind)
// set current navigation command
_nav_cmd = cmd;
_flags.nav_cmd_loaded = true;
}else{
} else {
// set current do command
if (!_flags.do_cmd_loaded) {
_do_cmd = cmd;
@ -506,6 +506,79 @@ bool AP_Mission::set_current_cmd(uint16_t index, bool rewind)
return true;
}
// returns false on any issue at all.
bool AP_Mission::set_item(uint16_t index, mavlink_mission_item_int_t& src_packet)
{
// this is the on-storage format
AP_Mission::Mission_Command cmd;
// can't handle request for anything bigger than the mission size+1...
if (index > num_commands() ) {
return false;
}
// convert from mavlink-ish format to storage format, if we can.
if (mavlink_int_to_mission_cmd(src_packet, cmd) != MAV_MISSION_ACCEPTED) {
return false;
}
// A request to set the 'next' item after the end is how we add an extra
// item to the list, thus allowing us to write entire missions if needed.
if (index == num_commands() ) {
return add_cmd( cmd);
}
// replacing an existing mission item...
return AP_Mission::replace_cmd( index, cmd);
}
bool AP_Mission::get_item(uint16_t index, mavlink_mission_item_int_t& ret_packet)
{
// setting ret_packet.command = -1 and/or returning false
// means it contains invalid data after it leaves here.
// this is the on-storage format
AP_Mission::Mission_Command cmd;
// can't handle request for anything bigger than the mission size...
if (index >= num_commands() ) {
ret_packet.command = -1;
return false;
}
// minimal placeholder values during read-from-storage
ret_packet.target_system = 1; // unused sysid
ret_packet.target_component = 1; // unused compid
// 0=home, higher number/s = mission item number.
ret_packet.seq = index;
// retrieve mission from eeprom
if (!read_cmd_from_storage(ret_packet.seq, cmd)) {
ret_packet.command = -1;
return false;
}
// convert into mavlink-ish format for lua and friends.
if (!mission_cmd_to_mavlink_int(cmd, ret_packet)) {
ret_packet.command = -1;
return false;
}
// set packet's current field to 1 if this is the command being executed
if (cmd.id == (uint16_t)get_current_nav_cmd().index) {
ret_packet.current = 1;
} else {
ret_packet.current = 0;
}
// set auto continue to 1, becasue that's what's done elsewhere.
ret_packet.autocontinue = 1; // 1 (true), 0 (false)
ret_packet.command = cmd.id;
return true;
}
struct PACKED Packed_Location_Option_Flags {
uint8_t relative_alt : 1; // 1 if altitude is relative to home
uint8_t unused1 : 1; // unused flag (defined so that loiter_ccw uses the correct bit)
@ -699,7 +772,8 @@ void AP_Mission::write_home_to_storage()
write_cmd_to_storage(0,home_cmd);
}
MAV_MISSION_RESULT AP_Mission::sanity_check_params(const mavlink_mission_item_int_t& packet) {
MAV_MISSION_RESULT AP_Mission::sanity_check_params(const mavlink_mission_item_int_t& packet)
{
uint8_t nan_mask;
switch (packet.command) {
case MAV_CMD_NAV_WAYPOINT:
@ -762,8 +836,7 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_
// this is reserved for storing 16 bit command IDs
return MAV_MISSION_INVALID;
case MAV_CMD_NAV_WAYPOINT: // MAV ID: 16
{
case MAV_CMD_NAV_WAYPOINT: { // MAV ID: 16
/*
the 15 byte limit means we can't fit both delay and radius
in the cmd structure. When we expand the mission structure
@ -791,8 +864,7 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_
cmd.content.location.loiter_ccw = (packet.param3 < 0); // -1 = counter clockwise, +1 = clockwise
break;
case MAV_CMD_NAV_LOITER_TURNS: // MAV ID: 18
{
case MAV_CMD_NAV_LOITER_TURNS: { // MAV ID: 18
uint16_t num_turns = packet.param1; // param 1 is number of times to circle is held in low p1
uint16_t radius_m = fabsf(packet.param3); // param 3 is radius in meters is held in high p1
cmd.p1 = (radius_m<<8) | (num_turns & 0x00FF); // store radius in high byte of p1, num turns in low byte of p1
@ -1472,7 +1544,7 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c
packet.z = cmd.content.location.alt / 100.0f; // cmd alt in cm to m
if (cmd.content.location.relative_alt) {
packet.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT;
}else{
} else {
packet.frame = MAV_FRAME_GLOBAL;
}
#if AP_TERRAIN_AVAILABLE
@ -1543,7 +1615,7 @@ bool AP_Mission::advance_current_nav_cmd(uint16_t starting_index)
if (cmd_index == AP_MISSION_CMD_INDEX_NONE) {
// start from beginning of the mission command list
cmd_index = AP_MISSION_FIRST_REAL_COMMAND;
}else{
} else {
// start from one position past the current nav command
cmd_index++;
}
@ -1575,7 +1647,7 @@ bool AP_Mission::advance_current_nav_cmd(uint16_t starting_index)
}
// save a loaded wp index in history array for when _repeat_dist is set via MAV_CMD_DO_SET_RESUME_REPEAT_DIST
// and prevent history being re-written until vehicle returns to interupted position
if(_repeat_dist > 0 && !_flags.resuming_mission && _nav_cmd.index != AP_MISSION_CMD_INDEX_NONE && !(_nav_cmd.content.location.lat == 0 && _nav_cmd.content.location.lng == 0)) {
if (_repeat_dist > 0 && !_flags.resuming_mission && _nav_cmd.index != AP_MISSION_CMD_INDEX_NONE && !(_nav_cmd.content.location.lat == 0 && _nav_cmd.content.location.lng == 0)) {
// update mission history. last index position is always the most recent wp loaded.
for (uint8_t i=0; i<AP_MISSION_MAX_WP_HISTORY-1; i++) {
_wp_index_history[i] = _wp_index_history[i+1];
@ -1589,7 +1661,7 @@ bool AP_Mission::advance_current_nav_cmd(uint16_t starting_index)
_flags.resuming_mission = false;
}
}else{
} else {
// set current do command and start it (if not already set)
if (!_flags.do_cmd_loaded) {
_do_cmd = cmd;
@ -1628,7 +1700,7 @@ void AP_Mission::advance_current_do_cmd()
uint16_t cmd_index = _do_cmd.index;
if (cmd_index == AP_MISSION_CMD_INDEX_NONE) {
cmd_index = AP_MISSION_FIRST_REAL_COMMAND;
}else{
} else {
// start from one position past the current do command
cmd_index = _do_cmd.index + 1;
}
@ -1659,7 +1731,7 @@ bool AP_Mission::get_next_cmd(uint16_t start_index, Mission_Command& cmd, bool i
// search until the end of the mission command list
uint8_t max_loops = 64;
while(cmd_index < (unsigned)_cmd_total) {
while (cmd_index < (unsigned)_cmd_total) {
// load the next command
if (!read_cmd_from_storage(cmd_index, temp_cmd)) {
// this should never happen because of check above but just in case
@ -1695,7 +1767,7 @@ bool AP_Mission::get_next_cmd(uint16_t start_index, Mission_Command& cmd, bool i
if (temp_cmd.content.jump.num_times == AP_MISSION_JUMP_REPEAT_FOREVER) {
// continue searching from jump target
cmd_index = temp_cmd.content.jump.target;
}else{
} else {
// get number of times jump command has already been run
int16_t jump_times_run = get_jump_times_run(temp_cmd);
if (jump_times_run < temp_cmd.content.jump.num_times) {
@ -1705,12 +1777,12 @@ bool AP_Mission::get_next_cmd(uint16_t start_index, Mission_Command& cmd, bool i
}
// continue searching from jump target
cmd_index = temp_cmd.content.jump.target;
}else{
} else {
// jump has been run specified number of times so move search to next command in mission
cmd_index++;
}
}
}else{
} else {
// this is a non-jump command so return it
cmd = temp_cmd;
return true;
@ -1738,10 +1810,10 @@ bool AP_Mission::get_next_do_cmd(uint16_t start_index, Mission_Command& cmd)
if (!get_next_cmd(start_index, temp_cmd, false)) {
// no more commands so return failure
return false;
}else if (is_nav_cmd(temp_cmd)) {
} else if (is_nav_cmd(temp_cmd)) {
// if it's a "navigation" command then return false because we do not progress past nav commands
return false;
}else{
} else {
// this must be a "do" or "conditional" and is not a do-jump command so return it
cmd = temp_cmd;
return true;
@ -1755,7 +1827,7 @@ bool AP_Mission::get_next_do_cmd(uint16_t start_index, Mission_Command& cmd)
// init_jump_tracking - initialise jump_tracking variables
void AP_Mission::init_jump_tracking()
{
for(uint8_t i=0; i<AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS; i++) {
for (uint8_t i=0; i<AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS; i++) {
_jump_tracking[i].index = AP_MISSION_CMD_INDEX_NONE;
_jump_tracking[i].num_times_run = 0;
}
@ -1774,7 +1846,7 @@ int16_t AP_Mission::get_jump_times_run(const Mission_Command& cmd)
for (uint8_t i=0; i<AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS; i++) {
if (_jump_tracking[i].index == cmd.index) {
return _jump_tracking[i].num_times_run;
}else if(_jump_tracking[i].index == AP_MISSION_CMD_INDEX_NONE) {
} else if (_jump_tracking[i].index == AP_MISSION_CMD_INDEX_NONE) {
// we've searched through all known jump commands and haven't found it so allocate new space in _jump_tracking array
_jump_tracking[i].index = cmd.index;
_jump_tracking[i].num_times_run = 0;
@ -1804,7 +1876,7 @@ void AP_Mission::increment_jump_times_run(Mission_Command& cmd, bool send_gcs_ms
gcs().send_text(MAV_SEVERITY_INFO, "Mission: %u Jump %i/%i", _jump_tracking[i].index, _jump_tracking[i].num_times_run, cmd.content.jump.num_times);
}
return;
}else if(_jump_tracking[i].index == AP_MISSION_CMD_INDEX_NONE) {
} else if (_jump_tracking[i].index == AP_MISSION_CMD_INDEX_NONE) {
// we've searched through all known jump commands and haven't found it so allocate new space in _jump_tracking array
_jump_tracking[i].index = cmd.index;
_jump_tracking[i].num_times_run = 1;
@ -2071,8 +2143,9 @@ bool AP_Mission::is_landing_type_cmd(uint16_t id) const
}
}
const char *AP_Mission::Mission_Command::type() const {
switch(id) {
const char *AP_Mission::Mission_Command::type() const
{
switch (id) {
case MAV_CMD_NAV_WAYPOINT:
return "WP";
case MAV_CMD_NAV_SPLINE_WAYPOINT:
@ -2293,7 +2366,8 @@ bool AP_Mission::calc_rewind_pos(Mission_Command& rewind_cmd)
// singleton instance
AP_Mission *AP_Mission::_singleton;
namespace AP {
namespace AP
{
AP_Mission *mission()
{

84
libraries/AP_Mission/AP_Mission.h
View File

@ -45,7 +45,8 @@
/// @class AP_Mission
/// @brief Object managing Mission
class AP_Mission {
class AP_Mission
{
public:
// jump command structure
@ -314,7 +315,8 @@ public:
}
// get singleton instance
static AP_Mission *get_singleton() {
static AP_Mission *get_singleton()
{
return _singleton;
}
@ -337,11 +339,17 @@ public:
void init();
/// status - returns the status of the mission (i.e. Mission_Started, Mission_Complete, Mission_Stopped
mission_state state() const { return _flags.state; }
mission_state state() const
{
return _flags.state;
}
/// num_commands - returns total number of commands in the mission
/// this number includes offset 0, the home location
uint16_t num_commands() const { return _cmd_total; }
uint16_t num_commands() const
{
return _cmd_total;
}
/// num_commands_max - returns maximum number of commands that can be stored
uint16_t num_commands_max() const;
@ -394,31 +402,48 @@ public:
static bool is_nav_cmd(const Mission_Command& cmd);
/// get_current_nav_cmd - returns the current "navigation" command
const Mission_Command& get_current_nav_cmd() const { return _nav_cmd; }
const Mission_Command& get_current_nav_cmd() const
{
return _nav_cmd;
}
/// get_current_nav_index - returns the current "navigation" command index
/// Note that this will return 0 if there is no command. This is
/// used in MAVLink reporting of the mission command
uint16_t get_current_nav_index() const {
return _nav_cmd.index==AP_MISSION_CMD_INDEX_NONE?0:_nav_cmd.index; }
uint16_t get_current_nav_index() const
{
return _nav_cmd.index==AP_MISSION_CMD_INDEX_NONE?0:_nav_cmd.index;
}
/// get_current_nav_id - return the id of the current nav command
uint16_t get_current_nav_id() const { return _nav_cmd.id; }
uint16_t get_current_nav_id() const
{
return _nav_cmd.id;
}
/// get_prev_nav_cmd_id - returns the previous "navigation" command id
/// if there was no previous nav command it returns AP_MISSION_CMD_ID_NONE
/// we do not return the entire command to save on RAM
uint16_t get_prev_nav_cmd_id() const { return _prev_nav_cmd_id; }
uint16_t get_prev_nav_cmd_id() const
{
return _prev_nav_cmd_id;
}
/// get_prev_nav_cmd_index - returns the previous "navigation" commands index (i.e. position in the mission command list)
/// if there was no previous nav command it returns AP_MISSION_CMD_INDEX_NONE
/// we do not return the entire command to save on RAM
uint16_t get_prev_nav_cmd_index() const { return _prev_nav_cmd_index; }
uint16_t get_prev_nav_cmd_index() const
{
return _prev_nav_cmd_index;
}
/// get_prev_nav_cmd_with_wp_index - returns the previous "navigation" commands index that contains a waypoint (i.e. position in the mission command list)
/// if there was no previous nav command it returns AP_MISSION_CMD_INDEX_NONE
/// we do not return the entire command to save on RAM
uint16_t get_prev_nav_cmd_with_wp_index() const { return _prev_nav_cmd_wp_index; }
uint16_t get_prev_nav_cmd_with_wp_index() const
{
return _prev_nav_cmd_wp_index;
}
/// get_next_nav_cmd - gets next "navigation" command found at or after start_index
/// returns true if found, false if not found (i.e. reached end of mission command list)
@ -431,10 +456,16 @@ public:
int32_t get_next_ground_course_cd(int32_t default_angle);
/// get_current_do_cmd - returns active "do" command
const Mission_Command& get_current_do_cmd() const { return _do_cmd; }
const Mission_Command& get_current_do_cmd() const
{
return _do_cmd;
}
/// get_current_do_cmd_id - returns id of the active "do" command
uint16_t get_current_do_cmd_id() const { return _do_cmd.id; }
uint16_t get_current_do_cmd_id() const
{
return _do_cmd.id;
}
// set_current_cmd - jumps to command specified by index
bool set_current_cmd(uint16_t index, bool rewind = false);
@ -470,7 +501,10 @@ public:
static bool mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& cmd, mavlink_mission_item_int_t& packet);
// return the last time the mission changed in milliseconds
uint32_t last_change_time_ms(void) const { return _last_change_time_ms; }
uint32_t last_change_time_ms(void) const
{
return _last_change_time_ms;
}
// find the nearest landing sequence starting point (DO_LAND_START) and
// return its index. Returns 0 if no appropriate DO_LAND_START point can
@ -489,14 +523,21 @@ public:
bool is_best_land_sequence(void);
// set in_landing_sequence flag
void set_in_landing_sequence_flag(bool flag) { _flags.in_landing_sequence = flag; }
void set_in_landing_sequence_flag(bool flag)
{
_flags.in_landing_sequence = flag;
}
// force mission to resume when start_or_resume() is called
void set_force_resume(bool force_resume) { _force_resume = force_resume; }
void set_force_resume(bool force_resume)
{
_force_resume = force_resume;
}
// get a reference to the AP_Mission semaphore, allowing an external caller to lock the
// storage while working with multiple waypoints
HAL_Semaphore &get_semaphore(void) {
HAL_Semaphore &get_semaphore(void)
{
return _rsem;
}
@ -509,6 +550,10 @@ public:
// user settable parameters
static const struct AP_Param::GroupInfo var_info[];
// allow lua to get/set any WP items in any order in a mavlink-ish kinda way.
bool get_item(uint16_t index, mavlink_mission_item_int_t& result) ;
bool set_item(uint16_t index, mavlink_mission_item_int_t& source) ;
private:
static AP_Mission *_singleton;
@ -638,6 +683,7 @@ private:
bool command_do_set_repeat_dist(const AP_Mission::Mission_Command& cmd);
};
namespace AP {
AP_Mission *mission();
namespace AP
{
AP_Mission *mission();
};

18
libraries/AP_Scripting/generator/description/bindings.desc
View File

@ -215,6 +215,24 @@ singleton AP_Mission method get_prev_nav_cmd_id uint16_t
singleton AP_Mission method get_current_nav_id uint16_t
singleton AP_Mission method get_current_do_cmd_id uint16_t
singleton AP_Mission method num_commands uint16_t
singleton AP_Mission method get_item boolean uint16_t 0 UINT16_MAX mavlink_mission_item_int_t'Null
singleton AP_Mission method set_item boolean uint16_t 0 UINT16_MAX mavlink_mission_item_int_t
userdata mavlink_mission_item_int_t field param1 float read write -FLT_MAX FLT_MAX
userdata mavlink_mission_item_int_t field param2 float read write -FLT_MAX FLT_MAX
userdata mavlink_mission_item_int_t field param3 float read write -FLT_MAX FLT_MAX
userdata mavlink_mission_item_int_t field param4 float read write -FLT_MAX FLT_MAX
userdata mavlink_mission_item_int_t field x int32_t read write -INT32_MAX INT32_MAX
userdata mavlink_mission_item_int_t field y int32_t read write -INT32_MAX INT32_MAX
userdata mavlink_mission_item_int_t field z float read write -FLT_MAX FLT_MAX
userdata mavlink_mission_item_int_t field seq uint16_t read write 0 UINT16_MAX
userdata mavlink_mission_item_int_t field command uint16_t read write 0 UINT16_MAX
-- userdata mavlink_mission_item_int_t field target_system uint8_t read write 0 UINT8_MAX
-- userdata mavlink_mission_item_int_t field target_component uint8_t read write 0 UINT8_MAX
userdata mavlink_mission_item_int_t field frame uint8_t read write 0 UINT8_MAX
userdata mavlink_mission_item_int_t field current uint8_t read write 0 UINT8_MAX
-- userdata mavlink_mission_item_int_t field autocontinue uint8_t read write 0 UINT8_MAX
include AP_RPM/AP_RPM.h
singleton AP_RPM alias RPM

0
misc.txt Normal file
View File