Commit 0f991ae2 authored by maruggv's avatar maruggv
Browse files

work done for integrator buttons

parent 9969d92e
......@@ -145,9 +145,9 @@
#define DRONEX_LAND 9
#define DRONEX_ABORT 10
#define DRONEX_INTEGRATOR_ON 11
#define DRONEX_INTEGRATOR_OFF 12
#define DRONEX_INTEGRATOR_RESET 13
#define DRONEX_INTEGRATOR_ON 15
#define DRONEX_INTEGRATOR_OFF 16
#define DRONEX_INTEGRATOR_RESET 17
// Universal constants
......
......@@ -103,6 +103,10 @@
#define DRONEX_BUTTON_3 13
#define DRONEX_BUTTON_4 14
#define DRONEX_INTEGRATOR_ON 15
#define DRONEX_INTEGRATOR_OFF 16
#define DRONEX_INTEGRATOR_RESET 17
#define DRONEX_STATE_GROUND 0
#define DRONEX_STATE_TAKING_OFF 1
#define DRONEX_STATE_LAND_ON_GROUND 2
......@@ -111,9 +115,8 @@
#define DRONEX_STATE_LAND_ON_MOTHERSHIP 5
#define DRONEX_STATE_ON_MOTHERSHIP 6
#define INTEGRATOR_ON 1
#define INTEGRATOR_OFF 2
#define INTEGRATOR_RESET 3
// These constants define the modes that can be used for controller the Crazyflie 2.0,
// the constants defined here need to be in agreement with those defined in the
......
......@@ -219,17 +219,17 @@ void buttonPressed_abort(){
void buttonPressed_integrator_on(){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Turn ON integrator");
integratorFlag = INTEGRATOR_ON;
integratorFlag = DRONEX_INTEGRATOR_ON;
}
void buttonPressed_integrator_off(){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] Turn OFF integrator");
integratorFlag = INTEGRATOR_OFF;
integratorFlag = DRONEX_INTEGRATOR_OFF;
}
void buttonPressed_integrator_reset(){
ROS_INFO("[DRONEX CONTROLLER-DroneXControllerService] RESET integrator to zero");
integratorFlag = INTEGRATOR_RESET;
integratorFlag = DRONEX_INTEGRATOR_RESET;
}
......@@ -669,7 +669,7 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
//integrator for x,y,z
if(integratorFlag == INTEGRATOR_ON)
if(integratorFlag == DRONEX_INTEGRATOR_ON)
{
for(int i=0; i < 3; i++)
{
......@@ -677,7 +677,7 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
}
}
if(integratorFlag == INTEGRATOR_RESET)
if(integratorFlag == DRONEX_INTEGRATOR_RESET)
{
for(int i=0; i < 3; i++)
{
......@@ -1034,6 +1034,17 @@ void buttonPressedCallback(const std_msgs::Int32& msg)
//ROS_INFO("DRONEX_ABORT");
buttonPressed_abort();
break;
case DRONEX_INTEGRATOR_ON:
buttonPressed_integrator_on();
break;
case DRONEX_INTEGRATOR_OFF:
buttonPressed_integrator_off();
break;
case DRONEX_INTEGRATOR_RESET:
buttonPressed_integrator_reset();
break;
default:
{
// Let the user know that the command was not recognised
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment