The autopilot control interface

The interface forms an abstract layer upon the hardware dependent serial port interface, and shouldn't in theory be modified to port the program to another architecture, like the main MAVLink control source code(mavlink_control.cpp) This layer is the one that offers the control through the offboard control mode, which is well defined in the PX4 flight stack.

The interface is contained withininterface.cppandinterface.h,and callsserial_port.hwhich is hardware dependent.

Let's begin by defining the basic requirements for this interface :

It needs to identify the messages sent from the autopilot (through serial port), and forms a structure containing the received messages - Methods : read_messages()and aMavlink_messagesstructure/type.
It needs to use the acquired messages (position, status, battery charge, altitude, attitude, etc..) to construct a new message and send it back to the pixhawk autopilot through serial port, and even send new instructions like set velocity and set point (set position) - Methods : autopilot_write_message(),autopilot_write(),autopilot_write_setpoint(), autopilot_update_setpoint().
It needs a specific instruction/method to send the arming/disarm commands to the pixhawk - Methods : autopilot_arm(),autopilot_disarm()and a toggle for these 2 methods :toggle_arm_disarm()
It needs an instruction for ordering the pixhawk to receiver orders from external controller - Methods : enable_offboard_control(),disable_offboard_controland a toggle :toggle_offboard_control().
It needs control functions, like setpoint, set velocity and more abstract commands : set_position(),set__(), set_velocity(),set_yaw(),position_and_speed_set()
It needs a message and commands timestamps - Methods : get_time_usec(),Time_stamp() constructre, andreset_timestamp() as well as the Time_Stamps structure/type.
It needs to initialize the correct target autopilot and system IDs, and lock down intial position - Methods : autopilot_intialize(),autopilot_start().

Initialization

First, we define wether the autopilot is in offboard control mode or armed (inautopilot_intialize())

control_status = 0;      // whether the autopilot is in offboard control mode
arm_status = 0;          // whether the autopilot is armed or not

Define the target system, autopilot IDs and local companion computer ID, and affect these to the current target system and autopilot IDs.

system_id    = 1; // system id
autopilot_id = 1; // autopilot component id
companion_id = 0; // companion computer component id

current_messages.sysid  = system_id; // Set current system id to predefined one
current_messages.compid = autopilot_id; // Set current autopilot id to predefined one

Theautopilot_intialize()is the very first function to be executed once the program starts.

Inautopilot_start(), we lock down the initial position data (x,y,z), (Vx, Vy, Vz), yaw and yaw rate.

This function is called directly after the firstread_messages(). As we will see later, theread_messages()function loops until it receives the first HEARTBEAT and LOCAL_POSITION_NED IDs and locks down, then depends on the HIGHRES Message to keep going.

This function is called only once on each operation (or sequence) in commands.cpp and mavlink_control.cpp. To make sure the redefinition of the intial coordinates never occurs twice in the same operation, a lock was added : initial_position_lock.

Read MAVLink messages from serial

This function interact directly with received MAVLink messages through the serial port :

bool success;               // receive success flag
bool received_all = false;  // Loop untill the specified MAVLink message ID is received 
Time_Stamps this_timestamps; // Define a Timestamp structure

The received_all boolean is used to check for the reception of the specified items which are defined later :

if (lock_read_messages == 0){
            received_all =
                    this_timestamps.heartbeat  &&
                    this_timestamps.local_position_ned;        
            } else {
                received_all = this_timestamps.highres_imu;    
                if (highres_flag == 0) break;
            }
lock_read_messages = 1;

For the first run, (first time theread_messages()function is called), the function waits for the HEARTBEAT and LOCAL_POSITION_NED MAVLink messages types. Once received, the locklock_read_messages turns on and the function will only wait for the HIGHRES_IMU MAVLink message to continue.

This function reads a message assembled from serial port :

mavlink_message_t message;
success = serial_read_message(message);

If a read was successful, interpret the message (dependent on the message ID) : e.g : The LOCAL_POSITION_NED MAVLink message :

Identify the message ID :

case MAVLINK_MSG_ID_LOCAL_POSITION_NED:{

Decode the message based on the message ID :

mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned));

Assign a timestamp to the message received throughget_time_usec()function

current_messages.time_stamps.local_position_ned = get_time_usec();
this_timestamps.local_position_ned = current_messages.time_stamps.local_position_ned;
break;
}

Same procedure for all other messages.

Notes : Each timeread_messages()is called, the Timestamp constructor erases the previous timestamps withreset_timestamps().

The same reset happens each time we define a new MAVLink message structureMavlink_Messages.

Write a MAVLink message to serial port

This is done directly through theautopilot_write_message() function, which calls the serial port function directly : serial_write_message()

autopilot_write()

The general writing is done byautopilot_write() function (which serves as a void setpoint command)

First we define a mavlink message :

mavlink_set_position_target_local_ned_t set_point;

Then we define the type_mask :

set_point.type_mask = MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_VELOCITY & MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_YAW_RATE;

The type_mask is a Bitmask that is computed on the pixhawk part. It indicates which values should be ignored by the vehicle. These bit_masks are defined ininterface.h:

#define MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_POSITION     0b0000110111111000
#define MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_VELOCITY     0b0000110111000111
#define MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_ACCELERATION 0b0000110000111111
#define MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_FORCE        0b0000111000111111
#define MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_YAW_ANGLE    0b0000100111111111
#define MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_YAW_RATE     0b0000010111111111

As well as the coordinate frame, yaw rate and intial velocity in the 3 axis :

set_point.coordinate_frame = MAV_FRAME_LOCAL_NED;
set_point.vx       = 0.0;
set_point.vy       = 0.0;
set_point.vz       = 0.0;
set_point.yaw_rate = 0.0;

Finaly, it updates the current_setpoint variable and then write the message to serial port withautopilot_write_setpoint()

current_setpoint = set_point;
autopilot_write_setpoint();

autopilot_write_setpoint()

This forms the main function that writes MAVLink messages to the serial port (or the pixhawk) :

We define the required setpoint to be sent :

mavlink_set_position_target_local_ned_t set_point = current_setpoint;

Affect the destination of the setpoint to the required system (predefined in initialization) :

    set_point.target_system    = system_id;
    set_point.target_component = autopilot_id;

Encode and write the setpoint to the serial port :

mavlink_message_t message;
mavlink_msg_set_position_target_local_ned_encode(system_id, companion_id, &message, &set_point);

autopilot_write_message(message);

Toggle offboard control

The offboard control mode needs to be activated in order for the drone to receive the setpoint commands. Note : We need to send at least void setpoint commands (can be done withautopilot_write()since it only sets the default yaw rate and a null initial speed) before sending the offboard activation command (delay of 0.5 seconds)

Bothenable_offboard_control()anddisable_offboard_control()depends on the control_status variable which specify the stats of the offboard control (defines if the offboard control mode is activated from the prespective of the program)

What actually fullfill the job is thetoggle_offboard_control()function, which depending on the flag, it sends a command long message (a type of MAVLink messages) and a message ID to the pixhawk in order to activate the offboard control mode.

First, define a command long structure, then assign the required command, target system and component IDs, and the parameter.

mavlink_command_long_t com = { 0 };
com.target_system    = system_id;
com.target_component = autopilot_id;
com.command          = MAV_CMD_NAV_GUIDED_ENABLE;
com.confirmation     = true;
com.param1           = (float) flag; // flag >0.5 => start, <0.5 => stop

Encode and the send the message as a command long :

mavlink_message_t message;
mavlink_msg_command_long_encode(system_id, companion_id, &message, &com);

int len = serial_write_message(message);

Toggle arm/disarm

In our application, we needed to actiavte the drone remotely then start sending the offboard control and the required setpoint commands. Altho this is not necessary, these functions are very similar to the offboard command functions (both of the mavlink command long types). The type of the sent command and parameter changes :

autopilot_status.command          = MAV_CMD_COMPONENT_ARM_DISARM;
autopilot_status.param1           = (float) flag; // flag = 1 => arm, flag = 0 => disarm

The locking flag for this command isarm_status

Control functions

Most like theautopilot_write() function, these functions sets the bitmask to ignore all other commands except the desired functionnality, the coordinate frame and the local coordinates in the 3 axis : e.g :

void set_position(float x, float y, float z, mavlink_set_position_target_local_ned_t &set_position){
    set_position.type_mask = MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_POSITION;
    set_position.coordinate_frame = MAV_FRAME_LOCAL_NED;

    set_position.x   = x; set_position.y   = y; set_position.z   = z;

    }

It's important to note that, since this is a pure sequential program without any use of threads/Posix threads, a new function had been added : set__(), which defines the required setpoint, update the current_setpoint variable, and write it automatically to the serial port:

void set__(float x, float y, float z, mavlink_set_position_target_local_ned_t &final_set_point){
        set_position( x , y  , z, final_set_point);
        autopilot_update_setpoint(final_set_point);
        autopilot_write_setpoint();
    }

Timestamps :

To correctly time the moment of reception of the message, we need a timestamping mechanism. Therefor, we define a Timestamp structure which rest itself each time it is called, and assign the timestamp through theget_time_usec() function (in microseconds).

MAVLink messages structure :

The MAVLink messages data structure defines the type of each MAVLink message and makes it ready to be manipulated throught out the program. New types of MAVLink messages can be added, as well as custom ones.

Each time we define the same MAVLink messages structure, we reset its timestamps.

The autopilot control interface

The autopilot control interface

Initialization

Read MAVLink messages from serial

Write a MAVLink message to serial port

autopilot_write()

autopilot_write_setpoint()

Toggle offboard control

Toggle arm/disarm

Control functions

Timestamps :

MAVLink messages structure :

results matching ""

No results matching ""