void matlabNavModeCallback(const std_msgs::Bool::ConstPtr & message)
{
motor1 = 0;
motor2 = 0;
servo1 = 0;
servo2 = 0;
bool result;
if (message->data) {
result = reachNavState(SB_NAV_RAW, 0.5); // Navigation raw mode
desired_nav_mode = 1;
if (result) {
// reachNavState not successful -> send error state 8 to matlab
geometry_msgs::Quaternion control_mode;
control_mode.x = 8;
control_mode.y = 0;
control_mode.z = 0;
control_mode.w = 0;
control_mode_pub.publish(control_mode);
}
} else {
reachNavState(SB_NAV_STOP, 0.5); // Navigation stop mode
desired_nav_mode = 0;
}
}
bool CoaxSimpleControl::setControlMode(coax_simple_control::SetControlMode::Request &req, coax_simple_control::SetControlMode::Response &out) {
out.result = 0;
switch (req.mode)
{
case 1:
if (CONTROL_MODE != CONTROL_LANDED){
ROS_INFO("Start can only be executed from mode CONTROL_LANDED");
out.result = -1;
break;
}
if (battery_voltage < 10.5) {
ROS_INFO("Battery Low!!! (%f V) Start denied",battery_voltage);
LOW_POWER_DETECTED = true;
out.result = -1;
break;
}
if (coax_nav_mode != SB_NAV_RAW) {
if (coax_nav_mode != SB_NAV_STOP) {
reachNavState(SB_NAV_STOP, 0.5);
ros::Duration(0.5).sleep(); // make sure CoaX is in SB_NAV_STOP mode
}
reachNavState(SB_NAV_RAW, 0.5);
}
// switch to start procedure
INIT_DESIRE = false;
init_count = 0;
rotor_ready_count = 0;
CONTROL_MODE = CONTROL_START;
motor1_des = 0;
motor2_des = 0;
servo1_des = 0.0;
ROS_INFO("servo1_des %f", servo1_des);
servo2_des = 0;
break;
case 9:
motor1_des = 0;
motor2_des = 0;
servo1_des = 0;
servo2_des = 0;
//roll_trim = 0;
//pitch_trim = 0;
reachNavState(SB_NAV_STOP, 0.5);
rotor_ready_count = -1;
CONTROL_MODE = CONTROL_LANDED;
break;
default:
ROS_INFO("Non existent control mode request!");
out.result = -1;
}
return true;
}
bool setControlMode(coax_interface::SetControlMode::Request &req, coax_interface::SetControlMode::Response &out)
{
if (req.mode !=8) {
geometry_msgs::Quaternion control_mode;
control_mode.x = req.mode;
control_mode.y = 0;
control_mode.z = 0;
control_mode.w = 0;
control_mode_pub.publish(control_mode);
if (req.mode == 9) {
reachNavState(SB_NAV_STOP,0.5);
desired_nav_mode = 0;
}
}else {
ROS_INFO("Cannot manually call control mode 8");
}
/*
char * key;
key = req.mode;
if (!strcmp(key, "s")) {
control_mode.x = 1;
control_mode_pub.publish(control_mode);
} else if (!strcmp(key, "h")) {
control_mode.x = 2;
control_mode_pub.publish(control_mode);
} else if (!strcmp(key, "g")) {
control_mode.x = 3;
control_mode_pub.publish(control_mode);
} else if (!strcmp(key, "t")) {
control_mode.x = 4;
control_mode_pub.publish(control_mode);
} else if (!strcmp(key, "l")) {
control_mode.x = 5;
control_mode_pub.publish(control_mode);
} else if (!strcmp(key, "q")) {
control_mode.x = 9;
control_mode_pub.publish(control_mode);
} else {
printf("Usage: [s]=start, [h]=hover, [g]=go to position, [t]= trajectory following, [l]=land, [q]=quit \n");
}
*/
out.result = 1;
return true;
}
bool OnboardControl::setControlMode(coax_vision::SetControlMode::Request &req, coax_vision::SetControlMode::Response &out) {
out.result = 0;
switch (req.mode)
{
case 1:
if (CONTROL_MODE != CONTROL_LANDED){
ROS_INFO("Start can only be executed from mode CONTROL_LANDED");
out.result = -1;
break;
}
if (coax_nav_mode != SB_NAV_RAW) {
if (coax_nav_mode != SB_NAV_STOP) {
reachNavState(SB_NAV_STOP, 0.5);
ros::Duration(0.5).sleep(); // make sure CoaX is in SB_NAV_STOP mode
}
reachNavState(SB_NAV_RAW, 0.5);
}
// switch to start procedure
INIT_DESIRE = false;
init_count = 0;
rotor_ready_count = 0;
CONTROL_MODE = CONTROL_START;
stage = 0;
onboard_stage.stage = stage;
onboard_stage_pub.publish(onboard_stage);
break;
case 2:
if(stage==-1||stage==2){
stage = 1;
onboard_stage.stage=stage;
onboard_stage_pub.publish(onboard_stage);
break;
}
out.result = -1;
ROS_WARN("check the state, not ready for mode HOVER");
break;
case 3:
if(stage!=1){
out.result = -1;
ROS_WARN("check the state,not ready for mode LOCALIZE");
break;
}
stage = 2;
break;
case 4:
if(stage ==1||stage ==2){
stage = 3;
ROS_INFO("LANDING...");
break;}
out.result = -1;
ROS_WARN("check the state, not ready for LANDING");
break;
default:
ROS_INFO("Non existent control mode request!");
out.result = -1;
}
return true;
}
bool CoaxVisionControl::setControlMode(coax_vision::SetControlMode::Request &req, coax_vision::SetControlMode::Response &out) {
out.result = 0;
switch (req.mode)
{
case 1:
if (CONTROL_MODE != CONTROL_LANDED) {
ROS_INFO("Start can only be executed from mode CONTROL_LANDED");
out.result = -1;
break;
}
if (coax_nav_mode != SB_NAV_RAW) {
if (coax_nav_mode != SB_NAV_STOP) {
reachNavState(SB_NAV_STOP, 0.5);
ros::Duration(0.5).sleep(); // make sure CoaX is in SB_NAV_STOP mode
}
reachNavState(SB_NAV_RAW, 0.5);
}
// switch to start procedure
INIT_DESIRE = false;
init_count = 0;
rotor_ready_count = 0;
CONTROL_MODE = CONTROL_START;
motor1_des = 0;
motor2_des = 0;
servo1_des = 0;
servo2_des = 0;
stage = 0;
break;
case 2:
if(stage==-1||stage==2) {
stage = 1;
break;
}
out.result = -1;
ROS_WARN("check the state, not ready for mode HOVER");
break;
case 3:
if(stage!=1) {
out.result = -1;
ROS_WARN("check the state,not ready for mode LOCALIZE");
break;
}
stage = 2;
initial_pos=0;
break;
case 9:
motor1_des = 0;
motor2_des = 0;
servo1_des = 0;
servo2_des = 0;
roll_trim = 0;
pitch_trim = 0;
reachNavState(SB_NAV_STOP, 0.5);
rotor_ready_count = -1;
CONTROL_MODE = CONTROL_LANDED;
break;
default:
ROS_INFO("Non existent control mode request!");
out.result = -1;
}
return true;
}
