Commands

The commands are the actual operations executed by the microcontroller (or the nix system). These commands in general are divided in sequences, which each sequence is executed one time by the scheduler for a specific amount of time.

Taking as example theautomatic_takeoff() operation : First of all, like the general architecture explained, we callread_messages()first, then autopilot_start(), and a first write withautopilot_write_helper().

Then we define the MAVLink setpoint at the LOCAL NED :

mavlink_set_position_target_local_ned_t set_point;

Control functions

arm_sequence()

We quickly enters the switch, and the arm sequnce : This sequence quickly checks for the base_mode from the heartbeat messages to a predefined state and the first arm.

if (current_messages.heartbeat.base_mode != ARMED_BASE_MODE && arm_lock == 0){

Incommands.h,we defineARMED_BASE_MODE :

#define ARMED_BASE_MODE 209

This corresponds to a mode where the pixhawk clearly states that's armed. This represents an acknowledgment procedure. We shouldn't arm twice (sending the same command twice), since the scheduler executes the same commands for a certain time, so we add an lock which blocks the execution for a single time.

offboard_control_sequence()

After arming, we need to force the pixhawk to enter the offboard control mode, to do so, we need to send commands at least at 4 Hz, and by sending void vommands withautopilot_write()and then the right set of setpoint commands, we will force the drone to takeoff and execute the required commands.

Like the arm sequence, if the offboard change failed to occur, we put a condition to resend the same command again while maintaining the flow of void commands :

    if (current_messages.heartbeat.base_mode != OFFBOARD_CONTROL_BASE_MODE){
        control_status = false;
        autopilot_write();
        enable_offboard_control();
        autopilot_write();
    }

OFFBOARD_CONTROL_BASE_MODEis defined incommands.h

#define OFFBOARD_CONTROL_BASE_MODE 157

set__(), set_yaw()

We start to use the control commands provided by the autopilot control interface to control the drone.

set_yaw (ip.yaw, set_point);
set__( ip.x , ip.y , ip.z - 2.00 , set_point); break;

disable_offboard_control_sequence()

This sequence is simply sending the offboard control disabling command.

Disarm sequence

For the STM32F4, we write a void setpoint again, the we issue theautopilot_disarm()function, and recheck the base_mode for actual disarm :

if (current_messages.heartbeat.base_mode == ARMED_BASE_MODE){
        autopilot_disarm();
        }

program_counter_sequence()

This sequence is different on each architecture : For STM32F4, thetim2_isr()hardware Interrupt Service routines increments the seconds. If the count is larger than the specified timer, it will reset the seconds counter and increment the Program Counter variable by one.

if (seconds >= timer){
    Program_counter++;
    seconds = 0;
}

For the PC architecture, we define a start time.

#ifndef STM32F4
    time_t end;
    time_t begin =  time(NULL);
#endif

Each iteration of thewhile(1)loop, we compare the end time with the begin time, if this time is bigger than the specified time, we increment the Program counter.

end =  time(NULL);

if ((end - begin) >= timer){
    begin = time(NULL);
    printf("Operation : %d \n", Program_counter);
    Program_counter++;
}

Customization

These commands can be modified at will. You can add as many setpoints and velocity set points as you want, as long as you keep the arm/offboard control and disable offboard control/disarm sequence intact.

void operation (float timer);
void operation_extended (float timer);
void square_operation (float timer);
void circle_operation (float timer);
void automatic_takeoff(float timer);
void flight_control_sequence(float timer);