I like to refer my answer for people who want to use MAV_CMD_DO_SET_MODE because I was also stuck in the same problem.