diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 817f61d317..e288bc9410 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -51,9 +51,6 @@ void AP_Mission::init() // command list will be cleared if they do not match check_eeprom_version(); - // changes in Content size break the storage - static_assert(sizeof(union Content) == 12, "AP_Mission: Content must be 12 bytes"); - // 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"); @@ -465,6 +462,41 @@ bool AP_Mission::set_current_cmd(uint16_t index) 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) + uint8_t loiter_ccw : 1; // 0 if clockwise, 1 if counter clockwise + uint8_t terrain_alt : 1; // this altitude is above terrain + uint8_t origin_alt : 1; // this altitude is above ekf origin + uint8_t loiter_xtrack : 1; // 0 to crosstrack from center of waypoint, 1 to crosstrack from tangent exit location +}; + +struct PACKED PackedLocation { + union { + Packed_Location_Option_Flags flags; ///< options bitmask (1<<0 = relative altitude) + uint8_t options; /// allows writing all flags to eeprom as one byte + }; + // by making alt 24 bit we can make p1 in a command 16 bit, + // allowing an accurate angle in centi-degrees. This keeps the + // storage cost per mission item at 15 bytes, and allows mission + // altitudes of up to +/- 83km + int32_t alt:24; ///< param 2 - Altitude in centimeters (meters * 100) see LOCATION_ALT_MAX_M + int32_t lat; ///< param 3 - Latitude * 10**7 + int32_t lng; ///< param 4 - Longitude * 10**7 +}; + +union PackedContent { + // location + PackedLocation location; // Waypoint location + + // 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]; + +}; + +assert_storage_size assert_storage_size_PackedContent; + /// load_cmd_from_storage - load command from storage /// true is return if successful bool AP_Mission::read_cmd_from_storage(uint16_t index, Mission_Command& cmd) const @@ -488,15 +520,34 @@ 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); + PackedContent packed_content; + 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); + _storage.read_block(packed_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); + _storage.read_block(packed_content.bytes, pos_in_storage+3, 12); + } + + if (stored_in_location(cmd.id)) { + // Location is not PACKED; field-wise copy it: + cmd.content.location.relative_alt = packed_content.location.flags.relative_alt; + cmd.content.location.loiter_ccw = packed_content.location.flags.loiter_ccw; + cmd.content.location.terrain_alt = packed_content.location.flags.terrain_alt; + cmd.content.location.origin_alt = packed_content.location.flags.origin_alt; + cmd.content.location.loiter_xtrack = packed_content.location.flags.loiter_xtrack; + cmd.content.location.alt = packed_content.location.alt; + cmd.content.location.lat = packed_content.location.lat; + cmd.content.location.lng = packed_content.location.lng; + } else { + // all other options in Content are assumed to be packed: + static_assert(sizeof(cmd.content) >= 12, + "content is big enough to take bytes"); + memcpy(&cmd.content, packed_content.bytes, 12); } // set command's index to it's position in eeprom @@ -507,6 +558,31 @@ bool AP_Mission::read_cmd_from_storage(uint16_t index, Mission_Command& cmd) con return true; } +bool AP_Mission::stored_in_location(uint16_t id) +{ + switch (id) { + case MAV_CMD_NAV_WAYPOINT: + case MAV_CMD_NAV_LOITER_UNLIM: + case MAV_CMD_NAV_LOITER_TURNS: + case MAV_CMD_NAV_LOITER_TIME: + case MAV_CMD_NAV_LAND: + case MAV_CMD_NAV_TAKEOFF: + case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT: + case MAV_CMD_NAV_LOITER_TO_ALT: + case MAV_CMD_NAV_SPLINE_WAYPOINT: + case MAV_CMD_DO_SET_HOME: + case MAV_CMD_DO_LAND_START: + case MAV_CMD_DO_GO_AROUND: + case MAV_CMD_DO_SET_ROI: + case MAV_CMD_NAV_VTOL_TAKEOFF: + case MAV_CMD_NAV_VTOL_LAND: + case MAV_CMD_NAV_PAYLOAD_PLACE: + return true; + default: + return false; + } +} + /// write_cmd_to_storage - write a command to storage /// index is used to calculate the storage location /// true is returned if successful @@ -519,19 +595,35 @@ bool AP_Mission::write_cmd_to_storage(uint16_t index, Mission_Command& cmd) return false; } + PackedContent packed; + if (stored_in_location(cmd.id)) { + // Location is not PACKED; field-wise copy it: + packed.location.flags.relative_alt = cmd.content.location.relative_alt; + packed.location.flags.loiter_ccw = cmd.content.location.loiter_ccw; + packed.location.flags.terrain_alt = cmd.content.location.terrain_alt; + packed.location.flags.origin_alt = cmd.content.location.origin_alt; + packed.location.flags.loiter_xtrack = cmd.content.location.loiter_xtrack; + packed.location.alt = cmd.content.location.alt; + packed.location.lat = cmd.content.location.lat; + packed.location.lng = cmd.content.location.lng; + } else { + // all other options in Content are assumed to be packed: + memcpy(packed.bytes, &cmd.content.jump, sizeof(packed.bytes)); + } + // calculate where in storage the command should be placed uint16_t pos_in_storage = 4 + (index * AP_MISSION_EEPROM_COMMAND_SIZE); 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); + _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); _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); + _storage.write_block(pos_in_storage+5, packed.bytes, 10); } // remember when the mission last changed @@ -597,12 +689,10 @@ MAV_MISSION_RESULT AP_Mission::sanity_check_params(const mavlink_mission_item_in // return MAV_MISSION_ACCEPTED on success, MAV_MISSION_RESULT error on failure MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_item_int_t& packet, AP_Mission::Mission_Command& cmd) { - bool copy_location = false; - // command's position in mission list and mavlink id cmd.index = packet.seq; cmd.id = packet.command; - cmd.content.location.options = 0; + memset(&cmd.content.location, 0, sizeof(cmd.content.location)); MAV_MISSION_RESULT param_check = sanity_check_params(packet); if (param_check != MAV_MISSION_ACCEPTED) { @@ -618,7 +708,6 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ case MAV_CMD_NAV_WAYPOINT: // MAV ID: 16 { - copy_location = true; /* the 15 byte limit means we can't fit both delay and radius in the cmd structure. When we expand the mission structure @@ -642,45 +731,39 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ break; case MAV_CMD_NAV_LOITER_UNLIM: // MAV ID: 17 - copy_location = true; cmd.p1 = fabsf(packet.param3); // store radius as 16bit since no other params are competing for space - cmd.content.location.flags.loiter_ccw = (packet.param3 < 0); // -1 = counter clockwise, +1 = clockwise + cmd.content.location.loiter_ccw = (packet.param3 < 0); // -1 = counter clockwise, +1 = clockwise break; case MAV_CMD_NAV_LOITER_TURNS: // MAV ID: 18 { - copy_location = true; 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.flags.loiter_ccw = (packet.param3 < 0); - cmd.content.location.flags.loiter_xtrack = (packet.param4 > 0); // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location + 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; case MAV_CMD_NAV_LOITER_TIME: // MAV ID: 19 - copy_location = true; cmd.p1 = packet.param1; // loiter time in seconds uses all 16 bits, 8bit seconds is too small. No room for radius. - cmd.content.location.flags.loiter_ccw = (packet.param3 < 0); - cmd.content.location.flags.loiter_xtrack = (packet.param4 > 0); // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location + 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; case MAV_CMD_NAV_RETURN_TO_LAUNCH: // MAV ID: 20 break; case MAV_CMD_NAV_LAND: // MAV ID: 21 - copy_location = true; cmd.p1 = packet.param1; // abort target altitude(m) (plane only) - cmd.content.location.flags.loiter_ccw = is_negative(packet.param4); // yaw direction, (plane deepstall only) + cmd.content.location.loiter_ccw = is_negative(packet.param4); // yaw direction, (plane deepstall only) break; case MAV_CMD_NAV_TAKEOFF: // MAV ID: 22 - copy_location = true; // only altitude is used cmd.p1 = packet.param1; // minimum pitch (plane only) break; case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT: // MAV ID: 30 - copy_location = true; // lat/lng used for heading lock cmd.p1 = packet.param1; // Climb/Descend // 0 = Neutral, cmd complete at +/- 5 of indicated alt. // 1 = Climb, cmd complete at or above indicated alt. @@ -688,14 +771,12 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ break; case MAV_CMD_NAV_LOITER_TO_ALT: // MAV ID: 31 - copy_location = true; cmd.p1 = fabsf(packet.param2); // param2 is radius in meters - cmd.content.location.flags.loiter_ccw = (packet.param2 < 0); - cmd.content.location.flags.loiter_xtrack = (packet.param4 > 0); // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location + cmd.content.location.loiter_ccw = (packet.param2 < 0); + cmd.content.location.loiter_xtrack = (packet.param4 > 0); // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location break; case MAV_CMD_NAV_SPLINE_WAYPOINT: // MAV ID: 82 - copy_location = true; cmd.p1 = packet.param1; // delay at waypoint in seconds break; @@ -741,7 +822,6 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ break; case MAV_CMD_DO_SET_HOME: - copy_location = true; cmd.p1 = packet.param1; // p1=0 means use current location, p=1 means use provided location break; @@ -769,15 +849,12 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ break; case MAV_CMD_DO_LAND_START: // MAV ID: 189 - copy_location = true; break; case MAV_CMD_DO_GO_AROUND: // MAV ID: 191 - copy_location = true; break; case MAV_CMD_DO_SET_ROI: // MAV ID: 201 - copy_location = true; cmd.p1 = packet.param1; // 0 = no roi, 1 = next waypoint, 2 = waypoint number, 3 = fixed location, 4 = given target (not supported) break; @@ -845,11 +922,9 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ break; case MAV_CMD_NAV_VTOL_TAKEOFF: - copy_location = true; break; case MAV_CMD_NAV_VTOL_LAND: - copy_location = true; break; case MAV_CMD_DO_VTOL_TRANSITION: @@ -868,7 +943,6 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ case MAV_CMD_NAV_PAYLOAD_PLACE: cmd.p1 = packet.param1*100; // copy max-descend parameter (m->cm) - copy_location = true; break; case MAV_CMD_NAV_SET_YAW_SPEED: @@ -890,7 +964,7 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ } // copy location from mavlink to command - if (copy_location) { + if (stored_in_location(cmd.id)) { // sanity check location if (!check_lat(packet.x)) { @@ -912,20 +986,20 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ case MAV_FRAME_MISSION: case MAV_FRAME_GLOBAL: - cmd.content.location.flags.relative_alt = 0; + cmd.content.location.relative_alt = 0; break; case MAV_FRAME_GLOBAL_RELATIVE_ALT: - cmd.content.location.flags.relative_alt = 1; + cmd.content.location.relative_alt = 1; break; #if AP_TERRAIN_AVAILABLE case MAV_FRAME_GLOBAL_TERRAIN_ALT: // we mark it as a relative altitude, as it doesn't have // home alt added - cmd.content.location.flags.relative_alt = 1; + cmd.content.location.relative_alt = 1; // mark altitude as above terrain, not above home - cmd.content.location.flags.terrain_alt = 1; + cmd.content.location.terrain_alt = 1; break; #endif @@ -1056,8 +1130,6 @@ MAV_MISSION_RESULT AP_Mission::mavlink_cmd_long_to_mission_cmd(const mavlink_com // return true on success, false on failure bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& cmd, mavlink_mission_item_int_t& packet) { - bool copy_location = false; - // command's position in mission list and mavlink id packet.seq = cmd.index; packet.command = cmd.id; @@ -1077,7 +1149,6 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c return false; case MAV_CMD_NAV_WAYPOINT: // MAV ID: 16 - copy_location = true; #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) // acceptance radius in meters @@ -1090,50 +1161,44 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c break; case MAV_CMD_NAV_LOITER_UNLIM: // MAV ID: 17 - copy_location = true; packet.param3 = (float)cmd.p1; - if (cmd.content.location.flags.loiter_ccw) { + if (cmd.content.location.loiter_ccw) { packet.param3 *= -1; } break; case MAV_CMD_NAV_LOITER_TURNS: // MAV ID: 18 - copy_location = true; packet.param1 = LOWBYTE(cmd.p1); // number of times to circle is held in low byte of p1 packet.param3 = HIGHBYTE(cmd.p1); // radius is held in high byte of p1 - if (cmd.content.location.flags.loiter_ccw) { + if (cmd.content.location.loiter_ccw) { packet.param3 = -packet.param3; } - packet.param4 = cmd.content.location.flags.loiter_xtrack; // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location + packet.param4 = cmd.content.location.loiter_xtrack; // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location break; case MAV_CMD_NAV_LOITER_TIME: // MAV ID: 19 - copy_location = true; packet.param1 = cmd.p1; // loiter time in seconds - if (cmd.content.location.flags.loiter_ccw) { + if (cmd.content.location.loiter_ccw) { packet.param3 = -1; } else { packet.param3 = 1; } - packet.param4 = cmd.content.location.flags.loiter_xtrack; // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location + packet.param4 = cmd.content.location.loiter_xtrack; // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location break; case MAV_CMD_NAV_RETURN_TO_LAUNCH: // MAV ID: 20 break; case MAV_CMD_NAV_LAND: // MAV ID: 21 - copy_location = true; packet.param1 = cmd.p1; // abort target altitude(m) (plane only) - packet.param4 = cmd.content.location.flags.loiter_ccw ? -1 : 1; // yaw direction, (plane deepstall only) + packet.param4 = cmd.content.location.loiter_ccw ? -1 : 1; // yaw direction, (plane deepstall only) break; case MAV_CMD_NAV_TAKEOFF: // MAV ID: 22 - copy_location = true; // only altitude is used packet.param1 = cmd.p1; // minimum pitch (plane only) break; case MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT: // MAV ID: 30 - copy_location = true; // lat/lng used for heading lock packet.param1 = cmd.p1; // Climb/Descend // 0 = Neutral, cmd complete at +/- 5 of indicated alt. // 1 = Climb, cmd complete at or above indicated alt. @@ -1141,16 +1206,14 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c break; case MAV_CMD_NAV_LOITER_TO_ALT: // MAV ID: 31 - copy_location = true; packet.param2 = cmd.p1; // loiter radius(m) - if (cmd.content.location.flags.loiter_ccw) { + if (cmd.content.location.loiter_ccw) { packet.param2 = -packet.param2; } - packet.param4 = cmd.content.location.flags.loiter_xtrack; // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location + packet.param4 = cmd.content.location.loiter_xtrack; // 0 to xtrack from center of waypoint, 1 to xtrack from tangent exit location break; case MAV_CMD_NAV_SPLINE_WAYPOINT: // MAV ID: 82 - copy_location = true; packet.param1 = cmd.p1; // delay at waypoint in seconds break; @@ -1196,7 +1259,6 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c break; case MAV_CMD_DO_SET_HOME: // MAV ID: 179 - copy_location = true; packet.param1 = cmd.p1; // p1=0 means use current location, p=1 means use provided location break; @@ -1224,15 +1286,12 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c break; case MAV_CMD_DO_LAND_START: // MAV ID: 189 - copy_location = true; break; case MAV_CMD_DO_GO_AROUND: // MAV ID: 191 - copy_location = true; break; case MAV_CMD_DO_SET_ROI: // MAV ID: 201 - copy_location = true; packet.param1 = cmd.p1; // 0 = no roi, 1 = next waypoint, 2 = waypoint number, 3 = fixed location, 4 = given target (not supported) break; @@ -1304,11 +1363,9 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c break; case MAV_CMD_NAV_VTOL_TAKEOFF: - copy_location = true; break; case MAV_CMD_NAV_VTOL_LAND: - copy_location = true; break; case MAV_CMD_DO_VTOL_TRANSITION: @@ -1322,7 +1379,6 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c break; case MAV_CMD_NAV_PAYLOAD_PLACE: - copy_location = true; packet.param1 = cmd.p1/100.0f; // copy max-descend parameter (m->cm) break; @@ -1345,20 +1401,20 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c } // copy location from mavlink to command - if (copy_location) { + if (stored_in_location(cmd.id)) { packet.x = cmd.content.location.lat; packet.y = cmd.content.location.lng; packet.z = cmd.content.location.alt / 100.0f; // cmd alt in cm to m - if (cmd.content.location.flags.relative_alt) { + if (cmd.content.location.relative_alt) { packet.frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; }else{ packet.frame = MAV_FRAME_GLOBAL; } #if AP_TERRAIN_AVAILABLE - if (cmd.content.location.flags.terrain_alt) { + if (cmd.content.location.terrain_alt) { // this is a above-terrain altitude - if (!cmd.content.location.flags.relative_alt) { + if (!cmd.content.location.relative_alt) { // refuse to return non-relative terrain mission // items. Internally we do have these, and they // have home.alt added, but we should never be @@ -1372,7 +1428,7 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c } #else // don't ever return terrain mission items if no terrain support - if (cmd.content.location.flags.terrain_alt) { + if (cmd.content.location.terrain_alt) { return false; } #endif diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index 11aa0437ed..e329fa263a 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -191,7 +191,7 @@ public: float release_rate; // release rate in meters/second }; - union PACKED Content { + union Content { // jump structure Jump_Command jump; @@ -257,10 +257,6 @@ public: // location Location location; // Waypoint location - - // 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 @@ -489,6 +485,8 @@ private: static StorageAccess _storage; + static bool stored_in_location(uint16_t id); + struct Mission_Flags { mission_state state; uint8_t nav_cmd_loaded : 1; // true if a "navigation" command has been loaded into _nav_cmd