|
|
@ -336,15 +336,15 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t |
|
|
|
break; |
|
|
|
break; |
|
|
|
|
|
|
|
|
|
|
|
case MAV_MISSION_TYPE_RALLY: { // Read a safe point / rally point
|
|
|
|
case MAV_MISSION_TYPE_RALLY: { // Read a safe point / rally point
|
|
|
|
mission_save_point_s mission_save_point; |
|
|
|
mission_safe_point_s mission_safe_point; |
|
|
|
read_success = dm_read(DM_KEY_SAFE_POINTS, seq + 1, &mission_save_point, sizeof(mission_save_point_s)) == |
|
|
|
read_success = dm_read(DM_KEY_SAFE_POINTS, seq + 1, &mission_safe_point, sizeof(mission_safe_point_s)) == |
|
|
|
sizeof(mission_save_point_s); |
|
|
|
sizeof(mission_safe_point_s); |
|
|
|
|
|
|
|
|
|
|
|
mission_item.nav_cmd = MAV_CMD_NAV_RALLY_POINT; |
|
|
|
mission_item.nav_cmd = MAV_CMD_NAV_RALLY_POINT; |
|
|
|
mission_item.frame = mission_save_point.frame; |
|
|
|
mission_item.frame = mission_safe_point.frame; |
|
|
|
mission_item.lat = mission_save_point.lat; |
|
|
|
mission_item.lat = mission_safe_point.lat; |
|
|
|
mission_item.lon = mission_save_point.lon; |
|
|
|
mission_item.lon = mission_safe_point.lon; |
|
|
|
mission_item.altitude = mission_save_point.alt; |
|
|
|
mission_item.altitude = mission_safe_point.alt; |
|
|
|
} |
|
|
|
} |
|
|
|
break; |
|
|
|
break; |
|
|
|
|
|
|
|
|
|
|
@ -1113,13 +1113,13 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) |
|
|
|
break; |
|
|
|
break; |
|
|
|
|
|
|
|
|
|
|
|
case MAV_MISSION_TYPE_RALLY: { // Write a safe point / rally point
|
|
|
|
case MAV_MISSION_TYPE_RALLY: { // Write a safe point / rally point
|
|
|
|
mission_save_point_s mission_save_point; |
|
|
|
mission_safe_point_s mission_safe_point; |
|
|
|
mission_save_point.lat = mission_item.lat; |
|
|
|
mission_safe_point.lat = mission_item.lat; |
|
|
|
mission_save_point.lon = mission_item.lon; |
|
|
|
mission_safe_point.lon = mission_item.lon; |
|
|
|
mission_save_point.alt = mission_item.altitude; |
|
|
|
mission_safe_point.alt = mission_item.altitude; |
|
|
|
mission_save_point.frame = mission_item.frame; |
|
|
|
mission_safe_point.frame = mission_item.frame; |
|
|
|
write_failed = dm_write(DM_KEY_SAFE_POINTS, wp.seq + 1, DM_PERSIST_POWER_ON_RESET, &mission_save_point, |
|
|
|
write_failed = dm_write(DM_KEY_SAFE_POINTS, wp.seq + 1, DM_PERSIST_POWER_ON_RESET, &mission_safe_point, |
|
|
|
sizeof(mission_save_point_s)) != sizeof(mission_save_point_s); |
|
|
|
sizeof(mission_safe_point_s)) != sizeof(mission_safe_point_s); |
|
|
|
} |
|
|
|
} |
|
|
|
break; |
|
|
|
break; |
|
|
|
|
|
|
|
|
|
|
|