Commit 8562e694 authored by Ian McInerney's avatar Ian McInerney

Merged refactored groundstation code

parents 8702c155 ff3db774
......@@ -136,7 +136,6 @@ static void commanderXYZ_WatchdogTick(void) {
// If the timeout occurs, drop the quad to the ground
if ((tickNow - crtpPosCache.timestamp) > COMMANDER_XYZ_WDT_TIMEOUT_SHUTDOWN) {
watchdogTriggered = 1;
// DEBUG_PRINT("Watchdog");
}
}
......@@ -189,9 +188,6 @@ uint32_t commanderXYZ_GetInactivityTime(void) {
}
void commanderXYZ_GetSetpoint(setpoint_t *setpoint, const state_t *state, uint8_t *resetControllers) {
// Check the watchdog
commanderXYZ_WatchdogTick();
// Tell the controller everything should be in absolute mode
setpoint->mode.x = modeAbs;
setpoint->mode.y = modeAbs;
......@@ -221,6 +217,9 @@ void commanderXYZ_GetSetpoint(setpoint_t *setpoint, const state_t *state, uint8_
}
void commanderXYZ_GetPosition(state_t *state, const uint32_t tick) {
// Check the watchdog and feed it
commanderXYZ_WatchdogTick();
// Save the previous position values
static float oldX = 0;
static float oldY = 0;
......
# Compiler to use
g++ = g++ -Wall
all: eris_vrpn
# Where are things located
INC_DIR=inc
SRC_DIR=src
eris_vrpn: eris_vrpn.cpp vrpn.cpp CCrazyflie.cpp CCrazyRadio.cpp CCRTPPacket.cpp CTOC.cpp multicast.cpp quadcopterData.cpp callbacks.cpp userInput.cpp userOutput.cpp logData.cpp controllerHelperFunctions.cpp
$(g++) -Iincludes -g -O0 -o eris_vrpn eris_vrpn.cpp vrpn.cpp CCrazyflie.cpp CCrazyRadio.cpp CCRTPPacket.cpp CTOC.cpp multicast.cpp quadcopterData.cpp callbacks.cpp userInput.cpp userOutput.cpp logData.cpp controllerHelperFunctions.cpp -lquat -lvrpn -lpthread -lusb-1.0
# What the final executable is called
EXE_NAME=crazyflieGroundStation
# All the libraries needed
LIBS=-lquat -lvrpn -lpthread -lusb-1.0
# The compile flags
FLAGS=-g -o0 -Wall
# Determine what source files exist
STRUCTURE := $(shell find $(SRC_DIR) -type d)
ALL_FILES := $(addsuffix /*,$(STRUCTURE))
ALL_FILES := $(wildcard $(ALL_FILES))
SRC_FILES := $(filter %.cpp,$(ALL_FILES))
all:
$(g++) -I$(INC_DIR) $(FLAGS) -o $(EXE_NAME) $(SRC_FILES) $(LIBS)
clean:
rm -f *.o eris_vrpn
rm -f *.o $(EXE_NAME)
#include <iostream>
#include "quadcopterData.h"
using namespace std;
/*
* This function will go through and perform a stepped takeoff method.
*/
void takeoffRoutine(int crazyflieNum,
CONTROLLER_DATA_t *controllerData,
CCrazyflie *cflieCopter,
TAKEOFF_PROFILE_t *takeoff ) {
// The current time
double time = cflieCopter->currentTime();
// Check if the next step uses time or altitude
float stepType = takeoff->profile[ takeoff->curStep*3 + 1];
if ( stepType == step_time ) {
// The next step is time based
if ( abs( time - takeoff->lastStepTime ) > takeoff->profile[ takeoff->curStep*3 + 2 ] ) {
// The setpoint has elapsed, move to the next one
takeoff->curStep++;
if (takeoff->curStep > takeoff->numTakeoffSteps) {
takeoff->curStep = takeoff->numTakeoffSteps;
}
}
} else if ( stepType == step_height ) {
// The next step is height based
if ( controllerData->zPosition < takeoff->profile[ takeoff->curStep*3 + 2 ]) {
// The setpoint has passed, move to the next one
takeoff->curStep++;
if (takeoff->curStep > takeoff->numTakeoffSteps) {
takeoff->curStep = takeoff->numTakeoffSteps;
}
}
} else if ( takeoff->profile[ takeoff->curStep*3 + 1 ] == step_final ) {
// Renable the Z controller
cflieCopter->enableThrustController();
// If it is the final step, then put it into hover mode
cflieCopter->m_enumFlightMode = HOVER_MODE;
}
// Set the base thrust
controllerData->baseThrust = takeoff->profile[ takeoff->curStep*3 + 0 ];
}
\ No newline at end of file
#ifndef BRIDGE_H_
#define BRIDGE_H_
#include "structures.h"
#include <vrpn_Tracker.h>
class Bridge {
private:
// Location of the two endpoints
lateralPosition endpoint1;
lateralPosition endpoint2;
bool endpoint1Valid;
bool endpoint2Valid;
public:
Bridge();
~Bridge();
/**
* This function will find the point located between the two endpoints.
* If the second endpoint is missing, then the 1st endpoint is returned
*
* @param pos Pointer to a lateral position struct
* @param loc Location between the two endpoints to find (loc must be in [0,1])
* @return True if a location is valid, false if no location is able to found
*/
bool getInterpolatedPosition(lateralPosition *pos, int loc);
/**
* Callbacks for the VRPN trackers on either end of the bridge
*/
static void VRPN_CALLBACK end1Callback(void* inst, const vrpn_TRACKERCB t);
static void VRPN_CALLBACK end2Callback(void* inst, const vrpn_TRACKERCB t);
};
#endif
\ No newline at end of file
......@@ -45,16 +45,12 @@
// Private
#include "CCrazyRadio.h"
#include "CTOC.h"
#include "structures.h"
#define PID_BUFFER_SIZE 12
#include "vrpn_Connection.h"
#include "vrpn_Tracker.h"
struct PID_ControllerParams {
float Kp;
float Ki;
float Kd;
float iLimit;
uint8_t controllerID;
};
#define PID_BUFFER_SIZE 12
// Numbers specified here must match the quadcopter ID numbers
enum PID_Controller_IDs {
......@@ -92,7 +88,6 @@ enum FlightMode{
BRIDGE_TAKEOFF_MODE,
IDLE_MODE
};
// Backward declaration of the CCrazyflie class to make the compiler happy
class CCrazyRadio;
......@@ -128,18 +123,28 @@ class CCrazyflie {
/*! \brief The current desired control set point (position/yaw to
reach) */
// Structures to hold the current position and orientation of the quad
lateralPosition m_currentCameraPosition;
angularPosition m_currentCameraAngle;
quaternionPosition m_currentCameraQuat;
// Structures to hold the desired setpoints
lateralPosition m_desiredPosition;
float m_desiredYaw;
uint16_t m_baseThrust;
// An array to hold the takeoff profile
takeoffProfileStep *takeoffProfile;
int numTakeoffSteps;
// Variables needed for the takeoff routine
int currentStep;
double lastStepTime;
float m_startTime;
float xPosition;
float yPosition;
float zPosition;
float yawPosition;
float xSetpoint;
float ySetpoint;
float zSetpoint;
float yawSetpoint;
uint16_t baseThrust;
uint8_t resetController;
bool m_resetController;
// File stream for the console output
std::ofstream file_console;
......@@ -149,7 +154,7 @@ class CCrazyflie {
// Various things for setting the PID parameters of the quadcopter
uint8_t m_bufferEnd;
struct PID_ControllerParams bufferedParams[PID_BUFFER_SIZE];
PID_ControllerParams bufferedParams[PID_BUFFER_SIZE];
// Control related parameters
/*! \brief Maximum absolute value for the roll that will be sent to
......@@ -167,7 +172,6 @@ class CCrazyflie {
int m_nMinThrust;
double m_dSendSetpointPeriod;
double m_dSetpointLastSent;
bool m_bSendsSetpoints;
CTOC *m_tocParameters;
CTOC *m_tocLogs;
enum State m_enumState;
......@@ -175,6 +179,10 @@ class CCrazyflie {
// Function to hold the received logging packets in
std::list<CCRTPPacket*> m_lstLoggingPackets;
// Flags to say whether to send packets
bool m_sendSetpoints;
bool m_sendPosition;
// Functions
bool readTOCParameters();
bool readTOCLogs();
......@@ -193,6 +201,14 @@ class CCrazyflie {
\param sThrust The desired thrust value.
\return Boolean value denoting whether or not the command could be sent successfully. */
/**
* Modify if the thrust controller is active or not.
*
* @param enabled True to enable controller, false to disable
* @return True if packet received successfully
*/
bool modifyThrustController(bool enabled);
public:
/*! \brief Constructor for the copter convenience class
......@@ -210,6 +226,15 @@ class CCrazyflie {
CCrazyRadio radio instance given in the constructor). */
~CCrazyflie();
/*
* Callback called whenever there is a new VRPN position packet.
* Note: This function is static in the class.
*
* @param crazyflie Pointer to the crazyflie object (cast as a void *)
* @param t The tracker results
*/
static void VRPN_CALLBACK vrpnTrackerCallback(void *crazyflie, const vrpn_TRACKERCB t);
/**
* Query the type of controller that the Crazyflie is using
*
......@@ -233,59 +258,81 @@ class CCrazyflie {
*/
bool modifyThrustController(bool enabled);
/*! \brief Set the thrust control set point
The thrust value that will be sent to the internal copter
controller as a set point.
\param nThrust The thrust value to send (> 10000) */
void setThrust(int nThrust);
/*! \brief Returns the current thrust
\return The current thrust value as reported by the copter */
int thrust();
void setCameraPosition(lateralPosition cameraPosition);
void setCameraAngle(angularPosition cameraAngle);
void setCameraQuat(quaternionPosition cameraQuat);
/*! \brief Set the roll control set point
The roll value that will be sent to the internal copter
controller as a set point.
\param fRoll The roll value to send */
void setRoll(float fRoll);
/*! \brief Returns the current roll
Roll values are in degree, ranging from -180.0deg to 180.0deg.
\return The current roll value as reported by the copter */
float roll();
/*! \brief Set the pitch control set point
/*
* These functions will return the current quadcopter
* lateral position as reported by the camera system.
*/
float getCameraX();
float getCameraY();
float getCameraZ();
The pitch value that will be sent to the internal copter
controller as a set point.
/*
* These functions will return the current quadcopter
* angular position as reported by the quadcopter sensors.
*/
float getQuadRoll();
float getQuadPitch();
float getQuadYaw();
\param fPitch The pitch value to send */
void setPitch(float fPitch);
/*! \brief Returns the current pitch
/*
* These functions will return the current quadcopter
* angular position as reported by the camera system.
*/
float getCameraRoll();
float getCameraPitch();
float getCameraYaw();
Pitch values are in degree, ranging from -180.0deg to 180.0deg.
/*
* These functions will return the current controller setpoints.
*/
float getSetpointX();
float getSetpointY();
float getSetpointZ();
float getSetpointYaw();
uint16_t getBaseThrust();
\return The current pitch value as reported by the copter */
float pitch();
/**
* These functions will set the setpoints of the quadcopter
*/
void setSetpointX(float setpoint);
void setSetpointY(float setpoint);
void setSetpointZ(float setpoint);
void setSetpointYaw(float setpoint);
void setBaseThrust(uint16_t thrust);
/*! \brief Set the yaw control set point
/**
* Enable or disable the thrust controller.
*/
bool enableThrustController();
bool disableThrustController();
The yaw value that will be sent to the internal copter
controller as a set point.
/**
* Reset the controller on the quadcopter
*/
void resetController();
\param fYaw The yaw value to send */
void setYaw(float fYaw);
/*! \brief Returns the current yaw
/**
* This function will set the flight mode of the Crazyflie
*/
void setFlightMode( enum FlightMode mode);
Yaw values are in degree, ranging from -180.0deg to 180.0deg.
/*
* This function will set the takeoff profile to use.
*
* @param profile Pointer to the takeoff profile to use
* @param numSteps The number of steps in the takeoff profile
*
*/
void setTakeoffProfile( takeoffProfileStep *profile, int numSteps );
\return The current yaw value as reported by the copter */
float yaw();
/*
* This function will cycle through the takeoff profile for the Crazyflie.
*/
void takeoffCycle();
/*! \brief Manages internal calculation operations
......@@ -317,48 +364,26 @@ class CCrazyflie {
copter communication. */
bool isInitialized();
/*! \brief Set whether setpoints are currently sent while cycle()
While performing the cycle() function, the currently set setpoint
is sent to the copter regularly. If this is not the case, dummy
packets are sent. Using this mechanism, you can effectively switch
off sending new commands to the copter.
Default value: `false`
\param bSendSetpoints When set to `true`, the current setpoint is
sent while cycle(). Otherwise, not. */
void setSendSetpoints(bool bSendSetpoints);
/*! \brief Whether or not setpoints are currently sent to the copter
\return Boolean value denoting whether or not the current setpoint
is sent to the copter while performing cycle(). */
bool sendsSetpoints();
uint8_t setPID_Constants( struct PID_ControllerParams params );
bool sendPID_Constants( struct PID_ControllerParams params );
uint8_t setPID_Constants( PID_ControllerParams params );
bool sendPID_Constants( PID_ControllerParams params );
/*
void setXYZYawPosition(float xPosition, float yPosition, float zPosition,
float yawPosition);
void setXYZYawSetpoint(float xSetpoint, float ySetpoint, float zSetpoint,
float yawSetpoint, uint16_t baseThrust,
uint8_t resetController);
*/
/*
* Send setpoints for the quadcopter
*/
bool sendXYZYawSetpoint(float xSetpoint, float ySetpoint, float zSetpoint,
float yawSetpoint, uint16_t baseThrust,
uint8_t resetController);
bool sendPositionSetpoint();
/*
* Send the current position to the quad
*/
bool sendXYZYawPosition(float xPosition, float yPosition, float zPosition,
float yawPosition);
bool sendCameraData();
void displayLoggingBlocksInitialized();
......
......@@ -7,32 +7,12 @@
#include <math.h>
#include <quat.h>
enum bridge_landing_state {
NOT_CLOSE, // If the quadcopter is not even close
SAME_XY, // If the quadcopter is in the right X & Y position
LANDED // If the quadcopter has landed on the bridge
};
// Struct for the bridge position
struct bridge_Position {
float x;
float y;
float z;
float isSet;
enum bridge_landing_state landingState;
};
// Bridge position
extern struct bridge_Position bridge1;
extern struct bridge_Position bridge2;
// Function prototypes for the VRPN callback routines
void VRPN_CALLBACK handle_hand(void*, const vrpn_TRACKERCB t);
void VRPN_CALLBACK callbackBridge1(void*, const vrpn_TRACKERCB t);
void VRPN_CALLBACK callbackBridge2(void*, const vrpn_TRACKERCB t);
// Function prototypes for the trackable callbacks for the quadcopters
void VRPN_CALLBACK handle_pos(void*, const vrpn_TRACKERCB t);
//void VRPN_CALLBACK handle_pos(void*, const vrpn_TRACKERCB t);
void VRPN_CALLBACK handle_Crazyflie2(void*, const vrpn_TRACKERCB t);
void VRPN_CALLBACK handle_Crazyflie3(void*, const vrpn_TRACKERCB t);
void VRPN_CALLBACK handle_Crazyflie4(void*, const vrpn_TRACKERCB t);
......
#ifndef ERIS_VRPN
#define ERIS_VRPN
#ifndef CRAZYFLIE_GROUND_STATION_H_
#define CRAZYFLIE_GROUND_STATION_H_
#include <iostream>
#include <iomanip>
......@@ -14,8 +14,12 @@
#include <quat.h>
#include "Bridge.h"
extern uint8_t exitProgram;
extern Bridge *bridge;
// The lowest battery level to fly with
#define LOW_BATTERY_LEVEL 3.2
......
......@@ -35,11 +35,11 @@
crazyflie_info[i].controllerData.rollPosition = Q_RAD_TO_DEG(euler[2]); \
})
#define SET_X(quadNumber, setpoint) ( crazyflie_info[quadNumber-1].controllerData.xSetpoint = setpoint )
#define SET_Y(quadNumber, setpoint) ( crazyflie_info[quadNumber-1].controllerData.ySetpoint = setpoint )
#define SET_Z(quadNumber, setpoint) ( crazyflie_info[quadNumber-1].controllerData.zSetpoint = setpoint )
#define SET_YAW(quadNumber, setpoint) ( crazyflie_info[quadNumber-1].controllerData.yawSetpoint = setpoint )
#define SET_X(quadNumber, setpoint) ( crazyflie_info[quadNumber-1].cflieCopter->setSetpointX( setpoint ) )
#define SET_Y(quadNumber, setpoint) ( crazyflie_info[quadNumber-1].cflieCopter->setSetpointY( setpoint ) )
#define SET_Z(quadNumber, setpoint) ( crazyflie_info[quadNumber-1].cflieCopter->setSetpointZ( setpoint ) )
#define SET_YAW(quadNumber, setpoint) ( crazyflie_info[quadNumber-1].cflieCopter->setSetpointYaw( setpoint ) )
#define SET_FLIGHTMODE(quadNumber, mode) ( crazyflie_info[quadNumber-1].cflieCopter->m_enumFlightMode = mode)
#define SET_FLIGHTMODE(quadNumber, mode) ( crazyflie_info[quadNumber-1].cflieCopter->setFlightMode( mode ) )
#endif
\ No newline at end of file
......@@ -21,19 +21,6 @@
#define Ki 0
#define Kd 750
enum takeoffStepType {
step_final = 0,
step_time,
step_height
};
typedef struct TAKEOFF_PROFILE {
uint8_t curStep;
double lastStepTime;
uint8_t numTakeoffSteps;
float* profile;
} TAKEOFF_PROFILE_t;
typedef struct CONTROLLER_DATA {
double loopTimeStartPrevious; // The start time of the last loop to run
double loopTime; // The time taken by running the last loop
......@@ -62,7 +49,6 @@ typedef struct CONTROLLER_DATA {
typedef struct QUADCOPTERS {
// VRPN variables
const char* vrpn_trackableName; // The VPRN trackable name
void VRPN_CALLBACK (*vrpn_callback)(void*, const vrpn_TRACKERCB t); // The VRPN callback function pointer
vrpn_Tracker_Remote *vrpn_tracker; // The VRPN tracker object
CCrazyflie *cflieCopter; // The crazyflie copter object
......@@ -74,7 +60,6 @@ typedef struct QUADCOPTERS {
const char* baselogfileName; // The base name for the log file
struct PID_ControllerParams pidParams[9]; // The PID parameters for all 9 internal controllers
TAKEOFF_PROFILE_t takeoff;
} QUADCOPTERS_t;
// This structure contains the information about each radio in use
......@@ -111,6 +96,8 @@ extern RADIOS_t radios[];
// Array to hold the hand data
extern HAND_t hand;
extern takeoffProfileStep quad1_takeoffSteps[];
// Array to hold the offsets for mirror flight
extern float mirrorOffsets[][2];
......
#ifndef STRUCTURES_H_
#define STRUCTURES_H_
#include <stdint.h>
enum takeoffStepType {
step_final = 0,
step_time,
step_height
};
struct PID_ControllerParams {
float Kp;
float Ki;
float Kd;
float iLimit;
uint8_t controllerID;
};
typedef struct angularPosition {
float roll;
float pitch;
float yaw;
} angularPosition;
typedef struct lateralPosition {
float x;
float y;
float z;
} lateralPosition;
typedef struct quaternionPosition {
float q0;
float q1;
float q2;
float q3;
} quaternionPosition;
typedef struct takeoffProfileStep {
uint16_t thrust;
enum takeoffStepType type;
double condition;
} takeoffProfileStep;
#endif
\ No newline at end of file
#ifndef VRPN_H
#define VRPN_H
#ifndef VRPN_H_
#define VRPN_H_
#include <string>
#include "vrpn_Connection.h"
#include "vrpn_Tracker.h"
#include "eris_vrpn.h"
#include "crazyflieGroundStation.h"
#include <stdio.h>
#include <sys/select.h>
#include <termios.h> /* POSIX terminal control definitions */
......
#ifndef CONTROLLERHELPERFUNCTIONS_H_
#define CONTROLLERHELPERFUNCTIONS_H_
void takeoffRoutine(int crazyflieNum,
CONTROLLER_DATA_t *controllerData,
CCrazyflie *cflieCopter,
TAKEOFF_PROFILE_t *takeoff );
#endif
\ No newline at end of file
/*
multicast.c
The following program sends or receives multicast packets. If invoked
with one argument, it sends a packet containing the current time to an
arbitrarily chosen multicast group and UDP port. If invoked with no
arguments, it receives and prints these packets. Start it as a sender on
just one host and as a receiver on all the other hosts
*/
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <time.h>
#include <stdio.h>
</