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; }