diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 4d968a4c18..e9667083c4 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -717,10 +717,11 @@ bool AP_Mission::read_cmd_from_storage(uint16_t index, Mission_Command& cmd) con PackedContent packed_content {}; const uint8_t b1 = _storage.read_byte(pos_in_storage); - if (b1 == 0) { + if (b1 == 0 || b1 == 1) { cmd.id = _storage.read_uint16(pos_in_storage+1); cmd.p1 = _storage.read_uint16(pos_in_storage+3); _storage.read_block(packed_content.bytes, pos_in_storage+5, 10); + format_conversion(b1, cmd, packed_content); } else { cmd.id = b1; cmd.p1 = _storage.read_uint16(pos_in_storage+1); @@ -825,12 +826,21 @@ bool AP_Mission::write_cmd_to_storage(uint16_t index, const Mission_Command& cmd uint16_t pos_in_storage = 4 + (index * AP_MISSION_EEPROM_COMMAND_SIZE); if (cmd.id < 256) { + // for commands below 256 we store up to 12 bytes _storage.write_byte(pos_in_storage, cmd.id); _storage.write_uint16(pos_in_storage+1, cmd.p1); _storage.write_block(pos_in_storage+3, packed.bytes, 12); } else { - // if the command ID is above 256 we store a 0 followed by the 16 bit command ID - _storage.write_byte(pos_in_storage, 0); + // if the command ID is above 256 we store a tag byte followed + // by the 16 bit command ID. The tag byte is 1 for commands + // where we have changed the storage format (see + // format_conversion), 0 otherwise + uint8_t tag_byte = 0; + // currently the only converted structure is NAV_SCRIPT_TIME + if (cmd.id == MAV_CMD_NAV_SCRIPT_TIME) { + tag_byte = 1; + } + _storage.write_byte(pos_in_storage, tag_byte); _storage.write_uint16(pos_in_storage+1, cmd.id); _storage.write_uint16(pos_in_storage+3, cmd.p1); _storage.write_block(pos_in_storage+5, packed.bytes, 10); @@ -913,8 +923,8 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ // command specific conversions from mavlink packet to mission command switch (cmd.id) { - case 0: - // this is reserved for storing 16 bit command IDs + case 0 ... 1: + // these are reserved for storing 16 bit command IDs return MAV_MISSION_INVALID; case MAV_CMD_NAV_WAYPOINT: { // MAV ID: 16 @@ -1209,8 +1219,8 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ cmd.content.nav_script_time.timeout_s = packet.param2; cmd.content.nav_script_time.arg1.set(packet.param3); cmd.content.nav_script_time.arg2.set(packet.param4); - cmd.content.nav_script_time.arg3.set(packet.x); - cmd.content.nav_script_time.arg4.set(packet.y); + cmd.content.nav_script_time.arg3 = int16_t(packet.x); + cmd.content.nav_script_time.arg4 = int16_t(packet.y); break; case MAV_CMD_NAV_ATTITUDE_TIME: @@ -1703,8 +1713,8 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c packet.param2 = cmd.content.nav_script_time.timeout_s; packet.param3 = cmd.content.nav_script_time.arg1.get(); packet.param4 = cmd.content.nav_script_time.arg2.get(); - packet.x = cmd.content.nav_script_time.arg3.get(); - packet.y = cmd.content.nav_script_time.arg4.get(); + packet.x = cmd.content.nav_script_time.arg3; + packet.y = cmd.content.nav_script_time.arg4; break; case MAV_CMD_NAV_ATTITUDE_TIME: @@ -2624,6 +2634,28 @@ bool AP_Mission::calc_rewind_pos(Mission_Command& rewind_cmd) return true; } +/* + handle format conversion of storage format to allow us to update + format to take advantage of new packing. This is particularly useful + for conversion to Float16 to get extra parameter space +*/ +void AP_Mission::format_conversion(uint8_t tag_byte, const Mission_Command &cmd, PackedContent &packed_content) const +{ + // currently only one conversion needed, more can be added + if (tag_byte == 0 && cmd.id == MAV_CMD_NAV_SCRIPT_TIME) { + struct nav_script_time_Command_tag0 old_fmt; + struct nav_script_time_Command new_fmt; + memcpy((void*)&old_fmt, packed_content.bytes, sizeof(old_fmt)); + new_fmt.command = old_fmt.command; + new_fmt.timeout_s = old_fmt.timeout_s; + new_fmt.arg1.set(old_fmt.arg1); + new_fmt.arg2.set(old_fmt.arg2); + new_fmt.arg3 = 0; + new_fmt.arg4 = 0; + memcpy(packed_content.bytes, (void*)&new_fmt, sizeof(new_fmt)); + } +} + // singleton instance AP_Mission *AP_Mission::_singleton; diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index cb62bb1a1c..f631d4dd77 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -52,6 +52,8 @@ #define AP_MISSION_MAX_WP_HISTORY 7 // The maximum number of previous wp commands that will be stored from the active missions history #define LAST_WP_PASSED (AP_MISSION_MAX_WP_HISTORY-2) +union PackedContent; + /// @class AP_Mission /// @brief Object managing Mission class AP_Mission @@ -219,14 +221,23 @@ public: float p3; }; - // Scripting NAV command (with verify) + // Scripting NAV command old version of storage format + struct PACKED nav_script_time_Command_tag0 { + uint8_t command; + uint8_t timeout_s; + float arg1; + float arg2; + }; + + // Scripting NAV command, new version of storage format struct PACKED nav_script_time_Command { uint8_t command; uint8_t timeout_s; Float16_t arg1; Float16_t arg2; - Float16_t arg3; - Float16_t arg4; + // last 2 arguments need to be integers due to MISSION_ITEM_INT encoding + int16_t arg3; + int16_t arg4; }; // Scripting NAV command (with verify) @@ -782,6 +793,12 @@ private: bool start_command_do_sprayer(const AP_Mission::Mission_Command& cmd); bool start_command_do_scripting(const AP_Mission::Mission_Command& cmd); bool start_command_do_gimbal_manager_pitchyaw(const AP_Mission::Mission_Command& cmd); + + /* + handle format conversion of storage format to allow us to update + format to take advantage of new packing + */ + void format_conversion(uint8_t tag_byte, const Mission_Command &cmd, PackedContent &packed_content) const; }; namespace AP