README.md

Overview

Controller interfaces (IPos, IVel, IAtt, ...)

These interfaces require a specific set_xxx_command and provide the corresponding structure. ITorque is the lowest level and represents servo_mix. Vehicle will contain a controller of this type.

IController interfaces

Provides generic command structure icommand_t and member icommand_t* command_, which is used to indicate the most recently setted command. The lowest cascade level should implement this interface to provide command_ to the cascade

Default controllers used in cascade (Position_controller, Velocity_controller, ...)

Controllers that handle one cascade level. They inherit from the next lower cascade level. The next lower cascade level is templated, allowing to combine controllers freely. As a command for a specific controller level is set, the pointer command_ will point to the level's own command structure, indicating that this is the currently valid command structure. When updated, the level that was last setted will set the command for its subordinate level and call the subordinate's update function.

Advantages:

• Using the suggested cascade structure, the cascade can be create using any number of default or custom controller levels by using only templates to specifiy which level's to use.
• set_xxx_command functions of lower levels do not have to be implemented (they are exposed through polymorphism)
• There is not enum/list of available controllers, which allows to add custom modules w/o modifiying existing controllers or enums

Dangers

• a controller level could not set IController::command_ in its set_xxx_command function: Upon update, a higher level controller would overwrite the command for this controller

• a controller level could set the command of a lower level controller in its update() even if it has no valid command (forgotten if(TTT::command_ == xxx_command_))

controller_interfaces.hpp

/****************************************
 *       Interfaces
 ****************************************/

class IController
{
public:
	struct icommand_t{};
protected:
	icommand_t* command_;
};

class IPos
{
public:
	struct pos_command_t : public IController::icommand_t {};

	virtual bool set_pos_command(const pos_command_t& pos_command)=0;
	virtual void update()=0;
};

class IVel
{
public:
	struct vel_command_t : public IController::icommand_t {};

	virtual bool set_vel_command(const vel_command_t& vel_command)=0;
	virtual void update()=0;
};

class IAtt
{
public:
	struct att_command_t : public IController::icommand_t {};

	virtual bool set_att_command(const att_command_t& att_command)=0;
	virtual void update()=0;
};

class IRate
{
public:
	struct rate_command_t : public IController::icommand_t {};

	virtual bool set_rate_command(const rate_command_t& rate_command)=0;
	virtual void update()=0;
};

class ITorque
{
public:
	struct torq_command_t : public IController::icommand_t {};

	virtual bool set_torq_command(const torq_command_t& torq_command)=0;
	virtual void update()=0;
};

controllers.hpp

#include <iostream>
#include "controller_interfaces.hpp"
using namespace std;


/****************************************
 *       Cascade Implementation
 ****************************************/


template<class TVel>
class Position_controller : public IPos, public TVel
{
public:
	Position_controller() {};

	bool set_pos_command(const pos_command_t& pos_command){
		pos_command_ = pos_command;
		TVel::command_ = &pos_command_;
		return true;
	};
	
	virtual void update(){
		if(TVel::command_ == &pos_command_)
		{
			IVel::vel_command_t vel_command = calc_vel_command(pos_command_);
			TVel::update_cascade(vel_command);
		}else
		{
			cout << "Position_controller: pass through " << endl;
			TVel::update();
		}
	};

protected:
	void update_cascade(const pos_command_t &pos_command)
	{
		pos_command_ = pos_command;
		IVel::vel_command_t vel_command = calc_vel_command(pos_command_);
		TVel::update_cascade(vel_command);
	}

	IVel::vel_command_t calc_vel_command(const pos_command_t& pos_command)
	{
		IVel::vel_command_t vel_command;
		/*calc velocity command */
		cout << "Position_controller: calculating velocity command " << endl;		
	}

private:
		pos_command_t pos_command_;

};


template<class TAtt>
class Velocity_controller : public IVel, public TAtt
{
public:
	Velocity_controller() {};

	bool set_vel_command(const vel_command_t& vel_command){
		vel_command_ = vel_command;
		TAtt::command_ = &vel_command_;
		return true;
	};

	virtual void update(){
		if(TAtt::command_ == &vel_command_)
		{
			IAtt::att_command_t att_command = calc_att_command(vel_command_);
			TAtt::update_cascade(att_command);
		}else
		{
			cout << "Velocity_controller: pass through " << endl;
			TAtt::update();
		}
	};

protected:
	void update_cascade(const vel_command_t &vel_command)
	{
		vel_command_ = vel_command;
		IAtt::att_command_t att_command = calc_att_command(vel_command_);
		TAtt::update_cascade(att_command);
	}

	IAtt::att_command_t calc_att_command(const vel_command_t& vel_command)
	{
		IAtt::att_command_t att_command;
		/*calc attitude command */
		cout << "Velocity_controller: calculating attitude command " << endl;
		return att_command;
	}

private:
	vel_command_t vel_command_;
};


template<class TRate>
class Attitude_controller : public IAtt, public TRate
{
public:
	Attitude_controller() {};

	bool set_att_command(const att_command_t& att_command){
		att_command_ = att_command;
		TRate::command_ = &att_command_;
		return true;
	};

	virtual void update(){
		if(TRate::command_ == &att_command_)
		{
			IRate::rate_command_t rate_command = calc_rate_command(att_command_);
			TRate::update_cascade(rate_command);
		}else
		{
			cout << "Attitude_controller: pass through " << endl;
			TRate::update();
		}
	};

protected:
	void update_cascade(const att_command_t &att_command)
	{
		att_command_ = att_command;
		IRate::rate_command_t rate_command = calc_rate_command(att_command_);
		TRate::update_cascade(rate_command);
	}

	IRate::rate_command_t calc_rate_command(const att_command_t& att_command)
	{
		IRate::rate_command_t rate_command;
		/*calc rate command */
		cout << "Attitude_controller: calculating rate command " << endl;
		return rate_command;
	}

private:
	att_command_t att_command_;
};


template<class TTorque>
class Rate_controller : public IRate, public TTorque
{
public:
	Rate_controller(){};

	bool set_rate_command(const rate_command_t& rate_command){
		rate_command_ = rate_command_;
		TTorque::command_ = &rate_command_;
		return true;
	};

	virtual void update(){
		if(TTorque::command_ == &rate_command_)
		{
			ITorque::torq_command_t torq_command = calc_torq_command(rate_command_);
			TTorque::update_cascade(torq_command);
		}else
		{
			cout << "Rate_controller: pass through " << endl;
			TTorque::update();
		}
	};

protected:
	void update_cascade(const rate_command_t& rate_command)
	{
		rate_command_ = rate_command;
		ITorque::torq_command_t torq_command = calc_torq_command(rate_command_);
		TTorque::update_cascade(torq_command);
	}

	ITorque::torq_command_t calc_torq_command(const rate_command_t& rate_command)
	{
		ITorque::torq_command_t torq_command;
		/*calc rate command */
		cout << "Rate_controller: calculating torque command " << endl;
		return torq_command;
	}

private:
	rate_command_t rate_command_;
};


class Torque_copter_diag : public ITorque, public IController
{
public:
	Torque_copter_diag(){};

	bool set_torq_command(const torq_command_t& torq_command){
		torq_command_ = torq_command;
		command_ = &torq_command_;
		return true;
	};

	virtual void update(){
		cout << "Torque_controller: setting motors " << endl;
	};

protected:
	void update_cascade(const torq_command_t& torq_command)
	{
		Torque_copter_diag::update();
	}

private: 
	torq_command_t torq_command_;
};

main.cpp

#include "controllers.hpp"
#include <iostream>
using namespace std;



int main(int argc, char** args)
{
    cout << "--- start ---" << endl;

    /* pos_ctrl is a full cascade controller */
    typedef Position_controller<Velocity_controller<Attitude_controller< Rate_controller< Torque_copter_diag> > > > Cascade_controller;
    
    Cascade_controller pos_ctrl;
    IPos& mypos = pos_ctrl;

    cout << "---- setting position command ----" << endl;
    IPos::pos_command_t pos_command;
    pos_ctrl.set_pos_command(pos_command);
    pos_ctrl.update();
    pos_ctrl.update();

    cout << "---- setting velocity command ----" << endl;
    IVel::vel_command_t vel_command;
    pos_ctrl.set_vel_command(vel_command);
    pos_ctrl.update();
    pos_ctrl.update();

    cout << "---- setting attitude command ----" << endl;
    IAtt::att_command_t att_command;
    pos_ctrl.set_att_command(att_command);
    pos_ctrl.update();
    pos_ctrl.update();

    /* att_ctrl is an cascade controller where Attitude_controller is the heighest level */
    Attitude_controller<Rate_controller<Torque_copter_diag> > att_ctrl;

    cout << "--- finished ---" << endl;

    return 0;
}