diff --git a/settings/ParameterList.xml b/settings/ParameterList.xml
index b1cb8f47e903a5555a305b69ee54ab5832cb31b9..4a84dc1f7c6ed0a144bcb6ac58bfba84496a64bf 100644
--- a/settings/ParameterList.xml
+++ b/settings/ParameterList.xml
@@ -18,7 +18,38 @@
-
+
+
+
+
+
+
+
+
@@ -27,7 +58,7 @@
-
diff --git a/src/comm/OpalLink.cc b/src/comm/OpalLink.cc
index 1c29ef43cd839676afc434f8d1525f01e51cc4c7..dc2039ec19e0a4c4859baf34b1f3883299687049 100644
--- a/src/comm/OpalLink.cc
+++ b/src/comm/OpalLink.cc
@@ -157,17 +157,29 @@ void OpalLink::writeBytes(const char *bytes, qint64 length)
/* AILERON SERVO */
if (params->contains(OpalRT::SERVO_INPUTS, "AIL_RIGHT_IN"))
params->getParameter(OpalRT::SERVO_INPUTS, "AIL_RIGHT_IN").setValue(((radio.aileron[0]>900 /*in us?*/)?radio.aileron[0]/1000:radio.aileron[0]));
+ if (params->contains(OpalRT::SERVO_OUTPUTS, "AIL_RIGHT_OUT"))
+ params->getParameter(OpalRT::SERVO_OUTPUTS, "AIL_RIGHT_OUT").setValue(((radio.aileron[0]>900 /*in us?*/)?radio.aileron[0]/1000:radio.aileron[0]));
if (params->contains(OpalRT::SERVO_INPUTS, "AIL_CENTER_IN"))
params->getParameter(OpalRT::SERVO_INPUTS, "AIL_CENTER_IN").setValue(((radio.aileron[1]>900 /*in us?*/)?radio.aileron[1]/1000:radio.aileron[1]));
+ if (params->contains(OpalRT::SERVO_OUTPUTS, "AIL_CENTER_OUT"))
+ params->getParameter(OpalRT::SERVO_OUTPUTS, "AIL_CENTER_OUT").setValue(((radio.aileron[1]>900 /*in us?*/)?radio.aileron[1]/1000:radio.aileron[1]));
if (params->contains(OpalRT::SERVO_INPUTS, "AIL_LEFT_IN"))
params->getParameter(OpalRT::SERVO_INPUTS, "AIL_LEFT_IN").setValue(((radio.aileron[2]>900 /*in us?*/)?radio.aileron[2]/1000:radio.aileron[2]));
+ if (params->contains(OpalRT::SERVO_OUTPUTS, "AIL_LEFT_OUT"))
+ params->getParameter(OpalRT::SERVO_OUTPUTS, "AIL_LEFT_OUT").setValue(((radio.aileron[2]>900 /*in us?*/)?radio.aileron[2]/1000:radio.aileron[2]));
/* ELEVATOR SERVO */
if (params->contains(OpalRT::SERVO_INPUTS, "ELE_DOWN_IN"))
params->getParameter(OpalRT::SERVO_INPUTS, "ELE_DOWN_IN").setValue(((radio.elevator[0]>900 /*in us?*/)?radio.elevator[0]/1000:radio.elevator[0]));
+ if (params->contains(OpalRT::SERVO_OUTPUTS, "ELE_DOWN_OUT"))
+ params->getParameter(OpalRT::SERVO_OUTPUTS, "ELE_DOWN_OUT").setValue(((radio.elevator[0]>900 /*in us?*/)?radio.elevator[0]/1000:radio.elevator[0]));
if (params->contains(OpalRT::SERVO_INPUTS, "ELE_CENTER_IN"))
params->getParameter(OpalRT::SERVO_INPUTS, "ELE_CENTER_IN").setValue(((radio.elevator[1]>900 /*in us?*/)?radio.elevator[1]/1000:radio.elevator[1]));
+ if (params->contains(OpalRT::SERVO_OUTPUTS, "ELE_CENTER_OUT"))
+ params->getParameter(OpalRT::SERVO_OUTPUTS, "ELE_CENTER_OUT").setValue(((radio.elevator[1]>900 /*in us?*/)?radio.elevator[1]/1000:radio.elevator[1]));
if (params->contains(OpalRT::SERVO_INPUTS, "ELE_UP_IN"))
params->getParameter(OpalRT::SERVO_INPUTS, "ELE_UP_IN").setValue(((radio.elevator[2]>900 /*in us?*/)?radio.elevator[2]/1000:radio.elevator[2]));
+ if (params->contains(OpalRT::SERVO_OUTPUTS, "ELE_UP_OUT"))
+ params->getParameter(OpalRT::SERVO_OUTPUTS, "ELE_UP_OUT").setValue(((radio.elevator[2]>900 /*in us?*/)?radio.elevator[2]/1000:radio.elevator[2]));
/* THROTTLE SERVO */
if (params->contains(OpalRT::SERVO_INPUTS, "THR_SET0_IN"))
params->getParameter(OpalRT::SERVO_INPUTS, "THR_SET0_IN").setValue(((radio.throttle[0]>900 /*in us?*/)?radio.throttle[0]/1000:radio.throttle[0]));
@@ -205,6 +217,23 @@ void OpalLink::writeBytes(const char *bytes, qint64 length)
}
break;
#endif
+ case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
+ {
+ mavlink_request_data_stream_t stream;
+ mavlink_msg_request_data_stream_decode(&msg, &stream);
+ switch (stream.req_stream_id)
+ {
+ case 4:
+ if (stream.start_stop == 1)
+ sendRawController = true;
+ else
+ sendRawController = false;
+ break;
+ default:
+ qDebug() << __FILE__ << __LINE__ << "Received Unknown Data Strem Request with ID" << stream.req_stream_id;
+ }
+ }
+ break;
default:
{
qDebug() << "OpalLink::writeBytes(): Unknown mavlink packet";
@@ -344,6 +373,18 @@ void OpalLink::getSignals()
);
receiveMessage(rc);
}
+ if (sendRawController)
+ {
+ mavlink_message_t rawController;
+ mavlink_msg_attitude_controller_output_pack(systemID, componentID, &rawController,
+ 1,
+ rescaleControllerOutput(values[OpalRT::CONTROLLER_AILERON]),
+ rescaleControllerOutput(values[OpalRT::CONTROLLER_ELEVATOR]),
+ 0, // yaw not used
+ 0 // thrust not used
+ );
+ receiveMessage(rawController);
+ }
}
else if (returnVal != EAGAIN) // if returnVal == EAGAIN => data just wasn't ready
{
@@ -416,10 +457,12 @@ uint8_t OpalLink::rescaleNorm(double norm, int ch)
return static_cast(norm*255);
break;
}
-
-
}
+int8_t OpalLink::rescaleControllerOutput(double raw)
+{
+ return static_cast((raw>=0?raw*127:raw*128));
+}
bool OpalLink::connect()
{
diff --git a/src/comm/OpalLink.h b/src/comm/OpalLink.h
index 6a9d43d1ebaa79965986e8a4e81d354015aa7bd1..d257aaaf225e399c9a2ca2f7b28ed910f8c0feef 100644
--- a/src/comm/OpalLink.h
+++ b/src/comm/OpalLink.h
@@ -153,8 +153,10 @@ protected:
uint16_t duty2PulseMicros(double duty);
uint8_t rescaleNorm(double norm, int ch);
+ int8_t rescaleControllerOutput(double raw);
bool sendRCValues;
+ bool sendRawController;
};
#endif // OPALLINK_H
diff --git a/src/comm/OpalRT.h b/src/comm/OpalRT.h
index 6e3d7074d85251ea0e970b15f862d6b4ee3b9c59..70bc5b4795700724c4723858969ed6052d4ae056 100644
--- a/src/comm/OpalRT.h
+++ b/src/comm/OpalRT.h
@@ -41,7 +41,7 @@ namespace OpalRT
Configuration info for the model
*/
- const unsigned short NUM_OUTPUT_SIGNALS=42;
+ const unsigned short NUM_OUTPUT_SIGNALS=48;
/* ------------------------------ Outputs ------------------------------
*
@@ -103,7 +103,13 @@ namespace OpalRT
NORM_CHANNEL_7,
NORM_CHANNEL_8,
CONTROLLER_AILERON,
- CONTROLLER_ELEVATOR
+ CONTROLLER_ELEVATOR,
+ AIL_POUT,
+ AIL_IOUT,
+ AIL_DOUT,
+ ELE_POUT,
+ ELE_IOUT,
+ ELE_DOUT
};
/** Component IDs of the parameters. Currently they are all 1 becuase there is no advantage
@@ -112,9 +118,9 @@ namespace OpalRT
*/
enum SubsystemIds
{
- NAV_ID = 1,
- LOG_ID,
- CONTROLLER_ID,
+ NAV = 1,
+ LOG,
+ CONTROLLER,
SERVO_OUTPUTS,
SERVO_INPUTS
};
diff --git a/src/comm/ParameterList.cc b/src/comm/ParameterList.cc
index 682489ee54464aaf4e47b0efc94d9f8bfabb7d4e..809db6f28c1038decc2e6810fee1e85251bdafba 100644
--- a/src/comm/ParameterList.cc
+++ b/src/comm/ParameterList.cc
@@ -45,9 +45,15 @@ ParameterList::ParameterList()
reqdServoParams->append("AIL_RIGHT_IN");
reqdServoParams->append("AIL_CENTER_IN");
reqdServoParams->append("AIL_LEFT_IN");
+ reqdServoParams->append("AIL_RIGHT_OUT");
+ reqdServoParams->append("AIL_CENTER_OUT");
+ reqdServoParams->append("AIL_LEFT_OUT");
reqdServoParams->append("ELE_DOWN_IN");
reqdServoParams->append("ELE_CENTER_IN");
reqdServoParams->append("ELE_UP_IN");
+ reqdServoParams->append("ELE_DOWN_OUT");
+ reqdServoParams->append("ELE_CENTER_OUT");
+ reqdServoParams->append("ELE_UP_OUT");
reqdServoParams->append("RUD_LEFT_IN");
reqdServoParams->append("RUD_CENTER_IN");
reqdServoParams->append("RUD_RIGHT_IN");
@@ -285,6 +291,15 @@ bool ParameterList::read(QIODevice *device)
child = child.nextSiblingElement("Block");
}
+ if (!reqdServoParams->empty())
+ {
+ qDebug() << __FILE__ << __LINE__ << "Missing the following required servo parameters";
+ foreach(QString s, *reqdServoParams)
+ {
+ qDebug() << s;
+ }
+ }
+
delete paramConfig;
return true;
}
@@ -297,9 +312,9 @@ void ParameterList::parseBlock(const QDomElement &block)
Parameter *p;
SubsystemIds id;
if (block.attribute("name") == "Navigation")
- id = OpalRT::NAV_ID;
+ id = OpalRT::NAV;
else if (block.attribute("name") == "Controller")
- id = OpalRT::CONTROLLER_ID;
+ id = OpalRT::CONTROLLER;
else if (block.attribute("name") == "ServoOutputs")
id = OpalRT::SERVO_OUTPUTS;
else if (block.attribute("name") == "ServoInputs")
@@ -332,13 +347,6 @@ void ParameterList::parseBlock(const QDomElement &block)
}
}
- if (!reqdServoParams->empty())
- {
- qDebug() << __FILE__ << __LINE__ << "Missing the following required servo parameters";
- foreach(QString s, *reqdServoParams)
- {
- qDebug() << s;
- }
- }
+
}