From e1e3979ed5fe10c8e19d82da4b0d436c3cc8d3b6 Mon Sep 17 00:00:00 2001 From: Buzz Date: Tue, 5 May 2020 10:27:56 +1000 Subject: [PATCH 1/2] AP_Scripting: allow to get/set/create arbitrary mission items 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 ) --- libraries/AP_Mission/AP_Mission.cpp | 228 ++++++++++++------ libraries/AP_Mission/AP_Mission.h | 96 ++++++-- .../generator/description/bindings.desc | 18 ++ misc.txt | 0 4 files changed, 240 insertions(+), 102 deletions(-) create mode 100644 misc.txt diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 87a640040a2be..fb1fa8b3fb350 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -55,7 +55,7 @@ void AP_Mission::init() // If Mission Clear bit is set then it should clear the mission, otherwise retain the mission. if (AP_MISSION_MASK_MISSION_CLEAR & _options) { gcs().send_text(MAV_SEVERITY_INFO, "Clearing Mission"); - clear(); + clear(); } _last_change_time_ms = AP_HAL::millis(); @@ -116,7 +116,7 @@ void AP_Mission::resume() if (_repeat_dist > 0 && _wp_index_history[LAST_WP_PASSED] != AP_MISSION_CMD_INDEX_NONE) { // if not already in a resume state calculate the position to rewind to Mission_Command tmp_cmd; - if (!_flags.resuming_mission && calc_rewind_pos(tmp_cmd)) { + if (!_flags.resuming_mission && calc_rewind_pos(tmp_cmd)) { _resume_cmd = tmp_cmd; } @@ -231,7 +231,7 @@ bool AP_Mission::clear() /// trucate - truncate any mission items beyond index void AP_Mission::truncate(uint16_t index) { - if ((unsigned)_cmd_total > index) { + if ((unsigned)_cmd_total > index) { _cmd_total.set_and_save(index); } } @@ -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 @@ -287,7 +287,7 @@ void AP_Mission::update() bool AP_Mission::verify_command(const Mission_Command& cmd) { switch (cmd.id) { - // do-commands always return true for verify: + // do-commands always return true for verify: case MAV_CMD_DO_GRIPPER: case MAV_CMD_DO_SET_SERVO: case MAV_CMD_DO_SET_RELAY: @@ -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) @@ -643,7 +716,7 @@ bool AP_Mission::stored_in_location(uint16_t id) bool AP_Mission::write_cmd_to_storage(uint16_t index, const Mission_Command& cmd) { WITH_SEMAPHORE(_rsem); - + // range check cmd's index if (index >= num_commands_max()) { return false; @@ -699,27 +772,28 @@ 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: - nan_mask = ~(1 << 3); // param 4 can be nan - break; - case MAV_CMD_NAV_LAND: - nan_mask = ~(1 << 3); // param 4 can be nan - break; - case MAV_CMD_NAV_TAKEOFF: - nan_mask = ~(1 << 3); // param 4 can be nan - break; - case MAV_CMD_NAV_VTOL_TAKEOFF: - nan_mask = ~(1 << 3); // param 4 can be nan - break; - case MAV_CMD_NAV_VTOL_LAND: - nan_mask = ~((1 << 2) | (1 << 3)); // param 3 and 4 can be nan - break; - default: - nan_mask = 0xff; - break; + case MAV_CMD_NAV_WAYPOINT: + nan_mask = ~(1 << 3); // param 4 can be nan + break; + case MAV_CMD_NAV_LAND: + nan_mask = ~(1 << 3); // param 4 can be nan + break; + case MAV_CMD_NAV_TAKEOFF: + nan_mask = ~(1 << 3); // param 4 can be nan + break; + case MAV_CMD_NAV_VTOL_TAKEOFF: + nan_mask = ~(1 << 3); // param 4 can be nan + break; + case MAV_CMD_NAV_VTOL_LAND: + nan_mask = ~((1 << 2) | (1 << 3)); // param 3 and 4 can be nan + break; + default: + nan_mask = 0xff; + break; } if (((nan_mask & (1 << 0)) && isnan(packet.param1)) || @@ -761,9 +835,8 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ case 0: // 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 @@ -784,22 +857,21 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ cmd.p1 = packet.param1; #endif } - break; + break; case MAV_CMD_NAV_LOITER_UNLIM: // MAV ID: 17 cmd.p1 = fabsf(packet.param3); // store radius as 16bit since no other params are competing for space 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 cmd.content.location.loiter_ccw = (packet.param3 < 0); cmd.content.location.loiter_xtrack = (packet.param4 > 0); // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location } - break; + break; case MAV_CMD_NAV_LOITER_TIME: // MAV ID: 19 cmd.p1 = packet.param1; // loiter time in seconds uses all 16 bits, 8bit seconds is too small. No room for radius. @@ -821,9 +893,9 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT: // MAV ID: 30 cmd.p1 = packet.param1; // Climb/Descend - // 0 = Neutral, cmd complete at +/- 5 of indicated alt. - // 1 = Climb, cmd complete at or above indicated alt. - // 2 = Descend, cmd complete at or below indicated alt. + // 0 = Neutral, cmd complete at +/- 5 of indicated alt. + // 1 = Climb, cmd complete at or above indicated alt. + // 2 = Descend, cmd complete at or below indicated alt. break; case MAV_CMD_NAV_LOITER_TO_ALT: // MAV ID: 31 @@ -996,7 +1068,7 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ cmd.content.do_engine_control.start_control = (packet.param1>0); cmd.content.do_engine_control.cold_start = (packet.param2>0); cmd.content.do_engine_control.height_delay_cm = packet.param3*100; - break; + break; case MAV_CMD_NAV_PAYLOAD_PLACE: cmd.p1 = packet.param1*100; // copy max-descend parameter (m->cm) @@ -1077,7 +1149,7 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ } MAV_MISSION_RESULT AP_Mission::convert_MISSION_ITEM_to_MISSION_ITEM_INT(const mavlink_mission_item_t &packet, - mavlink_mission_item_int_t &mav_cmd) + mavlink_mission_item_int_t &mav_cmd) { // TODO: rename mav_cmd to mission_item_int // TODO: rename packet to mission_item @@ -1094,7 +1166,7 @@ MAV_MISSION_RESULT AP_Mission::convert_MISSION_ITEM_to_MISSION_ITEM_INT(const ma mav_cmd.current = packet.current; mav_cmd.autocontinue = packet.autocontinue; mav_cmd.mission_type = packet.mission_type; - + /* the strategy for handling both MISSION_ITEM and MISSION_ITEM_INT is to pass the lat/lng in MISSION_ITEM_INT straight through, and @@ -1127,7 +1199,7 @@ MAV_MISSION_RESULT AP_Mission::convert_MISSION_ITEM_to_MISSION_ITEM_INT(const ma } MAV_MISSION_RESULT AP_Mission::convert_MISSION_ITEM_INT_to_MISSION_ITEM(const mavlink_mission_item_int_t &item_int, - mavlink_mission_item_t &item) + mavlink_mission_item_t &item) { item.param1 = item_int.param1; item.param2 = item_int.param2; @@ -1169,10 +1241,10 @@ MAV_MISSION_RESULT AP_Mission::convert_MISSION_ITEM_INT_to_MISSION_ITEM(const ma // mavlink_cmd_long_to_mission_cmd - converts a mavlink cmd long to an AP_Mission::Mission_Command object which can be stored to eeprom // return MAV_MISSION_ACCEPTED on success, MAV_MISSION_RESULT error on failure -MAV_MISSION_RESULT AP_Mission::mavlink_cmd_long_to_mission_cmd(const mavlink_command_long_t& packet, AP_Mission::Mission_Command& cmd) +MAV_MISSION_RESULT AP_Mission::mavlink_cmd_long_to_mission_cmd(const mavlink_command_long_t& packet, AP_Mission::Mission_Command& cmd) { mavlink_mission_item_int_t miss_item = {0}; - + miss_item.param1 = packet.param1; miss_item.param2 = packet.param2; miss_item.param3 = packet.param3; @@ -1206,7 +1278,7 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c case 0: // this is reserved for 16 bit command IDs return false; - + case MAV_CMD_NAV_WAYPOINT: // MAV ID: 16 #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) // acceptance radius in meters @@ -1259,9 +1331,9 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT: // MAV ID: 30 packet.param1 = cmd.p1; // Climb/Descend - // 0 = Neutral, cmd complete at +/- 5 of indicated alt. - // 1 = Climb, cmd complete at or above indicated alt. - // 2 = Descend, cmd complete at or below indicated alt. + // 0 = Neutral, cmd complete at +/- 5 of indicated alt. + // 1 = Climb, cmd complete at or above indicated alt. + // 2 = Descend, cmd complete at or below indicated alt. break; case MAV_CMD_NAV_LOITER_TO_ALT: // MAV ID: 31 @@ -1436,7 +1508,7 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c packet.param1 = cmd.content.do_engine_control.start_control?1:0; packet.param2 = cmd.content.do_engine_control.cold_start?1:0; packet.param3 = cmd.content.do_engine_control.height_delay_cm*0.01f; - break; + break; case MAV_CMD_NAV_PAYLOAD_PLACE: packet.param1 = cmd.p1/100.0f; // copy max-descend parameter (m->cm) @@ -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=0; i--) { - // to get this far there has to be at least one 'passed wp' stored in history. This is to check incase + // to get this far there has to be at least one 'passed wp' stored in history. This is to check incase // of history array no being completely filled with valid waypoints upon resume. if (_wp_index_history[i] == AP_MISSION_CMD_INDEX_NONE) { // no more stored history @@ -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() { diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index 10aa592d56dfc..500cee15ffb33 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -45,7 +45,8 @@ /// @class AP_Mission /// @brief Object managing Mission -class AP_Mission { +class AP_Mission +{ public: // jump command structure @@ -137,7 +138,7 @@ class AP_Mission { // set cam trigger distance command structure struct PACKED Cam_Trigg_Distance { float meters; // distance - uint8_t trigger; // triggers one image capture immediately + uint8_t trigger; // triggers one image capture immediately }; // gripper command structure @@ -166,7 +167,7 @@ class AP_Mission { uint8_t target_state; }; - // navigation delay command structure + // navigation delay command structure struct PACKED Navigation_Delay_Command { float seconds; // period of delay in seconds int8_t hour_utc; // absolute time's hour (utc) @@ -314,14 +315,15 @@ class AP_Mission { } // get singleton instance - static AP_Mission *get_singleton() { + static AP_Mission *get_singleton() + { return _singleton; } /* Do not allow copies */ AP_Mission(const AP_Mission &other) = delete; - AP_Mission &operator=(const AP_Mission&) = delete; - + AP_Mission &operator=(const AP_Mission&) = delete; + // mission state enumeration enum mission_state { MISSION_STOPPED=0, @@ -337,11 +339,17 @@ class AP_Mission { 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 @@ class AP_Mission { 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 @@ class AP_Mission { 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); @@ -453,9 +484,9 @@ class AP_Mission { void write_home_to_storage(); static MAV_MISSION_RESULT convert_MISSION_ITEM_to_MISSION_ITEM_INT(const mavlink_mission_item_t &mission_item, - mavlink_mission_item_int_t &mission_item_int) WARN_IF_UNUSED; + mavlink_mission_item_int_t &mission_item_int) WARN_IF_UNUSED; static MAV_MISSION_RESULT convert_MISSION_ITEM_INT_to_MISSION_ITEM(const mavlink_mission_item_int_t &mission_item_int, - mavlink_mission_item_t &mission_item) WARN_IF_UNUSED; + mavlink_mission_item_t &mission_item) WARN_IF_UNUSED; // mavlink_int_to_mission_cmd - converts mavlink message to an AP_Mission::Mission_Command object which can be stored to eeprom // return MAV_MISSION_ACCEPTED on success, MAV_MISSION_RESULT error on failure @@ -470,7 +501,10 @@ class AP_Mission { 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 @@ class AP_Mission { 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 @@ class AP_Mission { // 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 @@ class AP_Mission { bool command_do_set_repeat_dist(const AP_Mission::Mission_Command& cmd); }; -namespace AP { - AP_Mission *mission(); +namespace AP +{ +AP_Mission *mission(); }; diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index eea36b8ece383..2facbda10f806 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -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 diff --git a/misc.txt b/misc.txt new file mode 100644 index 0000000000000..e69de29bb2d1d From d055a0c202770a7993bf0cb9349521879c4de383 Mon Sep 17 00:00:00 2001 From: Buzz Date: Tue, 5 May 2020 11:58:05 +1000 Subject: [PATCH 2/2] AP_Scripting: rebuild bindings --- .../AP_Scripting/lua_generated_bindings.cpp | 279 ++++++++++++++++++ .../AP_Scripting/lua_generated_bindings.h | 2 + 2 files changed, 281 insertions(+) diff --git a/libraries/AP_Scripting/lua_generated_bindings.cpp b/libraries/AP_Scripting/lua_generated_bindings.cpp index 0b1390b983049..f7aaa3201e3ac 100644 --- a/libraries/AP_Scripting/lua_generated_bindings.cpp +++ b/libraries/AP_Scripting/lua_generated_bindings.cpp @@ -41,6 +41,16 @@ static int binding_argcheck(lua_State *L, int expected_arg_count) { return 0; } +int new_mavlink_mission_item_int_t(lua_State *L) { + luaL_checkstack(L, 2, "Out of stack"); + void *ud = lua_newuserdata(L, sizeof(mavlink_mission_item_int_t)); + memset(ud, 0, sizeof(mavlink_mission_item_int_t)); + new (ud) mavlink_mission_item_int_t(); + luaL_getmetatable(L, "mavlink_mission_item_int_t"); + lua_setmetatable(L, -2); + return 1; +} + int new_Vector2f(lua_State *L) { luaL_checkstack(L, 2, "Out of stack"); void *ud = lua_newuserdata(L, sizeof(Vector2f)); @@ -71,6 +81,11 @@ int new_Location(lua_State *L) { return 1; } +mavlink_mission_item_int_t * check_mavlink_mission_item_int_t(lua_State *L, int arg) { + void *data = luaL_checkudata(L, arg, "mavlink_mission_item_int_t"); + return (mavlink_mission_item_int_t *)data; +} + Vector2f * check_Vector2f(lua_State *L, int arg) { void *data = luaL_checkudata(L, arg, "Vector2f"); return (Vector2f *)data; @@ -100,6 +115,204 @@ AP_HAL::UARTDriver ** check_AP_HAL__UARTDriver(lua_State *L, int arg) { return (AP_HAL::UARTDriver **)data; } +static int mavlink_mission_item_int_t_current(lua_State *L) { + mavlink_mission_item_int_t *ud = check_mavlink_mission_item_int_t(L, 1); + switch(lua_gettop(L)) { + case 1: + lua_pushinteger(L, ud->current); + return 1; + case 2: { + const lua_Integer raw_data_2 = luaL_checkinteger(L, 2); + luaL_argcheck(L, ((raw_data_2 >= MAX(0, 0)) && (raw_data_2 <= MIN(UINT8_MAX, UINT8_MAX))), 2, "current out of range"); + const uint8_t data_2 = static_cast(raw_data_2); + ud->current = data_2; + return 0; + } + default: + return luaL_argerror(L, lua_gettop(L), "too many arguments"); + } +} + +static int mavlink_mission_item_int_t_frame(lua_State *L) { + mavlink_mission_item_int_t *ud = check_mavlink_mission_item_int_t(L, 1); + switch(lua_gettop(L)) { + case 1: + lua_pushinteger(L, ud->frame); + return 1; + case 2: { + const lua_Integer raw_data_2 = luaL_checkinteger(L, 2); + luaL_argcheck(L, ((raw_data_2 >= MAX(0, 0)) && (raw_data_2 <= MIN(UINT8_MAX, UINT8_MAX))), 2, "frame out of range"); + const uint8_t data_2 = static_cast(raw_data_2); + ud->frame = data_2; + return 0; + } + default: + return luaL_argerror(L, lua_gettop(L), "too many arguments"); + } +} + +static int mavlink_mission_item_int_t_command(lua_State *L) { + mavlink_mission_item_int_t *ud = check_mavlink_mission_item_int_t(L, 1); + switch(lua_gettop(L)) { + case 1: + lua_pushinteger(L, ud->command); + return 1; + case 2: { + const lua_Integer raw_data_2 = luaL_checkinteger(L, 2); + luaL_argcheck(L, ((raw_data_2 >= MAX(0, 0)) && (raw_data_2 <= MIN(UINT16_MAX, UINT16_MAX))), 2, "command out of range"); + const uint16_t data_2 = static_cast(raw_data_2); + ud->command = data_2; + return 0; + } + default: + return luaL_argerror(L, lua_gettop(L), "too many arguments"); + } +} + +static int mavlink_mission_item_int_t_seq(lua_State *L) { + mavlink_mission_item_int_t *ud = check_mavlink_mission_item_int_t(L, 1); + switch(lua_gettop(L)) { + case 1: + lua_pushinteger(L, ud->seq); + return 1; + case 2: { + const lua_Integer raw_data_2 = luaL_checkinteger(L, 2); + luaL_argcheck(L, ((raw_data_2 >= MAX(0, 0)) && (raw_data_2 <= MIN(UINT16_MAX, UINT16_MAX))), 2, "seq out of range"); + const uint16_t data_2 = static_cast(raw_data_2); + ud->seq = data_2; + return 0; + } + default: + return luaL_argerror(L, lua_gettop(L), "too many arguments"); + } +} + +static int mavlink_mission_item_int_t_z(lua_State *L) { + mavlink_mission_item_int_t *ud = check_mavlink_mission_item_int_t(L, 1); + switch(lua_gettop(L)) { + case 1: + lua_pushnumber(L, ud->z); + return 1; + case 2: { + const float raw_data_2 = luaL_checknumber(L, 2); + luaL_argcheck(L, ((raw_data_2 >= MAX(-FLT_MAX, -INFINITY)) && (raw_data_2 <= MIN(FLT_MAX, INFINITY))), 2, "z out of range"); + const float data_2 = raw_data_2; + ud->z = data_2; + return 0; + } + default: + return luaL_argerror(L, lua_gettop(L), "too many arguments"); + } +} + +static int mavlink_mission_item_int_t_y(lua_State *L) { + mavlink_mission_item_int_t *ud = check_mavlink_mission_item_int_t(L, 1); + switch(lua_gettop(L)) { + case 1: + lua_pushinteger(L, ud->y); + return 1; + case 2: { + const lua_Integer raw_data_2 = luaL_checkinteger(L, 2); + luaL_argcheck(L, ((raw_data_2 >= MAX(-INT32_MAX, INT32_MIN)) && (raw_data_2 <= MIN(INT32_MAX, INT32_MAX))), 2, "y out of range"); + const int32_t data_2 = raw_data_2; + ud->y = data_2; + return 0; + } + default: + return luaL_argerror(L, lua_gettop(L), "too many arguments"); + } +} + +static int mavlink_mission_item_int_t_x(lua_State *L) { + mavlink_mission_item_int_t *ud = check_mavlink_mission_item_int_t(L, 1); + switch(lua_gettop(L)) { + case 1: + lua_pushinteger(L, ud->x); + return 1; + case 2: { + const lua_Integer raw_data_2 = luaL_checkinteger(L, 2); + luaL_argcheck(L, ((raw_data_2 >= MAX(-INT32_MAX, INT32_MIN)) && (raw_data_2 <= MIN(INT32_MAX, INT32_MAX))), 2, "x out of range"); + const int32_t data_2 = raw_data_2; + ud->x = data_2; + return 0; + } + default: + return luaL_argerror(L, lua_gettop(L), "too many arguments"); + } +} + +static int mavlink_mission_item_int_t_param4(lua_State *L) { + mavlink_mission_item_int_t *ud = check_mavlink_mission_item_int_t(L, 1); + switch(lua_gettop(L)) { + case 1: + lua_pushnumber(L, ud->param4); + return 1; + case 2: { + const float raw_data_2 = luaL_checknumber(L, 2); + luaL_argcheck(L, ((raw_data_2 >= MAX(-FLT_MAX, -INFINITY)) && (raw_data_2 <= MIN(FLT_MAX, INFINITY))), 2, "param4 out of range"); + const float data_2 = raw_data_2; + ud->param4 = data_2; + return 0; + } + default: + return luaL_argerror(L, lua_gettop(L), "too many arguments"); + } +} + +static int mavlink_mission_item_int_t_param3(lua_State *L) { + mavlink_mission_item_int_t *ud = check_mavlink_mission_item_int_t(L, 1); + switch(lua_gettop(L)) { + case 1: + lua_pushnumber(L, ud->param3); + return 1; + case 2: { + const float raw_data_2 = luaL_checknumber(L, 2); + luaL_argcheck(L, ((raw_data_2 >= MAX(-FLT_MAX, -INFINITY)) && (raw_data_2 <= MIN(FLT_MAX, INFINITY))), 2, "param3 out of range"); + const float data_2 = raw_data_2; + ud->param3 = data_2; + return 0; + } + default: + return luaL_argerror(L, lua_gettop(L), "too many arguments"); + } +} + +static int mavlink_mission_item_int_t_param2(lua_State *L) { + mavlink_mission_item_int_t *ud = check_mavlink_mission_item_int_t(L, 1); + switch(lua_gettop(L)) { + case 1: + lua_pushnumber(L, ud->param2); + return 1; + case 2: { + const float raw_data_2 = luaL_checknumber(L, 2); + luaL_argcheck(L, ((raw_data_2 >= MAX(-FLT_MAX, -INFINITY)) && (raw_data_2 <= MIN(FLT_MAX, INFINITY))), 2, "param2 out of range"); + const float data_2 = raw_data_2; + ud->param2 = data_2; + return 0; + } + default: + return luaL_argerror(L, lua_gettop(L), "too many arguments"); + } +} + +static int mavlink_mission_item_int_t_param1(lua_State *L) { + mavlink_mission_item_int_t *ud = check_mavlink_mission_item_int_t(L, 1); + switch(lua_gettop(L)) { + case 1: + lua_pushnumber(L, ud->param1); + return 1; + case 2: { + const float raw_data_2 = luaL_checknumber(L, 2); + luaL_argcheck(L, ((raw_data_2 >= MAX(-FLT_MAX, -INFINITY)) && (raw_data_2 <= MIN(FLT_MAX, INFINITY))), 2, "param1 out of range"); + const float data_2 = raw_data_2; + ud->param1 = data_2; + return 0; + } + default: + return luaL_argerror(L, lua_gettop(L), "too many arguments"); + } +} + static int Vector2f_y(lua_State *L) { Vector2f *ud = check_Vector2f(L, 1); switch(lua_gettop(L)) { @@ -510,6 +723,21 @@ static int Location_get_distance(lua_State *L) { return 1; } +const luaL_Reg mavlink_mission_item_int_t_meta[] = { + {"current", mavlink_mission_item_int_t_current}, + {"frame", mavlink_mission_item_int_t_frame}, + {"command", mavlink_mission_item_int_t_command}, + {"seq", mavlink_mission_item_int_t_seq}, + {"z", mavlink_mission_item_int_t_z}, + {"y", mavlink_mission_item_int_t_y}, + {"x", mavlink_mission_item_int_t_x}, + {"param4", mavlink_mission_item_int_t_param4}, + {"param3", mavlink_mission_item_int_t_param3}, + {"param2", mavlink_mission_item_int_t_param2}, + {"param1", mavlink_mission_item_int_t_param1}, + {NULL, NULL} +}; + const luaL_Reg Vector2f_meta[] = { {"y", Vector2f_y}, {"x", Vector2f_x}, @@ -594,6 +822,53 @@ static int AP_RPM_get_rpm(lua_State *L) { return 1; } +static int AP_Mission_set_item(lua_State *L) { + AP_Mission * ud = AP_Mission::get_singleton(); + if (ud == nullptr) { + return luaL_argerror(L, 1, "mission not supported on this firmware"); + } + + binding_argcheck(L, 3); + const lua_Integer raw_data_2 = luaL_checkinteger(L, 2); + luaL_argcheck(L, ((raw_data_2 >= MAX(0, 0)) && (raw_data_2 <= MIN(UINT16_MAX, UINT16_MAX))), 2, "argument out of range"); + const uint16_t data_2 = static_cast(raw_data_2); + mavlink_mission_item_int_t & data_3 = *check_mavlink_mission_item_int_t(L, 3); + AP::scheduler().get_semaphore().take_blocking(); + const bool data = ud->set_item( + data_2, + data_3); + + AP::scheduler().get_semaphore().give(); + lua_pushboolean(L, data); + return 1; +} + +static int AP_Mission_get_item(lua_State *L) { + AP_Mission * ud = AP_Mission::get_singleton(); + if (ud == nullptr) { + return luaL_argerror(L, 1, "mission not supported on this firmware"); + } + + binding_argcheck(L, 2); + const lua_Integer raw_data_2 = luaL_checkinteger(L, 2); + luaL_argcheck(L, ((raw_data_2 >= MAX(0, 0)) && (raw_data_2 <= MIN(UINT16_MAX, UINT16_MAX))), 2, "argument out of range"); + const uint16_t data_2 = static_cast(raw_data_2); + mavlink_mission_item_int_t data_5003 = {}; + AP::scheduler().get_semaphore().take_blocking(); + const bool data = ud->get_item( + data_2, + data_5003); + + AP::scheduler().get_semaphore().give(); + if (data) { + new_mavlink_mission_item_int_t(L); + *check_mavlink_mission_item_int_t(L, -1) = data_5003; + } else { + lua_pushnil(L); + } + return 1; +} + static int AP_Mission_num_commands(lua_State *L) { AP_Mission * ud = AP_Mission::get_singleton(); if (ud == nullptr) { @@ -2407,6 +2682,8 @@ const luaL_Reg AP_RPM_meta[] = { }; const luaL_Reg AP_Mission_meta[] = { + {"set_item", AP_Mission_set_item}, + {"get_item", AP_Mission_get_item}, {"num_commands", AP_Mission_num_commands}, {"get_current_do_cmd_id", AP_Mission_get_current_do_cmd_id}, {"get_current_nav_id", AP_Mission_get_current_nav_id}, @@ -2693,6 +2970,7 @@ struct userdata_meta { }; const struct userdata_meta userdata_fun[] = { + {"mavlink_mission_item_int_t", mavlink_mission_item_int_t_meta, NULL}, {"Vector2f", Vector2f_meta, NULL}, {"Vector3f", Vector3f_meta, NULL}, {"Location", Location_meta, NULL}, @@ -2800,6 +3078,7 @@ const struct userdata { const char *name; const lua_CFunction fun; } new_userdata[] = { + {"mavlink_mission_item_int_t", new_mavlink_mission_item_int_t}, {"Vector2f", new_Vector2f}, {"Vector3f", new_Vector3f}, {"Location", new_Location}, diff --git a/libraries/AP_Scripting/lua_generated_bindings.h b/libraries/AP_Scripting/lua_generated_bindings.h index e0c043519e3c7..6e0f3de242b65 100644 --- a/libraries/AP_Scripting/lua_generated_bindings.h +++ b/libraries/AP_Scripting/lua_generated_bindings.h @@ -31,6 +31,8 @@ #endif // !defined(AP_TERRAIN_AVAILABLE) || (AP_TERRAIN_AVAILABLE != 1) +int new_mavlink_mission_item_int_t(lua_State *L); +mavlink_mission_item_int_t * check_mavlink_mission_item_int_t(lua_State *L, int arg); int new_Vector2f(lua_State *L); Vector2f * check_Vector2f(lua_State *L, int arg); int new_Vector3f(lua_State *L);