MAV'RIC
|
Public Member Functions | |
Mission_handler_navigating (const INS &ins, const Mavlink_stream &mavlink_stream, Mavlink_waypoint_handler &waypoint_handler) | |
Initialize the navigating mission planner handler. | |
virtual bool | can_handle (const Waypoint &wpt) const |
Checks if the waypoint is a navigating waypoint. | |
virtual bool | setup (const Waypoint &wpt) |
Sets up this handler class for a first time initialization. | |
virtual Mission_handler::update_status_t | update () |
Handles the mission every iteration. | |
virtual bool | write_flight_command (Flight_controller &flight_controller) const |
Provides control commands to the flight controller. | |
virtual Mission_planner::internal_state_t | handler_mission_state () const |
Returns that the mission state is in MISSION. | |
Protected Member Functions | |
void | send_nav_time (const Mavlink_stream *mavlink_stream, mavlink_message_t *msg) |
Sends the travel time between the last two waypoints. | |
Protected Attributes | |
const INS & | ins_ |
The reference to the ins interface. | |
const Mavlink_stream & | mavlink_stream_ |
The reference to the mavlink object. | |
Mavlink_waypoint_handler & | waypoint_handler_ |
The reference to the mavlink waypoint handler. | |
Waypoint | waypoint_ |
The waypoint that we are heading towards. | |
bool | waypoint_reached_ |
Flag stating if this waypoint has been reached or not. | |
uint64_t | start_time_ |
The start time for travelling to this waypoint. | |
uint32_t | travel_time_ |
The travel time between two waypoints, updated once the MAV arrives at its next waypoint. | |
position_command_t | position_command_ |
Desired position command. |
Mission_handler_navigating::Mission_handler_navigating | ( | const INS & | ins, |
const Mavlink_stream & | mavlink_stream, | ||
Mavlink_waypoint_handler & | waypoint_handler | ||
) |
Initialize the navigating mission planner handler.
ins | The reference to the ins |
mavlink_stream | The reference to the MAVLink stream structure |
waypoint_handler | The handler for the manual control state |
bool Mission_handler_navigating::can_handle | ( | const Waypoint & | wpt | ) | const [virtual] |
Checks if the waypoint is a navigating waypoint.
Checks if the inputted waypoint is a: MAV_CMD_NAV_WAYPOINT
wpt | The waypoint class |
Implements Mission_handler.
Reimplemented in Mission_handler_critical_navigating.
Mission_planner::internal_state_t Mission_handler_navigating::handler_mission_state | ( | ) | const [virtual] |
Returns that the mission state is in MISSION.
Implements Mission_handler.
void Mission_handler_navigating::send_nav_time | ( | const Mavlink_stream * | mavlink_stream, |
mavlink_message_t * | msg | ||
) | [protected] |
Sends the travel time between the last two waypoints.
waypoint_handler | The pointer to the waypoint handler structure |
mavlink_stream | The pointer to the MAVLink stream structure |
msg | The pointer to the MAVLink message |
bool Mission_handler_navigating::setup | ( | const Waypoint & | wpt | ) | [virtual] |
Sets up this handler class for a first time initialization.
Records the waypoint reference
wpt | The waypoint class |
Implements Mission_handler.
Mission_handler::update_status_t Mission_handler_navigating::update | ( | void | ) | [virtual] |
Handles the mission every iteration.
Handles the navigation to the waypoint and determines the status code. The status code is MISSION_IN_PROGRESS for currently navigating to the waypoint, MISSION_FINISHED for waypoint reached and autocontinue, MISSION_FAILED for control command failed.
Implements Mission_handler.
bool Mission_handler_navigating::write_flight_command | ( | Flight_controller & | flight_controller | ) | const [virtual] |
Provides control commands to the flight controller.
Implements Flight_command_source.