I've discovered a bug in the AP_Mission command management and I can't find a reference to it in the forum. I was hoping to either educate people of the bug, or be directed to a way around it.
tldr; The first command saved to storage never gets read. AP_Mission always starts mission sequences at 0, when read from storage 0 is treated as a special index.
If you follow the suggested state machine for setting mission commands: qgroundcontrol.org/mavlink/waypoint_protocol ... You'll notice that the AP_Mission sends a MISSION_REQUEST with a sequence which starts at 0. When the requested sequence is received, the index is not modified to fit the AP_MISSION_FIRST_REAL_COMMAND. It's written to index 0 in function write_cmd_to_storage . Notice that all mission functions rely on function read_cmd_from_storage which purposefully skips index 0. So I never read command 0. I see this in reality because my UAV never goes to the first waypoint.
Does anyone have a solution other than repeat the first waypoint twice?