diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index a85f900c12..c14a41930b 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -316,8 +316,7 @@ public: _mission_complete_fn(mission_complete_fn), _prev_nav_cmd_id(AP_MISSION_CMD_ID_NONE), _prev_nav_cmd_index(AP_MISSION_CMD_INDEX_NONE), - _prev_nav_cmd_wp_index(AP_MISSION_CMD_INDEX_NONE), - _last_change_time_ms(0) + _prev_nav_cmd_wp_index(AP_MISSION_CMD_INDEX_NONE) { #if CONFIG_HAL_BOARD == HAL_BOARD_SITL if (_singleton != nullptr) { @@ -332,14 +331,6 @@ public: // clear commands _nav_cmd.index = AP_MISSION_CMD_INDEX_NONE; _do_cmd.index = AP_MISSION_CMD_INDEX_NONE; - - // initialise other internal variables - _flags.state = MISSION_STOPPED; - _flags.nav_cmd_loaded = false; - _flags.do_cmd_loaded = false; - _flags.in_landing_sequence = false; - _flags.resuming_mission = false; - _force_resume = false; } // get singleton instance @@ -609,11 +600,11 @@ private: struct Mission_Flags { mission_state state; - uint8_t nav_cmd_loaded : 1; // true if a "navigation" command has been loaded into _nav_cmd - uint8_t do_cmd_loaded : 1; // true if a "do"/"conditional" command has been loaded into _do_cmd - uint8_t do_cmd_all_done : 1; // true if all "do"/"conditional" commands have been completed (stops unnecessary searching through eeprom for do commands) - bool in_landing_sequence : 1; // true if the mission has jumped to a landing - bool resuming_mission : 1; // true if the mission is resuming and set false once the aircraft attains the interrupted WP + bool nav_cmd_loaded; // true if a "navigation" command has been loaded into _nav_cmd + bool do_cmd_loaded; // true if a "do"/"conditional" command has been loaded into _do_cmd + bool do_cmd_all_done; // true if all "do"/"conditional" commands have been completed (stops unnecessary searching through eeprom for do commands) + bool in_landing_sequence; // true if the mission has jumped to a landing + bool resuming_mission; // true if the mission is resuming and set false once the aircraft attains the interrupted WP } _flags; // mission WP resume history @@ -685,24 +676,25 @@ private: /// sanity checks that the masked fields are not NaN's or infinite static MAV_MISSION_RESULT sanity_check_params(const mavlink_mission_item_int_t& packet); - // parameters - AP_Int16 _cmd_total; // total number of commands in the mission - AP_Int8 _restart; // controls mission starting point when entering Auto mode (either restart from beginning of mission or resume from last command run) - AP_Int16 _options; // bitmask options for missions, currently for mission clearing on reboot but can be expanded as required - // pointer to main program functions mission_cmd_fn_t _cmd_start_fn; // pointer to function which will be called when a new command is started mission_cmd_fn_t _cmd_verify_fn; // pointer to function which will be called repeatedly to ensure a command is progressing mission_complete_fn_t _mission_complete_fn; // pointer to function which will be called when mission completes + // parameters + AP_Int16 _cmd_total; // total number of commands in the mission + AP_Int16 _options; // bitmask options for missions, currently for mission clearing on reboot but can be expanded as required + AP_Int8 _restart; // controls mission starting point when entering Auto mode (either restart from beginning of mission or resume from last command run) + // internal variables + bool _force_resume; // when set true it forces mission to resume irrespective of MIS_RESTART param. + uint16_t _repeat_dist; // Distance to repeat on mission resume (m), can be set with MAV_CMD_DO_SET_RESUME_REPEAT_DIST struct Mission_Command _nav_cmd; // current "navigation" command. It's position in the command list is held in _nav_cmd.index struct Mission_Command _do_cmd; // current "do" command. It's position in the command list is held in _do_cmd.index struct Mission_Command _resume_cmd; // virtual wp command that is used to resume mission if the mission needs to be rewound on resume. uint16_t _prev_nav_cmd_id; // id of the previous "navigation" command. (WAYPOINT, LOITER_TO_ALT, ect etc) uint16_t _prev_nav_cmd_index; // index of the previous "navigation" command. Rarely used which is why we don't store the whole command uint16_t _prev_nav_cmd_wp_index; // index of the previous "navigation" command that contains a waypoint. Rarely used which is why we don't store the whole command - bool _force_resume; // when set true it forces mission to resume irrespective of MIS_RESTART param. struct Location _exit_position; // the position in the mission that the mission was exited // jump related variables @@ -714,8 +706,6 @@ private: // last time that mission changed uint32_t _last_change_time_ms; - // Distance to repeat on mission resume (m), can be set with MAV_CMD_DO_SET_RESUME_REPEAT_DIST - uint16_t _repeat_dist; // multi-thread support. This is static so it can be used from // const functions