void Pgoto_point(){
if(strcmp(buff,"x")==0 && (~set)&1){
set=set|1;
next=1;
rdp=Pset_coord;
}
else if(strcmp(buff,"y")==0 && (~set)&2){
set=set|2;
next=2;
rdp=Pset_coord;
}
else if(strcmp(buff,"z")==0 && (~set)&4){
set=set|4;
next=4;
rdp=Pset_coord;
}
else if(strcmp(buff,"w")==0 && (~set)&8){
set=set|8;
next=8;
rdp=Pset_coord;
}
else if(strcmp(buff,"!")==0){//valid exit point
rdp=Pstart;
set=0;
start_path();
}
else{
Serial.print("Error: invalid or redundant coordinate:\t");Serial.println(buff);
Serial.println("Waiting for '!'.");
set=0;
rdp=Pinvalid;
}
}
void SvgSerializer::write(const IfcGeom::BRepElement<double>* o) {
IfcSchema::IfcBuildingStorey* storey = 0;
IfcSchema::IfcObjectDefinition* obdef = static_cast<IfcSchema::IfcObjectDefinition*>(file->entityById(o->id()));
#ifndef USE_IFC4
typedef IfcSchema::IfcRelDecomposes decomposition_element;
#else
typedef IfcSchema::IfcRelAggregates decomposition_element;
#endif
while (true) {
// Iterate over the decomposing element to find the parent IfcBuildingStorey
decomposition_element::list::ptr decomposes = obdef->Decomposes();
if (!decomposes->size()) {
if (obdef->is(IfcSchema::Type::IfcElement)) {
IfcSchema::IfcRelContainedInSpatialStructure::list::ptr containment = ((IfcSchema::IfcElement*)obdef)->ContainedInStructure();
if (!containment->size()) {
break;
}
for (IfcSchema::IfcRelContainedInSpatialStructure::list::it it = containment->begin(); it != containment->end(); ++it) {
IfcSchema::IfcRelContainedInSpatialStructure* container = *it;
if (container->RelatingStructure() != obdef) {
obdef = container->RelatingStructure();
}
}
} else {
break;
}
} else {
for (decomposition_element::list::it it = decomposes->begin(); it != decomposes->end(); ++it) {
decomposition_element* decompose = *it;
if (decompose->RelatingObject() != obdef) {
obdef = decompose->RelatingObject();
}
}
}
if (obdef->is(IfcSchema::Type::IfcBuildingStorey)) {
storey = static_cast<IfcSchema::IfcBuildingStorey*>(obdef);
break;
}
}
if (!storey) return;
path_object& p = start_path(storey, nameElement(o));
for (IfcGeom::IfcRepresentationShapeItems::const_iterator it = o->geometry().begin(); it != o->geometry().end(); ++ it) {
gp_GTrsf gtrsf = it->Placement();
gp_Trsf o_trsf;
const std::vector<double>& matrix = o->transformation().matrix().data();
o_trsf.SetValues(
matrix[0], matrix[3], matrix[6], matrix[ 9],
matrix[1], matrix[4], matrix[7], matrix[10],
matrix[2], matrix[5], matrix[8], matrix[11]
#if OCC_VERSION_HEX < 0x60800
, Precision::Angular(), Precision::Confusion()
#endif
);
gtrsf.PreMultiply(o_trsf);
const TopoDS_Shape& s = it->Shape();
bool trsf_valid = false;
gp_Trsf trsf;
try {
trsf = gtrsf.Trsf();
trsf_valid = true;
} catch (...) {}
const TopoDS_Shape moved_shape = trsf_valid
? BRepBuilderAPI_Transform(s, trsf, true).Shape()
: BRepBuilderAPI_GTransform(s, gtrsf, true).Shape();
const double inf = std::numeric_limits<double>::infinity();
double zmin = inf;
double zmax = -inf;
{TopExp_Explorer exp(moved_shape, TopAbs_VERTEX);
for (; exp.More(); exp.Next()) {
const TopoDS_Vertex& vertex = TopoDS::Vertex(exp.Current());
gp_Pnt pnt = BRep_Tool::Pnt(vertex);
if (pnt.Z() < zmin) { zmin = pnt.Z(); }
if (pnt.Z() > zmax) { zmax = pnt.Z(); }
}}
if (section_height) {
if (zmin > section_height || zmax < section_height) continue;
} else {
if (zmin == inf || (zmax - zmin) < 1.) continue;
}
const double cut_z = section_height.get_value_or(zmin + 1.);
// Create a horizontal cross section 1 meter above the bottom point of the shape
TopoDS_Shape result = BRepAlgoAPI_Section(moved_shape, gp_Pln(gp_Pnt(0, 0, cut_z), gp::DZ()));
Handle(TopTools_HSequenceOfShape) edges = new TopTools_HSequenceOfShape();
Handle(TopTools_HSequenceOfShape) wires = new TopTools_HSequenceOfShape();
{TopExp_Explorer exp(result, TopAbs_EDGE);
for (; exp.More(); exp.Next()) {
edges->Append(exp.Current());
}}
ShapeAnalysis_FreeBounds::ConnectEdgesToWires(edges, 1e-5, false, wires);
gp_Pnt prev;
for (int i = 1; i <= wires->Length(); ++i) {
const TopoDS_Wire& wire = TopoDS::Wire(wires->Value(i));
write(p, wire);
}
}
}
void process_cmd(struct cmd_frame_t *cmd)
{
int res;
reply.status = ACK; // change to NACK as necessary
reply.cmd = cmd->cmd;
switch (cmd->cmd) {
case CMD_ECHO:
reply.data[0] = cmd->echo.length;
memcpy(&reply.data[1], cmd->echo.data, cmd->echo.length);
send_reply(&reply, cmd->echo.length+3);
break;
case CMD_RESET:
if (cmd->reset_magic == RESET_MAGIC) {
usb_peripheral_reset();
CREG_M4MEMMAP = 0x10400000;
scb_reset_system();
} else {
send_nack();
}
break;
case CMD_GET_EVENT_COUNTERS:
{
struct event_counters counters = get_last_event_counters();
memcpy(reply.data, &counters, sizeof(struct event_counters));
send_reply(&reply, 2+sizeof(struct event_counters));
break;
}
case CMD_GET_STAGE_GAINS:
memcpy(reply.data, stage_fb_gains, sizeof(stage_fb_gains));
send_reply(&reply, 2+sizeof(stage_fb_gains));
break;
case CMD_SET_STAGE_GAINS:
memcpy(stage_fb_gains, cmd->set_stage_gains, sizeof(stage_fb_gains));
send_ack();
break;
case CMD_GET_STAGE_SETPOINT:
memcpy(reply.data, stage_fb_setpoint, sizeof(stage_fb_setpoint));
send_reply(&reply, 2+sizeof(stage_fb_setpoint));
break;
case CMD_SET_STAGE_SETPOINT:
memcpy(stage_fb_setpoint, cmd->set_stage_setpoint, sizeof(stage_fb_setpoint));
send_ack();
break;
case CMD_GET_PSD_GAINS:
memcpy(reply.data, psd_fb_gains, sizeof(psd_fb_gains));
send_reply(&reply, 2+sizeof(psd_fb_gains));
break;
case CMD_SET_PSD_GAINS:
memcpy(psd_fb_gains, cmd->set_psd_gains, sizeof(psd_fb_gains));
send_ack();
break;
case CMD_GET_PSD_SETPOINT:
memcpy(reply.data, psd_fb_setpoint, sizeof(psd_fb_setpoint));
send_reply(&reply, 2+sizeof(psd_fb_setpoint));
break;
case CMD_SET_PSD_SETPOINT:
memcpy(psd_fb_setpoint, cmd->set_psd_setpoint, sizeof(psd_fb_setpoint));
send_ack();
break;
case CMD_GET_MAX_ERROR:
memcpy(reply.data, &max_error, sizeof(max_error));
send_reply(&reply, 2+sizeof(max_error));
break;
case CMD_SET_MAX_ERROR:
max_error = cmd->set_max_error;
send_ack();
break;
case CMD_GET_OUTPUT_GAINS:
{
fixed16_t* tmp = (fixed16_t*) reply.data;
for (unsigned int i=0; i<STAGE_OUTPUTS; i++) {
*tmp = stage_outputs[i].p_gain; tmp++;
*tmp = stage_outputs[i].i_gain; tmp++;
}
send_reply(&reply, 2+2*STAGE_OUTPUTS*sizeof(fixed16_t));
break;
}
case CMD_SET_OUTPUT_GAINS:
{
for (unsigned int i=0; i<STAGE_OUTPUTS; i++) {
stage_outputs[i].p_gain = cmd->set_output_gains[i][0];
stage_outputs[i].i_gain = cmd->set_output_gains[i][1];
}
send_ack();
break;
}
case CMD_GET_OUTPUT_TAUS:
{
for (unsigned int i=0; i<STAGE_OUTPUTS; i++) {
reply.data[i] = pi_get_tau(&stage_outputs[i]);
}
send_reply(&reply, 2+3);
break;
}
case CMD_SET_OUTPUT_TAUS:
{
for (unsigned int i=0; i<STAGE_OUTPUTS; i++) {
pi_set_tau(&stage_outputs[i], cmd->set_output_taus[i]);
}
send_ack();
break;
}
case CMD_GET_ADC_FREQ:
reply.data32[0] = 0; //TODO adc_get_trigger_freq();
send_reply(&reply, 2+4);
break;
case CMD_SET_ADC_FREQ:
adc_set_trigger_freq(cmd->set_adc_freq);
send_ack();
break;
case CMD_GET_ADC_TRIGGER_MODE:
reply.data[0] = adc_get_trigger_mode();
send_reply(&reply, 2+1);
break;
case CMD_SET_ADC_TRIGGER_MODE:
adc_set_trigger_mode(cmd->set_adc_trigger_mode);
send_ack();
break;
case CMD_START_ADC_STREAM:
adc_streaming = true;
send_ack();
break;
case CMD_STOP_ADC_STREAM:
adc_streaming = false;
adc_flush();
send_ack();
break;
case CMD_FLUSH_ADC_STREAM:
if (adc_flush() == 0) {
send_ack();
} else {
send_nack();
}
break;
case CMD_GET_ADC_DECIMATION:
{
reply.data32[0] = adc_get_decimation();
send_reply(&reply, 2+4);
break;
}
case CMD_SET_ADC_DECIMATION:
if (adc_set_decimation(cmd->set_adc_decimation) == 0) {
send_ack();
} else {
send_nack();
}
break;
case CMD_GET_FEEDBACK_FREQ:
{
unsigned int freq = feedback_get_loop_freq();
reply.data32[0] = freq;
send_reply(&reply, 2+4);
break;
}
case CMD_SET_FEEDBACK_FREQ:
feedback_set_loop_freq(cmd->set_feedback_freq);
send_ack();
break;
case CMD_GET_FEEDBACK_MODE:
reply.data[0] = feedback_get_mode();
send_reply(&reply, 3);
break;
case CMD_SET_FEEDBACK_MODE:
feedback_set_mode(cmd->set_feedback_mode);
send_ack();
break;
case CMD_SET_RAW_POS:
if (feedback_set_position(cmd->set_raw_pos) == 0) {
send_ack();
} else {
send_nack();
}
break;
case CMD_CLEAR_PATH:
clear_path();
send_ack();
break;
case CMD_ENQUEUE_POINTS:
res = 0;
if (cmd->enqueue_points.npts > 0)
res = enqueue_points((uint16_t*) &cmd->enqueue_points.points, cmd->enqueue_points.npts);
if (res == -1 || res == 0) {
reply.data[0] = res != -1; // were the points added?
reply.data[1] = is_path_running();
send_reply(&reply, 4);
} else {
send_nack();
}
break;
case CMD_START_PATH:
if (start_path(cmd->start_path.freq, cmd->start_path.synchronous_adc) == 0) {
send_ack();
} else {
send_nack();
}
break;
case CMD_SET_EXCITATION:
{
struct set_excitation* const se = &cmd->set_excitation;
struct excitation_buffer* const exc = &excitations[se->channel];
if ((se->length + se->offset > MAX_EXCITATION_LENGTH)
|| (se->channel >= 3))
{
send_nack();
break;
}
exc->length = 0;
exc->offset = 0;
if (se->length != 0) {
memcpy(&exc->samples[se->offset],
&se->samples,
sizeof(uint16_t) * se->length);
}
exc->length = se->total_length;
send_ack();
break;
}
case CMD_GET_SEARCH_STEP:
memcpy(reply.data, search_fb_step,
sizeof(search_fb_step));
send_reply(&reply, 2+sizeof(search_fb_step));
break;
case CMD_SET_SEARCH_STEP:
memcpy(search_fb_step, cmd->set_search_step,
sizeof(search_fb_step));
send_ack();
break;
case CMD_GET_SEARCH_OBJ_GAINS:
memcpy(reply.data, search_obj_gains,
sizeof(search_obj_gains));
send_reply(&reply, 2+sizeof(search_obj_gains));
break;
case CMD_SET_SEARCH_OBJ_GAINS:
memcpy(search_obj_gains, cmd->set_search_obj_gains,
sizeof(search_obj_gains));
send_ack();
break;
case CMD_GET_SEARCH_OBJ_THRESH:
memcpy(reply.data, &search_obj_thresh,
sizeof(search_obj_thresh));
send_reply(&reply, 2+sizeof(search_obj_thresh));
break;
case CMD_SET_SEARCH_OBJ_THRESH:
search_obj_thresh = cmd->set_search_obj_thresh,
send_ack();
break;
case CMD_GET_COARSE_FB_PARAMS:
memcpy(reply.data, &coarse_fb_channels,
sizeof(coarse_fb_channels));
send_reply(&reply, 2+sizeof(coarse_fb_channels));
break;
case CMD_SET_COARSE_FB_PARAMS:
memcpy(coarse_fb_channels, cmd->set_coarse_fb_params,
sizeof(coarse_fb_channels));
send_ack();
break;
default:
send_nack();
return;
}
}
