ballist->setForceStabilisation(sm->force_stabilised);
ballist->setExternalForce(sm->ext_force);
ballist->setForcePath(sm->force_path.c_str());
- ballist->setXoffset(sm->x_offset);
- ballist->setYoffset(sm->y_offset);
- ballist->setZoffset(sm->z_offset);
+ ballist->setXoffset(_x_offset);
+ ballist->setYoffset(_y_offset);
+ ballist->setZoffset(_z_offset);
ballist->setPitchoffset(sm->pitch_offset->get_value());
ballist->setYawoffset(sm->yaw_offset->get_value());
ballist->setParentNodes(_selected_ac);
userpos.setLongitudeDeg(IC.lon);
userpos.setElevationFt(IC.alt);
- _x_offset = sm->x_offset;
- _y_offset = sm->y_offset;
- _z_offset = sm->z_offset;
+ if (sm->offsets_in_meter) {
+ _x_offset = -sm->x_offset->get_value() * SG_METER_TO_FEET;
+ _y_offset = sm->y_offset->get_value() * SG_METER_TO_FEET;
+ _z_offset = sm->z_offset->get_value() * SG_METER_TO_FEET;
+ }
+ else {
+ _x_offset = sm->x_offset->get_value();
+ _y_offset = sm->y_offset->get_value();
+ _z_offset = sm->z_offset->get_value();
+ }
setOffsetPos();
- double yaw_offset = sm->yaw_offset->get_value();
- double pitch_offset = sm->pitch_offset->get_value();
+ // Compute initial orientation using yaw and pitch offsets and parent's orientation
+ const double yaw_offset = sm->yaw_offset->get_value();
+ const double pitch_offset = sm->pitch_offset->get_value();
- // Compute azimuth and elevation given the yaw and pitch offsets
SGQuatd ic_quat = SGQuatd::fromYawPitchRollDeg(IC.azimuth, IC.elevation, IC.roll);
ic_quat *= SGQuatd::fromYawPitchRollDeg(yaw_offset, pitch_offset, 0.0);
- double ic_roll;
- ic_quat.getEulerDeg(IC.azimuth, IC.elevation, ic_roll);
-
- // Calculate the total speed north
- IC.total_speed_north = sm->speed * cos(IC.elevation * SG_DEGREES_TO_RADIANS)
- * cos(IC.azimuth * SG_DEGREES_TO_RADIANS) + IC.speed_north_fps;
-
- // Calculate the total speed east
- IC.total_speed_east = sm->speed * cos(IC.elevation * SG_DEGREES_TO_RADIANS)
- * sin(IC.azimuth * SG_DEGREES_TO_RADIANS) + IC.speed_east_fps;
-
- // Calculate the total speed down
- IC.total_speed_down = sm->speed * -sin(IC.elevation * SG_DEGREES_TO_RADIANS)
- + IC.speed_down_fps;
-
- // Re-calculate speed, elevation and azimuth
- IC.speed = sqrt(IC.total_speed_north * IC.total_speed_north
- + IC.total_speed_east * IC.total_speed_east
- + IC.total_speed_down * IC.total_speed_down);
+ // Calculate total speed using speeds of submodel and parent
+ SGVec3d total_speed = SGVec3d(IC.speed_north_fps, IC.speed_east_fps, IC.speed_down_fps);
+ total_speed += ic_quat.rotate(SGVec3d(sm->speed, 0, 0));
- cout << "sm speed: " << sm->speed << " IC speed: " << IC.speed << endl;
- cout << "az1: " << IC.azimuth << " el1: " << IC.elevation << endl;
+ IC.speed = length(total_speed);
// If speeds are low this calculation can become unreliable
if (IC.speed > 1) {
- IC.azimuth = atan2(IC.total_speed_east, IC.total_speed_north) * SG_RADIANS_TO_DEGREES;
+ const double total_speed_north = total_speed.x();
+ const double total_speed_east = total_speed.y();
+ const double total_speed_down = total_speed.z();
+
+ IC.azimuth = atan2(total_speed_east, total_speed_north) * SG_RADIANS_TO_DEGREES;
// Rationalize the output
if (IC.azimuth < 0)
else if (IC.azimuth >= 360)
IC.azimuth -= 360;
- IC.elevation = -atan(IC.total_speed_down / sqrt(IC.total_speed_north
- * IC.total_speed_north + IC.total_speed_east * IC.total_speed_east))
+ IC.elevation = -atan(total_speed_down / sqrt(total_speed_north
+ * total_speed_north + total_speed_east * total_speed_east))
* SG_RADIANS_TO_DEGREES;
}
-
- cout << "az2: " << IC.azimuth << " el2: " << IC.elevation << endl;
+ else {
+ double ic_roll;
+ ic_quat.getEulerDeg(IC.azimuth, IC.elevation, ic_roll);
+ }
}
void FGSubmodelMgr::updatelat(double lat)
sm->delay = entry_node->getDoubleValue("delay", 0.25);
sm->count = entry_node->getIntValue("count", 1);
sm->slaved = entry_node->getBoolValue("slaved", false);
- sm->x_offset = entry_node->getDoubleValue("x-offset", 0.0);
- sm->y_offset = entry_node->getDoubleValue("y-offset", 0.0);
- sm->z_offset = entry_node->getDoubleValue("z-offset", 0.0);
sm->drag_area = entry_node->getDoubleValue("eda", 0.034);
sm->life = entry_node->getDoubleValue("life", 900.0);
sm->buoyancy = entry_node->getDoubleValue("buoyancy", 0);
SGPropertyNode_ptr prop_root = fgGetNode("/", true);
SGPropertyNode n;
+ SGPropertyNode_ptr a, b;
+
+ // Offsets
+ a = entry_node->getNode("offsets", false);
+ sm->offsets_in_meter = (a != 0);
+
+ if (a) {
+ b = a->getNode("x-m");
+ sm->x_offset = new FGXMLAutopilot::InputValue(*prop_root, b ? *b : n);
- SGPropertyNode_ptr a = entry_node->getNode("yaw-offset");
- sm->yaw_offset = new FGXMLAutopilot::InputValue(*prop_root, a ? *a : n);
+ b = a->getNode("y-m");
+ sm->y_offset = new FGXMLAutopilot::InputValue(*prop_root, b ? *b : n);
- a = entry_node->getNode("pitch-offset");
- sm->pitch_offset = new FGXMLAutopilot::InputValue(*prop_root, a ? *a : n);
+ b = a->getNode("z-m");
+ sm->z_offset = new FGXMLAutopilot::InputValue(*prop_root, b ? *b : n);
+
+ b = a->getNode("heading-deg");
+ sm->yaw_offset = new FGXMLAutopilot::InputValue(*prop_root, b ? *b : n);
+
+ b = a->getNode("pitch-deg");
+ sm->pitch_offset = new FGXMLAutopilot::InputValue(*prop_root, b ? *b : n);
+ }
+ else {
+ bool old = false;
+
+ b = entry_node->getNode("x-offset");
+ sm->x_offset = new FGXMLAutopilot::InputValue(*prop_root, b ? *b : n);
+ if (b) old = true;
+
+ b = entry_node->getNode("y-offset");
+ sm->y_offset = new FGXMLAutopilot::InputValue(*prop_root, b ? *b : n);
+ if (b) old = true;
+
+ b = entry_node->getNode("z-offset");
+ sm->z_offset = new FGXMLAutopilot::InputValue(*prop_root, b ? *b : n);
+ if (b) old = true;
+
+ b = entry_node->getNode("yaw-offset");
+ sm->yaw_offset = new FGXMLAutopilot::InputValue(*prop_root, b ? *b : n);
+ if (b) old = true;
+
+ b = entry_node->getNode("pitch-offset");
+ sm->pitch_offset = new FGXMLAutopilot::InputValue(*prop_root, b ? *b : n);
+ if (b) old = true;
+
+#if defined(ENABLE_DEV_WARNINGS)
+ if (old) {
+ SG_LOG(SG_AI, SG_WARN, "Submodels: <*-offset> is deprecated. Use <offsets> instead");
+ }
+#endif
+ }
// Randomness
a = entry_node->getNode("randomness", true);
- SGPropertyNode_ptr b;
// Maximum azimuth randomness error in degrees
b = a->getNode("azimuth");
projects / flightgear.git / blobdiff