static void status_message(int status_code)
{
if (status_code == 0) {
int32_t* pos=get_current_position();
printPgmString(PSTR("ok"));
printPgmString(PSTR(" W:"));
printInteger(pos[0]);
printPgmString(PSTR(" X:"));
printInteger(pos[1]);
printPgmString(PSTR(" Y:"));
printInteger(pos[2]);
printPgmString(PSTR(" Z:"));
printInteger(pos[3]);
printPgmString(PSTR("\r\n"));
} else {
printPgmString(PSTR("error: "));
switch(status_code) {
case STATUS_BAD_NUMBER_FORMAT:
printPgmString(PSTR("Bad number format\r\n")); break;
case STATUS_EXPECTED_COMMAND_LETTER:
printPgmString(PSTR("Expected command letter\r\n")); break;
case STATUS_UNSUPPORTED_STATEMENT:
printPgmString(PSTR("Unsupported statement\r\n")); break;
case STATUS_FLOATING_POINT_ERROR:
printPgmString(PSTR("Floating point error\r\n")); break;
default:
printInteger(status_code);
printPgmString(PSTR("\r\n"));
}
}
}
void HapticsPSM::compute_total_deflection(tf::Vector3 &delta_v){
tf::Vector3 v1,v2,v3;
v1 = coll_psm.locked_position;
get_current_position(v2);
v3 = (v1 - v2) + (coll_psm.cur_normal * coll_psm.spr_radius);
//ROS_INFO("v3 full vx = %f vy = %f vz = %f ", v3.getX(),v3.getY(),v3.getZ());
delta_v = v3;
}
/*
This function return the index of current character.
Stack:
1: the instance table
2: the accessed key
*/
int lui_input_get_index(lua_State *L)
{
INPUT *i;
luasoul_checkclass(L, 1, INPUT_CLASS, i); /* get input */
lua_pushnumber(L, get_current_position(i) + 1); /* lua 'array' start at 1 */
return 1;
}
static int64_t op1a_get_next_frame_number(MXFReader *reader)
{
if (mxf_file_is_seekable(reader->mxfFile))
{
return get_current_position(reader->essenceReader->data->index);
}
else
{
return reader->essenceReader->data->nsIndex.currentPosition;
}
}
int
TraceLog::step_forward(const unsigned int steps)
{
int i=0;
if (i<steps && _current_position<get_number_of_commands())
{
do_current_command();
++i; ++_current_position;
}
return get_current_position();
}
static int op1a_read_next_frame(MXFReader *reader, MXFReaderListener *listener)
{
MXFFile *mxfFile = reader->mxfFile;
EssenceReader *essenceReader = reader->essenceReader;
EssenceReaderData *data = essenceReader->data;
/* set position at start of the current content package */
if (mxf_file_is_seekable(mxfFile))
{
if (end_of_essence(data->index))
{
return -1;
}
CHK_ORET(set_position(mxfFile, data->index, get_current_position(data->index)));
if (end_of_essence(data->index))
{
return -1;
}
CHK_ORET(read_content_package(reader, 0, listener));
increment_current_position(data->index);
}
else
{
if (ns_end_of_essence(&data->nsIndex))
{
return -1;
}
CHK_ORET(ns_pos_at_next_frame(reader));
if (ns_end_of_essence(&data->nsIndex))
{
return -1;
}
CHK_ORET(ns_read_content_package(reader, 0, listener));
data->nsIndex.currentPosition++;
}
return 1;
}
/*
input:addch(ch)
put a wide character
*/
int lui_addch_input(lua_State *L)
{
INPUT *i;
wchar_t *wstr;
size_t len;
cchar_t ch;
luasoul_checkclass(L, 1, INPUT_CLASS, i); /* get input */
luasoul_checklwcstr(L, 2, wstr, len); /* get wstring */
/* FIXME: correct attributs to setcchar() ? */
if (setcchar(&ch, wstr, WA_NORMAL, 0, NULL) == OK
&& wins_wch(i->pad, &ch) == OK)
{
/* man setcchar()
...contain at most one spacing character, which must be the first */
len = (size_t) wcwidth(*wstr);
if (len != (size_t) -1 && len) /* don't move for 0 width */
set_current_position(i, get_current_position(i) + len);
}
return 0;
}
/*
This function return the content of the buffer.
Stack:
1: the instance table
2: the accessed key
FIXME: use win_wchnstr, handle multiple lines ?
*/
int lui_input_get_buff(lua_State *L)
{
INPUT *i;
int pos;
wchar_t *buff;
int len;
luasoul_checkclass(L, 1, INPUT_CLASS, i); /* get input */
pos = get_current_position(i); /* save cursor position */
/* assume i->height == 1 */
len = i->width * i->off;
buff = malloc((len + 1) * sizeof(*buff));
len = mvwinnwstr(i->pad, 0, 0, buff, len);
while (iswspace(buff[--len])) /* remove leading space characters */
{}
buff[len + 1] = L'\0';
luasoul_pushwstring(L, buff);
set_current_position(i, pos); /* restore cursor position */
return 1;
}
int
TraceLog::step_backward(const unsigned int steps)
{
return get_current_position();
}
void HapticsPSM::run_haptic_alg(){
if(check_collison()){
//Step 1:
get_collision_normals_and_points(coll_res.contacts, coll_psm.cur_normal_arr, coll_psm.cur_contact_pnts_arr, coll_psm.insertion_depths);
if (coll_psm.contact_cnts_prev != coll_res.contacts.size()){
ROS_INFO("Number of Contact Points = %i", coll_res.contacts.size());
}
//Step 2:
compute_average_position(coll_psm.cur_contact_pnts_arr, coll_psm.locked_position);
compute_average_normal(coll_psm.cur_normal_arr, coll_psm.cur_normal);
if(coll_psm._first_contact){
ROS_INFO("First contact occured");
tf::Vector3 diff, cur_pos;
get_current_position(cur_pos);
diff = coll_psm.locked_position - cur_pos;
coll_psm.spr_radius = diff.length();
ROS_INFO("SPR Radius Detected to be %f",coll_psm.spr_radius);
coll_psm._first_contact = false;
ROS_INFO("Normal is nx = %f ny = %f nz = %f", coll_psm.cur_normal.getX(),coll_psm.cur_normal.getY(),coll_psm.cur_normal.getZ());
}
//Step 3:
if(has_normal_changed(coll_psm.cur_normal, coll_psm.pre_normal)){
// ROS_INFO("Normal has Changed nx = %f ny = %f nz = %f", coll_psm.cur_normal.getX(),coll_psm.cur_normal.getY(),coll_psm.cur_normal.getZ());
// ROS_INFO("Normal has Changed pnx = %f pny = %f pnz = %f", coll_psm.pre_normal.getX(),coll_psm.pre_normal.getY(),coll_psm.pre_normal.getZ());
}
//Step 5:
compute_total_deflection(coll_psm.def_total);
//Step 6:
compute_deflection_along_normal(coll_psm.cur_normal, coll_psm.def_total, coll_psm.def_along_n);
//Step 7:
compute_deflection_force(spr_haptic_force.wrench,coll_psm.def_along_n);
//Step 8:
compute_force_in_tip_frame(spr_haptic_force.wrench);
//Step 9:
coll_psm.pre_normal = coll_psm.cur_normal;
coll_psm.contact_cnts_prev = coll_res.contacts.size();
if (coll_psm.cur_normal.angle(coll_psm.def_along_n) > coll_psm.epsilon){
ROS_INFO("Angle between normal and deflection force %f", coll_psm.cur_normal.angle(coll_psm.def_along_n));
}
mesh_normal.wrench.force.x = coll_psm.cur_normal.getX();
mesh_normal.wrench.force.y = coll_psm.cur_normal.getY();
mesh_normal.wrench.force.z = coll_psm.cur_normal.getZ();
normal_pub.publish(mesh_normal);
}
else{
if(!coll_psm._first_contact){
ROS_INFO("First contact Released");
coll_psm._first_contact = true;
}
coll_psm.def_along_n.setValue(0,0,0);
coll_psm.cur_normal.setValue(0,0,0);
spr_haptic_force.wrench.force.x=0;
spr_haptic_force.wrench.force.y=0;
spr_haptic_force.wrench.force.z=0;
}
haptic_deflection.x = coll_psm.def_along_n.getX();
haptic_deflection.y = coll_psm.def_along_n.getY();
haptic_deflection.z = coll_psm.def_along_n.getZ();
haptic_deflection_pub.publish(haptic_deflection);
spr_haptic_pub.publish(spr_haptic_force);
pose_pub.publish(group->getCurrentPose());
}
void HapticsPSM::adjust_locked_position_along_new_n(tf::Vector3 &old_d, tf::Vector3&new_n, tf::Vector3 &locked_p){
tf::Vector3 new_d;
get_current_position(locked_p); //Record the current position, this would be inside the collision mesh as the normal just changed while in collision
compute_deflection_along_normal(new_n, old_d, new_d);
locked_p = locked_p + new_d; //If we are already inside a body, this would take the locked proxy point outside the body along the deflection along normal.
}
void HapticsPSM::lock_current_position_for_proxy(tf::Vector3 &v){
get_current_position(v); //Whatever the current end position is, set v equal to it.
}
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;
}
}
static int read_content_package(MXFReader *reader, int skip, MXFReaderListener *listener)
{
MXFFile *mxfFile = reader->mxfFile;
EssenceReader *essenceReader = reader->essenceReader;
EssenceReaderData *data = essenceReader->data;
FileIndex *index = data->index;
EssenceTrack *essenceTrack;
uint8_t *buffer;
uint64_t bufferSize;
mxfKey key;
uint8_t llen;
uint64_t len;
uint64_t cpLen;
uint64_t cpCount;
int trackIndex;
get_next_kl(index, &key, &llen, &len);
cpLen = get_cp_len(index);
cpCount = mxfKey_extlen + llen;
/* process essence elements in content package */
while (cpCount <= cpLen)
{
if (!skip && mxf_is_gc_essence_element(&key))
{
/* send data to listener */
if (get_essence_track_with_tracknumber(essenceReader, mxf_get_track_number(&key), &essenceTrack,
&trackIndex))
{
if (accept_frame(listener, trackIndex))
{
CHK_ORET(read_frame(reader, listener, trackIndex, len, &buffer, &bufferSize));
CHK_ORET(send_frame(reader, listener, trackIndex, buffer, bufferSize));
}
else
{
CHK_ORET(mxf_skip(mxfFile, len));
}
cpCount += len;
}
else if (element_is_known_system_item(&key))
{
CHK_ORET(extract_system_item_info(reader, &key, len,
get_current_position(reader->essenceReader->data->index)));
cpCount += len;
}
else
{
CHK_ORET(mxf_skip(mxfFile, len));
cpCount += len;
}
}
else
{
CHK_ORET(mxf_skip(mxfFile, len));
cpCount += len;
}
if (!mxf_read_kl(mxfFile, &key, &llen, &len))
{
CHK_ORET(mxf_file_eof(mxfFile));
set_next_kl(index, &g_Null_Key, 0, 0);
break;
}
set_next_kl(index, &key, llen, len);
cpCount += mxfKey_extlen + llen;
}
CHK_ORET(cpCount == cpLen + mxfKey_extlen + llen);
return 1;
}
