From Skybotix

Contents 1 Connection functions 1.1 sbOpenSerialCommunication 1.2 sbOpenSocketCommunication 1.3 sbCloseCommunication 1.4 sbLockCommunication 1.5 sbUnlockCommunication 1.6 sbConnect 1.7 sbDisconnect 2 Configuration fonctions 2.1 sbConfigureOAMode 2.2 sbConfigureComm 2.3 sbConfigureControl 2.4 sbConfigureTimeout 3 Control functions 3.1 sbSetNavMode 3.2 sbSetControl 4 State functions 4.1 sbGetSensorList 4.2 sbRequestState 4.3 sbReceiveState 4.4 sbRegisterStateCallback 5 Navigation modes 5.1 Stop 5.2 Idle 5.3 Take Off 5.4 Land 5.5 Hover 5.6 Controlled -- Ctrlled 5.7 Sink

Connection functions

sbOpenSerialCommunication

int sbOpenSerialCommunication(struct SBControlContext *control,
       const char *device, unsigned int baudrate);

This function opens a serial communication channel to the helicopter. Under linux, device may be (for instance) /dev/tty0 or /dev/rfcomm0 for bluetooth, and baudrate is a define symbol as defined in bit/termios.h (e.g. B115200).

sbOpenSocketCommunication

int sbOpenSocketCommunication(struct SBControlContext *control,
       const char *hostname, unsigned int port);

This function opens a network communication channel to the helicopter. This may be used in simulation or possibly to communicate via an on-board network-enabled processor. Argument hostname is the hostname or IP of the destination host and port is the UDP port of the communication.

Currently, there is no server responding to this communication request.

sbCloseCommunication

int sbCloseCommunication(struct SBControlContext *control);

Closes the communication channel and release any resource if required.

sbLockCommunication

int sbLockCommunication(struct SBControlContext *control);

Locks the communication channel to prevent concurrent access from different threads. Currently, this will only work on POSIX systems.

sbUnlockCommunication

int sbUnlockCommunication(struct SBControlContext *control);

Unlocks the communication channel

sbConnect

int sbConnect(struct SBControlContext *control);

This function is seen as a placeholder for the connection mechanism to the helicopter. In current implementation, if the connection has been established correctly, it always returns successfully.

Future implementation may change the behaviour.

sbDisconnect

int sbDisconnect(struct SBControlContext *control);

When (and if) a proper connection mechanism is designed, then this function will perform the disconnection from the platform.

Configuration fonctions

sbConfigureOAMode

int sbConfigureOAMode(struct SBControlContext *control, unsigned int oavoidMode);

Configures the obstacle avoidance behavior on the helicopter. oavoidMode describes the obstacle avoidance behavior using flags defined in sbconst.h:

• SB_OA_NONE: no obstacle avoidance.
• SB_OA_VERTICAL: only vertical obstacle avoidance, using downward

looking range sensor.

• SB_OA_HORIZONTAL: only horizontal obstacle avoidance, using horizontal

range sensors if available.

• A OR of the above to combine the behaviors.

sbConfigureComm

int sbConfigureComm(struct SBControlContext *control, 
       unsigned int commMode,
       unsigned int frequency,
       unsigned long contents);

Configures the communication behavior, in particular if the controlling computer will access the helicopter state via polling or receive it at fixed frequency. commMode is set using a flag defined from sbconst.h. It can be:

• SB_COM_ONREQUEST: the helicopter state is only sent when requested with

sbRequestState. Arguments frequency and contents are ignored.

• SB_COM_CONTINUOUS: the helicopter will send its state continuously,

frequency time per second. Only state variable flagged in contents (see section on state) will be sent. Maximum frequency achievable on a bluetooth link depends on the number of state item requested, and how often the communication direction is switched. Values between 10 and 200 seem possible.

sbConfigureControl

int sbConfigureControl(struct SBControlContext *control, 
       unsigned int roll,
       unsigned int pitch,
       unsigned int yaw,
       unsigned int altitude,
       unsigned int horizontal);

Configures the individual axis control modes. Each axis can be controlled either in position (SB_CTRL_POS), velocity (SB_CTRL_VEL) or relative displacement (SB_CTRL_REL), or not controlled (SB_CTRL_NONE). Not all modes apply to all axis, and some axis are coupled, preventing some control to be requested simultaneously. Control modes are defined sbconst.h.

Acceptable control (currently implemented/foreseen) are as follows:

• Roll: Position control only, Horizontal control is not possible
• Pitch: Position control only, Horizontal control is not possible
• Yaw (eq. Heading): Position and Velocity control
• Altitude: Position and Velocity control
• Horizontal: Velocity only, Roll and Pitch control is not possible

sbConfigureTimeout

int sbConfigureTimeout(struct SBControlContext *control, 
       unsigned short control_timeout_ms,
       unsigned short watchdog_timeout_ms);

Configures the timeout policy for the control and general communication watchdog. For both timeout, a value of zero does not change the current setting. A value of 0xFFFF disactivate the timeout. The control timeout is used only when the navigation mode (see section on controls) is SB_NAV_CTRLLED. In this case, if the controlling application does not issue commands within the timeout, the helicopter automatically switch back to SB_NAV_HOVER (constant height, constant yaw).

The watchdog timeout is used at any time, but really only makes sense when the helicopter is controlled or hovering (i.e. in modes SB_NAV_CTRLLED and SB_NAV_HOVER). In these mode, if no sbSetNavMode request are received within the timeout, the system automatically switch to SB_NAV_SINK (constant heading and slowly decreasing altitude until touch down). This watchdog is useful to implement a safe behavior in case the communication link gets broken.

Control functions

sbSetNavMode

int sbSetNavMode(struct SBControlContext *control, 
       unsigned int navmode);

Set the navigation mode, with navMode as a flag defined in sbconst.h. The semantic of navigation mode is as follows:

• SB_NAV_STOP: the helicopter is stopped, on the ground, rotors stopped.
• SB_NAV_IDLE: the helicopter is on the ground, rotors rotating but not enough for taking off.
• SB_NAV_TAKEOFF: the helicopter takes off, and transition to SB_NAV_HOVER when take off succeeded.
• SB_NAV_HOVER: the helicopter stays at constant height and yaw angle, with no pitch or roll. No horizontal stabilisation is active. If a watchdog is active, and no sbSetNavMode request is received after this timeout, the helicopter transition to SB_NAV_SINK.
• SB_NAV_LAND: the helicopter comes to the ground, and transition to SB_NAV_IDLE automatically after touch down.
• SB_NAV_CTRLLED: the helicopter is ready to receive user commands using sbSetControl. If a control timeout is active, and no control is received after this timeout, then the helicopter goes back to SB_NAV_HOVER.

sbSetNavMode is also used as a keep-alive function, indicating that the controlling process is still active, and as a result, should be called regularly (at least once per second with the default settings). See sbConfigureTimeout for more details.

sbSetControl

int sbSetControl(struct SBControlContext *control, 
       double roll, double pitch, double yaw,
       double altitude, double hx, double hy);

Gives the set-points of the helicopter control laws. The semantic of the set-point depends of the control modes defined with sbConfigureControl. Command clipping may be implemented, depending on the helicopter platform. If obstacle avoidance is active, it may also take precedence over requested control. Control axis where the control mode has been configured to SB_CTRL_NONE can be set to any value (0 is recommended).

Only International System units are used in this function: rad, rad/s, m, m/s

State functions

Semantic of the content flags

Content flag	Semantic
SBS_MODES	System mode set using the configuration function navigation state, communication mode, control modes, obstacle avoidance.
SBS_TIMESTAMP	System timestamp.
SBS_RPY	Roll, pitch, yaw from IMU.
SBS_GYRO	Roll, pitch, yaw rate from IMU.
SBS_ACCEL	Acceleration from IMU.
SBS_MAGNETO	Magnetic field vector from IMU.
SBS_IMUTEMP	Temperature from IMU.
SBS_ZRANGE	Range to the floor or nearest obstacle downward.
SBS_ZPRESSURE	Altitude from pressure sensor.
SBS_HRANGES	Horizontal ranges to obstacles.
SBS_XY_REL	Distance to closest object.
SBS_BATTERY	Battery status.
SBS_TIMEOUT	Timeout currently used, configured using sbConfigureTimeout.
SBS_O_ATTITUDE	Output of Attitude control.
SBS_O_ALTITUDE	Output of Altitude control.
SBS_O_TOL	Output of TakeOff/Landing control.
SBS_O_XY	Output of XY control.
SBS_O_OAVOID	Output of Obstacle avoidance control.
Combined flags for convenience
SBS_ALL	All of the above.
SBS_IMU_ALL	All the IMU data.
SBS_RANGES_ALL	All the ranges data.
SBS_ALTITUDE_ALL	All the altitude data (pressure and range).
SBS_OUTPUT_ALL	All the controller output.

Detailed content of the state structure

C Structure	State flags from content table and details
typedef struct {
unsigned char errorFlags;	Always included
unsigned long content;	Always included. Use a AND of the state content flags SBS_... above to check the content, e.g: if (state.content || SBS_RPY) { compute_odo(state.roll,state.pitch,state.yaw); } This content should correspond to what has been configured in sbConfigureComm or requested in sbRequestState
unsigned long timeStamp;	SBS_TIMESTAMP, timestamp in ms, from helicopter startup.
unsigned short controlTimeout;	SBS_TIMEOUT, as configured with sbConfigureTimeout.
unsigned short watchdogTimeout;	SBS_TIMEOUT, as configured with sbConfigureTimeout.
struct {	SBS_MODES
unsigned char navigation;	as set with sbSetNavMode.
unsigned char communication;	as set with sbConfigureComm.
unsigned char oavoid;	as set with sbConfigureOAMode.
unsigned char rollAxis;	as set with sbConfigureControl, when in SB_NAV_CTRLLED mode.
unsigned char pitchAxis;	idem.
unsigned char yawAxis;	idem.
unsigned char altAxis;	idem.
unsigned char horizAxis;	idem.
} mode;
float roll, pitch, yaw;	SBS_RPY, from IMU.
float gyro[3];	SBS_GYRO, from IMU.
float accel[3];	SBS_ACCEL, from IMU.
float magneto[3];	SBS_MAGNETO, from IMU.
float imutemp;	SBS_IMUTEMP, from IMU.
float zrange;	SBS_ZRANGE, from downward looking range sensor.
float zpressure;	SBS_ZPRESSURE
float hranges[3];	SBS_HRANGES, from horizontal range sensors.
float xrel, yrel;	SBS_XY_REL, distance to closest object, computed from range sensors.
float battery;	SBS_BATTERY
unsigned short o_attitude[3];	SBS_O_ATTITUDE, semantic TBD
unsigned short o_altitude;	SBS_O_ALTITUDE, semantic TBD
unsigned short o_tol;	SBS_O_TOL, semantic TBD
unsigned short o_xy[2];	SBS_O_XY, semantic TBD
unsigned short o_oavoid[2];	SBS_O_OAVOID, semantic TBD
SBHeliState;

sbGetSensorList

int sbGetSensorList(struct SBControlContext *control, unsigned long *list);

This functions returns the list of sensing modalities implemented on this platform. The output is returned in list under the form of a OR'ed mask of flags used to request helicopter state.

This mask can then be used with a AND operator when requesting the helicopter state, in order to request only relevant data.

sbRequestState

int sbRequestState(struct SBControlContext *control,
       unsigned long contents, SBHeliState *state);

Requests the helicopter state. contents is a OR'ed mask of flags defined above. Only the relevant fields of state are modified by the function. A detailed description of the SBHeliState type is given below.

This function can be used even when the communication mode is continuous (see sbConfigureComm), for instance, to request different data from time to time. Given that changing communication direction is very costly with bluetooth, it may be better to just augment the number of requested item in continuous mode.

sbReceiveState

int sbReceiveState(struct SBControlContext *control,SBHeliState *state,
       unsigned int timeout_ms);

Receives a state message, as specified in sbConfigureComm. timeout_ms represent a timeout (in ms) for the reception of messages. A value of zero implies an infinite wait. On success, the field of state specified in the contents argument of sbConfigureComm are updated.

sbRegisterStateCallback

int sbRegisterStateCallback(struct SBControlContext *control, 
   void (*stateCallback)(SBHeliState *state, void *userData),
   void *userData);

This function illustrates the spirit of this API. At any one time, only one process/thread should use the communication channel. In synchronous mode, the dialogue is only question/response, but when the communication is continuous, the helicopter may send state information at any time. When a question/response mode (control, configuration) is started, some state information may arrive after the question. while waiting for the answer. This callback can be setup to handle this piece of data in this case, instead of discarding it.

In practice, each time a question/response interaction is started, if a state message is received while waiting for the response, then stateCallback is called with the decoded state and the pointer userData used when registering the callback.

// Global variable to store the state, for illustration
SBHeliState state;
void * receive_thread(void *) {
    ...
        while (1) {
            sbLockCommunication(&control);
            sbReceiveState(control,&state);
            sbUnlockCommunication(&control);
        }
    ...
}

void recCB(SBHeliState *rstate, void *user_data) {
    memcpy(&state,&rstate,sizeof(SBHeliState);
}

int main() {
    struct SBControlContext control;
    
    sbOpenSerialCommunication(&control,"/dev/rfcomm0",B115200)
    sbConnect(&control);
    sbRegisterStateCallback(&control,recCB,NULL);
    sbConfigureComm(SB_COM_CONTINUOUS,30,SBS_RPY | SBS_ZRANGES);

    start receive_thread()

    while (1) {
        wait something
        sbLockCommunication(&control);
        // Any state received here will be handle by the callback
        sbSetNavMode(&control,SB_NAV_HOVER);
        sbUnlockCommunication(&control);
    }
    sbDisconnect(&control);
    ...
}

Navigation modes

The figure above is a state automaton showing the various behaviour modes expected from a MAV. It is important to notice that these modes may be virtual modes: they don't have to correspond to what is really implemented in the controller, they describes various behaviours, semantically consistent and with the same set of constraints. The remaining of this section details the expected behaviour in each mode, the transition conditions and the accepted commands.

For the safety of the MAV, it is assumed that a control watchdog is implemented. This watchdog is triggered when the connection between the MAV and the host computer is broken, or if the host controller stop sending control commands.

Stop

Description

• System is checked up
• All the motors are powered off
• The pressure sensor reference is initialised

Transition conditions

1. Transition A : on user request.

Accepted commands

• Set Mode to Idle

Idle

Description

• All systems powered on
• All motors rotating slowly

Transition conditions

1. Transition B : on user request or on watchdog timeout
2. Transition C : on user request: mode set to Take Off or Hover

Accepted commands

• Set Mode to Stop, Take Off or Hover

Take Off

Description

• The MAV stabilise itself to hovering at 10cm above ground (TBC), based on

pressure sensor. The resulting displacement is relative.

Transition conditions

1. Transition F : the system is stable at the required height.
2. Transition D : on user request, watchdog timeout, action timeout or exception.

Accepted commands

• Set Mode to Land

Land

Description

• Bring the MAV to touch down. Downward range sensor is used to estimate

touch down. The maximum velocity downward is XX (TBD).

Transition conditions

1. Transition E : Contact has been detected.

Accepted commands

• None

Hover

Description

• Yaw and altitude are kept constant, roll and pitch kept to zero. The MAV

will certainly be drifting in this mode.

• If available, the horizontal obstacle avoidance is active.
• Downward obstacle avoidance is active
• Data fusion for altitude estimation is active.

Transition conditions

1. Transition H : on user request
2. Transition I : on watchdog timeout
3. Transition M : on user request

Accepted commands

• Set Mode to Land
• Set Mode to Controlled

Controlled -- Ctrlled

Description

• Altitude, yaw and position are controlled as requested
• Downward obstacle avoidance may be activated
• If available, horizontal obstacle avoidance may be activated

Transition conditions

1. Transition G : on user request or on control timeout (i.e. the watchdog

has not been triggered, but no remote commands have been received).

1. Transition J : on watchdog timeout
2. Transition K : on user request

Accepted commands

• Set Mode to Hover
• Set Mode to Land
• Desired altitude, yaw and horizontal controls. In position or rate as

available.

Sink

Description

• Altitude is slowly controlled down (for instance 5cm/s) until touch down
• Any desired control is ignored until the connection is reset (see section
• If available, the horizontal obstacle avoidance is active.

Transition conditions

1. Transition N : on user request after connection reset.
2. Transition O : on user request after connection reset.
3. Transition L : on user request after connection reset, or when the

distance to ground becomes smaller than 10cm (TBC).

Accepted commands

• Set Mode to Hover
• Set Mode to Land
• Set Mode to Controlled