void MMS_Handle()
{
switch(currentState)
{
case ON_GROUND_NO_HOME:
//ROS_INFO("MMS_CURRENT_STATE:ON_GROUND_NO_HOME");
/*outputRef_.Latitude = 0;
outputRef_.Longitude = 0;
outputRef_.AltitudeRelative = -50; // mm @ frequency: verical speed
outputRef_.Yawangle = 0;
outputRef_.Mode = 100;
get_current_position();
outputRef_.AltitudeRelative = outputRef_.AltitudeRelative-5000;
pubToReference_.publish(outputRef_);
ROS_INFO("PUSHING THE DRONE DOWN ...");*/
if (ARMED)
{
set_events_false();
ARMED = true;
get_current_position();
outputRef_.AltitudeRelative = outputRef_.AltitudeRelative-5000;
pubToReference_.publish(outputRef_);
ROS_INFO("MMS->NAV: REFERENCE = ON_GROUND");
outputArm_.arm_disarm = false;
outputArm_.new_arm_disarm = true;
pubToArm_.publish(outputArm_);
ROS_INFO("MMS->APM: DISARMING");
currentState = ON_GROUND_NO_HOME;
ROS_INFO("MMS_CURRENT_STATE: ON_GROUND_NO_HOME");
break;
}
if (SET_HOME)
{
set_events_false();
currentState = SETTING_HOME;
ROS_INFO("MMS_CURRENT_STATE: SETTING_HOME");
break;
}
//}
break;
case SETTING_HOME:
//ROS_INFO("MMS_CURRENT_STATE:SETTING_HOME");
/*outputRef_.Latitude = 0;
outputRef_.Longitude = 0;
outputRef_.AltitudeRelative = -50; // mm @ frequency: verical speed
outputRef_.Yawangle = 0;
outputRef_.Mode = 100;
get_current_position();
outputRef_.AltitudeRelative = outputRef_.AltitudeRelative-5000;
pubToReference_.publish(outputRef_);
ROS_INFO("PUSHING THE DRONE DOWN ...");*/
Home.lat = inputPos_.lat;
Home.lon = inputPos_.lon;
Home.alt = inputPos_.alt;
Home.relative_alt = inputPos_.relative_alt;
Home.hdg = inputPos_.hdg;
ROS_INFO("HOME POSITION: Lat, %d, Lon, %d, AltRel, %d, Yaw, %f",Home.lat, Home.lon, Home.relative_alt, Home.hdg);
//ROS_INFO("Home AMSL, %d, rel, %d", Home.alt, Home.relative_alt);
currentState = ON_GROUND_DISARMED;
ROS_INFO("MMS_CURRENT_STATE: ON_GROUND_DISARMED");
break;
case ON_GROUND_DISARMED:
// ROS_INFO("MMS_CURRENT_STATE:ON_GROUND_DISARMED");
/*outputRef_.Latitude = 0;
outputRef_.Longitude = 0;
outputRef_.AltitudeRelative = -50; // mm @ frequency: verical speed
outputRef_.Yawangle = 0;
outputRef_.Mode = 100;
get_current_position();
outputRef_.AltitudeRelative = outputRef_.AltitudeRelative-5000;
pubToReference_.publish(outputRef_);
ROS_INFO("PUSHING THE DRONE DOWN ...");*/
if (TAKEOFF)
{
set_events_false();
TAKEOFF = true;
ARMED = false;
/*outputRef_.Latitude = 0;
outputRef_.Longitude = 0;
outputRef_.AltitudeRelative = -50; // mm @ frequency: verical speed
outputRef_.Yawangle = 0;
outputRef_.Mode = 100;*/
get_current_position();
outputRef_.AltitudeRelative = outputRef_.AltitudeRelative-5000;
pubToReference_.publish(outputRef_);
ROS_INFO("MMS->NAV: REFERENCE = ON_GROUND");
outputArm_.arm_disarm = true;
outputArm_.new_arm_disarm = true;
pubToArm_.publish(outputArm_);
ROS_INFO("MMS->APM: ARMING");
counter_ = 0; //start timing to rearm
currentState = ARMING;
ROS_INFO("MMS_CURRENT_STATE: ARMING");
break;
}
if (SET_HOME)
{
set_events_false();
outputAckCmd_.mission_item_reached = true;
outputAckCmd_.mav_mission_accepted = false;
outputAckCmd_.mav_cmd_id = 300;
pubToAckCmd_.publish(outputAckCmd_);
ROS_INFO("MMS->GCS: MISSION_ACCEPTED");
currentState = SETTING_HOME;
ROS_INFO("MMS_CURRENT_STATE: SETTING_HOME");
break;
}
break;
case ARMING:
/* get_current_position();
outputRef_.AltitudeRelative = outputRef_.AltitudeRelative-5000;
pubToReference_.publish(outputRef_);
ROS_INFO("PUSHING THE DRONE DOWN ...");*/
if (LAND)
{
set_events_false();
ARMED = true;
get_current_position();
outputRef_.AltitudeRelative = outputRef_.AltitudeRelative-5000;
pubToReference_.publish(outputRef_);
ROS_INFO("MMS->NAV: REFERENCE = ON_GROUND");
outputArm_.arm_disarm = false;
outputArm_.new_arm_disarm = true;
pubToArm_.publish(outputArm_);
ROS_INFO("MMS->APM: DISARMING");
currentState = DISARMING;
ROS_INFO("MMS_CURRENT_STATE: DISARMING");
break;
}
if (ARMED)
{
ROS_INFO("!!! DRONE ARMED !!!");
set_events_false();
TAKEOFF = true; // to complete automatic TO
currentState = ON_GROUND_ARMED;
ROS_INFO("MMS_CURRENT_STATE: ON_GROUND_ARMED");
break;
}
if (counter_>=50) //5 seconds
{
currentState = ON_GROUND_DISARMED;
ROS_INFO("MMS_ARMING FAILED. BACK TO STATE: ON_GROUND_DISARMED");
}
break;
case DISARMING:
/*get_current_position();
outputRef_.AltitudeRelative = outputRef_.AltitudeRelative-5000;
pubToReference_.publish(outputRef_);
ROS_INFO("PUSHING THE DRONE DOWN ...");*/
if (ARMED == false)
{
ROS_INFO("DRONE DISARMED!");
set_events_false();
currentState = ON_GROUND_DISARMED;
ROS_INFO("MMS_CURRENT_STATE: ON_GROUND_DISARMED");
}
break;
case ON_GROUND_ARMED:
/*get_current_position();
outputRef_.AltitudeRelative = outputRef_.AltitudeRelative-5000;
pubToReference_.publish(outputRef_);
ROS_INFO("PUSHING THE DRONE DOWN ...");*/
//ROS_INFO("MMS_CURRENT_STATE:ON_GROUND_ARMED");
if (LAND)
{
set_events_false();
ARMED = true;
outputArm_.arm_disarm = false;
outputArm_.new_arm_disarm = true;
pubToArm_.publish(outputArm_);
ROS_INFO("MMS->APM: DISARMING");
currentState = DISARMING;
ROS_INFO("MMS_CURRENT_STATE: DISARMING");
break;
}
if (TAKEOFF)
{
set_events_false();
outputAckCmd_.mission_item_reached = false;
outputAckCmd_.mav_mission_accepted = true;
outputAckCmd_.mav_cmd_id = 22;
pubToAckCmd_.publish(outputAckCmd_);
ROS_INFO("MMS->GCS: MISSION_ACCEPTED");
currentState = ON_GROUND_READY_TO_TAKEOFF;
ROS_INFO("MMS_CURRENT_STATE: READY_TO_TAKEOFF");
break;
}
break;
case ON_GROUND_READY_TO_TAKEOFF:
// ROS_INFO("MMS_CURRENT_STATE:ON_GROUND_READY_TO_TAKEOFF");
/*get_current_position();
outputRef_.AltitudeRelative = outputRef_.AltitudeRelative-5000;
pubToReference_.publish(outputRef_);
ROS_INFO("PUSHING THE DRONE DOWN ...");*/
if (LAND)
{
set_events_false();
ARMED = true;
outputAckCmd_.mission_item_reached = false;
outputAckCmd_.mav_mission_accepted = true;
outputAckCmd_.mav_cmd_id = 21;
pubToAckCmd_.publish(outputAckCmd_);
ROS_INFO("MMS->GCS: MISSION_ACCEPTED");
outputArm_.arm_disarm = false;
outputArm_.new_arm_disarm = true;
pubToArm_.publish(outputArm_);
ROS_INFO("MMS->APM: DISARMING");
currentState = DISARMING;
ROS_INFO("MMS_CURRENT_STATE: DISARMING");
break;
}
if (MISSION_START)
{
set_events_false();
/*outputRef_.Latitude = 0;
outputRef_.Longitude = 0;
outputRef_.AltitudeRelative = 1000;
outputRef_.Yawangle = 0;
outputRef_.Mode = 100;*/
get_current_position();
outputRef_.AltitudeRelative = outputRef_.AltitudeRelative+Dh_TO;
pubToReference_.publish(outputRef_);
ROS_INFO("MMS->NAV: REFERENCE = TAKEOFF");
currentState = PERFORMING_TAKEOFF;
ROS_INFO("MMS_CURRENT_STATE:PERFORMING_TAKEOFF");
break;
}
break;
case PERFORMING_TAKEOFF:
//ROS_INFO("MMS_CURRENT_STATE:PERFORMING_TAKEOFF");
distance(); // error_to_t,
if (LAND)
{
set_events_false();
LAND = true;
outputAckCmd_.mission_item_reached = false;
outputAckCmd_.mav_mission_accepted = true;
outputAckCmd_.mav_cmd_id = 21;
pubToAckCmd_.publish(outputAckCmd_);
ROS_INFO("MMS->GCS: MISSION_ACCEPTED");
/*outputRef_.Latitude = 0;
outputRef_.Longitude = 0;
outputRef_.AltitudeRelative = 0;
outputRef_.Yawangle = 0;
outputRef_.Mode = 100;*/
get_current_position();
pubToReference_.publish(outputRef_);
ROS_INFO("MMS->NAV: REFERENCE = CURRENT POSITION");
currentState = READY_TO_LAND;
ROS_INFO("MMS_CURRENT_STATE: READY_TO_LAND");
break;
}
if (error_to_t.error_pos < eps_TO and error_to_t.error_ang < eps_YAW)
//if (abs(inputPos_.relative_alt - Dh_TO) < eps_TO and error_to_t.error_ang < eps_YAW)
{
set_events_false();
outputAckCmd_.mission_item_reached = true;
outputAckCmd_.mav_mission_accepted = false;
outputAckCmd_.mav_cmd_id = 22;
pubToAckCmd_.publish(outputAckCmd_);
ROS_INFO("MMS->GCS: MISSION_ITEM_REACHED");
currentState = IN_FLIGHT;
ROS_INFO("MMS_CURRENT_STATE: IN_FLIGHT");
break;
}
break;
case IN_FLIGHT:
//ROS_INFO("MMS_CURRENT_STATE:IN_FLIGHT");
if (LAND)
{
set_events_false();
LAND = true;
outputAckCmd_.mission_item_reached = false;
outputAckCmd_.mav_mission_accepted = true;
outputAckCmd_.mav_cmd_id = 21;
pubToAckCmd_.publish(outputAckCmd_);
ROS_INFO("MMS->GCS: MISSION_ACCEPTED");
/*outputRef_.Latitude = 0;
outputRef_.Longitude = 0;
outputRef_.AltitudeRelative = 0;
outputRef_.Yawangle = 0;
outputRef_.Mode = 100;*/
get_current_position();
pubToReference_.publish(outputRef_);
ROS_INFO("MMS->NAV: REFERENCE = CURRENT POSITION");
currentState = READY_TO_LAND;
ROS_INFO("MMS_CURRENT_STATE: READY_TO_LAND");
break;
}
if (WAYPOINT)
{
set_events_false();
outputAckCmd_.mission_item_reached = false;
outputAckCmd_.mav_mission_accepted = true;
outputAckCmd_.mav_cmd_id = 16;
pubToAckCmd_.publish(outputAckCmd_);
ROS_INFO("MMS->GCS: MISSION_ACCEPTED");
currentState = READY_TO_GO;
ROS_INFO("MMS_CURRENT_STATE: READY_TO_GO");
break;
}
break;
case READY_TO_GO:
// ROS_INFO("MMS_CURRENT_STATE:READY_TO_GO");
if (LAND)
{
set_events_false();
LAND = true;
outputAckCmd_.mission_item_reached = false;
outputAckCmd_.mav_mission_accepted = true;
outputAckCmd_.mav_cmd_id = 21;
pubToAckCmd_.publish(outputAckCmd_);
ROS_INFO("MMS->GCS: MISSION_ACCEPTED");
/*outputRef_.Latitude = 0;
outputRef_.Longitude = 0;
outputRef_.AltitudeRelative = 0;
outputRef_.Yawangle = 0;
outputRef_.Mode = 100;*/
get_current_position();
pubToReference_.publish(outputRef_);
ROS_INFO("MMS->NAV: REFERENCE = CURRENT POSITION");
currentState = READY_TO_LAND;
ROS_INFO("MMS_CURRENT_STATE: READY_TO_LAND");
break;
}
if (WAYPOINT)
{
set_events_false();
outputAckCmd_.mission_item_reached = false;
outputAckCmd_.mav_mission_accepted = true;
outputAckCmd_.mav_cmd_id = 16;
pubToAckCmd_.publish(outputAckCmd_);
ROS_INFO("MMS->GCS: MISSION_ACCEPTED");
/*outputRef_.Latitude = 0;
outputRef_.Longitude = 0;
outputRef_.AltitudeRelative = 0;
outputRef_.Yawangle = 0;
outputRef_.Mode = 100;*/
get_current_position(); //TODO check not used
pubToReference_.publish(outputRef_);
ROS_INFO("MMS->NAV: REFERENCE = CURRENT POSITION");
currentState = READY_TO_GO;
ROS_INFO("MMS_CURRENT_STATE: READY_TO_GO");
break;
}
if (MISSION_START)
{
set_events_false();
//get_target_position();
pubToReference_.publish(target_);
outputRef_ = target_;
ROS_INFO("MMS->NAV: REFERENCE = TARGET WAYPOINT");
currentState = PERFORMING_GO_TO;
ROS_INFO("MMS_CURRENT_STATE: PERFORMING_GO_TO");
break;
}
break;
case PERFORMING_GO_TO:
if (counter_print>=9){
ROS_INFO("MMS PERFOMING_GO_TO");
}
// ROS_INFO("MMS_CURRENT_STATE:PERFOMING_GO_TO");
distance(); // error_to_t,
if (LAND)
{
set_events_false();
LAND = true;
outputAckCmd_.mission_item_reached = false;
outputAckCmd_.mav_mission_accepted = true;
outputAckCmd_.mav_cmd_id = 21;
pubToAckCmd_.publish(outputAckCmd_);
ROS_INFO("MMS->GCS: MISSION_ACCEPTED");
/*outputRef_.Latitude = 0;
outputRef_.Longitude = 0;
outputRef_.AltitudeRelative = 0;
outputRef_.Yawangle = 0;
outputRef_.Mode = 100;*/
get_current_position();
pubToReference_.publish(outputRef_);
ROS_INFO("MMS->NAV: REFERENCE = CURRENT POSITION");
currentState = READY_TO_LAND;
ROS_INFO("MMS_CURRENT_STATE: READY_TO_LAND");
break;
}
if (WAYPOINT)
{
set_events_false();
outputAckCmd_.mission_item_reached = false;
outputAckCmd_.mav_mission_accepted = true;
outputAckCmd_.mav_cmd_id = 16;
pubToAckCmd_.publish(outputAckCmd_);
ROS_INFO("MMS->GCS: MISSION_ACCEPTED");
/*outputRef_.Latitude = 0;
outputRef_.Longitude = 0;
outputRef_.AltitudeRelative = 0;
outputRef_.Yawangle = 0;
outputRef_.Mode = 100;*/
get_current_position(); //TODO check not used
pubToReference_.publish(outputRef_);
ROS_INFO("MMS->NAV: REFERENCE = CURRENT POSITION");
currentState = READY_TO_GO;
ROS_INFO("MMS_CURRENT_STATE: READY_TO_GO");
break;
}
if (error_to_t.error_pos < eps_WP and error_to_t.error_ang < eps_YAW)
{
set_events_false();
outputAckCmd_.mission_item_reached = true;
outputAckCmd_.mav_mission_accepted = false;
outputAckCmd_.mav_cmd_id = 16;
pubToAckCmd_.publish(outputAckCmd_);
ROS_INFO("MMS->GCS: MISSION_ITEM_REACHED");
currentState = IN_FLIGHT;
ROS_INFO("MMS_CURRENT_STATE: IN_FLIGHT");
break;
}
break;
case READY_TO_LAND:
//ROS_INFO("MMS_CURRENT_STATE: READY_TO_LAND");
if (MISSION_START)
{
set_events_false();
LAND = true;
outputRef_.Latitude = inputPos_.lat;
outputRef_.Longitude = inputPos_.lon;
outputRef_.AltitudeRelative = inputPos_.relative_alt; // 5 cm @ frequencey
outputRef_.Yawangle = inputPos_.hdg*3.14/100/360;
outputRef_.Mode = 0;
//pubToReference_.publish(outputRef_);
//ROS_INFO("MMS->NAV: REFERENCE = VERT. LAND SPEED");
currentState = PERFORMING_LANDING;
ROS_INFO("MMS_CURRENT_STATE: PERFORMING_LANDING");
break;
}
if (WAYPOINT)
{
set_events_false();
outputAckCmd_.mission_item_reached = false;
outputAckCmd_.mav_mission_accepted = true;
outputAckCmd_.mav_cmd_id = 16;
pubToAckCmd_.publish(outputAckCmd_);
ROS_INFO("MMS->GCS: MISSION_ACCEPTED");
/*outputRef_.Latitude = 0;
outputRef_.Longitude = 0;
outputRef_.AltitudeRelative = 0;
outputRef_.Yawangle = 0;
outputRef_.Mode = 100;*/
get_current_position(); //TODO check not used
pubToReference_.publish(outputRef_);
ROS_INFO("MMS->NAV: REFERENCE = CURRENT POSITION");
currentState = READY_TO_GO;
ROS_INFO("MMS_CURRENT_STATE: READY_TO_GO");
break;
}
break;
case PERFORMING_LANDING:
// ROS_INFO("MMS_CURRENT_STATE:PERFORMING_LANDING");
//outputRef_.Latitude = inputPos_.lat;
//outputRef_.Longitude = inputPos_.lon;
outputRef_.AltitudeRelative -= 80; // 5 cm @ frequencey
//outputRef_.Yawangle = 0;
//outputRef_.Mode = 0;
pubToReference_.publish(outputRef_);
ROS_INFO("MMS->NAV: REFERENCE = VERT. LAND SPEED");
if (inputPos_.relative_alt - outputRef_.AltitudeRelative > eps_LAND)
{
set_events_false();
LAND = true;
outputAckCmd_.mission_item_reached = true;
outputAckCmd_.mav_mission_accepted = false;
outputAckCmd_.mav_cmd_id = 21;
pubToAckCmd_.publish(outputAckCmd_);
ROS_INFO("MMS->GCS: MISSION_ITEM_REACHED");
currentState = ON_GROUND_ARMED;
ROS_INFO("MMS_CURRENT_STATE: ON_GROUND_ARMED");
}
break;
}
}
void MMS_Handle()
{
switch(currentState)
{
case ON_GROUND_NO_HOME:
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = FRAME_BARO;//inputCmd_.frame;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO_ONCE("MMS->REF: CURRENT_STATE = ON_GROUND_NO_HOME");
if (ARMED)
{
set_events_false();
ARMED = true;
outputArm_.arm_disarm = false;
outputArm_.new_arm_disarm = true;
// pubToArm_.publish(outputArm_); // TODO automatic disarming
ROS_INFO("MMS->APM: DISARMING");
currentState = ON_GROUND_NO_HOME;
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = FRAME_BARO;//inputCmd_.frame;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS->REF: CURRENT_STATE = ON_GROUND_NO_HOME");
break;
}
if (SET_HOME)
{
outputAckMission_.mission_item_reached = false;
outputAckMission_.seq = inputCmd_.seq;
outputAckMission_.mav_mission_accepted = true;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: MISSION_ACCEPTED");
set_events_false();
currentState = SETTING_HOME;
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = FRAME_BARO;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS->REF: CURRENT_STATE = SETTING_HOME");
break;
}
// Use these lines to introduce the mission_not_accepted for those commands which cannot be executed in this state
if (TAKEOFF || LAND || WAYPOINT)
{
set_events_false();
outputAckMission_.mission_item_reached = false;
outputAckMission_.seq = inputCmd_.seq;
outputAckMission_.mav_mission_accepted = false;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: MISSION_NOT_ACCEPTED");
break;
}
break;
case SETTING_HOME:
outputAckMission_.mission_item_reached = true;
outputAckMission_.seq = seq_number;
outputAckMission_.mav_mission_accepted = false;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: MISSION_ITEM_REACHED");
currentState = ON_GROUND_DISARMED;
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = FRAME_BARO;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS->REF: CURRENT_STATE = ON_GROUND_DISARMED");
break;
case ON_GROUND_DISARMED:
if (TAKEOFF)
{
set_events_false();
TAKEOFF = true;
ARMED = false;
outputArm_.arm_disarm = true;
outputArm_.new_arm_disarm = true;
pubToArm_.publish(outputArm_);
ROS_INFO("MMS->APM: ARMING");
counter_ = 0; //start timing to rearm
currentState = ARMING;
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = FRAME_BARO;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS->REF: CURRENT_STATE = ARMING");
break;
}
if (SET_HOME)
{
set_events_false();
outputAckMission_.mission_item_reached = false;
outputAckMission_.seq = inputCmd_.seq;
outputAckMission_.mav_mission_accepted = true;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: MISSION_ACCEPTED");
currentState = SETTING_HOME;
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = FRAME_BARO;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS->REF: CURRENT_STATE = SETTING_HOME");
break;
}
// Use these lines to introduce the mission_not_accepted for those commands which cannot be executed in this state
if (LAND || WAYPOINT)
{
set_events_false();
outputAckMission_.mission_item_reached = false;
outputAckMission_.seq = inputCmd_.seq;
outputAckMission_.mav_mission_accepted = false;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: MISSION_NOT_ACCEPTED");
break;
}
break;
case ARMING:
if (LAND)
{
set_events_false();
ARMED = true;
outputAckMission_.mission_item_reached = false;
outputAckMission_.seq = inputCmd_.seq;
outputAckMission_.mav_mission_accepted = true;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: MISSION_ACCEPTED");
outputArm_.arm_disarm = false;
outputArm_.new_arm_disarm = true;
// pubToArm_.publish(outputArm_); // TODO automatic disarming
ROS_INFO("MMS->APM: DISARMING");
currentState = DISARMING;
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = FRAME_BARO;//inputCmd_.frame;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS->REF: CURRENT_STATE = DISARMING");
counter_ = 0; //start timing to rearm
break;
}
if (ARMED)
{
ROS_INFO("MMS: !!! DRONE ARMED !!!");
set_events_false();
TAKEOFF = true; // to complete automatic TO
currentState = ON_GROUND_ARMED;
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = FRAME_BARO;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS->REF: CURRENT_STATE = ON_GROUND_ARMED");
break;
}
if (counter_>=50) //5 seconds
{
currentState = ON_GROUND_DISARMED;
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = FRAME_BARO;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS: ARMING FAILED");
ROS_INFO("MMS->REF: CURRENT_STATE = ON_GROUND_DISARMED");
}
if (SET_HOME || WAYPOINT)
{
set_events_false();
outputAckMission_.mission_item_reached = false;
outputAckMission_.seq = inputCmd_.seq;
outputAckMission_.mav_mission_accepted = false;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: MISSION_NOT_ACCEPTED");
break;
}
break;
case DISARMING: // TODO automatic disarming
if (ARMED == false)
{
ROS_INFO("MMS: DRONE DISARMED!");
set_events_false();
outputAckMission_.mission_item_reached = true;
outputAckMission_.seq = seq_number;
outputAckMission_.mav_mission_accepted = false;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: MISSION_ITEM_REACHED");
currentState = ON_GROUND_DISARMED;
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = FRAME_BARO;//inputCmd_.frame;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS->REF: CURRENT_STATE = ON_GROUND_DISARMED");
break;
}
if (counter_>=50) //5 seconds
{
currentState = ON_GROUND_ARMED;
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = FRAME_BARO;//inputCmd_.frame;
pubToMmsStatus_.publish(outputMmsStatus_);
LAND = true;
ROS_INFO("MMS: DISARMING FAILED");
ROS_INFO("MMS->REF: CURRENT_STATE = ON_GROUND_ARMED");
}
if (TAKEOFF || LAND || SET_HOME || WAYPOINT)
{
set_events_false();
outputAckMission_.mission_item_reached = false;
outputAckMission_.seq = inputCmd_.seq;
outputAckMission_.mav_mission_accepted = false;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: MISSION_NOT_ACCEPTED");
break;
}
break;
case ON_GROUND_ARMED:
if (LAND) // TODO automatic disarming
{
set_events_false();
ARMED = true;
outputAckMission_.mission_item_reached = false;
outputAckMission_.seq = inputCmd_.seq;
outputAckMission_.mav_mission_accepted = true;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: MISSION_ACCEPTED");
outputArm_.arm_disarm = false;
outputArm_.new_arm_disarm = true;
// pubToArm_.publish(outputArm_); // TODO automatic disarming
ROS_INFO("MMS->APM: DISARMING");
currentState = DISARMING;
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = FRAME_BARO;//inputCmd_.frame;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS->REF: CURRENT_STATE = DISARMING");
counter_ = 0; //start timing to rearm
break;
}
if (TAKEOFF)
{
set_events_false();
outputAckMission_.mission_item_reached = false;
outputAckMission_.seq = inputCmd_.seq;
outputAckMission_.mav_mission_accepted = true;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: MISSION_ACCEPTED");
currentState = PERFORMING_TAKEOFF;// ON_GROUND_READY_TO_TAKEOFF; // with this modification the state that needs the MISSION_START is excluded
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = FRAME_BARO;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS->REF: CURRENT_STATE = PERFORMING_TAKEOFF");//READY_TO_TAKEOFF");
break;
}
if (SET_HOME || WAYPOINT)
{
set_events_false();
outputAckMission_.mission_item_reached = false;
outputAckMission_.seq = inputCmd_.seq;
outputAckMission_.mav_mission_accepted = false;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: MISSION_NOT_ACCEPTED");
break;
}
break;
case PERFORMING_TAKEOFF:
if (SAFETY_ON){ //PUT THIS FOR ROLLING BACK FROM MANUAL_FLIGHT
set_events_false();
previousState = currentState; //save last state in previousState
currentState = MANUAL_FLIGHT;
outputMmsStatus_.mms_state = currentState;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS->REF: CURRENT_STATE = MANUAL_FLIGHT");
break;
}
if (LAND)
{
set_events_false();
LAND = true;
outputAckMission_.mission_item_reached = false;
outputAckMission_.seq = inputCmd_.seq;
outputAckMission_.mav_mission_accepted = true;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: MISSION_ACCEPTED");
currentState = PERFORMING_LANDING;//READY_TO_LAND;
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = target_frame;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS->REF: CURRENT_STATE = PERFORMING_LANDING");//READY_TO_LAND");
break;
}
//ROS_INFO("MMS DEBUG: Command: %d - Seq: %d - Seq_req: %d", inputDist_.command, inputDist_.seq, seq_number);
if (inputDist_.command == 22 && seq_number == inputDist_.seq)
{
ROS_INFO_ONCE("MMS: REACHING THE TAKEOFF TARGET");
//ROS_INFO("MMS: Distances: %.3f - %.3f - %.3f", inputDist_.error_pos, inputDist_.error_ang, inputDist_.error_alt);
if (inputDist_.error_ang < eps_YAW && inputDist_.error_alt < eps_alt)
{
set_events_false();
outputAckMission_.mission_item_reached = true;
outputAckMission_.seq = seq_number;
outputAckMission_.mav_mission_accepted = false;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: MISSION_ITEM_REACHED");
currentState = IN_FLIGHT;
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = target_frame;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS->REF: CURRENT_STATE = IN_FLIGHT");
// break; // comment this line to execute also the following if()
}
}
if (TAKEOFF || SET_HOME || WAYPOINT)
{
set_events_false();
outputAckMission_.mission_item_reached = false;
outputAckMission_.seq = inputCmd_.seq;
outputAckMission_.mav_mission_accepted = false;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: MISSION_NOT_ACCEPTED");
break;
}
break;
case IN_FLIGHT:
if (SAFETY_ON){ //PUT THIS FOR ROLLING BACK FROM MANUAL_FLIGHT
set_events_false();
previousState = currentState; //save last state in previousState
currentState = MANUAL_FLIGHT;
outputMmsStatus_.mms_state = currentState;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS->REF: CURRENT_STATE = MANUAL_FLIGHT");
break;
}
if (LAND)
{
set_events_false();
LAND = true;
outputAckMission_.mission_item_reached = false;
outputAckMission_.seq = inputCmd_.seq;
outputAckMission_.mav_mission_accepted = true;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: MISSION_ACCEPTED");
currentState = PERFORMING_LANDING;// READY_TO_LAND; // with this modification the state that needs the MISSION_START is excluded
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = target_frame;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS-REF: CURRENT_STATE = PERFORMING_LANDING"); // READY_TO_LAND");
break;
}
if (WAYPOINT)
{
set_events_false();
outputAckMission_.mission_item_reached = false;
outputAckMission_.seq = inputCmd_.seq;
outputAckMission_.mav_mission_accepted = true;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: MISSION_ACCEPTED");
currentState = PERFORMING_GO_TO; // READY_TO_GO; // with this modification the state that needs the MISSION_START is excluded
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = target_frame;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS->REF: CURRENT_STATE = PERFORMING_GO_TO"); // READY_TO_GO");
break;
}
if (GRID_EVENT) //TODO add this event in other states too (in waypoint and ??)
{
set_events_false();
outputAckMission_.mission_item_reached = false;
outputAckMission_.seq = inputCmd_.seq;
outputAckMission_.mav_mission_accepted = true;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: MISSION_ACCEPTED");
currentState = GRID; //
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = target_frame;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS->REF: CURRENT_STATE = GRID"); // READY_TO_GO");
break;
}
if (TAKEOFF || SET_HOME)
{
set_events_false();
outputAckMission_.mission_item_reached = false;
outputAckMission_.seq = inputCmd_.seq;
outputAckMission_.mav_mission_accepted = false;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: MISSION_NOT_ACCEPTED");
break;
}
if (LEASHING_START) //TODO maybe add LEASHING_START in other states
{
set_events_false();
outputAckMission_.mission_item_reached = false;
outputAckMission_.seq = inputCmd_.seq;
outputAckMission_.mav_mission_accepted = true;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: MISSION_ACCEPTED (LEASHING)");
currentState = LEASHING;
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = target_frame;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS->REF: CURRENT_STATE = LEASHING");
}
break;
case GRID:
if (SAFETY_ON){ //PUT THIS FOR ROLLING BACK FROM MANUAL_FLIGHT
set_events_false();
previousState = currentState; //save last state in previousState
currentState = MANUAL_FLIGHT;
outputMmsStatus_.mms_state = currentState;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS->REF: CURRENT_STATE = MANUAL_FLIGHT");
break;
}
if (LAND){
set_events_false();
LAND = true;
outputAckMission_.mission_item_reached = false;
outputAckMission_.seq = inputCmd_.seq;
outputAckMission_.mav_mission_accepted = true;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: MISSION_ACCEPTED");
currentState = PERFORMING_LANDING; // READY_TO_LAND;
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = target_frame;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS->REF: CURRENT_STATE = PERFORMING_LANDING");//READY_TO_LAND");
break;
}
if (WAYPOINT){
set_events_false();
outputAckMission_.mission_item_reached = false;
outputAckMission_.seq = inputCmd_.seq;
outputAckMission_.mav_mission_accepted = true;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: MISSION_ACCEPTED");
currentState = PERFORMING_GO_TO;//READY_TO_GO;
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = target_frame;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS->REF: CURRENT_STATE = PERFORMING_GO_TO");//READY_TO_GO");
break;
}
if (GRID_ENDED){
set_events_false();
currentState = IN_FLIGHT;
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = target_frame;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS->REF: CURRENT_STATE = IN_FLIGHT");
if (Grid_ack_.completion_type == 1){ //success
outputAckMission_.mission_item_reached = true;
outputAckMission_.seq = seq_number;
outputAckMission_.mav_mission_accepted = false;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: GRID FINISHED SUCCESFULLY");
} else { //failure
outputAckMission_.mission_item_reached = false;
outputAckMission_.seq = inputCmd_.seq;
outputAckMission_.mav_mission_accepted = false;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: GRID FINISHED NOT SUCCESFULLY");
}
}
if (PAUSE){
set_events_false();
previousState = currentState; //save last state in previousState
currentState = PAUSED;
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = target_frame;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS->REF: CURRENT_STATE = PAUSED");
}
break;
case PERFORMING_GO_TO:
if (SAFETY_ON){ //PUT THIS FOR ROLLING BACK FROM MANUAL_FLIGHT
set_events_false();
previousState = currentState; //save last state in previousState
currentState = MANUAL_FLIGHT;
outputMmsStatus_.mms_state = currentState;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS->REF: CURRENT_STATE = MANUAL_FLIGHT");
break;
}
if (LAND)
{
set_events_false();
LAND = true;
outputAckMission_.mission_item_reached = false;
outputAckMission_.seq = inputCmd_.seq;
outputAckMission_.mav_mission_accepted = true;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: MISSION_ACCEPTED");
currentState = PERFORMING_LANDING; // READY_TO_LAND;
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = target_frame;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS->REF: CURRENT_STATE = PERFORMING_LANDING");//READY_TO_LAND");
break;
}
if (WAYPOINT)
{
set_events_false();
outputAckMission_.mission_item_reached = false;
outputAckMission_.seq = inputCmd_.seq;
outputAckMission_.mav_mission_accepted = true;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: MISSION_ACCEPTED");
currentState = PERFORMING_GO_TO;//READY_TO_GO;
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = target_frame;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS->REF: CURRENT_STATE = PERFORMING_GO_TO");//READY_TO_GO");
break;
}
//ROS_INFO("-------------- command %d --- seq %d --- seq2: %d ------", inputDist_.command, seq_number, inputDist_.seq);
if (inputDist_.command == 16 && seq_number == inputDist_.seq)
{
ROS_INFO_ONCE("MMS: REACHING THE WAYPOINT TARGET");
//ROS_INFO("----------MMS: Distances: %.3f - %.3f - %.3f", inputDist_.error_pos, inputDist_.error_ang, inputDist_.error_alt);
if (inputDist_.error_pos < eps_WP && inputDist_.error_ang < eps_YAW && inputDist_.error_alt < eps_alt)
{
set_events_false();
outputAckMission_.mission_item_reached = true;
outputAckMission_.seq = seq_number;
outputAckMission_.mav_mission_accepted = false;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: MISSION_ITEM_REACHED");
currentState = IN_FLIGHT;
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = target_frame;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS->REF: CURRENT_STATE = IN_FLIGHT");
// break;
}
}
if (TAKEOFF || SET_HOME)
{
set_events_false();
outputAckMission_.mission_item_reached = false;
outputAckMission_.seq = inputCmd_.seq;
outputAckMission_.mav_mission_accepted = false;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: MISSION_NOT_ACCEPTED");
break;
}
break;
case PERFORMING_LANDING:
set_events_false(); // TODO automatic disarming (eliminate)
//if (inputDist_.command == 21 && seq_number == inputDist_.seq)
//{
ROS_INFO_ONCE("MMS: REACHING THE LANDING TARGET");
/*if (inputDist_.error_alt > eps_LAND)
{
set_events_false();
LAND = true;
currentState = ON_GROUND_ARMED;
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = FRAME_BARO;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS->REF: CURRENT_STATE = ON_GROUND_ARMED");
}*/
//}
break;
case MANUAL_FLIGHT:
if (SAFETY_OFF)
{
set_events_false();
currentState = previousState; //rolling back to last state
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = target_frame;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS: CURRENT_STATE = BACK TO OLD ONE: %d",currentState);
}
break;
case PAUSED:
if (CONTINUE){
set_events_false();
currentState = previousState; //rolling back to last state
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = target_frame;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS: CURRENT_STATE = BACK TO OLD ONE: %d",currentState);
}
break;
case LEASHING:
if (LAND){
set_events_false();
LAND = true;
outputAckMission_.mission_item_reached = false;
outputAckMission_.seq = inputCmd_.seq;
outputAckMission_.mav_mission_accepted = true;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: MISSION_ACCEPTED");
currentState = PERFORMING_LANDING;
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = target_frame;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS->REF: CURRENT_STATE = PERFORMING_LANDING");
break;
}
if (LEASHING_END){
set_events_false();
outputAckMission_.mission_item_reached = true;
outputAckMission_.seq = inputCmd_.seq;
outputAckMission_.mav_mission_accepted = false;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: MISSION_ITEM_REACHED (LEASHING)");
currentState = IN_FLIGHT;
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = target_frame;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS->REF: CURRENT_STATE = IN_FLIGHT");
}
if (LEASHING_FAILURE){
set_events_false();
outputAckMission_.mission_item_reached = false;
outputAckMission_.seq = inputCmd_.seq;
outputAckMission_.mav_mission_accepted = false;
pubToAckMission_.publish(outputAckMission_);
ROS_INFO("MMS->GCS: MISSION_ITEM_FAILED (LEASHING)");
currentState = IN_FLIGHT;
outputMmsStatus_.mms_state = currentState;
outputMmsStatus_.target_ref_frame = target_frame;
pubToMmsStatus_.publish(outputMmsStatus_);
ROS_INFO("MMS->REF: CURRENT_STATE = IN_FLIGHT");
}
break;
}
}
