aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Mission: allow for 16 bit command IDs

Browse Source 
this uses command ID 0 to allow for 16 bit command IDs. When used it
limits the content to just 10 bytes.
This commit is contained in:
Andrew Tridgell 2016-04-22 17:58:42 +10:00
parent bcc2838a37
commit 09c3c36c00
2 changed files with 33 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
libraries/AP_Mission/AP_Mission.cpp
View File

@ -424,9 +424,16 @@ bool AP_Mission::read_cmd_from_storage(uint16_t index, Mission_Command& cmd) con
// read WP position
uint16_t pos_in_storage = 4 + (index * AP_MISSION_EEPROM_COMMAND_SIZE);
cmd.id = _storage.read_byte(pos_in_storage);
cmd.p1 = _storage.read_uint16(pos_in_storage+1);
_storage.read_block(cmd.content.bytes, pos_in_storage+3, 12);
uint8_t b1 = _storage.read_byte(pos_in_storage);
if (b1 == 0) {
cmd.id = _storage.read_uint16(pos_in_storage+1);
cmd.p1 = _storage.read_uint16(pos_in_storage+3);
_storage.read_block(cmd.content.bytes, pos_in_storage+5, 10);
} else {
cmd.id = b1;
cmd.p1 = _storage.read_uint16(pos_in_storage+1);
_storage.read_block(cmd.content.bytes, pos_in_storage+3, 12);
}
// set command's index to it's position in eeprom
cmd.index = index;
@ -449,9 +456,17 @@ bool AP_Mission::write_cmd_to_storage(uint16_t index, Mission_Command& cmd)
// calculate where in storage the command should be placed
uint16_t pos_in_storage = 4 + (index * AP_MISSION_EEPROM_COMMAND_SIZE);
_storage.write_byte(pos_in_storage, cmd.id);
_storage.write_uint16(pos_in_storage+1, cmd.p1);
_storage.write_block(pos_in_storage+3, cmd.content.bytes, 12);
if (cmd.id < 256) {
_storage.write_byte(pos_in_storage, cmd.id);
_storage.write_uint16(pos_in_storage+1, cmd.p1);
_storage.write_block(pos_in_storage+3, cmd.content.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);
_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, cmd.content.bytes, 10);
}
// remember when the mission last changed
_last_change_time_ms = AP_HAL::millis();
@ -485,6 +500,10 @@ MAV_MISSION_RESULT AP_Mission::mavlink_to_mission_cmd(const mavlink_mission_item
// command specific conversions from mavlink packet to mission command
switch (cmd.id) {
case 0:
// this is reserved for storing 16 bit command IDs
return MAV_MISSION_INVALID;
case MAV_CMD_NAV_WAYPOINT: // MAV ID: 16
copy_location = true;
/*
@ -819,7 +838,10 @@ bool AP_Mission::mission_cmd_to_mavlink(const AP_Mission::Mission_Command& cmd,
// command specific conversions from mission command to mavlink packet
switch (cmd.id) {
case 0:
// this is reserved for 16 bit command IDs
return false;
case MAV_CMD_NAV_WAYPOINT: // MAV ID: 16
copy_location = true;
#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)

7
libraries/AP_Mission/AP_Mission.h
View File

@ -208,14 +208,15 @@ public:
// location
Location location; // Waypoint location
// raw bytes, for reading/writing to eeprom
// raw bytes, for reading/writing to eeprom. Note that only 10 bytes are available
// if a 16 bit command ID is used
uint8_t bytes[12];
};
// command structure
struct PACKED Mission_Command {
struct Mission_Command {
uint16_t index; // this commands position in the command list
uint8_t id; // mavlink command id
uint16_t id; // mavlink command id
uint16_t p1; // general purpose parameter 1
Content content;
};