Commit 57fa90c6 authored by Mariano Lizarraga's avatar Mariano Lizarraga

Merge branch 'experimental' of git@github.com:pixhawk/qgroundcontrol into mergeRemote

parents ce66c99f 39eabe5f
...@@ -422,9 +422,9 @@ contains(DEPENDENCIES_PRESENT, libfreenect) { ...@@ -422,9 +422,9 @@ contains(DEPENDENCIES_PRESENT, libfreenect) {
RESOURCES += mavground.qrc RESOURCES += mavground.qrc
# Include RT-LAB Library # Include RT-LAB Library
win32:exists(src/lib/opalrt/OpalApi.h):exists(C:\OPAL-RT\RT-LAB7.2.4\Common\bin) { win32:exists(src/lib/opalrt/OpalApi.h):exists(C:/OPAL-RT/RT-LAB7.2.4/Common/bin) {
message("Building support for Opal-RT") message("Building support for Opal-RT")
LIBS += -LC:\OPAL-RT\RT-LAB7.2.4\Common\bin \ LIBS += -LC:/OPAL-RT/RT-LAB7.2.4/Common/bin \
-lOpalApi -lOpalApi
INCLUDEPATH += src/lib/opalrt INCLUDEPATH += src/lib/opalrt
HEADERS += src/comm/OpalRT.h \ HEADERS += src/comm/OpalRT.h \
......
...@@ -18,7 +18,48 @@ ...@@ -18,7 +18,48 @@
<!--Parameters related to the controller block --> <!--Parameters related to the controller block -->
<Block name="Controller"> <Block name="Controller">
<!-- Aileron Control Parameters -->
<Parameter
SimulinkPath="avionics_src/sm_avionics/Attitude_Controller/PID_AIL/kp/"
SimulinkParameterName="Gain"
QGCParamID="AIL_KP"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Attitude_Controller/PID_AIL/ki/"
SimulinkParameterName="Gain"
QGCParamID="AIL_KI"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Attitude_Controller/PID_AIL/kd/"
SimulinkParameterName="Gain"
QGCParamID="AIL_KD"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Attitude_Controller/PID_AIL/Angle_Norm/Max_Min_Angle/"
SimulinkParameterName="Constant Value"
QGCParamID="ANG_AIL_MAX"
/>
<!-- Elevator Control Parameters -->
<Parameter
SimulinkPath="avionics_src/sm_avionics/Attitude_Controller/PID_ELE/kp/"
SimulinkParameterName="Gain"
QGCParamID="ELE_KP"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Attitude_Controller/PID_ELE/ki/"
SimulinkParameterName="Gain"
QGCParamID="ELE_KI"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Attitude_Controller/PID_ELE/kd/"
SimulinkParameterName="Gain"
QGCParamID="ELE_KD"
/>
<Parameter
SimulinkPath="avionics_src/sm_avionics/Attitude_Controller/PID_ELE/Angle_Norm/Max_Min_Angle/"
SimulinkParameterName="Constant Value"
QGCParamID="ANG_ELE_MAX"
/>
</Block> </Block>
<!-- Paremters for the Pilot Input/Raw RC block --> <!-- Paremters for the Pilot Input/Raw RC block -->
...@@ -27,7 +68,7 @@ ...@@ -27,7 +68,7 @@
<Parameter <Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/AileronInput/" SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/AileronInput/"
SimulinkParameterName="Setpoint0" SimulinkParameterName="Setpoint0"
QGCParamID="AIL_RIGHT_IN" QGCParamID="AIL_LEFT_IN"
/> />
<Parameter <Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/AileronInput/" SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/AileronInput/"
...@@ -37,7 +78,7 @@ ...@@ -37,7 +78,7 @@
<Parameter <Parameter
SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/AileronInput/" SimulinkPath="avionics_src/sm_avionics/Pilot_Inputs/AileronInput/"
SimulinkParameterName="Setpoint2" SimulinkParameterName="Setpoint2"
QGCParamID="AIL_LEFT_IN" QGCParamID="AIL_RIGHT_IN"
/> />
<!-- Settings for Elevator Servo --> <!-- Settings for Elevator Servo -->
<Parameter <Parameter
...@@ -143,45 +184,44 @@ ...@@ -143,45 +184,44 @@
<!-- Settings for Aileron Servo --> <!-- Settings for Aileron Servo -->
<Parameter <Parameter
SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/AileronOutput/" SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/AileronOutput/"
SimulinkParameterName="Low" SimulinkParameterName="Setpoint0"
QGCParamID="AIL_LOW_OUT" QGCParamID="AIL_LEFT_OUT"
/> />
<Parameter <Parameter
SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/AileronOutput/" SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/AileronOutput/"
SimulinkParameterName="Center" SimulinkParameterName="Setpoint1"
QGCParamID="AIL_CENTER_OUT" QGCParamID="AIL_CENTER_OUT"
/> />
<Parameter <Parameter
SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/AileronOutput/" SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/AileronOutput/"
SimulinkParameterName="High" SimulinkParameterName="Setpoint2"
QGCParamID="AIL_HIGH_OUT" QGCParamID="AIL_RIGHT_OUT"
/> />
<Parameter <Parameter
SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/AileronOutput/" SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/AileronMix/Controller_Mix/"
SimulinkParameterName="Reverse" SimulinkParameterName="Constant Value"
QGCParamID="AIL_REV_OUT" QGCParamID="MIX_AIL"
/> />
<!-- Settings for Elevator Servo --> <!-- Settings for Elevator Servo -->
<Parameter <Parameter
SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/ElevatorOutput/" SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/ElevatorOutput/"
SimulinkParameterName="Low" SimulinkParameterName="Setpoint0"
QGCParamID="ELE_LOW_OUT" QGCParamID="ELE_DOWN_OUT"
/> />
<Parameter <Parameter
SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/ElevatorOutput/" SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/ElevatorOutput/"
SimulinkParameterName="Center" SimulinkParameterName="Setpoint1"
QGCParamID="ELE_CENTER_OUT" QGCParamID="ELE_CENTER_OUT"
/> />
<Parameter <Parameter
SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/ElevatorOutput/" SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/ElevatorOutput/"
SimulinkParameterName="High" SimulinkParameterName="Setpoint2"
QGCParamID="ELE_HIGH_OUT" QGCParamID="ELE_UP_OUT"
/> />
<Parameter <Parameter
SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/ElevatorOutput/" SimulinkPath="avionics_src/sm_avionics/Servo_Outputs/ElevatorMix/Controller_Mix/"
SimulinkParameterName="Reverse" SimulinkParameterName="Constant Value"
QGCParamID="ELE_REV_OUT" QGCParamID="MIX_ELE"
/> />
</Block> </Block>
</ParameterList> </ParameterList>
...@@ -41,7 +41,9 @@ OpalLink::OpalLink() : ...@@ -41,7 +41,9 @@ OpalLink::OpalLink() :
componentID(1), componentID(1),
params(NULL), params(NULL),
opalInstID(101), opalInstID(101),
sendRCValues(false) sendRCValues(false),
sendRawController(false),
sendPosition(false)
{ {
start(QThread::LowPriority); start(QThread::LowPriority);
...@@ -134,13 +136,13 @@ void OpalLink::writeBytes(const char *bytes, qint64 length) ...@@ -134,13 +136,13 @@ void OpalLink::writeBytes(const char *bytes, qint64 length)
} }
} }
break; break;
case MAVLINK_MSG_ID_REQUEST_RC_CHANNELS: // case MAVLINK_MSG_ID_REQUEST_RC_CHANNELS:
{ // {
mavlink_request_rc_channels_t rc; // mavlink_request_rc_channels_t rc;
mavlink_msg_request_rc_channels_decode(&msg, &rc); // mavlink_msg_request_rc_channels_decode(&msg, &rc);
this->sendRCValues = static_cast<bool>(rc.enabled); // this->sendRCValues = static_cast<bool>(rc.enabled);
} // }
break; // break;
#ifdef MAVLINK_ENABLED_UALBERTA_MESSAGES #ifdef MAVLINK_ENABLED_UALBERTA_MESSAGES
case MAVLINK_MSG_ID_RADIO_CALIBRATION: case MAVLINK_MSG_ID_RADIO_CALIBRATION:
{ {
...@@ -157,17 +159,29 @@ void OpalLink::writeBytes(const char *bytes, qint64 length) ...@@ -157,17 +159,29 @@ void OpalLink::writeBytes(const char *bytes, qint64 length)
/* AILERON SERVO */ /* AILERON SERVO */
if (params->contains(OpalRT::SERVO_INPUTS, "AIL_RIGHT_IN")) 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])); 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")) 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])); 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")) 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])); 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 */ /* ELEVATOR SERVO */
if (params->contains(OpalRT::SERVO_INPUTS, "ELE_DOWN_IN")) 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])); 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")) 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])); 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")) 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])); 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 */ /* THROTTLE SERVO */
if (params->contains(OpalRT::SERVO_INPUTS, "THR_SET0_IN")) 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])); params->getParameter(OpalRT::SERVO_INPUTS, "THR_SET0_IN").setValue(((radio.throttle[0]>900 /*in us?*/)?radio.throttle[0]/1000:radio.throttle[0]));
...@@ -205,11 +219,50 @@ void OpalLink::writeBytes(const char *bytes, qint64 length) ...@@ -205,11 +219,50 @@ void OpalLink::writeBytes(const char *bytes, qint64 length)
} }
break; break;
#endif #endif
#ifdef MAVLINK_ENABLED_PIXHAWK
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 0: // All data types
break;
case 1: // Raw Sensor Data
break;
case 2: // extended system status
break;
case 3: // rc channel data
sendRCValues = (stream.start_stop == 1?true:false);
break;
case 4: // raw controller
if (stream.start_stop == 1)
sendRawController = true;
else
sendRawController = false;
break;
case 5: // raw sensor fusion
break;
case 6: // position
sendPosition = (stream.start_stop == 1?true:false);
break;
case 7: // extra 1
break;
case 8: // extra 2
break;
case 9: // extra 3
break;
default:
qDebug() << __FILE__ << __LINE__ << "Received Unknown Data Strem Request with ID" << stream.req_stream_id;
}
}
break;
default: default:
{ {
qDebug() << "OpalLink::writeBytes(): Unknown mavlink packet"; qDebug() << "OpalLink::writeBytes(): Unknown mavlink packet";
} }
} }
#endif
} }
} }
...@@ -282,17 +335,19 @@ void OpalLink::getSignals() ...@@ -282,17 +335,19 @@ void OpalLink::getSignals()
if (returnVal == EOK ) if (returnVal == EOK )
{ {
/* Send position info to qgroundcontrol */ /* Send position info to qgroundcontrol */
mavlink_message_t local_position; if (sendPosition)
mavlink_msg_local_position_pack(systemID, componentID, &local_position, {
(*timestep)*1000000, mavlink_message_t local_position;
values[OpalRT::X_POS], mavlink_msg_local_position_pack(systemID, componentID, &local_position,
values[OpalRT::Y_POS], (*timestep)*1000000,
values[OpalRT::Z_POS], values[OpalRT::X_POS],
values[OpalRT::X_VEL], values[OpalRT::Y_POS],
values[OpalRT::Y_VEL], values[OpalRT::Z_POS],
values[OpalRT::Z_VEL]); values[OpalRT::X_VEL],
receiveMessage(local_position); values[OpalRT::Y_VEL],
values[OpalRT::Z_VEL]);
receiveMessage(local_position);
}
/* send attitude info to qgroundcontrol */ /* send attitude info to qgroundcontrol */
mavlink_message_t attitude; mavlink_message_t attitude;
mavlink_msg_attitude_pack(systemID, componentID, &attitude, mavlink_msg_attitude_pack(systemID, componentID, &attitude,
...@@ -336,6 +391,18 @@ void OpalLink::getSignals() ...@@ -336,6 +391,18 @@ void OpalLink::getSignals()
); );
receiveMessage(rc); 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 else if (returnVal != EAGAIN) // if returnVal == EAGAIN => data just wasn't ready
{ {
...@@ -408,10 +475,12 @@ uint8_t OpalLink::rescaleNorm(double norm, int ch) ...@@ -408,10 +475,12 @@ uint8_t OpalLink::rescaleNorm(double norm, int ch)
return static_cast<uint8_t>(norm*255); return static_cast<uint8_t>(norm*255);
break; break;
} }
} }
int8_t OpalLink::rescaleControllerOutput(double raw)
{
return static_cast<int8_t>((raw>=0?raw*127:raw*128));
}
bool OpalLink::connect() bool OpalLink::connect()
{ {
......
...@@ -153,8 +153,11 @@ protected: ...@@ -153,8 +153,11 @@ protected:
uint16_t duty2PulseMicros(double duty); uint16_t duty2PulseMicros(double duty);
uint8_t rescaleNorm(double norm, int ch); uint8_t rescaleNorm(double norm, int ch);
int8_t rescaleControllerOutput(double raw);
bool sendRCValues; bool sendRCValues;
bool sendRawController;
bool sendPosition;
}; };
#endif // OPALLINK_H #endif // OPALLINK_H
...@@ -41,7 +41,7 @@ namespace OpalRT ...@@ -41,7 +41,7 @@ namespace OpalRT
Configuration info for the model Configuration info for the model
*/ */
const unsigned short NUM_OUTPUT_SIGNALS=42; const unsigned short NUM_OUTPUT_SIGNALS=48;
/* ------------------------------ Outputs ------------------------------ /* ------------------------------ Outputs ------------------------------
* *
...@@ -103,7 +103,13 @@ namespace OpalRT ...@@ -103,7 +103,13 @@ namespace OpalRT
NORM_CHANNEL_7, NORM_CHANNEL_7,
NORM_CHANNEL_8, NORM_CHANNEL_8,
CONTROLLER_AILERON, 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 /** Component IDs of the parameters. Currently they are all 1 becuase there is no advantage
...@@ -112,9 +118,9 @@ namespace OpalRT ...@@ -112,9 +118,9 @@ namespace OpalRT
*/ */
enum SubsystemIds enum SubsystemIds
{ {
NAV_ID = 1, NAV = 1,
LOG_ID, LOG,
CONTROLLER_ID, CONTROLLER,
SERVO_OUTPUTS, SERVO_OUTPUTS,
SERVO_INPUTS SERVO_INPUTS
}; };
......
...@@ -61,8 +61,8 @@ namespace OpalRT ...@@ -61,8 +61,8 @@ namespace OpalRT
const QGCParamID& getParamID() const {return *paramID;} const QGCParamID& getParamID() const {return *paramID;}
void setOpalID(unsigned short opalID) {this->opalID = opalID;} void setOpalID(unsigned short opalID) {this->opalID = opalID;}
const QString& getSimulinkPath() {return *simulinkPath;} const QString& getSimulinkPath() const {return *simulinkPath;}
const QString& getSimulinkName() {return *simulinkName;} const QString& getSimulinkName() const {return *simulinkName;}
uint8_t getComponentID() const {return componentID;} uint8_t getComponentID() const {return componentID;}
float getValue(); float getValue();
void setValue(float value); void setValue(float value);
......
...@@ -45,9 +45,15 @@ ParameterList::ParameterList() ...@@ -45,9 +45,15 @@ ParameterList::ParameterList()
reqdServoParams->append("AIL_RIGHT_IN"); reqdServoParams->append("AIL_RIGHT_IN");
reqdServoParams->append("AIL_CENTER_IN"); reqdServoParams->append("AIL_CENTER_IN");
reqdServoParams->append("AIL_LEFT_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_DOWN_IN");
reqdServoParams->append("ELE_CENTER_IN"); reqdServoParams->append("ELE_CENTER_IN");
reqdServoParams->append("ELE_UP_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_LEFT_IN");
reqdServoParams->append("RUD_CENTER_IN"); reqdServoParams->append("RUD_CENTER_IN");
reqdServoParams->append("RUD_RIGHT_IN"); reqdServoParams->append("RUD_RIGHT_IN");
...@@ -285,6 +291,15 @@ bool ParameterList::read(QIODevice *device) ...@@ -285,6 +291,15 @@ bool ParameterList::read(QIODevice *device)
child = child.nextSiblingElement("Block"); child = child.nextSiblingElement("Block");
} }
if (!reqdServoParams->empty())
{
qDebug() << __FILE__ << __LINE__ << "Missing the following required servo parameters";
foreach(QString s, *reqdServoParams)
{
qDebug() << s;
}
}
delete paramConfig; delete paramConfig;
return true; return true;
} }
...@@ -297,9 +312,9 @@ void ParameterList::parseBlock(const QDomElement &block) ...@@ -297,9 +312,9 @@ void ParameterList::parseBlock(const QDomElement &block)
Parameter *p; Parameter *p;
SubsystemIds id; SubsystemIds id;
if (block.attribute("name") == "Navigation") if (block.attribute("name") == "Navigation")
id = OpalRT::NAV_ID; id = OpalRT::NAV;
else if (block.attribute("name") == "Controller") else if (block.attribute("name") == "Controller")
id = OpalRT::CONTROLLER_ID; id = OpalRT::CONTROLLER;
else if (block.attribute("name") == "ServoOutputs") else if (block.attribute("name") == "ServoOutputs")
id = OpalRT::SERVO_OUTPUTS; id = OpalRT::SERVO_OUTPUTS;
else if (block.attribute("name") == "ServoInputs") else if (block.attribute("name") == "ServoInputs")
...@@ -332,13 +347,6 @@ void ParameterList::parseBlock(const QDomElement &block) ...@@ -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;
}
}
} }
...@@ -53,11 +53,11 @@ namespace OpalRT ...@@ -53,11 +53,11 @@ namespace OpalRT
const_iterator(const const_iterator& other); const_iterator(const const_iterator& other);
const_iterator& operator+=(int i) {index += i; return *this;} const_iterator& operator+=(int i) {index += i; return *this;}
bool operator<(const const_iterator& other) {return (this->paramList == other.paramList) bool operator<(const const_iterator& other) const {return (this->paramList == other.paramList)
&&(this->index<other.index);} &&(this->index<other.index);}
bool operator==(const const_iterator& other) {return (this->paramList == other.paramList) bool operator==(const const_iterator& other) const {return (this->paramList == other.paramList)
&&(this->index==other.index);} &&(this->index==other.index);}
bool operator!=(const const_iterator& other) {return !((*this) == other);} bool operator!=(const const_iterator& other) const {return !((*this) == other);}
const Parameter& operator*() const {return paramList[index];} const Parameter& operator*() const {return paramList[index];}
const Parameter* operator->() const {return &paramList[index];} const Parameter* operator->() const {return &paramList[index];}
......
...@@ -721,7 +721,8 @@ void HUD::paintHUD() ...@@ -721,7 +721,8 @@ void HUD::paintHUD()
painter.drawRoundedRect(compassRect, 2, 2); painter.drawRoundedRect(compassRect, 2, 2);
QString yawAngle; QString yawAngle;
const float yawDeg = ((values.value("yaw", 0.0f)/M_PI)*180.0f)+180.f; // const float yawDeg = ((values.value("yaw", 0.0f)/M_PI)*180.0f)+180.f;
const float yawDeg = ((values.value("yaw", 0.0f)/M_PI)*180.0f);
//qDebug() << "YAW: " << yawDeg; //qDebug() << "YAW: " << yawDeg;
yawAngle.sprintf("%03d", (int)yawDeg); yawAngle.sprintf("%03d", (int)yawDeg);
paintText(yawAngle, defaultColor, 3.5f, -3.7f, compassY+ 0.9f, &painter); paintText(yawAngle, defaultColor, 3.5f, -3.7f, compassY+ 0.9f, &painter);
......
...@@ -48,18 +48,14 @@ const float* RadioCalibrationData::operator [](int i) const ...@@ -48,18 +48,14 @@ const float* RadioCalibrationData::operator [](int i) const
return NULL; return NULL;
} }
const QVector<float>& RadioCalibrationData::operator ()(int i) const const QVector<float>& RadioCalibrationData::operator ()(const int i) const throw(std::out_of_range)
{ {
if (i < data->size()) if ((i < data->size()) && (i >=0))
{ {
return (*data)[i]; return (*data)[i];
} }
// FIXME Bryan throw std::out_of_range("Invalid channel index");
// FIXME James
// This is not good. If it is ever used after being returned it will cause a crash
// return QVector<float>();
} }
QString RadioCalibrationData::toString(RadioElement element) const QString RadioCalibrationData::toString(RadioElement element) const
......
...@@ -34,6 +34,7 @@ This file is part of the QGROUNDCONTROL project ...@@ -34,6 +34,7 @@ This file is part of the QGROUNDCONTROL project
#include <QDebug> #include <QDebug>
#include <QVector> #include <QVector>
#include <QString> #include <QString>
#include <stdexcept>
/** /**
...@@ -66,7 +67,7 @@ public: ...@@ -66,7 +67,7 @@ public:
}; };
const float* operator[](int i) const; const float* operator[](int i) const;
const QVector<float>& operator()(int i) const; const QVector<float>& operator()(int i) const throw(std::out_of_range);
void set(int element, int index, float value) {(*data)[element][index] = value;} void set(int element, int index, float value) {(*data)[element][index] = value;}
public slots: public slots:
......
...@@ -52,38 +52,38 @@ RadioCalibrationWindow::RadioCalibrationWindow(QWidget *parent) : ...@@ -52,38 +52,38 @@ RadioCalibrationWindow::RadioCalibrationWindow(QWidget *parent) :
setUASId(0); setUASId(0);
} }
void RadioCalibrationWindow::setChannelRaw(int ch, float raw) //void RadioCalibrationWindow::setChannelRaw(int ch, float raw)
{ //{
/** this expects a particular channel to function mapping // /** this expects a particular channel to function mapping
\todo allow run-time channel mapping // \todo allow run-time channel mapping
*/ // */
switch (ch) // switch (ch)
{ // {
case 0: // case 0:
aileron->channelChanged(raw); // aileron->channelChanged(raw);
break; // break;
case 1: // case 1:
elevator->channelChanged(raw); // elevator->channelChanged(raw);
break; // break;
case 2: // case 2:
throttle->channelChanged(raw); // throttle->channelChanged(raw);
break; // break;
case 3: // case 3:
rudder->channelChanged(raw); // rudder->channelChanged(raw);
break; // break;
case 4: // case 4:
gyro->channelChanged(raw); // gyro->channelChanged(raw);
break; // break;
case 5: // case 5:
pitch->channelChanged(raw); // pitch->channelChanged(raw);
break; // break;
} // }
} //}
void RadioCalibrationWindow::setChannelScaled(int ch, float normalized) //void RadioCalibrationWindow::setChannelScaled(int ch, float normalized)
{ //{
// FIXME James // FIXME James
// FIXME Bryan // FIXME Bryan
...@@ -113,9 +113,9 @@ void RadioCalibrationWindow::setChannelScaled(int ch, float normalized) ...@@ -113,9 +113,9 @@ void RadioCalibrationWindow::setChannelScaled(int ch, float normalized)
// } // }
} //}
void RadioCalibrationWindow::setChannel(int ch, float raw, float normalized) void RadioCalibrationWindow::setChannel(int ch, float raw)
{ {
/** this expects a particular channel to function mapping /** this expects a particular channel to function mapping
\todo allow run-time channel mapping \todo allow run-time channel mapping
......
...@@ -66,9 +66,10 @@ public: ...@@ -66,9 +66,10 @@ public:
explicit RadioCalibrationWindow(QWidget *parent = 0); explicit RadioCalibrationWindow(QWidget *parent = 0);
public slots: public slots:
void setChannel(int ch, float raw, float normalized); void setChannel(int ch, float raw);
void setChannelRaw(int ch, float raw); // @todo remove these functions if they are not needed - were added by lm on dec 14, 2010
void setChannelScaled(int ch, float normalized); // void setChannelRaw(int ch, float raw);
// void setChannelScaled(int ch, float normalized);
void loadFile(); void loadFile();
void saveFile(); void saveFile();
void send(); void send();
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment